GCompute - 计算三角形对的交点（1）
GCompute - 计算三角形对的交点（1）
本文是利用论文中的介绍进行实现的.
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绘图验证，如下图所示：
版权说明
作者： grassofsky
出处： http://www.cnblogs.com/grass-and-moon
本文版权归作者，欢迎转载，但未经作者同意必须保留此段声明，且在文章页面明显位置给出, 原文链接 如有问题， 可邮件（grass-of-sky@163.com）咨询.
