16 #ifndef SURGSIM_MATH_GEOMETRY_H
17 #define SURGSIM_MATH_GEOMETRY_H
19 #include <boost/container/static_vector.hpp>
21 #include <Eigen/Geometry>
48 static const double DistanceEpsilon = 1e-10;
51 static const double SquaredDistanceEpsilon = 1e-10;
54 static const double AngularEpsilon = 1e-10;
57 static const double ScalarEpsilon = 1e-10;
70 template <
class T,
int MOpt>
inline
72 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
73 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
74 Eigen::Matrix<T, 2, 1, MOpt>* coordinates)
76 const Eigen::Matrix<T, 3, 1, MOpt> line = sv1 - sv0;
77 const T length2 = line.squaredNorm();
78 if (length2 < Geometry::SquaredDistanceEpsilon)
80 coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
83 (*coordinates)[1] = (pt - sv0).dot(line) / length2;
84 (*coordinates)[0] =
static_cast<T
>(1) - (*coordinates)[1];
98 template <
class T,
int MOpt>
inline
100 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
101 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
102 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
103 const Eigen::Matrix<T, 3, 1, MOpt>& tn,
104 Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
106 const T signedTriAreaX2 = ((tv1 - tv0).cross(tv2 - tv0)).dot(tn);
107 if (signedTriAreaX2 < Geometry::SquaredDistanceEpsilon)
110 coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
113 (*coordinates)[0] = ((tv1 - pt).cross(tv2 - pt)).dot(tn) / signedTriAreaX2;
114 (*coordinates)[1] = ((tv2 - pt).cross(tv0 - pt)).dot(tn) / signedTriAreaX2;
115 (*coordinates)[2] = 1 - (*coordinates)[0] - (*coordinates)[1];
129 template <
class T,
int MOpt>
inline
131 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
132 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
133 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
134 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
135 Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
137 Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1 - tv0).cross(tv2 - tv0);
138 double norm = tn.norm();
139 if (norm < Geometry::DistanceEpsilon)
141 coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
158 template <
class T,
int MOpt>
inline
160 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
161 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
162 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
163 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
164 const Eigen::Matrix<T, 3, 1, MOpt>& tn)
166 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
169 baryCoords[0] >= -Geometry::ScalarEpsilon &&
170 baryCoords[1] >= -Geometry::ScalarEpsilon &&
171 baryCoords[2] >= -Geometry::ScalarEpsilon);
183 template <
class T,
int MOpt>
inline
185 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
186 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
187 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
188 const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
190 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
192 return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
193 baryCoords[1] >= -Geometry::ScalarEpsilon &&
194 baryCoords[2] >= -Geometry::ScalarEpsilon);
205 template <
class T,
int MOpt>
inline
207 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
208 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
209 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
210 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
211 const Eigen::Matrix<T, 3, 1, MOpt>& tn)
213 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
215 return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
216 baryCoords[1] >= -Geometry::ScalarEpsilon &&
217 baryCoords[2] >= -Geometry::ScalarEpsilon &&
218 baryCoords.minCoeff() <= Geometry::ScalarEpsilon);
230 template <
class T,
int MOpt>
inline
232 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
233 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
234 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
235 const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
237 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
239 return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
240 baryCoords[1] >= -Geometry::ScalarEpsilon &&
241 baryCoords[2] >= -Geometry::ScalarEpsilon &&
242 baryCoords.minCoeff() <= Geometry::ScalarEpsilon);
250 template <
class T,
int MOpt>
inline
252 const Eigen::Matrix<T, 3, 1, MOpt>& a,
253 const Eigen::Matrix<T, 3, 1, MOpt>& b,
254 const Eigen::Matrix<T, 3, 1, MOpt>& c,
255 const Eigen::Matrix<T, 3, 1, MOpt>& d)
257 return std::abs((c - a).dot((b - a).cross(d - c))) < Geometry::ScalarEpsilon;
265 template <
class T,
int MOpt>
inline
267 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
268 const Eigen::Matrix<T, 3, 1, MOpt>& v0,
269 const Eigen::Matrix<T, 3, 1, MOpt>& v1,
270 Eigen::Matrix<T, 3, 1, MOpt>* result)
275 Eigen::Matrix<T, 3, 1, MOpt> v01 = v1 - v0;
276 T v01_norm2 = v01.squaredNorm();
277 if (v01_norm2 <= Geometry::SquaredDistanceEpsilon)
280 T pv_norm2 = (pt - v0).squaredNorm();
281 return sqrt(pv_norm2);
283 T lambda = (v01).dot(pt - v0);
284 *result = v0 + lambda * v01 / v01_norm2;
285 return (*result - pt).norm();
296 template <
class T,
int MOpt>
inline
298 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
299 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
300 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
301 Eigen::Matrix<T, 3, 1, MOpt>* result)
303 Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
304 T v01Norm2 = v01.squaredNorm();
305 if (v01Norm2 <= Geometry::SquaredDistanceEpsilon)
308 return (pt - sv0).norm();
310 T lambda = v01.dot(pt - sv0);
315 else if (lambda >= v01Norm2)
321 *result = sv0 + lambda * v01 / v01Norm2;
323 return (*result - pt).norm();
336 template <
class T,
int MOpt>
inline
338 const Eigen::Matrix<T, 3, 1, MOpt>& l0v0,
339 const Eigen::Matrix<T, 3, 1, MOpt>& l0v1,
340 const Eigen::Matrix<T, 3, 1, MOpt>& l1v0,
341 const Eigen::Matrix<T, 3, 1, MOpt>& l1v1,
342 Eigen::Matrix<T, 3, 1, MOpt>* pt0,
343 Eigen::Matrix<T, 3, 1, MOpt>* pt1)
352 Eigen::Matrix<T, 3, 1, MOpt> l0v01 = l0v1 - l0v0;
353 T a = l0v01.squaredNorm();
354 if (a <= Geometry::SquaredDistanceEpsilon)
360 Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1 - l1v0;
361 T b = -l0v01.dot(l1v01);
362 T c = l1v01.squaredNorm();
363 if (c <= Geometry::SquaredDistanceEpsilon)
369 Eigen::Matrix<T, 3, 1, MOpt> l0v0_l1v0 = l0v0 - l1v0;
370 T d = l0v01.dot(l0v0_l1v0);
371 T e = -l1v01.dot(l0v0_l1v0);
372 T ratio = a * c - b * b;
373 if (std::abs(ratio) <= Geometry::ScalarEpsilon)
382 T inv_ratio = T(1) / ratio;
383 lambda0 = (b * e - c * d) * inv_ratio;
384 lambda1 = (b * d - a * e) * inv_ratio;
386 *pt0 = l0v0 + lambda0 * l0v01;
387 *pt1 = l1v0 + lambda1 * l1v01;
388 return ((*pt0) - (*pt1)).norm();
403 template <
class T,
int MOpt>
405 const Eigen::Matrix<T, 3, 1, MOpt>& s0v0,
406 const Eigen::Matrix<T, 3, 1, MOpt>& s0v1,
407 const Eigen::Matrix<T, 3, 1, MOpt>& s1v0,
408 const Eigen::Matrix<T, 3, 1, MOpt>& s1v1,
409 Eigen::Matrix<T, 3, 1, MOpt>* pt0,
410 Eigen::Matrix<T, 3, 1, MOpt>* pt1,
420 Eigen::Matrix<T, 3, 1, MOpt> s0v01 = s0v1 - s0v0;
421 T a = s0v01.squaredNorm();
422 if (a <= Geometry::SquaredDistanceEpsilon)
426 return distancePointSegment<T>(s0v0, s1v0, s1v1, pt1);
428 Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1 - s1v0;
429 T b = -s0v01.dot(s1v01);
430 T c = s1v01.squaredNorm();
431 if (c <= Geometry::SquaredDistanceEpsilon)
435 return distancePointSegment<T>(s1v0, s0v0, s0v1, pt0);
437 Eigen::Matrix<T, 3, 1, MOpt> tempLine = s0v0 - s1v0;
438 T d = s0v01.dot(tempLine);
439 T e = -s1v01.dot(tempLine);
440 T ratio = a * c - b * b;
445 if (1.0 - std::abs(s0v01.normalized().dot(s1v01.normalized())) >= Geometry::SquaredDistanceEpsilon)
520 enum edge_type { s0, s1, t0, t1, edge_skip, edge_invalid };
521 edge_type edge = edge_invalid;
732 *pt0 = s0v0 + s * (s0v01);
733 *pt1 = s1v0 + t * (s1v01);
734 if (s0t !=
nullptr && s1t !=
nullptr)
739 return ((*pt1) - (*pt0)).norm();
751 template <
class T,
int MOpt>
inline
753 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
754 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
755 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
756 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
757 Eigen::Matrix<T, 3, 1, MOpt>* result)
764 Eigen::Matrix<T, 3, 1, MOpt> tv01 = tv1 - tv0;
765 Eigen::Matrix<T, 3, 1, MOpt> tv02 = tv2 - tv0;
766 T a = tv01.squaredNorm();
767 if (a <= Geometry::SquaredDistanceEpsilon)
770 return distancePointSegment<T>(pt, tv0, tv2, result);
772 T b = tv01.dot(tv02);
773 T tCross = tv01.cross(tv02).squaredNorm();
774 if (tCross <= Geometry::SquaredDistanceEpsilon)
777 return distancePointSegment<T>(pt, tv0, tv1, result);
779 T c = tv02.squaredNorm();
780 if (c <= Geometry::SquaredDistanceEpsilon)
783 return distancePointSegment<T>(pt, tv0, tv1, result);
785 Eigen::Matrix<T, 3, 1, MOpt> tv0pv0 = tv0 - pt;
786 T d = tv01.dot(tv0pv0);
787 T e = tv02.dot(tv0pv0);
788 T ratio = a * c - b * b;
845 T numer, denom, tmp0, tmp1;
846 enum edge_type { s0, t0, s1t1, edge_skip, edge_invalid };
847 edge_type edge = edge_invalid;
852 numer = T(1) / ratio;
896 tmp1 = -a - d + b + e;
920 t = (-e >= c ? 1 : -e / c);
932 s = (-d >= a ? 1 : -d / a);
937 numer = c + e - b - d;
944 denom = a - 2 * b + c;
945 s = (numer >= denom ? 1 : numer / denom);
954 *result = tv0 + s * tv01 + t * tv02;
955 return ((*result) - pt).norm();
972 template <
class T,
int MOpt>
inline
974 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
975 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
976 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
977 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
978 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
979 const Eigen::Matrix<T, 3, 1, MOpt>& tn,
980 Eigen::Matrix<T, 3, 1, MOpt>* result)
983 Eigen::Matrix<T, 3, 1, MOpt> u = tv1 - tv0;
984 Eigen::Matrix<T, 3, 1, MOpt> v = tv2 - tv0;
987 Eigen::Matrix<T, 3, 1, MOpt> dir = sv1 - sv0;
988 Eigen::Matrix<T, 3, 1, MOpt> w0 = sv0 - tv0;
992 result->setConstant((std::numeric_limits<double>::quiet_NaN()));
995 if (std::abs(b) <= Geometry::AngularEpsilon)
997 if (std::abs(a) <= Geometry::AngularEpsilon)
1000 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1001 for (
int i = 0; i < 2; ++i)
1004 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1006 *result = (i == 0) ? sv0 : sv1;
1023 if (r < -Geometry::DistanceEpsilon)
1028 if (r > 1 + Geometry::DistanceEpsilon)
1034 Eigen::Matrix<T, 3, 1, MOpt> presumedIntersection = sv0 + r * dir;
1039 Eigen::Matrix<T, 3, 1, MOpt> w = presumedIntersection - tv0;
1042 T D = uv * uv - uu * vv;
1044 T s = (uv * wv - vv * wu) / D;
1046 if (s < -Geometry::DistanceEpsilon || s > 1 + Geometry::DistanceEpsilon)
1050 T t = (uv * wu - uu * wv) / D;
1052 if (t < -Geometry::DistanceEpsilon || (s + t) > 1 + Geometry::DistanceEpsilon)
1057 *result = sv0 + r * dir;
1071 template <
class T,
int MOpt>
inline
1073 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
1074 const Eigen::Matrix<T, 3, 1, MOpt>& n,
1076 Eigen::Matrix<T, 3, 1, MOpt>* result)
1078 T dist = n.dot(pt) + d;
1079 *result = pt - n * dist;
1098 template <
class T,
int MOpt>
inline
1100 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1101 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1102 const Eigen::Matrix<T, 3, 1, MOpt>& n,
1104 Eigen::Matrix<T, 3, 1, MOpt>* closestPointSegment,
1105 Eigen::Matrix<T, 3, 1, MOpt>* planeIntersectionPoint)
1107 T dist0 = n.dot(sv0) + d;
1108 T dist1 = n.dot(sv1) + d;
1110 Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1111 if (std::abs(n.dot(v01)) <= Geometry::AngularEpsilon)
1113 *closestPointSegment = (sv0 + sv1) * T(0.5);
1114 dist0 = n.dot(*closestPointSegment) + d;
1115 *planeIntersectionPoint = *closestPointSegment - dist0 * n;
1116 return (std::abs(dist0) < Geometry::DistanceEpsilon ? 0 : dist0);
1119 if ((dist0 > 0 && dist1 > 0) || (dist0 < 0 && dist1 < 0))
1121 if (std::abs(dist0) < std::abs(dist1))
1123 *closestPointSegment = sv0;
1124 *planeIntersectionPoint = sv0 - dist0 * n;
1129 *closestPointSegment = sv1;
1130 *planeIntersectionPoint = sv1 - dist1 * n;
1137 Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1138 T lambda = (-d - sv0.dot(n)) / v01.dot(n);
1139 *planeIntersectionPoint = sv0 + lambda * v01;
1140 *closestPointSegment = *planeIntersectionPoint;
1159 template <
class T,
int MOpt>
inline
1161 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1162 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1163 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1164 const Eigen::Matrix<T, 3, 1, MOpt>& n,
1166 Eigen::Matrix<T, 3, 1, MOpt>* closestPointTriangle,
1167 Eigen::Matrix<T, 3, 1, MOpt>* planeProjectionPoint)
1169 Eigen::Matrix<T, 3, 1, MOpt> distances(n.dot(tv0) + d, n.dot(tv1) + d, n.dot(tv2) + d);
1170 Eigen::Matrix<T, 3, 1, MOpt> t01 = tv1 - tv0;
1171 Eigen::Matrix<T, 3, 1, MOpt> t02 = tv2 - tv0;
1172 Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2 - tv1;
1174 closestPointTriangle->setConstant((std::numeric_limits<double>::quiet_NaN()));
1175 planeProjectionPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1180 if (std::abs(n.dot(t01)) <= Geometry::AngularEpsilon && std::abs(n.dot(t02)) <= Geometry::AngularEpsilon)
1182 *closestPointTriangle = (tv0 + tv1 + tv2) / T(3);
1183 *planeProjectionPoint = *closestPointTriangle - n * distances[0];
1184 return distances[0];
1188 if ((distances.array() < -Geometry::DistanceEpsilon).any() &&
1189 (distances.array() > Geometry::DistanceEpsilon).any())
1191 if (distances[0] * distances[1] < 0)
1193 *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t01) * t01;
1194 if (distances[0] * distances[2] < 0)
1196 *planeProjectionPoint = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1200 Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2 - tv1;
1201 *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1206 *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1207 *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1211 *closestPointTriangle = *planeProjectionPoint = (*closestPointTriangle + *planeProjectionPoint) * T(0.5);
1216 distances.cwiseAbs().minCoeff(&index);
1220 *closestPointTriangle = tv0;
1221 *planeProjectionPoint = tv0 - n * distances[0];
1222 return distances[0];
1224 *closestPointTriangle = tv1;
1225 *planeProjectionPoint = tv1 - n * distances[1];
1226 return distances[1];
1228 *closestPointTriangle = tv2;
1229 *planeProjectionPoint = tv2 - n * distances[2];
1230 return distances[2];
1233 return std::numeric_limits<T>::quiet_NaN();
1243 template <
class T,
int MOpt>
inline
1245 const Eigen::Matrix<T, 3, 1, MOpt>& pn0, T pd0,
1246 const Eigen::Matrix<T, 3, 1, MOpt>& pn1, T pd1,
1247 Eigen::Matrix<T, 3, 1, MOpt>* pt0,
1248 Eigen::Matrix<T, 3, 1, MOpt>* pt1)
1251 const Eigen::Matrix<T, 3, 1, MOpt> lineDir = pn0.cross(pn1);
1252 const T lineDirNorm2 = lineDir.squaredNorm();
1254 pt0->setConstant((std::numeric_limits<double>::quiet_NaN()));
1255 pt1->setConstant((std::numeric_limits<double>::quiet_NaN()));
1258 if (lineDirNorm2 <= Geometry::SquaredDistanceEpsilon)
1263 *pt0 = (pd1 * pn0 - pd0 * pn1).cross(lineDir) / lineDirNorm2;
1264 *pt1 = *pt0 + lineDir;
1279 template <
class T,
int MOpt>
inline
1281 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1282 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1283 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1284 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1285 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1286 Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1287 Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1289 Eigen::Matrix<T, 3, 1, MOpt> n = (tv1 - tv0).cross(tv2 - tv1);
1304 template <
class T,
int MOpt>
inline
1306 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1307 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1308 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1309 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1310 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1311 const Eigen::Matrix<T, 3, 1, MOpt>& normal,
1312 Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1313 Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1315 segmentPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1316 trianglePoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1319 const Eigen::Matrix<T, 3, 1, MOpt>& n = normal;
1321 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1323 const Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1324 const T v01DotTn = n.dot(v01);
1325 if (std::abs(v01DotTn) <= Geometry::AngularEpsilon)
1330 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1332 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1334 *segmentPoint = sv0;
1339 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1341 *segmentPoint = sv1;
1348 T lambda = -n.dot(sv0 - tv0) / v01DotTn;
1349 if (lambda >= 0 && lambda <= 1)
1351 *segmentPoint = *trianglePoint = sv0 + lambda * v01;
1353 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1361 Eigen::Matrix<T, 3, 1, MOpt> segColPt01, segColPt02, segColPt12, triColPt01, triColPt02, triColPt12;
1365 Eigen::Matrix<T, 3, 1, MOpt> ptTriCol0, ptTriCol1;
1368 if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1370 dstPtTri0 = std::numeric_limits<T>::max();
1374 if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1376 dstPtTri1 = std::numeric_limits<T>::max();
1380 Eigen::Matrix<double, 5, 1> distances;
1381 (distances << dst01, dst02, dst12, dstPtTri0, dstPtTri1).finished().minCoeff(&minIndex);
1385 *segmentPoint = segColPt01;
1386 *trianglePoint = triColPt01;
1389 *segmentPoint = segColPt02;
1390 *trianglePoint = triColPt02;
1393 *segmentPoint = segColPt12;
1394 *trianglePoint = triColPt12;
1397 *segmentPoint = sv0;
1398 *trianglePoint = ptTriCol0;
1401 *segmentPoint = sv1;
1402 *trianglePoint = ptTriCol1;
1407 return std::numeric_limits<T>::quiet_NaN();
1422 template <
class T,
int MOpt>
inline
1424 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1425 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1426 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1427 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1428 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1429 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1430 Eigen::Matrix<T, 3, 1, MOpt>* closestPoint0,
1431 Eigen::Matrix<T, 3, 1, MOpt>* closestPoint1)
1434 T minDst = std::numeric_limits<T>::max();
1436 Eigen::Matrix<T, 3, 1, MOpt> segPt, triPt;
1437 Eigen::Matrix<T, 3, 1, MOpt> n0 = (t0v1 - t0v0).cross(t0v2 - t0v0);
1439 Eigen::Matrix<T, 3, 1, MOpt> n1 = (t1v1 - t1v0).cross(t1v2 - t1v0);
1442 if (currDst < minDst)
1445 *closestPoint0 = segPt;
1446 *closestPoint1 = triPt;
1449 if (currDst < minDst)
1452 *closestPoint0 = segPt;
1453 *closestPoint1 = triPt;
1456 if (currDst < minDst)
1459 *closestPoint0 = segPt;
1460 *closestPoint1 = triPt;
1464 if (currDst < minDst)
1467 *closestPoint1 = segPt;
1468 *closestPoint0 = triPt;
1471 if (currDst < minDst)
1474 *closestPoint1 = segPt;
1475 *closestPoint0 = triPt;
1478 if (currDst < minDst)
1481 *closestPoint1 = segPt;
1482 *closestPoint0 = triPt;
1493 template <
class T,
int MOpt>
1495 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1496 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1497 const Eigen::AlignedBox<T, 3>& box,
1498 std::vector<Eigen::Matrix<T, 3, 1, MOpt>>* intersections)
1500 Eigen::Array<T, 3, 1, MOpt> v01 = sv1 - sv0;
1501 Eigen::Array<bool, 3, 1, MOpt> parallelToPlane = (v01.cwiseAbs().array() < Geometry::DistanceEpsilon);
1502 if (parallelToPlane.any())
1504 Eigen::Array<bool, 3, 1, MOpt> beyondMinCorner = (sv0.array() < box.min().array());
1505 Eigen::Array<bool, 3, 1, MOpt> beyondMaxCorner = (sv0.array() > box.max().array());
1506 if ((parallelToPlane && (beyondMinCorner || beyondMaxCorner)).any())
1515 Eigen::Array<T, 3, 2, MOpt> planeIntersectionAbscissas;
1516 planeIntersectionAbscissas.col(0) = (box.min().array() - sv0.array());
1517 planeIntersectionAbscissas.col(1) = (box.max().array() - sv0.array());
1521 planeIntersectionAbscissas.colwise() /= v01;
1523 T entranceAbscissa = planeIntersectionAbscissas.rowwise().minCoeff().maxCoeff();
1524 T exitAbscissa = planeIntersectionAbscissas.rowwise().maxCoeff().minCoeff();
1525 if (entranceAbscissa < exitAbscissa && exitAbscissa > T(0.0))
1527 if (entranceAbscissa >= T(0.0) && entranceAbscissa <= T(1.0))
1529 intersections->push_back(sv0 + v01.matrix() * entranceAbscissa);
1532 if (exitAbscissa >= T(0.0) && exitAbscissa <= T(1.0))
1534 intersections->push_back(sv0 + v01.matrix() * exitAbscissa);
1547 template <
class T,
int MOpt>
1549 const Eigen::Matrix<T, 3, 1, MOpt>& capsuleBottom,
1550 const Eigen::Matrix<T, 3, 1, MOpt>& capsuleTop,
1551 const T capsuleRadius,
1552 const Eigen::AlignedBox<T, 3>& box)
1554 Eigen::AlignedBox<double, 3> dilatedBox(box.min().array() - capsuleRadius, box.max().array() + capsuleRadius);
1555 std::vector<Vector3d> candidates;
1557 if (dilatedBox.contains(capsuleBottom))
1559 candidates.push_back(capsuleBottom);
1561 if (dilatedBox.contains(capsuleTop))
1563 candidates.push_back(capsuleTop);
1566 bool doesIntersect =
false;
1567 ptrdiff_t dimensionsOutsideBox;
1568 Vector3d clampedPosition, segmentPoint;
1569 for (
auto candidate = candidates.cbegin(); candidate != candidates.cend(); ++candidate)
1576 dimensionsOutsideBox = (candidate->array() > box.max().array()).count();
1577 dimensionsOutsideBox += (candidate->array() < box.min().array()).count();
1578 if (dimensionsOutsideBox >= 2)
1580 clampedPosition = (*candidate).array().min(box.max().array()).max(box.min().array());
1581 if (
distancePointSegment(clampedPosition, capsuleBottom, capsuleTop, &segmentPoint) > capsuleRadius)
1587 doesIntersect =
true;
1590 return doesIntersect;
1600 template <
class T,
int VOpt>
1602 const Eigen::Matrix<T, 3, 1, VOpt>& segment0,
const Eigen::Matrix<T, 3, 1, VOpt>& segment1)
1604 auto pointToSegmentStart = segment0 - point;
1605 auto segmentDirection = segment1 - segment0;
1606 auto squaredNorm = segmentDirection.squaredNorm();
1607 SURGSIM_ASSERT(squaredNorm != 0.0) <<
"Line is defined by two collocated points.";
1608 auto distance = -pointToSegmentStart.dot(segmentDirection) / squaredNorm;
1609 auto p0Proj = segment0 + distance * segmentDirection;
1623 template <
class T,
int MOpt>
inline
1625 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1626 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1627 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1628 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1629 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1630 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1631 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1632 const Eigen::Matrix<T, 3, 1, MOpt>& t1n);
1640 template <
class T,
int MOpt>
inline
1642 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1643 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1644 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1645 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1646 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1647 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2);
1668 template <
class T,
int MOpt>
inline
1670 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1671 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1672 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1673 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1674 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1675 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1676 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1677 const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
1678 T* penetrationDepth,
1679 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1680 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1681 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1700 template <
class T,
int MOpt>
inline
1702 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1703 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1704 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1705 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1706 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1707 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1708 T* penetrationDepth,
1709 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1710 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1711 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1730 template <
class T,
int MOpt>
inline
1732 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1733 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1734 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1735 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1736 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1737 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1738 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1739 const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
1740 T* penetrationDepth,
1741 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1742 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1743 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1758 template <
class T,
int MOpt>
inline
1760 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1761 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1762 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1763 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1764 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1765 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1766 T* penetrationDepth,
1767 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1768 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1769 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1788 template <
class T,
int MOpt>
inline
1790 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1791 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1792 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1793 const Eigen::Matrix<T, 3, 1, MOpt>& tn,
1794 const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
1795 const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
1797 T* penetrationDepth,
1798 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
1799 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
1800 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
1801 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
1819 template <
class T,
int MOpt>
inline
1821 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1822 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1823 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1824 const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
1825 const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
1827 T* penetrationDepth,
1828 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
1829 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
1830 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
1831 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
1839 template <
class T,
int MOpt>
1841 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
1842 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
1843 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
1844 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
1845 std::array<T, 3>* timesOfCoplanarity)
1862 Eigen::Matrix<T, 3, 1, MOpt> AB0 = B.first - A.first;
1863 Eigen::Matrix<T, 3, 1, MOpt> AC0 = C.first - A.first;
1864 Eigen::Matrix<T, 3, 1, MOpt> CD0 = D.first - C.first;
1865 Eigen::Matrix<T, 3, 1, MOpt> VA = (A.second - A.first);
1866 Eigen::Matrix<T, 3, 1, MOpt> VC = (C.second - C.first);
1867 Eigen::Matrix<T, 3, 1, MOpt> VAB = (B.second - B.first) - VA;
1868 Eigen::Matrix<T, 3, 1, MOpt> VAC = VC - VA;
1869 Eigen::Matrix<T, 3, 1, MOpt> VCD = (D.second - D.first) - VC;
1870 T a0 = AB0.cross(CD0).dot(AC0);
1871 T a1 = AB0.cross(CD0).dot(VAC) + (AB0.cross(VCD) + VAB.cross(CD0)).dot(AC0);
1872 T a2 = (AB0.cross(VCD) + VAB.cross(CD0)).dot(VAC) + VAB.cross(VCD).dot(AC0);
1873 T a3 = VAB.cross(VCD).dot(VAC);