GCompute - 計算三角形對的交點
GCompute - 計算三角形對的交點
本文是利用論文中的介紹進行實現的.
Moller T. A fast triangle-triangle intersection test[J]. Journal of Graphics Tools, 1997, 2(2): 25-30.
關於該論文的介紹參見:PaperRead - A fast triangle-triangle intersection test
基於該論文的相交測試實現,參見:PaperImpl - A fast triangle-triangle intersection test
三角形交點計算原理
論文介紹一文中分析如下:
直線L與三角形,相交段計算。假設我們需要計算三角形\(T_1\)和直線\(L\)之間的相交線段,假設\(V_0^1\)和\(V_2^1\)位於平面\(\pi_2\)的同一側,\(V_1^1\)位於平面的另一側。先將頂點投影到直線L上,如下:
\[p_{V_i^1} = D\cdot (V_i^1 - O) \]
論文中經過優化有,\(p_{V_i^1}\)的計算公式如下:
\[p_{V_i^1} = \left\{\begin{array}{rcl}V_{ix}^1, \ if |D_x| = max(|D_x|, |D_y|, |D_z|) \\V_{iy}^1, \ if |D_y| = max(|D_x|, |D_y|, |D_z|) \\V_{iz}^1, \ if |D_z| = max(|D_x|, |D_y|, |D_z|) \\\end{array},\ i=0,1,2.\right. \]
然後進一步計算t點,如下:
\[t_1 = p_{V_0^1} + (p_{V_1^1} - p_{V_0^1})\frac{d_{V_0^1}}{d_{V_0^1} - d_{V_1^1}} \]
此時計算出來的t值,就是在交點在\(max(|D_x|,|D_y|,|D_z|)\)對應軸上的值。然後再將該值帶入兩個三角形的平面方程,就可以求得交點的具體座標。
原始碼實現
具體實現在之前相交測試程式碼的基礎上,增加了兩個平面相交求解的過程。完整程式碼如下:
#pragma once #include <iostream> #include <cmath> #include <algorithm> #include <assert.h> #include <vector> #include <Eigen/Dense> typedef Eigen::Vector3d Point3d; class Triangle { public: Triangle(Point3d pt0, Point3d pt1, Point3d pt2) : m_pt {pt0, pt1, pt2} { auto vecPt0TPt1 = pt1 - pt0; auto vecPt0TPt2 = pt2 - pt0; m_vecNormal = vecPt0TPt1.cross(vecPt0TPt2); m_vecNormal.normalize(); } double GetDistanceFromPointToTrianglePlane(Point3d pt) const { auto vecPtTPt0 = m_pt[0] - pt; return m_vecNormal.dot(vecPtTPt0); } void GetTriangleVertices(Point3d (&pt)[3]) const { pt[0] = m_pt[0]; pt[1] = m_pt[1]; pt[2] = m_pt[2]; } void GetNormal(Eigen::Vector3d &vecNormal) const { vecNormal = m_vecNormal; } private: Point3d m_pt[3]; Eigen::Vector3d m_vecNormal; }; #define EPSION 1e-7 enum IntersectionType { INTERSECTION, //< 有相交線段 DISJOINT, //< 不相交 COPLANE //< 共面 }; bool IsZero(double value, double epsion = EPSION) { return std::abs(value) < epsion; } bool IsEqual(double v1, double v2, double epsion = EPSION) { return IsZero(v1-v2, epsion); } bool IsPositive(double value, double epsion = EPSION) { return value - epsion > 0; } bool IsNegative(double value, double epsion = EPSION) { return value + epsion < 0; } int GetSignType(double value) { if (IsZero(value)) return 0; if (IsPositive(value)) return 1; return -1; } template<typename T> void Swap(T &a, T &b) { auto tmp = a; a = b; b = tmp; } void GetVertexNewOrder(const int (&disVTri1SignType)[3], const double (&disVTri1TPlaneTri2)[3], int (&vertexTri1NewOrder)[3]) { // 將頂點劃分成兩部分,0,2位於另一個三角形同一側,1位於另一個三角形另一側 vertexTri1NewOrder[0] = 0; vertexTri1NewOrder[1] = 1; vertexTri1NewOrder[2] = 2; int prodValue = disVTri1SignType[0] * disVTri1SignType[1] * disVTri1SignType[2]; // 如果乘積<0,則小於0的為單獨的點 if (prodValue < 0) { for (int i = 0; i < 3; ++i) { if (disVTri1TPlaneTri2[i] < 0) { Swap(vertexTri1NewOrder[i], vertexTri1NewOrder[1]); break; } } } // 如果乘積>0,則大於0的為單獨的點 else if (prodValue > 0) { for (int i = 0; i < 3; ++i) { if (disVTri1TPlaneTri2[i] > 0) { Swap(vertexTri1NewOrder[i], vertexTri1NewOrder[1]); break; } } } // 有點位於平面上 else { int sumValue = disVTri1SignType[0] + disVTri1SignType[1] + disVTri1SignType[2]; // 如果只有一個點等於0,並且另外兩個點同號,那麼等於0的點為單獨的點 if (std::abs(sumValue) == 2) { for (int i = 0; i < 3; ++i) { if (disVTri1TPlaneTri2[i] == 0) { Swap(vertexTri1NewOrder[i], vertexTri1NewOrder[1]); break; } } } // 如果只有一個點等於0,並且另外兩個點異號,那麼假定小於0的點為單獨的點 else if (std::abs(sumValue) == 0) { for (int i = 0; i < 3; ++i) { if (disVTri1TPlaneTri2[i] < 0) { Swap(vertexTri1NewOrder[i], vertexTri1NewOrder[1]); break; } } } // 如果兩個點等於0,那麼不等於0的點為單獨的點 else { for (int i = 0; i < 3; ++i) { if (disVTri1TPlaneTri2[i] != 0) { Swap(vertexTri1NewOrder[i], vertexTri1NewOrder[1]); break; } } } } } void CalculateT( const Eigen::Vector3d& vecNormalTri1, const double(&disVTri1TPlaneTri2)[3], const int (&vertexTri1NewOrder)[3], const Point3d (&verticesTri1)[3], double (&tTri1)[2], int& whichAxis) { int maxValueIndex = 0;; double maxValue = vecNormalTri1[0]; for (int i = 1; i < 3; ++i) { if (maxValue < vecNormalTri1[i]) { maxValue = vecNormalTri1[i]; maxValueIndex = i; } } double pTri1OnLine[3] = { verticesTri1[vertexTri1NewOrder[0]](maxValueIndex), verticesTri1[vertexTri1NewOrder[1]](maxValueIndex), verticesTri1[vertexTri1NewOrder[2]](maxValueIndex) }; tTri1[0] = pTri1OnLine[0] + (pTri1OnLine[1] - pTri1OnLine[0]) * disVTri1TPlaneTri2[vertexTri1NewOrder[0]] / (disVTri1TPlaneTri2[vertexTri1NewOrder[0]] - disVTri1TPlaneTri2[vertexTri1NewOrder[1]]); tTri1[1] = pTri1OnLine[2] + (pTri1OnLine[1] - pTri1OnLine[2]) * disVTri1TPlaneTri2[vertexTri1NewOrder[2]] / (disVTri1TPlaneTri2[vertexTri1NewOrder[2]] - disVTri1TPlaneTri2[vertexTri1NewOrder[1]]); whichAxis = maxValueIndex; } void CalculateIntersectionPoint( const Triangle& tri1, const Triangle& tri2, double t, int axisIndex, std::vector<Point3d>& interPts) { // function: // N1_x(P_x - V1_x) + N1_y(P_y - V1_y) + N1_z(P_z - V1_z) = 0 // N2_x(P_x - V2_x) + N2_y(P_y - V2_y) + N2_z(P_z - V2_z) = 0 // => // N1_xP_x + N1_yP_y + N1_zP_z = N1 \cdot V1 // N2_xP_x + N2_yP_y + N2_zP_z = N2 \cdot V2 // N: the normal of triangle // V: the point on triangle Eigen::Vector3d vecNormal1; tri1.GetNormal(vecNormal1); Point3d pts1[3]; tri1.GetTriangleVertices(pts1); Point3d ptV1 = pts1[0]; double d1 = vecNormal1.dot(ptV1); Eigen::Vector3d vecNormal2; tri2.GetNormal(vecNormal2); Point3d pts2[3]; tri2.GetTriangleVertices(pts2); Point3d ptV2 = pts2[0]; double d2 = vecNormal2.dot(ptV2); Point3d ptIntersect; if (axisIndex == 0) // x { ptIntersect[0] = t; // N1_yP_y + N1_zP_z = d1 - N1_x t // N2_yP_y + N2_zP_z = d2 - N2_x t double determinant = vecNormal1.y() * vecNormal2.z() - vecNormal1.z() * vecNormal2.y(); double determinantY = (d1 - vecNormal1.x()*t) * vecNormal2.z() - vecNormal1.z() * (d2 - vecNormal2.x()*t); double determinantZ = vecNormal1.y() * (d2 - vecNormal2.x()*t) - (d1 - vecNormal1.x()*t) * vecNormal2.y(); ptIntersect[1] = determinantY / determinant; ptIntersect[2] = determinantZ / determinant; } else if (axisIndex == 1) // y { ptIntersect[1] = t; // N1_xP_x + N1_zP_z = d1 - N1_y t // N2_xP_x + N2_zP_z = d2 - N2_y t double determinant = vecNormal1.x() * vecNormal2.z() - vecNormal1.z() * vecNormal2.x(); double determinantX = (d1 - vecNormal1.y()*t) * vecNormal2.z() - vecNormal1.z() * (d2 - vecNormal2.y()*t); double determinantZ = vecNormal1.x() * (d2 - vecNormal2.y()*t) - (d1 - vecNormal1.y()*t) * vecNormal2.x(); ptIntersect[0] = determinantX / determinant; ptIntersect[2] = determinantZ / determinant; } else // z { ptIntersect[2] = t; // N1_xP_x + N1_yP_y = d1 - N1_z t // N2_xP_x + N2_yP_y = d2 - N2_z t double determinant = vecNormal1.x() * vecNormal2.y() - vecNormal1.y() * vecNormal2.x(); double determinantX = (d1 - vecNormal1.z()*t) * vecNormal2.y() - vecNormal1.y() * (d2 - vecNormal2.z()*t); double determinantY = vecNormal1.x() * (d2 - vecNormal2.z()*t) - (d1 - vecNormal1.z()*t) * vecNormal2.x(); ptIntersect[0] = determinantX / determinant; ptIntersect[1] = determinantY / determinant; } interPts.push_back(ptIntersect); } void CalculateIntersectionPoints( const Triangle& tri1, const Triangle& tri2, double (&tTri1)[2], double (&tTri2)[2], int t1Index, int t2Index, std::vector<Point3d>& interPts) { // tTri1[0] tTri2[0] tTri1[1] tTri2[1] if (tTri1[0] < tTri2[0] && tTri2[0] < tTri1[1] && tTri1[1] < tTri2[1]) { CalculateIntersectionPoint(tri1, tri2, tTri2[0], t2Index, interPts); CalculateIntersectionPoint(tri1, tri2, tTri1[1], t1Index, interPts); } // tTri1[0] tTri2[0] tTri2[1] tTri1[1] else if (tTri1[0] < tTri2[0] && tTri2[0] < tTri2[1] && tTri2[1] < tTri1[1]) { CalculateIntersectionPoint(tri1, tri2, tTri2[0], t2Index, interPts); CalculateIntersectionPoint(tri1, tri2, tTri2[1], t2Index, interPts); } // tTri2[0] tTri1[0] tTri1[1] tTri2[1] else if (tTri2[0] < tTri1[0] && tTri1[0] < tTri1[1] && tTri1[1] < tTri2[1]) { CalculateIntersectionPoint(tri1, tri2, tTri1[0], t1Index, interPts); CalculateIntersectionPoint(tri1, tri2, tTri1[1], t1Index, interPts); } // tTri2[0] tTri1[0] tTri2[1] tTri1[1] else { CalculateIntersectionPoint(tri1, tri2, tTri1[0], t1Index, interPts); CalculateIntersectionPoint(tri1, tri2, tTri2[1], t2Index, interPts); } } IntersectionType TriangleIntersectionTest(const Triangle& tri1, const Triangle& tri2, std::vector<Point3d>& interPts) { Point3d verticesTri1[3], verticesTri2[3]; tri1.GetTriangleVertices(verticesTri1); tri2.GetTriangleVertices(verticesTri2); double disVTri2TPlaneTri1[3]; int disVTri2SignType[3]; double disVTri1TPlaneTri2[3]; int disVTri1SignType[3]; for (int i = 0; i < 3; ++i) { disVTri2TPlaneTri1[i] = tri1.GetDistanceFromPointToTrianglePlane(verticesTri2[i]); disVTri2SignType[i] = GetSignType(disVTri2TPlaneTri1[i]); } // 如果三角形Tri2的三個頂點在三角形Tri1的同一側,則不相交 if ((disVTri2SignType[0] > 0 && disVTri2SignType[1] > 0 && disVTri2SignType[2] > 0) || (disVTri2SignType[0] < 0 && disVTri2SignType[1] < 0 && disVTri2SignType[2] < 0)) { return DISJOINT; } // 都為0,則共面 if ((disVTri2SignType[0] | disVTri2SignType[1] | disVTri2SignType[2]) == 0) { return COPLANE; } for (int i = 0; i < 3; ++i) { disVTri1TPlaneTri2[i] = tri2.GetDistanceFromPointToTrianglePlane(verticesTri1[i]); disVTri1SignType[i] = GetSignType(disVTri1TPlaneTri2[i]); } // 如果三角形Tri1的三個頂點在三角形Tri2的同一側,則不相交 if ((disVTri1SignType[0] > 0 && disVTri1SignType[1] > 0 && disVTri1SignType[2] > 0) || (disVTri1SignType[0] < 0 && disVTri1SignType[1] < 0 && disVTri1SignType[2] < 0)) { return DISJOINT; } // 都為0,則共面 if ((disVTri1SignType[0] | disVTri1SignType[1] | disVTri1SignType[2]) == 0) { return COPLANE; } // 對頂點順序進行調整,如何論文中的描述 int vertexTri1NewOrder[3] = { 0, 1, 2 }; int vertexTri2NewOrder[3] = { 0, 1, 2 }; GetVertexNewOrder(disVTri1SignType, disVTri1TPlaneTri2, vertexTri1NewOrder); GetVertexNewOrder(disVTri2SignType, disVTri2TPlaneTri1, vertexTri2NewOrder); Eigen::Vector3d vecNormalTri1, vecNormalTri2; tri1.GetNormal(vecNormalTri1); tri2.GetNormal(vecNormalTri2); double tTri1[2], tTri2[2]; int t1Index, t2Index; // 計算相交線和每個三角形的相交線段 CalculateT(vecNormalTri1, disVTri1TPlaneTri2, vertexTri1NewOrder, verticesTri1, tTri1, t1Index); CalculateT(vecNormalTri2, disVTri2TPlaneTri1, vertexTri2NewOrder, verticesTri2, tTri2, t2Index); if (tTri1[0] > tTri1[1]) Swap(tTri1[0], tTri1[1]); if (tTri2[0] > tTri2[1]) Swap(tTri2[0], tTri2[1]); // 比較是否overlap if ((tTri2[0] > tTri1[0] && tTri2[0] < tTri1[1]) || (tTri2[1] > tTri1[0] && tTri2[1] < tTri1[1])) { CalculateIntersectionPoints(tri1, tri2, tTri1, tTri2, t1Index, t2Index, interPts); return INTERSECTION; } return DISJOINT; } void TriIntersectTestCase() { { Triangle tr1(Point3d(0, 0, 0), Point3d(1, 0, 1), Point3d(0, 1, 1)); Triangle tr2(Point3d(1, 1, 0), Point3d(1, 1, 1), Point3d(0, 0, 1)); std::vector<Point3d> pts; auto type = TriangleIntersectionTest(tr1, tr2, pts); assert(type == INTERSECTION); std::cout << "Intersection points: \n"; for (int i = 0; i < pts.size(); ++i) { std::cout << "=====" << "\n"; std::cout << pts[i] << "\n"; } } } int main() { TriIntersectTestCase(); return 0; }
測試結果如下:
Intersection points:
=====
0.333333
0.333333
0.666667
=====
0.5
0.5
1
用geogebra繪圖驗證,如下圖所示: