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