GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2011-2014, Willow Garage, Inc. |
||
5 |
* Copyright (c) 2014-2015, Open Source Robotics Foundation |
||
6 |
* All rights reserved. |
||
7 |
* |
||
8 |
* Redistribution and use in source and binary forms, with or without |
||
9 |
* modification, are permitted provided that the following conditions |
||
10 |
* are met: |
||
11 |
* |
||
12 |
* * Redistributions of source code must retain the above copyright |
||
13 |
* notice, this list of conditions and the following disclaimer. |
||
14 |
* * Redistributions in binary form must reproduce the above |
||
15 |
* copyright notice, this list of conditions and the following |
||
16 |
* disclaimer in the documentation and/or other materials provided |
||
17 |
* with the distribution. |
||
18 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
19 |
* contributors may be used to endorse or promote products derived |
||
20 |
* from this software without specific prior written permission. |
||
21 |
* |
||
22 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
23 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
24 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
25 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
26 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
27 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
28 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
29 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
30 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
31 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
32 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
33 |
* POSSIBILITY OF SUCH DAMAGE. |
||
34 |
*/ |
||
35 |
|||
36 |
/** \author Jia Pan */ |
||
37 |
|||
38 |
#include <hpp/fcl/internal/intersect.h> |
||
39 |
#include <iostream> |
||
40 |
#include <limits> |
||
41 |
#include <vector> |
||
42 |
#include <cmath> |
||
43 |
#include <hpp/fcl/internal/tools.h> |
||
44 |
|||
45 |
namespace hpp { |
||
46 |
namespace fcl { |
||
47 |
|||
48 |
bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, |
||
49 |
const Vec3f& v3, Vec3f* n, FCL_REAL* t) { |
||
50 |
Vec3f n_ = (v2 - v1).cross(v3 - v1); |
||
51 |
FCL_REAL norm2 = n_.squaredNorm(); |
||
52 |
if (norm2 > 0) { |
||
53 |
*n = n_ / sqrt(norm2); |
||
54 |
*t = n->dot(v1); |
||
55 |
return true; |
||
56 |
} |
||
57 |
return false; |
||
58 |
} |
||
59 |
|||
60 |
838026 |
void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, |
|
61 |
const Vec3f& B, Vec3f& VEC, Vec3f& X, |
||
62 |
Vec3f& Y) { |
||
63 |
✓✗ | 838026 |
Vec3f T; |
64 |
FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; |
||
65 |
✓✗ | 838026 |
Vec3f TMP; |
66 |
|||
67 |
✓✗✓✗ |
838026 |
T = Q - P; |
68 |
✓✗ | 838026 |
A_dot_A = A.dot(A); |
69 |
✓✗ | 838026 |
B_dot_B = B.dot(B); |
70 |
✓✗ | 838026 |
A_dot_B = A.dot(B); |
71 |
✓✗ | 838026 |
A_dot_T = A.dot(T); |
72 |
✓✗ | 838026 |
B_dot_T = B.dot(T); |
73 |
|||
74 |
// t parameterizes ray P,A |
||
75 |
// u parameterizes ray Q,B |
||
76 |
|||
77 |
FCL_REAL t, u; |
||
78 |
|||
79 |
// compute t for the closest point on ray P,A to |
||
80 |
// ray Q,B |
||
81 |
|||
82 |
838026 |
FCL_REAL denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; |
|
83 |
|||
84 |
838026 |
t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom; |
|
85 |
|||
86 |
// clamp result so t is on the segment P,A |
||
87 |
|||
88 |
✓✓✓✓ ✓✓ |
838026 |
if ((t < 0) || std::isnan(t)) |
89 |
343684 |
t = 0; |
|
90 |
✓✓ | 494342 |
else if (t > 1) |
91 |
364824 |
t = 1; |
|
92 |
|||
93 |
// find u for point on ray Q,B closest to point at t |
||
94 |
|||
95 |
838026 |
u = (t * A_dot_B - B_dot_T) / B_dot_B; |
|
96 |
|||
97 |
// if u is on segment Q,B, t and u correspond to |
||
98 |
// closest points, otherwise, clamp u, recompute and |
||
99 |
// clamp t |
||
100 |
|||
101 |
✓✓✗✓ ✓✓ |
838026 |
if ((u <= 0) || std::isnan(u)) { |
102 |
✓✗ | 332963 |
Y = Q; |
103 |
|||
104 |
332963 |
t = A_dot_T / A_dot_A; |
|
105 |
|||
106 |
✓✓✗✓ ✓✓ |
332963 |
if ((t <= 0) || std::isnan(t)) { |
107 |
✓✗ | 103970 |
X = P; |
108 |
✓✗✓✗ |
103970 |
VEC = Q - P; |
109 |
✓✓ | 228993 |
} else if (t >= 1) { |
110 |
✓✗✓✗ |
192168 |
X = P + A; |
111 |
✓✗✓✗ |
192168 |
VEC = Q - X; |
112 |
} else { |
||
113 |
✓✗✓✗ ✓✗ |
36825 |
X = P + A * t; |
114 |
✓✗ | 36825 |
TMP = T.cross(A); |
115 |
✓✗ | 36825 |
VEC = A.cross(TMP); |
116 |
} |
||
117 |
✓✓ | 505063 |
} else if (u >= 1) { |
118 |
✓✗✓✗ |
449660 |
Y = Q + B; |
119 |
|||
120 |
449660 |
t = (A_dot_B + A_dot_T) / A_dot_A; |
|
121 |
|||
122 |
✓✓✗✓ ✓✓ |
449660 |
if ((t <= 0) || std::isnan(t)) { |
123 |
✓✗ | 207309 |
X = P; |
124 |
✓✗✓✗ |
207309 |
VEC = Y - P; |
125 |
✓✓ | 242351 |
} else if (t >= 1) { |
126 |
✓✗✓✗ |
192265 |
X = P + A; |
127 |
✓✗✓✗ |
192265 |
VEC = Y - X; |
128 |
} else { |
||
129 |
✓✗✓✗ ✓✗ |
50086 |
X = P + A * t; |
130 |
✓✗✓✗ |
50086 |
T = Y - P; |
131 |
✓✗ | 50086 |
TMP = T.cross(A); |
132 |
✓✗ | 50086 |
VEC = A.cross(TMP); |
133 |
} |
||
134 |
} else { |
||
135 |
✓✗✓✗ ✓✗ |
55403 |
Y = Q + B * u; |
136 |
|||
137 |
✓✓✗✓ ✓✓ |
55403 |
if ((t <= 0) || std::isnan(t)) { |
138 |
✓✗ | 12676 |
X = P; |
139 |
✓✗ | 12676 |
TMP = T.cross(B); |
140 |
✓✗ | 12676 |
VEC = B.cross(TMP); |
141 |
✓✓ | 42727 |
} else if (t >= 1) { |
142 |
✓✗✓✗ |
26860 |
X = P + A; |
143 |
✓✗✓✗ |
26860 |
T = Q - X; |
144 |
✓✗ | 26860 |
TMP = T.cross(B); |
145 |
✓✗ | 26860 |
VEC = B.cross(TMP); |
146 |
} else { |
||
147 |
✓✗✓✗ ✓✗ |
15867 |
X = P + A * t; |
148 |
✓✗ | 15867 |
VEC = A.cross(B); |
149 |
✓✗✓✓ |
15867 |
if (VEC.dot(T) < 0) { |
150 |
✓✗✓✗ |
7885 |
VEC = VEC * (-1); |
151 |
} |
||
152 |
} |
||
153 |
} |
||
154 |
838026 |
} |
|
155 |
|||
156 |
331883 |
FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], |
|
157 |
Vec3f& P, Vec3f& Q) { |
||
158 |
// Compute vectors along the 6 sides |
||
159 |
|||
160 |
✓✓✓✗ |
1327532 |
Vec3f Sv[3]; |
161 |
✓✓✓✗ |
1327532 |
Vec3f Tv[3]; |
162 |
✓✗ | 331883 |
Vec3f VEC; |
163 |
|||
164 |
✓✗✓✗ |
331883 |
Sv[0] = S[1] - S[0]; |
165 |
✓✗✓✗ |
331883 |
Sv[1] = S[2] - S[1]; |
166 |
✓✗✓✗ |
331883 |
Sv[2] = S[0] - S[2]; |
167 |
|||
168 |
✓✗✓✗ |
331883 |
Tv[0] = T[1] - T[0]; |
169 |
✓✗✓✗ |
331883 |
Tv[1] = T[2] - T[1]; |
170 |
✓✗✓✗ |
331883 |
Tv[2] = T[0] - T[2]; |
171 |
|||
172 |
// For each edge pair, the vector connecting the closest points |
||
173 |
// of the edges defines a slab (parallel planes at head and tail |
||
174 |
// enclose the slab). If we can show that the off-edge vertex of |
||
175 |
// each triangle is outside of the slab, then the closest points |
||
176 |
// of the edges are the closest points for the triangles. |
||
177 |
// Even if these tests fail, it may be helpful to know the closest |
||
178 |
// points found, and whether the triangles were shown disjoint |
||
179 |
|||
180 |
✓✗✓✗ ✓✗✓✗ |
331883 |
Vec3f V, Z, minP, minQ; |
181 |
FCL_REAL mindd; |
||
182 |
331883 |
int shown_disjoint = 0; |
|
183 |
|||
184 |
✓✗✓✗ |
331883 |
mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high |
185 |
|||
186 |
✓✓ | 463989 |
for (int i = 0; i < 3; ++i) { |
187 |
✓✓ | 970132 |
for (int j = 0; j < 3; ++j) { |
188 |
// Find closest points on edges i & j, plus the |
||
189 |
// vector (and distance squared) between these points |
||
190 |
✓✗ | 838026 |
segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); |
191 |
|||
192 |
✓✗✓✗ |
838026 |
V = Q - P; |
193 |
✓✗ | 838026 |
FCL_REAL dd = V.dot(V); |
194 |
|||
195 |
// Verify this closest point pair only if the distance |
||
196 |
// squared is less than the minimum found thus far. |
||
197 |
|||
198 |
✓✓ | 838026 |
if (dd <= mindd) { |
199 |
✓✗ | 696152 |
minP = P; |
200 |
✓✗ | 696152 |
minQ = Q; |
201 |
696152 |
mindd = dd; |
|
202 |
|||
203 |
✓✗✓✗ |
696152 |
Z = S[(i + 2) % 3] - P; |
204 |
✓✗ | 696152 |
FCL_REAL a = Z.dot(VEC); |
205 |
✓✗✓✗ |
696152 |
Z = T[(j + 2) % 3] - Q; |
206 |
✓✗ | 696152 |
FCL_REAL b = Z.dot(VEC); |
207 |
|||
208 |
✓✓✓✓ |
696152 |
if ((a <= 0) && (b >= 0)) return dd; |
209 |
|||
210 |
✓✗ | 367591 |
FCL_REAL p = V.dot(VEC); |
211 |
|||
212 |
✓✓ | 367591 |
if (a < 0) a = 0; |
213 |
✓✓ | 367591 |
if (b > 0) b = 0; |
214 |
✓✓ | 367591 |
if ((p - a + b) > 0) shown_disjoint = 1; |
215 |
} |
||
216 |
} |
||
217 |
} |
||
218 |
|||
219 |
// No edge pairs contained the closest points. |
||
220 |
// either: |
||
221 |
// 1. one of the closest points is a vertex, and the |
||
222 |
// other point is interior to a face. |
||
223 |
// 2. the triangles are overlapping. |
||
224 |
// 3. an edge of one triangle is parallel to the other's face. If |
||
225 |
// cases 1 and 2 are not true, then the closest points from the 9 |
||
226 |
// edge pairs checks above can be taken as closest points for the |
||
227 |
// triangles. |
||
228 |
// 4. possibly, the triangles were degenerate. When the |
||
229 |
// triangle points are nearly colinear or coincident, one |
||
230 |
// of above tests might fail even though the edges tested |
||
231 |
// contain the closest points. |
||
232 |
|||
233 |
// First check for case 1 |
||
234 |
|||
235 |
✓✗ | 3322 |
Vec3f Sn; |
236 |
FCL_REAL Snl; |
||
237 |
|||
238 |
✓✗ | 3322 |
Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle |
239 |
✓✗ | 3322 |
Snl = Sn.dot(Sn); // Compute square of length of normal |
240 |
|||
241 |
// If cross product is long enough, |
||
242 |
|||
243 |
✓✗ | 3322 |
if (Snl > 1e-15) { |
244 |
// Get projection lengths of T points |
||
245 |
|||
246 |
✓✗ | 3322 |
Vec3f Tp; |
247 |
|||
248 |
✓✗✓✗ |
3322 |
V = S[0] - T[0]; |
249 |
✓✗✓✗ |
3322 |
Tp[0] = V.dot(Sn); |
250 |
|||
251 |
✓✗✓✗ |
3322 |
V = S[0] - T[1]; |
252 |
✓✗✓✗ |
3322 |
Tp[1] = V.dot(Sn); |
253 |
|||
254 |
✓✗✓✗ |
3322 |
V = S[0] - T[2]; |
255 |
✓✗✓✗ |
3322 |
Tp[2] = V.dot(Sn); |
256 |
|||
257 |
// If Sn is a separating direction, |
||
258 |
// find point with smallest projection |
||
259 |
|||
260 |
3322 |
int point = -1; |
|
261 |
✓✗✓✓ ✓✗✓✓ ✓✗✓✓ ✓✓ |
3322 |
if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) { |
262 |
✓✗✓✗ ✓✓ |
450 |
if (Tp[0] < Tp[1]) |
263 |
272 |
point = 0; |
|
264 |
else |
||
265 |
178 |
point = 1; |
|
266 |
✓✗✓✗ ✓✓ |
450 |
if (Tp[2] < Tp[point]) point = 2; |
267 |
✓✗✓✓ ✓✗✓✓ ✓✗✓✓ ✓✓ |
2872 |
} else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) { |
268 |
✓✗✓✗ ✓✓ |
1270 |
if (Tp[0] > Tp[1]) |
269 |
768 |
point = 0; |
|
270 |
else |
||
271 |
502 |
point = 1; |
|
272 |
✓✗✓✗ ✓✓ |
1270 |
if (Tp[2] > Tp[point]) point = 2; |
273 |
} |
||
274 |
|||
275 |
// If Sn is a separating direction, |
||
276 |
|||
277 |
✓✓ | 3322 |
if (point >= 0) { |
278 |
1720 |
shown_disjoint = 1; |
|
279 |
|||
280 |
// Test whether the point found, when projected onto the |
||
281 |
// other triangle, lies within the face. |
||
282 |
|||
283 |
✓✗✓✗ |
1720 |
V = T[point] - S[0]; |
284 |
✓✗ | 1720 |
Z = Sn.cross(Sv[0]); |
285 |
✓✗✓✓ |
1720 |
if (V.dot(Z) > 0) { |
286 |
✓✗✓✗ |
1642 |
V = T[point] - S[1]; |
287 |
✓✗ | 1642 |
Z = Sn.cross(Sv[1]); |
288 |
✓✗✓✓ |
1642 |
if (V.dot(Z) > 0) { |
289 |
✓✗✓✗ |
1554 |
V = T[point] - S[2]; |
290 |
✓✗ | 1554 |
Z = Sn.cross(Sv[2]); |
291 |
✓✗✓✓ |
1554 |
if (V.dot(Z) > 0) { |
292 |
// T[point] passed the test - it's a closest point for |
||
293 |
// the T triangle; the other point is on the face of S |
||
294 |
✓✗✓✗ ✓✗✓✗ |
1515 |
P = T[point] + Sn * (Tp[point] / Snl); |
295 |
✓✗ | 1515 |
Q = T[point]; |
296 |
✓✗✓✗ |
1515 |
return (P - Q).squaredNorm(); |
297 |
} |
||
298 |
} |
||
299 |
} |
||
300 |
} |
||
301 |
} |
||
302 |
|||
303 |
✓✗ | 1807 |
Vec3f Tn; |
304 |
FCL_REAL Tnl; |
||
305 |
|||
306 |
✓✗ | 1807 |
Tn = Tv[0].cross(Tv[1]); |
307 |
✓✗ | 1807 |
Tnl = Tn.dot(Tn); |
308 |
|||
309 |
✓✗ | 1807 |
if (Tnl > 1e-15) { |
310 |
✓✗ | 1807 |
Vec3f Sp; |
311 |
|||
312 |
✓✗✓✗ |
1807 |
V = T[0] - S[0]; |
313 |
✓✗✓✗ |
1807 |
Sp[0] = V.dot(Tn); |
314 |
|||
315 |
✓✗✓✗ |
1807 |
V = T[0] - S[1]; |
316 |
✓✗✓✗ |
1807 |
Sp[1] = V.dot(Tn); |
317 |
|||
318 |
✓✗✓✗ |
1807 |
V = T[0] - S[2]; |
319 |
✓✗✓✗ |
1807 |
Sp[2] = V.dot(Tn); |
320 |
|||
321 |
1807 |
int point = -1; |
|
322 |
✓✗✓✓ ✓✗✓✓ ✓✗✓✓ ✓✓ |
1807 |
if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) { |
323 |
✓✗✓✗ ✓✓ |
46 |
if (Sp[0] < Sp[1]) |
324 |
14 |
point = 0; |
|
325 |
else |
||
326 |
32 |
point = 1; |
|
327 |
✓✗✓✗ ✓✓ |
46 |
if (Sp[2] < Sp[point]) point = 2; |
328 |
✓✗✓✓ ✓✗✓✓ ✓✗✓✓ ✓✓ |
1761 |
} else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) { |
329 |
✓✗✓✗ ✓✓ |
275 |
if (Sp[0] > Sp[1]) |
330 |
132 |
point = 0; |
|
331 |
else |
||
332 |
143 |
point = 1; |
|
333 |
✓✗✓✗ ✓✓ |
275 |
if (Sp[2] > Sp[point]) point = 2; |
334 |
} |
||
335 |
|||
336 |
✓✓ | 1807 |
if (point >= 0) { |
337 |
321 |
shown_disjoint = 1; |
|
338 |
|||
339 |
✓✗✓✗ |
321 |
V = S[point] - T[0]; |
340 |
✓✗ | 321 |
Z = Tn.cross(Tv[0]); |
341 |
✓✗✓✗ |
321 |
if (V.dot(Z) > 0) { |
342 |
✓✗✓✗ |
321 |
V = S[point] - T[1]; |
343 |
✓✗ | 321 |
Z = Tn.cross(Tv[1]); |
344 |
✓✗✓✗ |
321 |
if (V.dot(Z) > 0) { |
345 |
✓✗✓✗ |
321 |
V = S[point] - T[2]; |
346 |
✓✗ | 321 |
Z = Tn.cross(Tv[2]); |
347 |
✓✗✓✗ |
321 |
if (V.dot(Z) > 0) { |
348 |
✓✗ | 321 |
P = S[point]; |
349 |
✓✗✓✗ ✓✗✓✗ |
321 |
Q = S[point] + Tn * (Sp[point] / Tnl); |
350 |
✓✗✓✗ |
321 |
return (P - Q).squaredNorm(); |
351 |
} |
||
352 |
} |
||
353 |
} |
||
354 |
} |
||
355 |
} |
||
356 |
|||
357 |
// Case 1 can't be shown. |
||
358 |
// If one of these tests showed the triangles disjoint, |
||
359 |
// we assume case 3 or 4, otherwise we conclude case 2, |
||
360 |
// that the triangles overlap. |
||
361 |
|||
362 |
✗✓ | 1486 |
if (shown_disjoint) { |
363 |
P = minP; |
||
364 |
Q = minQ; |
||
365 |
return mindd; |
||
366 |
} else |
||
367 |
1486 |
return 0; |
|
368 |
} |
||
369 |
|||
370 |
331883 |
FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, |
|
371 |
const Vec3f& S3, const Vec3f& T1, |
||
372 |
const Vec3f& T2, const Vec3f& T3, |
||
373 |
Vec3f& P, Vec3f& Q) { |
||
374 |
✓✓✓✗ |
1327532 |
Vec3f S[3]; |
375 |
✓✓✓✗ |
1327532 |
Vec3f T[3]; |
376 |
✓✗ | 331883 |
S[0] = S1; |
377 |
✓✗ | 331883 |
S[1] = S2; |
378 |
✓✗ | 331883 |
S[2] = S3; |
379 |
✓✗ | 331883 |
T[0] = T1; |
380 |
✓✗ | 331883 |
T[1] = T2; |
381 |
✓✗ | 331883 |
T[2] = T3; |
382 |
|||
383 |
✓✗ | 663766 |
return sqrTriDistance(S, T, P, Q); |
384 |
} |
||
385 |
|||
386 |
FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], |
||
387 |
const Matrix3f& R, const Vec3f& Tl, |
||
388 |
Vec3f& P, Vec3f& Q) { |
||
389 |
Vec3f T_transformed[3]; |
||
390 |
T_transformed[0] = R * T[0] + Tl; |
||
391 |
T_transformed[1] = R * T[1] + Tl; |
||
392 |
T_transformed[2] = R * T[2] + Tl; |
||
393 |
|||
394 |
return sqrTriDistance(S, T_transformed, P, Q); |
||
395 |
} |
||
396 |
|||
397 |
FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], |
||
398 |
const Transform3f& tf, Vec3f& P, |
||
399 |
Vec3f& Q) { |
||
400 |
Vec3f T_transformed[3]; |
||
401 |
T_transformed[0] = tf.transform(T[0]); |
||
402 |
T_transformed[1] = tf.transform(T[1]); |
||
403 |
T_transformed[2] = tf.transform(T[2]); |
||
404 |
|||
405 |
return sqrTriDistance(S, T_transformed, P, Q); |
||
406 |
} |
||
407 |
|||
408 |
328282 |
FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, |
|
409 |
const Vec3f& S3, const Vec3f& T1, |
||
410 |
const Vec3f& T2, const Vec3f& T3, |
||
411 |
const Matrix3f& R, const Vec3f& Tl, |
||
412 |
Vec3f& P, Vec3f& Q) { |
||
413 |
✓✗✓✗ ✓✗ |
328282 |
Vec3f T1_transformed = R * T1 + Tl; |
414 |
✓✗✓✗ ✓✗ |
328282 |
Vec3f T2_transformed = R * T2 + Tl; |
415 |
✓✗✓✗ ✓✗ |
328282 |
Vec3f T3_transformed = R * T3 + Tl; |
416 |
✓✗ | 328282 |
return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, |
417 |
656564 |
T3_transformed, P, Q); |
|
418 |
} |
||
419 |
|||
420 |
FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, |
||
421 |
const Vec3f& S3, const Vec3f& T1, |
||
422 |
const Vec3f& T2, const Vec3f& T3, |
||
423 |
const Transform3f& tf, Vec3f& P, |
||
424 |
Vec3f& Q) { |
||
425 |
Vec3f T1_transformed = tf.transform(T1); |
||
426 |
Vec3f T2_transformed = tf.transform(T2); |
||
427 |
Vec3f T3_transformed = tf.transform(T3); |
||
428 |
return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, |
||
429 |
T3_transformed, P, Q); |
||
430 |
} |
||
431 |
|||
432 |
33 |
Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, |
|
433 |
const Vec3f& p) { |
||
434 |
33 |
ProjectResult res; |
|
435 |
|||
436 |
✓✗✓✗ |
33 |
const Vec3f d = b - a; |
437 |
✓✗ | 33 |
const FCL_REAL l = d.squaredNorm(); |
438 |
|||
439 |
✓✗ | 33 |
if (l > 0) { |
440 |
✓✗✓✗ |
33 |
const FCL_REAL t = (p - a).dot(d); |
441 |
✓✓✓✓ |
33 |
res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); |
442 |
33 |
res.parameterization[0] = 1 - res.parameterization[1]; |
|
443 |
✓✓ | 33 |
if (t >= l) { |
444 |
✓✗✓✗ |
10 |
res.sqr_distance = (p - b).squaredNorm(); |
445 |
10 |
res.encode = 2; /* 0x10 */ |
|
446 |
✓✓ | 23 |
} else if (t <= 0) { |
447 |
✓✗✓✗ |
10 |
res.sqr_distance = (p - a).squaredNorm(); |
448 |
10 |
res.encode = 1; /* 0x01 */ |
|
449 |
} else { |
||
450 |
✓✗✓✗ ✓✗✓✗ |
13 |
res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); |
451 |
13 |
res.encode = 3; /* 0x00 */ |
|
452 |
} |
||
453 |
} |
||
454 |
|||
455 |
66 |
return res; |
|
456 |
} |
||
457 |
|||
458 |
26 |
Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, |
|
459 |
const Vec3f& c, |
||
460 |
const Vec3f& p) { |
||
461 |
26 |
ProjectResult res; |
|
462 |
|||
463 |
static const size_t nexti[3] = {1, 2, 0}; |
||
464 |
26 |
const Vec3f* vt[] = {&a, &b, &c}; |
|
465 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
26 |
const Vec3f dl[] = {a - b, b - c, c - a}; |
466 |
✓✗ | 26 |
const Vec3f& n = dl[0].cross(dl[1]); |
467 |
✓✗ | 26 |
const FCL_REAL l = n.squaredNorm(); |
468 |
|||
469 |
✓✗ | 26 |
if (l > 0) { |
470 |
26 |
FCL_REAL mindist = -1; |
|
471 |
✓✓ | 104 |
for (size_t i = 0; i < 3; ++i) { |
472 |
✓✗✓✗ ✓✗✓✓ |
78 |
if ((*vt[i] - p).dot(dl[i].cross(n)) > |
473 |
0) // origin is to the outside part of the triangle edge, then the |
||
474 |
// optimal can only be on the edge |
||
475 |
{ |
||
476 |
30 |
size_t j = nexti[i]; |
|
477 |
✓✗ | 30 |
ProjectResult res_line = projectLine(*vt[i], *vt[j], p); |
478 |
|||
479 |
✓✓✗✓ |
30 |
if (mindist < 0 || res_line.sqr_distance < mindist) { |
480 |
21 |
mindist = res_line.sqr_distance; |
|
481 |
21 |
res.encode = |
|
482 |
✓✓ | 21 |
static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) + |
483 |
✓✓ | 21 |
((res_line.encode & 2) ? 1 << j : 0)); |
484 |
21 |
res.parameterization[i] = res_line.parameterization[0]; |
|
485 |
21 |
res.parameterization[j] = res_line.parameterization[1]; |
|
486 |
21 |
res.parameterization[nexti[j]] = 0; |
|
487 |
} |
||
488 |
} |
||
489 |
} |
||
490 |
|||
491 |
✓✓ | 26 |
if (mindist < 0) // the origin project is within the triangle |
492 |
{ |
||
493 |
✓✗✓✗ |
5 |
FCL_REAL d = (a - p).dot(n); |
494 |
5 |
FCL_REAL s = sqrt(l); |
|
495 |
✓✗✓✗ |
5 |
Vec3f p_to_project = n * (d / l); |
496 |
✓✗ | 5 |
mindist = p_to_project.squaredNorm(); |
497 |
5 |
res.encode = 7; // m = 0x111 |
|
498 |
✓✗✓✗ ✓✗✓✗ |
5 |
res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s; |
499 |
✓✗✓✗ ✓✗✓✗ |
5 |
res.parameterization[1] = dl[2].cross(c - p - p_to_project).norm() / s; |
500 |
5 |
res.parameterization[2] = |
|
501 |
5 |
1 - res.parameterization[0] - res.parameterization[1]; |
|
502 |
} |
||
503 |
|||
504 |
26 |
res.sqr_distance = mindist; |
|
505 |
} |
||
506 |
|||
507 |
52 |
return res; |
|
508 |
} |
||
509 |
|||
510 |
15 |
Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, |
|
511 |
const Vec3f& b, |
||
512 |
const Vec3f& c, |
||
513 |
const Vec3f& d, |
||
514 |
const Vec3f& p) { |
||
515 |
15 |
ProjectResult res; |
|
516 |
|||
517 |
static const size_t nexti[] = {1, 2, 0}; |
||
518 |
15 |
const Vec3f* vt[] = {&a, &b, &c, &d}; |
|
519 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
15 |
const Vec3f dl[3] = {a - d, b - d, c - d}; |
520 |
✓✗ | 15 |
FCL_REAL vl = triple(dl[0], dl[1], dl[2]); |
521 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
15 |
bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0; |
522 |
✓✓✓✗ ✓✓ |
23 |
if (ng && |
523 |
8 |
std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng |
|
524 |
// is false, then the last vertex in the tetrahedron |
||
525 |
// does not grow toward the origin (in fact origin is |
||
526 |
// on the other side of the abc face) |
||
527 |
{ |
||
528 |
8 |
FCL_REAL mindist = -1; |
|
529 |
|||
530 |
✓✓ | 32 |
for (size_t i = 0; i < 3; ++i) { |
531 |
24 |
size_t j = nexti[i]; |
|
532 |
✓✗✓✗ ✓✗ |
24 |
FCL_REAL s = vl * (d - p).dot(dl[i].cross(dl[j])); |
533 |
✓✓ | 24 |
if (s > 0) // the origin is to the outside part of a triangle face, then |
534 |
// the optimal can only be on the triangle face |
||
535 |
{ |
||
536 |
✓✗ | 12 |
ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); |
537 |
✓✓✗✓ |
12 |
if (mindist < 0 || res_triangle.sqr_distance < mindist) { |
538 |
7 |
mindist = res_triangle.sqr_distance; |
|
539 |
7 |
res.encode = |
|
540 |
✓✓ | 7 |
static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) + |
541 |
✓✓ | 7 |
(res_triangle.encode & 2 ? 1 << j : 0) + |
542 |
7 |
(res_triangle.encode & 4 ? 8 : 0)); |
|
543 |
7 |
res.parameterization[i] = res_triangle.parameterization[0]; |
|
544 |
7 |
res.parameterization[j] = res_triangle.parameterization[1]; |
|
545 |
7 |
res.parameterization[nexti[j]] = 0; |
|
546 |
7 |
res.parameterization[3] = res_triangle.parameterization[2]; |
|
547 |
} |
||
548 |
} |
||
549 |
} |
||
550 |
|||
551 |
✓✓ | 8 |
if (mindist < 0) { |
552 |
1 |
mindist = 0; |
|
553 |
1 |
res.encode = 15; |
|
554 |
✓✗✓✗ ✓✗✓✗ |
1 |
res.parameterization[0] = triple(c - p, b - p, d - p) / vl; |
555 |
✓✗✓✗ ✓✗✓✗ |
1 |
res.parameterization[1] = triple(a - p, c - p, d - p) / vl; |
556 |
✓✗✓✗ ✓✗✓✗ |
1 |
res.parameterization[2] = triple(b - p, a - p, d - p) / vl; |
557 |
1 |
res.parameterization[3] = |
|
558 |
1 |
1 - (res.parameterization[0] + res.parameterization[1] + |
|
559 |
1 |
res.parameterization[2]); |
|
560 |
} |
||
561 |
|||
562 |
8 |
res.sqr_distance = mindist; |
|
563 |
✓✗ | 7 |
} else if (!ng) { |
564 |
✓✗ | 7 |
res = projectTriangle(a, b, c, p); |
565 |
7 |
res.parameterization[3] = 0; |
|
566 |
} |
||
567 |
30 |
return res; |
|
568 |
} |
||
569 |
|||
570 |
Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, |
||
571 |
const Vec3f& b) { |
||
572 |
ProjectResult res; |
||
573 |
|||
574 |
const Vec3f d = b - a; |
||
575 |
const FCL_REAL l = d.squaredNorm(); |
||
576 |
|||
577 |
if (l > 0) { |
||
578 |
const FCL_REAL t = -a.dot(d); |
||
579 |
res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); |
||
580 |
res.parameterization[0] = 1 - res.parameterization[1]; |
||
581 |
if (t >= l) { |
||
582 |
res.sqr_distance = b.squaredNorm(); |
||
583 |
res.encode = 2; /* 0x10 */ |
||
584 |
} else if (t <= 0) { |
||
585 |
res.sqr_distance = a.squaredNorm(); |
||
586 |
res.encode = 1; /* 0x01 */ |
||
587 |
} else { |
||
588 |
res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); |
||
589 |
res.encode = 3; /* 0x00 */ |
||
590 |
} |
||
591 |
} |
||
592 |
|||
593 |
return res; |
||
594 |
} |
||
595 |
|||
596 |
369111 |
Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, |
|
597 |
const Vec3f& b, |
||
598 |
const Vec3f& c) { |
||
599 |
369111 |
ProjectResult res; |
|
600 |
|||
601 |
static const size_t nexti[3] = {1, 2, 0}; |
||
602 |
369111 |
const Vec3f* vt[] = {&a, &b, &c}; |
|
603 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
369111 |
const Vec3f dl[] = {a - b, b - c, c - a}; |
604 |
✓✗ | 369111 |
const Vec3f& n = dl[0].cross(dl[1]); |
605 |
✓✗ | 369111 |
const FCL_REAL l = n.squaredNorm(); |
606 |
|||
607 |
✓✗ | 369111 |
if (l > 0) { |
608 |
369111 |
FCL_REAL mindist = -1; |
|
609 |
✓✓ | 1476444 |
for (size_t i = 0; i < 3; ++i) { |
610 |
✓✗✓✗ ✗✓ |
1107333 |
if (vt[i]->dot(dl[i].cross(n)) > |
611 |
0) // origin is to the outside part of the triangle edge, then the |
||
612 |
// optimal can only be on the edge |
||
613 |
{ |
||
614 |
size_t j = nexti[i]; |
||
615 |
ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); |
||
616 |
|||
617 |
if (mindist < 0 || res_line.sqr_distance < mindist) { |
||
618 |
mindist = res_line.sqr_distance; |
||
619 |
res.encode = |
||
620 |
static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) + |
||
621 |
((res_line.encode & 2) ? 1 << j : 0)); |
||
622 |
res.parameterization[i] = res_line.parameterization[0]; |
||
623 |
res.parameterization[j] = res_line.parameterization[1]; |
||
624 |
res.parameterization[nexti[j]] = 0; |
||
625 |
} |
||
626 |
} |
||
627 |
} |
||
628 |
|||
629 |
✓✗ | 369111 |
if (mindist < 0) // the origin project is within the triangle |
630 |
{ |
||
631 |
✓✗ | 369111 |
FCL_REAL d = a.dot(n); |
632 |
369111 |
FCL_REAL s = sqrt(l); |
|
633 |
✓✗✓✗ |
369111 |
Vec3f o_to_project = n * (d / l); |
634 |
✓✗ | 369111 |
mindist = o_to_project.squaredNorm(); |
635 |
369111 |
res.encode = 7; // m = 0x111 |
|
636 |
✓✗✓✗ ✓✗ |
369111 |
res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; |
637 |
✓✗✓✗ ✓✗ |
369111 |
res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; |
638 |
369111 |
res.parameterization[2] = |
|
639 |
369111 |
1 - res.parameterization[0] - res.parameterization[1]; |
|
640 |
} |
||
641 |
|||
642 |
369111 |
res.sqr_distance = mindist; |
|
643 |
} |
||
644 |
|||
645 |
738222 |
return res; |
|
646 |
} |
||
647 |
|||
648 |
21027 |
Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, |
|
649 |
const Vec3f& b, |
||
650 |
const Vec3f& c, |
||
651 |
const Vec3f& d) { |
||
652 |
21027 |
ProjectResult res; |
|
653 |
|||
654 |
static const size_t nexti[] = {1, 2, 0}; |
||
655 |
21027 |
const Vec3f* vt[] = {&a, &b, &c, &d}; |
|
656 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
21027 |
const Vec3f dl[3] = {a - d, b - d, c - d}; |
657 |
✓✗ | 21027 |
FCL_REAL vl = triple(dl[0], dl[1], dl[2]); |
658 |
✓✗✓✗ ✓✗✓✗ |
21027 |
bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0; |
659 |
✓✗✓✗ ✓✗ |
42054 |
if (ng && |
660 |
21027 |
std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng |
|
661 |
// is false, then the last vertex in the tetrahedron |
||
662 |
// does not grow toward the origin (in fact origin is |
||
663 |
// on the other side of the abc face) |
||
664 |
{ |
||
665 |
21027 |
FCL_REAL mindist = -1; |
|
666 |
|||
667 |
✓✓ | 84108 |
for (size_t i = 0; i < 3; ++i) { |
668 |
63081 |
size_t j = nexti[i]; |
|
669 |
✓✗✓✗ |
63081 |
FCL_REAL s = vl * d.dot(dl[i].cross(dl[j])); |
670 |
✗✓ | 63081 |
if (s > 0) // the origin is to the outside part of a triangle face, then |
671 |
// the optimal can only be on the triangle face |
||
672 |
{ |
||
673 |
ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); |
||
674 |
if (mindist < 0 || res_triangle.sqr_distance < mindist) { |
||
675 |
mindist = res_triangle.sqr_distance; |
||
676 |
res.encode = |
||
677 |
static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) + |
||
678 |
(res_triangle.encode & 2 ? 1 << j : 0) + |
||
679 |
(res_triangle.encode & 4 ? 8 : 0)); |
||
680 |
res.parameterization[i] = res_triangle.parameterization[0]; |
||
681 |
res.parameterization[j] = res_triangle.parameterization[1]; |
||
682 |
res.parameterization[nexti[j]] = 0; |
||
683 |
res.parameterization[3] = res_triangle.parameterization[2]; |
||
684 |
} |
||
685 |
} |
||
686 |
} |
||
687 |
|||
688 |
✓✗ | 21027 |
if (mindist < 0) { |
689 |
21027 |
mindist = 0; |
|
690 |
21027 |
res.encode = 15; |
|
691 |
✓✗ | 21027 |
res.parameterization[0] = triple(c, b, d) / vl; |
692 |
✓✗ | 21027 |
res.parameterization[1] = triple(a, c, d) / vl; |
693 |
✓✗ | 21027 |
res.parameterization[2] = triple(b, a, d) / vl; |
694 |
21027 |
res.parameterization[3] = |
|
695 |
21027 |
1 - (res.parameterization[0] + res.parameterization[1] + |
|
696 |
21027 |
res.parameterization[2]); |
|
697 |
} |
||
698 |
|||
699 |
21027 |
res.sqr_distance = mindist; |
|
700 |
} else if (!ng) { |
||
701 |
res = projectTriangleOrigin(a, b, c); |
||
702 |
res.parameterization[3] = 0; |
||
703 |
} |
||
704 |
42054 |
return res; |
|
705 |
} |
||
706 |
|||
707 |
} // namespace fcl |
||
708 |
|||
709 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |