16 #ifndef SURGSIM_MATH_GEOMETRY_H 17 #define SURGSIM_MATH_GEOMETRY_H 19 #include <boost/container/static_vector.hpp> 21 #include <Eigen/Geometry> 47 static const double DistanceEpsilon = 1e-10;
50 static const double SquaredDistanceEpsilon = 1e-10;
53 static const double AngularEpsilon = 1e-10;
56 static const double ScalarEpsilon = 1e-10;
70 template <
class T,
int MOpt>
inline 72 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
73 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
74 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
75 const Eigen::Matrix<T, 3, 1, MOpt>& tn,
76 Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
78 const T signedTriAreaX2 = ((tv1-tv0).cross(tv2-tv0)).dot(tn);
79 if (signedTriAreaX2 < Geometry::SquaredDistanceEpsilon)
82 coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
85 (*coordinates)[0] = ((tv1-pt).cross(tv2-pt)).dot(tn) / signedTriAreaX2;
86 (*coordinates)[1] = ((tv2-pt).cross(tv0-pt)).dot(tn) / signedTriAreaX2;
87 (*coordinates)[2] = 1 - (*coordinates)[0] - (*coordinates)[1];
101 template <
class T,
int MOpt>
inline 103 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
104 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
105 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
106 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
107 Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
109 const Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1-tv0).cross(tv2-tv0);
123 template <
class T,
int MOpt>
inline 125 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
126 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
127 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
128 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
129 const Eigen::Matrix<T, 3, 1, MOpt>& tn)
131 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
134 baryCoords[0] >= -Geometry::ScalarEpsilon &&
135 baryCoords[1] >= -Geometry::ScalarEpsilon &&
136 baryCoords[2] >= -Geometry::ScalarEpsilon);
148 template <
class T,
int MOpt>
inline 150 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
151 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
152 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
153 const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
155 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
157 return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
158 baryCoords[1] >= -Geometry::ScalarEpsilon &&
159 baryCoords[2] >= -Geometry::ScalarEpsilon);
167 template <
class T,
int MOpt>
inline 169 const Eigen::Matrix<T, 3, 1, MOpt>& a,
170 const Eigen::Matrix<T, 3, 1, MOpt>& b,
171 const Eigen::Matrix<T, 3, 1, MOpt>& c,
172 const Eigen::Matrix<T, 3, 1, MOpt>& d)
174 return std::abs((c - a).dot((b - a).cross(d - c))) < Geometry::ScalarEpsilon;
182 template <
class T,
int MOpt>
inline 184 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
185 const Eigen::Matrix<T, 3, 1, MOpt>& v0,
186 const Eigen::Matrix<T, 3, 1, MOpt>& v1,
187 Eigen::Matrix<T, 3, 1, MOpt>* result)
192 Eigen::Matrix<T, 3, 1, MOpt> v01 = v1-v0;
193 T v01_norm2 = v01.squaredNorm();
194 if (v01_norm2 <= Geometry::SquaredDistanceEpsilon)
197 T pv_norm2 = (pt-v0).squaredNorm();
198 return sqrt(pv_norm2);
200 T lambda = (v01).dot(pt-v0);
201 *result = v0 + lambda*v01/v01_norm2;
202 return (*result-pt).norm();
213 template <
class T,
int MOpt>
inline 215 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
216 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
217 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
218 Eigen::Matrix<T, 3, 1, MOpt>* result)
220 Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
221 T v01Norm2 = v01.squaredNorm();
222 if (v01Norm2 <= Geometry::SquaredDistanceEpsilon)
225 return (pt-sv0).norm();
227 T lambda = v01.dot(pt-sv0);
232 else if (lambda >= v01Norm2)
238 *result = sv0 + lambda*v01/v01Norm2;
240 return (*result-pt).norm();
253 template <
class T,
int MOpt>
inline 255 const Eigen::Matrix<T, 3, 1, MOpt>& l0v0,
256 const Eigen::Matrix<T, 3, 1, MOpt>& l0v1,
257 const Eigen::Matrix<T, 3, 1, MOpt>& l1v0,
258 const Eigen::Matrix<T, 3, 1, MOpt>& l1v1,
259 Eigen::Matrix<T, 3, 1, MOpt>* pt0,
260 Eigen::Matrix<T, 3, 1, MOpt>* pt1)
269 Eigen::Matrix<T, 3, 1, MOpt> l0v01 = l0v1-l0v0;
270 T a = l0v01.squaredNorm();
271 if (a <= Geometry::SquaredDistanceEpsilon)
277 Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1-l1v0;
278 T b = -l0v01.dot(l1v01);
279 T c = l1v01.squaredNorm();
280 if (c <= Geometry::SquaredDistanceEpsilon)
286 Eigen::Matrix<T, 3, 1, MOpt> l0v0_l1v0 = l0v0-l1v0;
287 T d = l0v01.dot(l0v0_l1v0);
288 T e = -l1v01.dot(l0v0_l1v0);
290 if (std::abs(ratio) <= Geometry::ScalarEpsilon)
299 T inv_ratio = T(1) / ratio;
300 lambda0 = (b*e - c*d) * inv_ratio;
301 lambda1 = (b*d - a*e) * inv_ratio;
303 *pt0 = l0v0 + lambda0 * l0v01;
304 *pt1 = l1v0 + lambda1 * l1v01;
305 return ((*pt0)-(*pt1)).norm();
320 template <
class T,
int MOpt>
322 const Eigen::Matrix<T, 3, 1, MOpt>& s0v0,
323 const Eigen::Matrix<T, 3, 1, MOpt>& s0v1,
324 const Eigen::Matrix<T, 3, 1, MOpt>& s1v0,
325 const Eigen::Matrix<T, 3, 1, MOpt>& s1v1,
326 Eigen::Matrix<T, 3, 1, MOpt>* pt0,
327 Eigen::Matrix<T, 3, 1, MOpt>* pt1,
337 Eigen::Matrix<T, 3, 1, MOpt> s0v01 = s0v1-s0v0;
338 T a = s0v01.squaredNorm();
339 if (a <= Geometry::SquaredDistanceEpsilon)
343 return distancePointSegment<T>(s0v0, s1v0, s1v1, pt1);
345 Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1-s1v0;
346 T b = -s0v01.dot(s1v01);
347 T c = s1v01.squaredNorm();
348 if (c <= Geometry::SquaredDistanceEpsilon)
352 return distancePointSegment<T>(s1v0, s0v0, s0v1, pt0);
354 Eigen::Matrix<T, 3, 1, MOpt> tempLine = s0v0-s1v0;
355 T d = s0v01.dot(tempLine);
356 T e = -s1v01.dot(tempLine);
362 if (1.0 - std::abs(s0v01.normalized().dot(s1v01.normalized())) >= Geometry::SquaredDistanceEpsilon)
437 enum edge_type { s0, s1, t0, t1, edge_skip, edge_invalid };
438 edge_type edge = edge_invalid;
649 *pt0 = s0v0 + s * (s0v01);
650 *pt1 = s1v0 + t * (s1v01);
651 if (s0t !=
nullptr && s1t !=
nullptr)
656 return ((*pt1)-(*pt0)).norm();
668 template <
class T,
int MOpt>
inline 670 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
671 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
672 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
673 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
674 Eigen::Matrix<T, 3, 1, MOpt>* result)
681 Eigen::Matrix<T, 3, 1, MOpt> tv01 = tv1-tv0;
682 Eigen::Matrix<T, 3, 1, MOpt> tv02 = tv2-tv0;
683 T a = tv01.squaredNorm();
684 if (a <= Geometry::SquaredDistanceEpsilon)
687 return distancePointSegment<T>(pt, tv0, tv2, result);
689 T b = tv01.dot(tv02);
690 T tCross = tv01.cross(tv02).squaredNorm();
691 if (tCross <= Geometry::SquaredDistanceEpsilon)
694 return distancePointSegment<T>(pt, tv0, tv1, result);
696 T c = tv02.squaredNorm();
697 if (c <= Geometry::SquaredDistanceEpsilon)
700 return distancePointSegment<T>(pt, tv0, tv1, result);
702 Eigen::Matrix<T, 3, 1, MOpt> tv0pv0 = tv0-pt;
703 T d = tv01.dot(tv0pv0);
704 T e = tv02.dot(tv0pv0);
762 T numer, denom, tmp0, tmp1;
763 enum edge_type { s0, t0, s1t1, edge_skip, edge_invalid };
764 edge_type edge = edge_invalid;
769 numer = T(1) / ratio;
837 t = (-e >= c ? 1 : -e/c);
849 s = (-d >= a ? 1 : -d/a);
862 s = (numer >= denom ? 1 : numer / denom);
871 *result = tv0 + s * tv01 + t * tv02;
872 return ((*result)-pt).norm();
889 template <
class T,
int MOpt>
inline 891 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
892 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
893 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
894 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
895 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
896 const Eigen::Matrix<T, 3, 1, MOpt>& tn,
897 Eigen::Matrix<T, 3, 1, MOpt>* result)
900 Eigen::Matrix<T, 3, 1, MOpt> u = tv1-tv0;
901 Eigen::Matrix<T, 3, 1, MOpt> v = tv2-tv0;
904 Eigen::Matrix<T, 3, 1, MOpt> dir = sv1-sv0;
905 Eigen::Matrix<T, 3, 1, MOpt> w0 = sv0-tv0;
909 result->setConstant((std::numeric_limits<double>::quiet_NaN()));
912 if (std::abs(b) <= Geometry::AngularEpsilon)
914 if (std::abs(a) <= Geometry::AngularEpsilon)
917 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
918 for (
int i=0; i<2; ++i)
921 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
923 *result = (i==0)?sv0:sv1;
940 if (r < -Geometry::DistanceEpsilon)
945 if (r > 1+Geometry::DistanceEpsilon)
951 Eigen::Matrix<T, 3, 1, MOpt> presumedIntersection = sv0 + r * dir;
956 Eigen::Matrix<T, 3, 1, MOpt> w = presumedIntersection - tv0;
959 T D = uv * uv - uu * vv;
961 T s = (uv * wv - vv * wu) / D;
963 if (s < -Geometry::DistanceEpsilon || s > 1 + Geometry::DistanceEpsilon)
967 T t = (uv * wu - uu * wv) / D;
969 if (t < -Geometry::DistanceEpsilon || (s + t) > 1 + Geometry::DistanceEpsilon)
974 *result = sv0 + r * dir;
988 template <
class T,
int MOpt>
inline 990 const Eigen::Matrix<T, 3, 1, MOpt>& pt,
991 const Eigen::Matrix<T, 3, 1, MOpt>& n,
993 Eigen::Matrix<T, 3, 1, MOpt>* result)
995 T dist = n.dot(pt) + d;
996 *result = pt - n*dist;
1015 template <
class T,
int MOpt>
inline 1017 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1018 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1019 const Eigen::Matrix<T, 3, 1, MOpt>& n,
1021 Eigen::Matrix<T, 3, 1, MOpt>* closestPointSegment,
1022 Eigen::Matrix<T, 3, 1, MOpt>* planeIntersectionPoint)
1024 T dist0 = n.dot(sv0) + d;
1025 T dist1 = n.dot(sv1) + d;
1027 Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1028 if (std::abs(n.dot(v01)) <= Geometry::AngularEpsilon)
1030 *closestPointSegment = (sv0 + sv1)*T(0.5);
1031 dist0 = n.dot(*closestPointSegment) + d;
1032 *planeIntersectionPoint = *closestPointSegment - dist0*n;
1033 return (std::abs(dist0) < Geometry::DistanceEpsilon ? 0 : dist0);
1036 if ((dist0 > 0 && dist1 > 0) || (dist0 < 0 && dist1 < 0))
1038 if (std::abs(dist0) < std::abs(dist1))
1040 *closestPointSegment = sv0;
1041 *planeIntersectionPoint = sv0 - dist0*n;
1046 *closestPointSegment = sv1;
1047 *planeIntersectionPoint = sv1 - dist1*n;
1054 Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
1055 T lambda= (-d-sv0.dot(n)) / v01.dot(n);
1056 *planeIntersectionPoint = sv0 + lambda * v01;
1057 *closestPointSegment = *planeIntersectionPoint;
1076 template <
class T,
int MOpt>
inline 1078 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1079 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1080 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1081 const Eigen::Matrix<T, 3, 1, MOpt>& n,
1083 Eigen::Matrix<T, 3, 1, MOpt>* closestPointTriangle,
1084 Eigen::Matrix<T, 3, 1, MOpt>* planeProjectionPoint)
1086 Eigen::Matrix<T, 3, 1, MOpt> distances(n.dot(tv0) + d, n.dot(tv1) + d, n.dot(tv2) + d);
1087 Eigen::Matrix<T, 3, 1, MOpt> t01 = tv1-tv0;
1088 Eigen::Matrix<T, 3, 1, MOpt> t02 = tv2-tv0;
1089 Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2-tv1;
1091 closestPointTriangle->setConstant((std::numeric_limits<double>::quiet_NaN()));
1092 planeProjectionPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1097 if (std::abs(n.dot(t01)) <= Geometry::AngularEpsilon && std::abs(n.dot(t02)) <= Geometry::AngularEpsilon)
1099 *closestPointTriangle = (tv0 + tv1 + tv2) / T(3);
1100 *planeProjectionPoint = *closestPointTriangle - n * distances[0];
1101 return distances[0];
1105 if ((distances.array() < -Geometry::DistanceEpsilon).any() &&
1106 (distances.array() > Geometry::DistanceEpsilon).any())
1108 if (distances[0] * distances[1] < 0)
1110 *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t01) * t01;
1111 if (distances[0] * distances[2] < 0)
1113 *planeProjectionPoint = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1117 Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2-tv1;
1118 *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1123 *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1124 *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1128 *closestPointTriangle = *planeProjectionPoint = (*closestPointTriangle + *planeProjectionPoint) * T(0.5);
1133 distances.cwiseAbs().minCoeff(&index);
1137 *closestPointTriangle = tv0;
1138 *planeProjectionPoint = tv0 - n * distances[0];
1139 return distances[0];
1141 *closestPointTriangle = tv1;
1142 *planeProjectionPoint = tv1 - n * distances[1];
1143 return distances[1];
1145 *closestPointTriangle = tv2;
1146 *planeProjectionPoint = tv2 - n * distances[2];
1147 return distances[2];
1150 return std::numeric_limits<T>::quiet_NaN();
1160 template <
class T,
int MOpt>
inline 1162 const Eigen::Matrix<T, 3, 1, MOpt>& pn0, T pd0,
1163 const Eigen::Matrix<T, 3, 1, MOpt>& pn1, T pd1,
1164 Eigen::Matrix<T, 3, 1, MOpt>* pt0,
1165 Eigen::Matrix<T, 3, 1, MOpt>* pt1)
1168 const Eigen::Matrix<T, 3, 1, MOpt> lineDir = pn0.cross(pn1);
1169 const T lineDirNorm2 = lineDir.squaredNorm();
1171 pt0->setConstant((std::numeric_limits<double>::quiet_NaN()));
1172 pt1->setConstant((std::numeric_limits<double>::quiet_NaN()));
1175 if (lineDirNorm2 <= Geometry::SquaredDistanceEpsilon)
1180 *pt0 = (pd1*pn0-pd0*pn1).cross(lineDir) / lineDirNorm2;
1181 *pt1 = *pt0 + lineDir;
1196 template <
class T,
int MOpt>
inline 1198 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1199 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1200 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1201 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1202 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1203 Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1204 Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1206 Eigen::Matrix<T, 3, 1, MOpt> n = (tv1 - tv0).cross(tv2 - tv1);
1221 template <
class T,
int MOpt>
inline 1223 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1224 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1225 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1226 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1227 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1228 const Eigen::Matrix<T, 3, 1, MOpt>& normal,
1229 Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1230 Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1232 segmentPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1233 trianglePoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1236 const Eigen::Matrix<T, 3, 1, MOpt>& n = normal;
1238 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1240 const Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
1241 const T v01DotTn = n.dot(v01);
1242 if (std::abs(v01DotTn) <= Geometry::AngularEpsilon)
1247 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1249 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1251 *segmentPoint = sv0;
1256 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1258 *segmentPoint = sv1;
1265 T lambda = -n.dot(sv0-tv0) / v01DotTn;
1266 if (lambda >= 0 && lambda <= 1)
1268 *segmentPoint = *trianglePoint = sv0 + lambda * v01;
1270 if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1278 Eigen::Matrix<T, 3, 1, MOpt> segColPt01, segColPt02, segColPt12, triColPt01, triColPt02, triColPt12;
1282 Eigen::Matrix<T, 3, 1, MOpt> ptTriCol0, ptTriCol1;
1285 if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1287 dstPtTri0 = std::numeric_limits<T>::max();
1291 if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1293 dstPtTri1 = std::numeric_limits<T>::max();
1297 Eigen::Matrix<double, 5, 1> distances;
1298 (distances << dst01, dst02, dst12, dstPtTri0, dstPtTri1).finished().minCoeff(&minIndex);
1302 *segmentPoint = segColPt01;
1303 *trianglePoint = triColPt01;
1306 *segmentPoint = segColPt02;
1307 *trianglePoint = triColPt02;
1310 *segmentPoint = segColPt12;
1311 *trianglePoint = triColPt12;
1314 *segmentPoint = sv0;
1315 *trianglePoint = ptTriCol0;
1318 *segmentPoint = sv1;
1319 *trianglePoint = ptTriCol1;
1324 return std::numeric_limits<T>::quiet_NaN();
1339 template <
class T,
int MOpt>
inline 1341 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1342 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1343 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1344 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1345 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1346 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1347 Eigen::Matrix<T, 3, 1, MOpt>* closestPoint0,
1348 Eigen::Matrix<T, 3, 1, MOpt>* closestPoint1)
1351 T minDst = std::numeric_limits<T>::max();
1353 Eigen::Matrix<T, 3, 1, MOpt> segPt, triPt;
1354 Eigen::Matrix<T, 3, 1, MOpt> n0 = (t0v1-t0v0).cross(t0v2-t0v0);
1356 Eigen::Matrix<T, 3, 1, MOpt> n1 = (t1v1-t1v0).cross(t1v2-t1v0);
1359 if (currDst < minDst)
1362 *closestPoint0 = segPt;
1363 *closestPoint1 = triPt;
1366 if (currDst < minDst)
1369 *closestPoint0 = segPt;
1370 *closestPoint1 = triPt;
1373 if (currDst < minDst)
1376 *closestPoint0 = segPt;
1377 *closestPoint1 = triPt;
1381 if (currDst < minDst)
1384 *closestPoint1 = segPt;
1385 *closestPoint0 = triPt;
1388 if (currDst < minDst)
1391 *closestPoint1 = segPt;
1392 *closestPoint0 = triPt;
1395 if (currDst < minDst)
1398 *closestPoint1 = segPt;
1399 *closestPoint0 = triPt;
1410 template <
class T,
int MOpt>
1412 const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1413 const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1414 const Eigen::AlignedBox<T, 3>& box,
1415 std::vector<Eigen::Matrix<T, 3, 1, MOpt> >* intersections)
1417 Eigen::Array<T, 3, 1, MOpt> v01 = sv1 - sv0;
1418 Eigen::Array<bool, 3, 1, MOpt> parallelToPlane = (v01.cwiseAbs().array() < Geometry::DistanceEpsilon);
1419 if (parallelToPlane.any())
1421 Eigen::Array<bool, 3, 1, MOpt> beyondMinCorner = (sv0.array() < box.min().array());
1422 Eigen::Array<bool, 3, 1, MOpt> beyondMaxCorner = (sv0.array() > box.max().array());
1423 if ((parallelToPlane && (beyondMinCorner || beyondMaxCorner)).any())
1432 Eigen::Array<T, 3, 2, MOpt> planeIntersectionAbscissas;
1433 planeIntersectionAbscissas.col(0) = (box.min().array() - sv0.array());
1434 planeIntersectionAbscissas.col(1) = (box.max().array() - sv0.array());
1438 planeIntersectionAbscissas.colwise() /= v01;
1440 T entranceAbscissa = planeIntersectionAbscissas.rowwise().minCoeff().maxCoeff();
1441 T exitAbscissa = planeIntersectionAbscissas.rowwise().maxCoeff().minCoeff();
1442 if (entranceAbscissa < exitAbscissa && exitAbscissa > T(0.0))
1444 if (entranceAbscissa >= T(0.0) && entranceAbscissa <= T(1.0))
1446 intersections->push_back(sv0 + v01.matrix() * entranceAbscissa);
1449 if (exitAbscissa >= T(0.0) && exitAbscissa <= T(1.0))
1451 intersections->push_back(sv0 + v01.matrix() * exitAbscissa);
1464 template <
class T,
int MOpt>
1466 const Eigen::Matrix<T, 3, 1, MOpt>& capsuleBottom,
1467 const Eigen::Matrix<T, 3, 1, MOpt>& capsuleTop,
1468 const T capsuleRadius,
1469 const Eigen::AlignedBox<T, 3>& box)
1471 Eigen::AlignedBox<double, 3> dilatedBox(box.min().array() - capsuleRadius, box.max().array() + capsuleRadius);
1472 std::vector<Vector3d> candidates;
1474 if (dilatedBox.contains(capsuleBottom))
1476 candidates.push_back(capsuleBottom);
1478 if (dilatedBox.contains(capsuleTop))
1480 candidates.push_back(capsuleTop);
1483 bool doesIntersect =
false;
1484 ptrdiff_t dimensionsOutsideBox;
1485 Vector3d clampedPosition, segmentPoint;
1486 for (
auto candidate = candidates.cbegin(); candidate != candidates.cend(); ++candidate)
1493 dimensionsOutsideBox = (candidate->array() > box.max().array()).count();
1494 dimensionsOutsideBox += (candidate->array() < box.min().array()).count();
1495 if (dimensionsOutsideBox >= 2)
1497 clampedPosition = (*candidate).array().min(box.max().array()).max(box.min().array());
1498 if (
distancePointSegment(clampedPosition, capsuleBottom, capsuleTop, &segmentPoint) > capsuleRadius)
1504 doesIntersect =
true;
1507 return doesIntersect;
1520 template <
class T,
int MOpt>
inline 1522 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1523 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1524 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1525 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1526 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1527 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1528 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1529 const Eigen::Matrix<T, 3, 1, MOpt>& t1n);
1537 template <
class T,
int MOpt>
inline 1539 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1540 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1541 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1542 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1543 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1544 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2);
1565 template <
class T,
int MOpt>
inline 1567 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1568 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1569 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1570 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1571 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1572 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1573 const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1574 const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
1575 T* penetrationDepth,
1576 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1577 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1578 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1597 template <
class T,
int MOpt>
inline 1599 const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1600 const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1601 const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1602 const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1603 const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1604 const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1605 T* penetrationDepth,
1606 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1607 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1608 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
T distanceTrianglePlane(const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *closestPointTriangle, Eigen::Matrix< T, 3, 1, MOpt > *planeProjectionPoint)
Calculate the distance of a triangle to a plane.
Definition: Geometry.h:1077
Definition: DriveElementFromInputBehavior.cpp:27
T distancePointTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the normal distance of a point from a triangle, the resulting point will be on the edge of ...
Definition: Geometry.h:669
T distancePointSegment(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, Eigen::Matrix< T, 3, 1, MOpt > *result)
Point segment distance, if the projection of the closest point is not within the segments, the closest segment point is used for the distance calculation.
Definition: Geometry.h:214
T distanceSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *segmentPoint, Eigen::Matrix< T, 3, 1, MOpt > *trianglePoint)
Calculate the distance of a line segment to a triangle.
Definition: Geometry.h:1197
bool doesIntersectPlanePlane(const Eigen::Matrix< T, 3, 1, MOpt > &pn0, T pd0, const Eigen::Matrix< T, 3, 1, MOpt > &pn1, T pd1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1)
Test if two planes are intersecting, if yes also calculate the intersection line. ...
Definition: Geometry.h:1161
T distanceTriangleTriangle(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, Eigen::Matrix< T, 3, 1, MOpt > *closestPoint0, Eigen::Matrix< T, 3, 1, MOpt > *closestPoint1)
Calculate the distance between two triangles.
Definition: Geometry.h:1340
T distancePointPlane(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the distance of a point to a plane.
Definition: Geometry.h:989
The convenience header that provides the entirety of the logging API.
bool doesIntersectBoxCapsule(const Eigen::Matrix< T, 3, 1, MOpt > &capsuleBottom, const Eigen::Matrix< T, 3, 1, MOpt > &capsuleTop, const T capsuleRadius, const Eigen::AlignedBox< T, 3 > &box)
Test if an axis aligned box intersects with a capsule.
Definition: Geometry.h:1465
T distancePointLine(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &v0, const Eigen::Matrix< T, 3, 1, MOpt > &v1, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the normal distance between a point and a line.
Definition: Geometry.h:183
void intersectionsSegmentBox(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::AlignedBox< T, 3 > &box, std::vector< Eigen::Matrix< T, 3, 1, MOpt > > *intersections)
Calculate the intersections between a line segment and an axis aligned box.
Definition: Geometry.h:1411
T distanceSegmentSegment(const Eigen::Matrix< T, 3, 1, MOpt > &s0v0, const Eigen::Matrix< T, 3, 1, MOpt > &s0v1, const Eigen::Matrix< T, 3, 1, MOpt > &s1v0, const Eigen::Matrix< T, 3, 1, MOpt > &s1v1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1, T *s0t=nullptr, T *s1t=nullptr)
Distance between two segments, if the project of the closest point is not on the opposing segment...
Definition: Geometry.h:321
bool isCoplanar(const Eigen::Matrix< T, 3, 1, MOpt > &a, const Eigen::Matrix< T, 3, 1, MOpt > &b, const Eigen::Matrix< T, 3, 1, MOpt > &c, const Eigen::Matrix< T, 3, 1, MOpt > &d)
Check whether the points are coplanar.
Definition: Geometry.h:168
T distanceSegmentPlane(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *closestPointSegment, Eigen::Matrix< T, 3, 1, MOpt > *planeIntersectionPoint)
Calculate the distance between a segment and a plane.
Definition: Geometry.h:1016
bool barycentricCoordinates(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn, Eigen::Matrix< T, 3, 1, MOpt > *coordinates)
Calculate the barycentric coordinates of a point with respect to a triangle.
Definition: Geometry.h:71
T distanceLineLine(const Eigen::Matrix< T, 3, 1, MOpt > &l0v0, const Eigen::Matrix< T, 3, 1, MOpt > &l0v1, const Eigen::Matrix< T, 3, 1, MOpt > &l1v0, const Eigen::Matrix< T, 3, 1, MOpt > &l1v1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1)
Determine the distance between two lines.
Definition: Geometry.h:254
Definitions of small fixed-size vector types.
bool calculateContactTriangleTriangle(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, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint0, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint1, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal)
Calculate the contact between two triangles.
Definition: TriangleTriangleContactCalculation-inl.h:216
bool isPointInsideTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn)
Check if a point is inside a triangle.
Definition: Geometry.h:124
bool doesCollideSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the intersection of a line segment with a triangle See http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle for the algorithm.
Definition: Geometry.h:890
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
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:56