MUDA
Loading...
Searching...
No Matches
distance_type.inl
1namespace muda::distance
2{
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)
6{
7 //return 0
8 return PointPointDistanceType::PP;
9}
10
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,
16 T& ratio)
17{
18 const Eigen::Vector<T, dim> e = e1 - e0;
19 ratio = e.dot(p - e0) / e.squaredNorm();
20 if(ratio <= 0)
21 {
22 // return 0;
23 return PointEdgeDistanceType::PP_PE0; // PP (p-e0)
24 }
25 else if(ratio >= 1)
26 {
27 // return 1;
28 return PointEdgeDistanceType::PP_PE1; // PP (p-e1)
29 }
30 else
31 {
32 // return 2;
33 return PointEdgeDistanceType::PE; // PE
34 }
35}
36
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)
42{
43 T ratio;
44 return point_edge_distance_type(p, e0, e1, ratio);
45}
46
47template <class T>
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)
53{
54 Eigen::Matrix<T, 2, 3> basis;
55 basis.row(0) = (t1 - t0).transpose();
56 basis.row(1) = (t2 - t0).transpose();
57
58 const Eigen::Vector<T, 3> nVec = basis.row(0).cross(basis.row(1));
59
60 Eigen::Matrix<T, 2, 3> param;
61
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)
65 {
66 // return 3;
67 return PointTriangleDistanceType::PE_PT0T1; // PE t0t1
68 }
69 else
70 {
71 basis.row(0) = (t2 - t1).transpose();
72
73 basis.row(1) = basis.row(0).cross(nVec);
74
75 param.col(1) = (basis * basis.transpose()).inverse() * (basis * (p - t1));
76
77 if(param(0, 1) > 0.0 && param(0, 1) < 1.0 && param(1, 1) >= 0.0)
78 {
79 // return 4;
80 return PointTriangleDistanceType::PE_PT1T2; // PE t1t2
81 }
82 else
83 {
84 basis.row(0) = (t0 - t2).transpose();
85
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)
89 {
90 // return 5
91 return PointTriangleDistanceType::PE_PT2T0; // PE t2t0
92 }
93 else
94 {
95 if(param(0, 0) <= 0.0 && param(0, 2) >= 1.0)
96 {
97 // return 0;
98 return PointTriangleDistanceType::PP_PT0; // PP t0
99 }
100 else if(param(0, 1) <= 0.0 && param(0, 0) >= 1.0)
101 {
102 // return 1;
103 return PointTriangleDistanceType::PP_PT1; // PP t1
104 }
105 else if(param(0, 2) <= 0.0 && param(0, 1) >= 1.0)
106 {
107 // return 2;
108 return PointTriangleDistanceType::PP_PT2; // PP t2
109 }
110 else
111 {
112 // return 6;
113 return PointTriangleDistanceType::PT; // PT
114 }
115 }
116 }
117 }
118}
119
120// a more robust implementation of http://geomalgorithms.com/a07-_distance.html
121template <class T>
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)
126{
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(); // always >= 0
131 T b = u.dot(v);
132 T c = v.squaredNorm(); // always >= 0
133 T d = u.dot(w);
134 T e = v.dot(w);
135 T D = a * c - b * b; // always >= 0
136 T tD = D; // tc = tN / tD, default tD = D >= 0
137 T sN, tN;
138
139 EdgeEdgeDistanceType defaultCase = EdgeEdgeDistanceType::EE; // 8
140
141 // compute the line parameters of the two closest points
142 sN = (b * e - c * d);
143 if(sN <= 0.0)
144 { // sc < 0 => the s=0 edge is visible
145 tN = e;
146 tD = c;
147 // defaultCase = 2;
148 defaultCase = EdgeEdgeDistanceType::PE_Ea0Eb0Eb1;
149 }
150 else if(sN >= D)
151 { // sc > 1 => the s=1 edge is visible
152 tN = e + b;
153 tD = c;
154 // defaultCase = 5;
155 defaultCase = EdgeEdgeDistanceType::PE_Ea1Eb0Eb1;
156 }
157 else
158 {
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))
162 {
163 // if (tN > 0.0 && tN < tD && (u.cross(v).dot(w) == 0.0 || u.cross(v).squaredNorm() == 0.0)) {
164 // std::cout << u.cross(v).squaredNorm() / (a * c) << ": " << sN << " " << D << ", " << tN << " " << tD << std::endl;
165 // avoid coplanar or nearly parallel EE
166 if(sN < D / 2)
167 {
168 tN = e;
169 tD = c;
170 // defaultCase = 2;
171 defaultCase = EdgeEdgeDistanceType::PE_Ea0Eb0Eb1;
172 }
173 else
174 {
175 tN = e + b;
176 tD = c;
177 // defaultCase = 5;
178 defaultCase = EdgeEdgeDistanceType::PE_Ea1Eb0Eb1;
179 }
180 }
181 // else defaultCase stays as 8
182 }
183
184 if(tN <= 0.0)
185 { // tc < 0 => the t=0 edge is visible
186 // recompute sc for this edge
187 if(-d <= 0.0)
188 {
189 // return 0;
190 return EdgeEdgeDistanceType::PP_Ea0Eb0;
191 }
192 else if(-d >= a)
193 {
194 // return 3;
195 return EdgeEdgeDistanceType::PP_Ea1Eb0;
196 }
197 else
198 {
199 // return 6;
200 return EdgeEdgeDistanceType::PE_Eb0Ea0Ea1;
201 }
202 }
203 else if(tN >= tD)
204 { // tc > 1 => the t=1 edge is visible
205 // recompute sc for this edge
206 if((-d + b) <= 0.0)
207 {
208 // return 1;
209 return EdgeEdgeDistanceType::PP_Ea0Eb1;
210 }
211 else if((-d + b) >= a)
212 {
213 // return 4;
214 return EdgeEdgeDistanceType::PP_Ea1Eb1;
215 }
216 else
217 {
218 // return 7;
219 return EdgeEdgeDistanceType::PE_Eb1Ea0Ea1;
220 }
221 }
222
223 return defaultCase;
224}
225
226} // namespace muda::distance