16 #ifndef SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H 17 #define SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H 21 static const double EPSILOND = 1e-12;
45 size_t* parametricIntersectionIndex)
48 static const T EPSILON =
static_cast<T
>(EPSILOND);
50 bool edgeFromUnderToAbove = dStart < 0.0 && dEnd >= 0.0;
51 bool edgeFromAboveToUnder = dStart > 0.0 && dEnd <= 0.0;
53 if (edgeFromUnderToAbove || edgeFromAboveToUnder)
55 if (std::abs(dStart - dEnd) < EPSILON)
58 parametricIntersection[(*parametricIntersectionIndex)++] = pvStart;
63 parametricIntersection[(*parametricIntersectionIndex)++] =
64 pvStart + (pvEnd - pvStart) * (dStart / (dStart - dEnd));
69 template <
class T,
int MOpt>
inline 71 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
72 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
73 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
74 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
75 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
76 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
77 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
78 const Eigen::Matrix<T, 3, 1, MOpt>& t1n)
80 typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
81 using SurgSim::Math::Geometry::DistanceEpsilon;
83 if (t0n.isZero() || t1n.isZero())
111 Vector3 d2(t0n.dot(t1v0 - t0v0), t0n.dot(t1v1 - t0v0), t0n.dot(t1v2 - t0v0));
113 if ((d2.array() < DistanceEpsilon).all() || (d2.array() > -DistanceEpsilon).all())
119 Vector3 d1(t1n.dot(t0v0 - t1v0), t1n.dot(t0v1 - t1v0), t1n.dot(t0v2 - t1v0));
121 if ((d1.array() < DistanceEpsilon).all() || (d1.array() > -DistanceEpsilon).all())
127 Vector3 D = t0n.cross(t1n).normalized();
130 Vector3 pv1(D.dot(t0v0), D.dot(t0v1), D.dot(t0v2));
131 Vector3 pv2(D.dot(t1v0), D.dot(t1v1), D.dot(t1v2));
141 for (
int i = 0; i < 3; ++i)
150 <<
"The intersection between the triangle and the separating axis is not a line segment." 151 <<
" This scenario cannot happen, at this point in the algorithm.";
158 return !(std::abs(s1[0] - s1[1]) <= DistanceEpsilon || std::abs(s2[0] - s2[1]) <= DistanceEpsilon) &&
159 !(s1[0] <= (s2[0] + DistanceEpsilon) && s1[0] <= (s2[1] + DistanceEpsilon) &&
160 s1[1] <= (s2[0] + DistanceEpsilon) && s1[1] <= (s2[1] + DistanceEpsilon)) &&
161 !(s1[0] >= (s2[0] - DistanceEpsilon) && s1[0] >= (s2[1] - DistanceEpsilon) &&
162 s1[1] >= (s2[0] - DistanceEpsilon) && s1[1] >= (s2[1] - DistanceEpsilon));
166 template <
class T,
int MOpt>
inline 168 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
169 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
170 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
171 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
172 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
173 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2)
175 Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
176 Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
177 if (t0n.isZero() || t1n.isZero())
193 #endif // SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H Definition: CompoundShapeToGraphics.cpp:29
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
bool doesIntersectTriangleTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, const Eigen::Matrix< T, 3, 1, MOpt > &t0n, const Eigen::Matrix< T, 3, 1, MOpt > &t1n)
Check if the two triangles intersect using separating axis test.
Definition: TriangleTriangleIntersection-inl.h:70
void edgeIntersection(T dStart, T dEnd, T pvStart, T pvEnd, T *parametricIntersection, size_t *parametricIntersectionIndex)
Two ends of the triangle edge are given in terms of the following vertex properties.
Definition: TriangleTriangleIntersection-inl.h:44