16 #ifndef SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
33 template <
class T,
int MOpt>
37 typedef Eigen::Matrix<T, 3, 1, MOpt>
Vector3;
38 typedef boost::container::static_vector<Vector3, CAPACITY>
Vertices;
69 for (
size_t i = 0; i < 3; ++i)
78 <<
"The distance from triangle is calculated as " << *penetrationDepth <<
". At this point in the"
79 <<
" algorithm, the depth is expected to be negative.";
81 *penetrationPoint1 = *penetrationPoint0 - (triangle.
m_normal * (*penetrationDepth));
82 *penetrationDepth = -(*penetrationDepth);
94 *planeNormal = planeNormal->cross(
m_normal);
95 planeNormal->normalize();
96 *planeD = -
m_vertices[index]->dot(*planeNormal);
123 clippedVertices.clear();
125 static const T EPSILON = T(Geometry::DistanceEpsilon);
128 boost::container::static_vector<T, CAPACITY> signedDistanceFromPlane;
129 for (
auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
131 signedDistanceFromPlane.push_back((*it).dot(planeN) + planeD);
138 typename boost::container::static_vector<Vector3, CAPACITY>::iterator end;
141 auto startSignedDistance = signedDistanceFromPlane.begin();
142 typename boost::container::static_vector<T, CAPACITY>::iterator endSignedDistance;
145 for (
auto start = originalVertices.begin(); start != originalVertices.end(); ++start, ++startSignedDistance)
149 endSignedDistance = startSignedDistance + 1;
150 if (end == originalVertices.end())
152 end = originalVertices.begin();
153 endSignedDistance = signedDistanceFromPlane.begin();
157 if (*startSignedDistance <= EPSILON)
159 clippedVertices.push_back(*start);
163 if ((*startSignedDistance < -EPSILON && *endSignedDistance > EPSILON) ||
164 (*startSignedDistance > EPSILON && *endSignedDistance < -EPSILON))
166 ratio = *startSignedDistance / (*startSignedDistance - *endSignedDistance);
167 clippedVertices.push_back(*start + (*end - *start) * ratio);
183 <<
"There are no vertices under the plane. This scenario should not arise according to the"
184 <<
" Triangle-Triangle Contact Calculation algorithm, because of the circumstances under which"
185 <<
" this function is set to be called.";
187 T signedDistanceFromPlane;
189 for (
auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
191 signedDistanceFromPlane = (*it).dot(planeN) + planeD;
192 if (signedDistanceFromPlane < *depth)
194 *depth = signedDistanceFromPlane;
215 template <
class T,
int MOpt>
inline
217 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
218 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
219 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
220 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
221 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
222 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
223 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
224 const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
226 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
227 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
228 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
230 typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
245 T penetrationDepths[2] = {T(0), T(0)};
246 Vector3 penetrationPoints[2][2];
250 triangle2, &penetrationDepths[0], &penetrationPoints[0][0], &penetrationPoints[0][1]);
253 triangle1, &penetrationDepths[1], &penetrationPoints[1][1], &penetrationPoints[1][0]);
256 if (penetrationDepths[0] < penetrationDepths[1])
258 *penetrationDepth = penetrationDepths[0];
259 *contactNormal = t1n;
260 *penetrationPoint0 = penetrationPoints[0][0];
261 *penetrationPoint1 = penetrationPoints[0][1];
265 *penetrationDepth = penetrationDepths[1];
266 *contactNormal = -t0n;
267 *penetrationPoint0 = penetrationPoints[1][0];
268 *penetrationPoint1 = penetrationPoints[1][1];
275 template <
class T,
int MOpt>
inline
277 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
278 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
279 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
280 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
281 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
282 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
284 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
285 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
286 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
288 Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
289 Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
290 if (t0n.isZero() || t1n.isZero())
298 penetrationPoint0, penetrationPoint1, contactNormal);
301 template <
class T,
int MOpt>
inline
303 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
304 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
305 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
306 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
307 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
308 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
309 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
310 const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
312 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
313 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
314 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
316 typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
317 using SurgSim::Math::Geometry::DistanceEpsilon;
318 using SurgSim::Math::Geometry::ScalarEpsilon;
320 SURGSIM_ASSERT(std::abs(t0n.norm() - 1.0) < ScalarEpsilon && std::abs(t1n.norm() - 1.0) < ScalarEpsilon)
321 <<
"The normals sent in are not normalized! t0n{" << t0n.transpose() <<
"}, t1n{" << t1n.transpose() <<
"}.";
326 std::array<Vector3, 2> d = {Vector3(t1n.dot(t0v0 - t1v0), t1n.dot(t0v1 - t1v0), t1n.dot(t0v2 - t1v0)),
327 Vector3(t0n.dot(t1v0 - t0v0), t0n.dot(t1v1 - t0v0), t0n.dot(t1v2 - t0v0))};
329 if ((d[0].array() < DistanceEpsilon).all() || (d[0].array() > -DistanceEpsilon).all() ||
330 (d[1].array() < DistanceEpsilon).all() || (d[1].array() > -DistanceEpsilon).all())
335 const std::array<std::array<Vector3, 3>, 2> tv = {{{t0v0, t0v1, t0v2}, {t1v0, t1v1, t1v2}}};
340 std::array<std::array<Vector3, 2>, 2> tsa;
341 for (
int i = 0; i < 2; ++i)
345 for (
int j = 0; j < 3; ++j)
350 if ((d[i][j] < 0.0 && d[i][k] >= 0.0) || (d[i][j] > 0.0 && d[i][k] <= 0.0))
353 auto ratio = std::abs(d[i][j]) / (std::abs(d[i][j]) + std::abs(d[i][k]));
354 tsa[i][index++] = tv[i][j] + ratio * (tv[i][k] - tv[i][j]);
359 <<
"The intersection between the edges of triangle " << i <<
" and the plane of the other triangle"
360 <<
" must result in two points exactly.";
364 const Vector3 D = t0n.cross(t1n).normalized();
368 distancePointLine(tsa[1][0], tsa[0][0], (tsa[0][0] + D).eval(), &result) < DistanceEpsilon &&
369 distancePointLine(tsa[1][1], tsa[0][0], (tsa[0][0] + D).eval(), &result) < DistanceEpsilon)
370 <<
"The intersection points on the triangles do not lie on the separating axis";
372 static const int DISTANCE = 0;
373 static const int TRIANGLE = 1;
374 static const int VERTEX = 2;
378 std::array<std::tuple<T, int, int>, 4> intervals =
379 {std::tuple<T, int, int>(0.0, 0, 0),
380 std::tuple<T, int, int>((tsa[0][1] - tsa[0][0]).dot(D), 0, 1),
381 std::tuple<T, int, int>((tsa[1][0] - tsa[0][0]).dot(D), 1, 0),
382 std::tuple<T, int, int>((tsa[1][1] - tsa[0][0]).dot(D), 1, 1)};
385 std::sort(intervals.begin(), intervals.end(), [](
const std::tuple<T, int, int>& i, std::tuple<T, int, int>& j)
387 return (std::get<DISTANCE>(i) < std::get<DISTANCE>(j));
397 enum {P = 0, Q, R, S};
398 if (std::get<TRIANGLE>(intervals[P]) == std::get<TRIANGLE>(intervals[Q]))
404 size_t indexLeft, indexRight;
405 if (std::get<TRIANGLE>(intervals[Q]) == std::get<TRIANGLE>(intervals[R]))
417 if ((std::get<DISTANCE>(intervals[Q]) - std::get<DISTANCE>(intervals[P])) <
418 (std::get<DISTANCE>(intervals[S]) - std::get<DISTANCE>(intervals[R])))
444 *penetrationDepth = std::get<DISTANCE>(intervals[indexRight]) - std::get<DISTANCE>(intervals[indexLeft]);
445 if (*penetrationDepth < DistanceEpsilon)
451 if (std::get<TRIANGLE>(intervals[indexLeft]) == 0)
453 *penetrationPoint0 = tsa[0][std::get<VERTEX>(intervals[indexLeft])];
454 *penetrationPoint1 = tsa[1][std::get<VERTEX>(intervals[indexRight])];
458 *penetrationPoint0 = tsa[0][std::get<VERTEX>(intervals[indexRight])];
459 *penetrationPoint1 = tsa[1][std::get<VERTEX>(intervals[indexLeft])];
462 if ((*penetrationPoint0 - *penetrationPoint1).dot(D) > 0.0)
475 template <
class T,
int MOpt>
inline
477 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
478 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
479 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
480 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
481 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
482 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
484 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
485 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
486 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
488 Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
489 Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
490 if (t0n.isZero() || t1n.isZero())
498 penetrationDepth, penetrationPoint0, penetrationPoint1, contactNormal);
506 #endif // SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H