1namespace muda::distance
3template <
class T,
int dim>
4MUDA_GENERIC PointPointDistanceType point_point_distance_type(
5 const Eigen::Vector<T, dim>& p0,
const Eigen::Vector<T, dim>& p1)
8 return PointPointDistanceType::PP;
11template <
class T,
int dim>
12MUDA_GENERIC PointEdgeDistanceType
13point_edge_distance_type(
const Eigen::Vector<T, dim>& p,
14 const Eigen::Vector<T, dim>& e0,
15 const Eigen::Vector<T, dim>& e1,
18 const Eigen::Vector<T, dim> e = e1 - e0;
19 ratio = e.dot(p - e0) / e.squaredNorm();
23 return PointEdgeDistanceType::PP_PE0;
28 return PointEdgeDistanceType::PP_PE1;
33 return PointEdgeDistanceType::PE;
37template <
class T,
int dim>
38MUDA_GENERIC PointEdgeDistanceType
39point_edge_distance_type(
const Eigen::Vector<T, dim>& p,
40 const Eigen::Vector<T, dim>& e0,
41 const Eigen::Vector<T, dim>& e1)
44 return point_edge_distance_type(p, e0, e1, ratio);
48MUDA_GENERIC PointTriangleDistanceType
49point_triangle_distance_type(
const Eigen::Vector<T, 3>& p,
50 const Eigen::Vector<T, 3>& t0,
51 const Eigen::Vector<T, 3>& t1,
52 const Eigen::Vector<T, 3>& t2)
54 Eigen::Matrix<T, 2, 3> basis;
55 basis.row(0) = (t1 - t0).transpose();
56 basis.row(1) = (t2 - t0).transpose();
58 const Eigen::Vector<T, 3> nVec = basis.row(0).cross(basis.row(1));
60 Eigen::Matrix<T, 2, 3> param;
62 basis.row(1) = basis.row(0).cross(nVec);
63 param.col(0) = (basis * basis.transpose()).inverse() * (basis * (p - t0));
64 if(param(0, 0) > 0.0 && param(0, 0) < 1.0 && param(1, 0) >= 0.0)
67 return PointTriangleDistanceType::PE_PT0T1;
71 basis.row(0) = (t2 - t1).transpose();
73 basis.row(1) = basis.row(0).cross(nVec);
75 param.col(1) = (basis * basis.transpose()).inverse() * (basis * (p - t1));
77 if(param(0, 1) > 0.0 && param(0, 1) < 1.0 && param(1, 1) >= 0.0)
80 return PointTriangleDistanceType::PE_PT1T2;
84 basis.row(0) = (t0 - t2).transpose();
86 basis.row(1) = basis.row(0).cross(nVec);
87 param.col(2) = (basis * basis.transpose()).inverse() * (basis * (p - t2));
88 if(param(0, 2) > 0.0 && param(0, 2) < 1.0 && param(1, 2) >= 0.0)
91 return PointTriangleDistanceType::PE_PT2T0;
95 if(param(0, 0) <= 0.0 && param(0, 2) >= 1.0)
98 return PointTriangleDistanceType::PP_PT0;
100 else if(param(0, 1) <= 0.0 && param(0, 0) >= 1.0)
103 return PointTriangleDistanceType::PP_PT1;
105 else if(param(0, 2) <= 0.0 && param(0, 1) >= 1.0)
108 return PointTriangleDistanceType::PP_PT2;
113 return PointTriangleDistanceType::PT;
122MUDA_GENERIC EdgeEdgeDistanceType edge_edge_distance_type(
const Eigen::Vector<T, 3>& ea0,
123 const Eigen::Vector<T, 3>& ea1,
124 const Eigen::Vector<T, 3>& eb0,
125 const Eigen::Vector<T, 3>& eb1)
127 Eigen::Vector<T, 3> u = ea1 - ea0;
128 Eigen::Vector<T, 3> v = eb1 - eb0;
129 Eigen::Vector<T, 3> w = ea0 - eb0;
130 T a = u.squaredNorm();
132 T c = v.squaredNorm();
139 EdgeEdgeDistanceType defaultCase = EdgeEdgeDistanceType::EE;
142 sN = (b * e - c * d);
148 defaultCase = EdgeEdgeDistanceType::PE_Ea0Eb0Eb1;
155 defaultCase = EdgeEdgeDistanceType::PE_Ea1Eb0Eb1;
159 tN = (a * e - b * d);
160 if(tN > 0.0 && tN < tD
161 && (u.cross(v).dot(w) == 0.0 || u.cross(v).squaredNorm() < 1.0e-20 * a * c))
171 defaultCase = EdgeEdgeDistanceType::PE_Ea0Eb0Eb1;
178 defaultCase = EdgeEdgeDistanceType::PE_Ea1Eb0Eb1;
190 return EdgeEdgeDistanceType::PP_Ea0Eb0;
195 return EdgeEdgeDistanceType::PP_Ea1Eb0;
200 return EdgeEdgeDistanceType::PE_Eb0Ea0Ea1;
209 return EdgeEdgeDistanceType::PP_Ea0Eb1;
211 else if((-d + b) >= a)
214 return EdgeEdgeDistanceType::PP_Ea1Eb1;
219 return EdgeEdgeDistanceType::PE_Eb1Ea0Ea1;