PaperImpl - A fast triangle-triangle intersection test

論文閱讀參見：https://www.cnblogs.com/grass-and-moon/p/13297665.html

對論文程式碼進行了實現具體如下，三角形的資料結構如下：

// geometry_structure.h
#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;
};
 

相交測試程式碼實現如下：

// triangle_intersection_test.h
#pragma once

#include <cmath>
#include <algorithm>
#include <assert.h>

#include "geometry_structure.h"

namespace IntersectionTest
{
#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 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) };

        double tTri1[2];
        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]]);
    }

    IntersectionType TriangleIntersectionTest(const Triangle& tri1, const Triangle& tri2) 
    {
        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];
        // 計算相交線和每個三角形的相交線段
        CalculateT(vecNormalTri1, disVTri1TPlaneTri2, vertexTri1NewOrder, verticesTri1, tTri1);
        CalculateT(vecNormalTri2, disVTri2TPlaneTri1, vertexTri2NewOrder, verticesTri2, tTri2);

        if (tTri1[0] > tTri1[1]) Swap(tTri1[0], tTri1[1]);

        // 比較是否overlap
        if ((tTri2[0] > tTri1[0] && tTri2[0] < tTri1[1]) ||
            (tTri2[1] > tTri1[0] && tTri2[1] < tTri1[1])) {
            return INTERSECTION;
        }

        return DISJOINT;
    }

    void TestCase()
    {
        {
            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));

            auto type = IntersectionTest::TriangleIntersectionTest(tr1, tr2);

            assert(type == IntersectionTest::INTERSECTION);
        }

        {

            Triangle tr1(Point3d(0, 0, 0), Point3d(0, 0, 1), Point3d(1, 1, 0));
            Triangle tr2(Point3d(1, 1, 0), Point3d(1, 1, 1), Point3d(0, 0, 1));

            auto type = IntersectionTest::TriangleIntersectionTest(tr1, tr2);

            assert(type == IntersectionTest::COPLANE);
        }

        {
            Triangle tr1(Point3d(0, 0, 0), Point3d(0, 0, 1), Point3d(1, 1, 0));
            Triangle tr2(Point3d(1, 0, 1), Point3d(0, 1, 1), Point3d(1, 1, 1));

            auto type = IntersectionTest::TriangleIntersectionTest(tr1, tr2);

            assert(type == IntersectionTest::DISJOINT);
        }
    }
};
 

測試main函式如下：

// main.cpp
#include "triangle_intersection_test.h"

int main()
{
    IntersectionTest::TestCase();
    return 0;
}