1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, 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 #ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H
40
41 #include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h"
42
43 namespace fcl
44 {
45
46 namespace detail
47 {
48
49 //==============================================================================
50 extern template
51 class FCL_EXPORT TriangleDistance<double>;
52
53 //==============================================================================
54 template <typename S>
segPoints(const Vector3<S> & P,const Vector3<S> & A,const Vector3<S> & Q,const Vector3<S> & B,Vector3<S> & VEC,Vector3<S> & X,Vector3<S> & Y)55 void TriangleDistance<S>::segPoints(const Vector3<S>& P, const Vector3<S>& A, const Vector3<S>& Q, const Vector3<S>& B,
56 Vector3<S>& VEC, Vector3<S>& X, Vector3<S>& Y)
57 {
58 Vector3<S> T;
59 S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
60 Vector3<S> TMP;
61
62 T = Q - P;
63 A_dot_A = A.dot(A);
64 B_dot_B = B.dot(B);
65 A_dot_B = A.dot(B);
66 A_dot_T = A.dot(T);
67 B_dot_T = B.dot(T);
68
69 // t parameterizes ray P,A
70 // u parameterizes ray Q,B
71
72 S t, u;
73
74 // compute t for the closest point on ray P,A to
75 // ray Q,B
76
77 S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
78
79 t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
80
81 // clamp result so t is on the segment P,A
82
83 if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1;
84
85 // find u for point on ray Q,B closest to point at t
86
87 u = (t*A_dot_B - B_dot_T) / B_dot_B;
88
89 // if u is on segment Q,B, t and u correspond to
90 // closest points, otherwise, clamp u, recompute and
91 // clamp t
92
93 if((u <= 0) || std::isnan(u))
94 {
95 Y = Q;
96
97 t = A_dot_T / A_dot_A;
98
99 if((t <= 0) || std::isnan(t))
100 {
101 X = P;
102 VEC = Q - P;
103 }
104 else if(t >= 1)
105 {
106 X = P + A;
107 VEC = Q - X;
108 }
109 else
110 {
111 X = P + A * t;
112 TMP = T.cross(A);
113 VEC = A.cross(TMP);
114 }
115 }
116 else if (u >= 1)
117 {
118 Y = Q + B;
119
120 t = (A_dot_B + A_dot_T) / A_dot_A;
121
122 if((t <= 0) || std::isnan(t))
123 {
124 X = P;
125 VEC = Y - P;
126 }
127 else if(t >= 1)
128 {
129 X = P + A;
130 VEC = Y - X;
131 }
132 else
133 {
134 X = P + A * t;
135 T = Y - P;
136 TMP = T.cross(A);
137 VEC= A.cross(TMP);
138 }
139 }
140 else
141 {
142 Y = Q + B * u;
143
144 if((t <= 0) || std::isnan(t))
145 {
146 X = P;
147 TMP = T.cross(B);
148 VEC = B.cross(TMP);
149 }
150 else if(t >= 1)
151 {
152 X = P + A;
153 T = Q - X;
154 TMP = T.cross(B);
155 VEC = B.cross(TMP);
156 }
157 else
158 {
159 X = P + A * t;
160 VEC = A.cross(B);
161 if(VEC.dot(T) < 0)
162 {
163 VEC = VEC * (-1);
164 }
165 }
166 }
167 }
168
169 //==============================================================================
170 template <typename S>
triDistance(const Vector3<S> T1[3],const Vector3<S> T2[3],Vector3<S> & P,Vector3<S> & Q)171 S TriangleDistance<S>::triDistance(const Vector3<S> T1[3], const Vector3<S> T2[3], Vector3<S>& P, Vector3<S>& Q)
172 {
173 // Compute vectors along the 6 sides
174
175 Vector3<S> Sv[3];
176 Vector3<S> Tv[3];
177 Vector3<S> VEC;
178
179 Sv[0] = T1[1] - T1[0];
180 Sv[1] = T1[2] - T1[1];
181 Sv[2] = T1[0] - T1[2];
182
183 Tv[0] = T2[1] - T2[0];
184 Tv[1] = T2[2] - T2[1];
185 Tv[2] = T2[0] - T2[2];
186
187 // For each edge pair, the vector connecting the closest points
188 // of the edges defines a slab (parallel planes at head and tail
189 // enclose the slab). If we can show that the off-edge vertex of
190 // each triangle is outside of the slab, then the closest points
191 // of the edges are the closest points for the triangles.
192 // Even if these tests fail, it may be helpful to know the closest
193 // points found, and whether the triangles were shown disjoint
194
195 Vector3<S> V;
196 Vector3<S> Z;
197 Vector3<S> minP = Vector3<S>::Zero();
198 Vector3<S> minQ = Vector3<S>::Zero();
199 S mindd;
200 int shown_disjoint = 0;
201
202 mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high
203
204 for(int i = 0; i < 3; ++i)
205 {
206 for(int j = 0; j < 3; ++j)
207 {
208 // Find closest points on edges i & j, plus the
209 // vector (and distance squared) between these points
210 segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q);
211
212 V = Q - P;
213 S dd = V.dot(V);
214
215 // Verify this closest point pair only if the distance
216 // squared is less than the minimum found thus far.
217
218 if(dd <= mindd)
219 {
220 minP = P;
221 minQ = Q;
222 mindd = dd;
223
224 Z = T1[(i+2)%3] - P;
225 S a = Z.dot(VEC);
226 Z = T2[(j+2)%3] - Q;
227 S b = Z.dot(VEC);
228
229 if((a <= 0) && (b >= 0)) return sqrt(dd);
230
231 S p = V.dot(VEC);
232
233 if(a < 0) a = 0;
234 if(b > 0) b = 0;
235 if((p - a + b) > 0) shown_disjoint = 1;
236 }
237 }
238 }
239
240 // No edge pairs contained the closest points.
241 // either:
242 // 1. one of the closest points is a vertex, and the
243 // other point is interior to a face.
244 // 2. the triangles are overlapping.
245 // 3. an edge of one triangle is parallel to the other's face. If
246 // cases 1 and 2 are not true, then the closest points from the 9
247 // edge pairs checks above can be taken as closest points for the
248 // triangles.
249 // 4. possibly, the triangles were degenerate. When the
250 // triangle points are nearly colinear or coincident, one
251 // of above tests might fail even though the edges tested
252 // contain the closest points.
253
254 // First check for case 1
255
256 Vector3<S> Sn;
257 S Snl;
258
259 Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle
260 Snl = Sn.dot(Sn); // Compute square of length of normal
261
262 // If cross product is long enough,
263
264 if(Snl > 1e-15)
265 {
266 // Get projection lengths of T2 points
267
268 Vector3<S> Tp;
269
270 V = T1[0] - T2[0];
271 Tp[0] = V.dot(Sn);
272
273 V = T1[0] - T2[1];
274 Tp[1] = V.dot(Sn);
275
276 V = T1[0] - T2[2];
277 Tp[2] = V.dot(Sn);
278
279 // If Sn is a separating direction,
280 // find point with smallest projection
281
282 int point = -1;
283 if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
284 {
285 if(Tp[0] < Tp[1]) point = 0; else point = 1;
286 if(Tp[2] < Tp[point]) point = 2;
287 }
288 else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
289 {
290 if(Tp[0] > Tp[1]) point = 0; else point = 1;
291 if(Tp[2] > Tp[point]) point = 2;
292 }
293
294 // If Sn is a separating direction,
295
296 if(point >= 0)
297 {
298 shown_disjoint = 1;
299
300 // Test whether the point found, when projected onto the
301 // other triangle, lies within the face.
302
303 V = T2[point] - T1[0];
304 Z = Sn.cross(Sv[0]);
305 if(V.dot(Z) > 0)
306 {
307 V = T2[point] - T1[1];
308 Z = Sn.cross(Sv[1]);
309 if(V.dot(Z) > 0)
310 {
311 V = T2[point] - T1[2];
312 Z = Sn.cross(Sv[2]);
313 if(V.dot(Z) > 0)
314 {
315 // T[point] passed the test - it's a closest point for
316 // the T2 triangle; the other point is on the face of T1
317 P = T2[point] + Sn * (Tp[point] / Snl);
318 Q = T2[point];
319 return (P - Q).norm();
320 }
321 }
322 }
323 }
324 }
325
326 Vector3<S> Tn;
327 S Tnl;
328
329 Tn = Tv[0].cross(Tv[1]);
330 Tnl = Tn.dot(Tn);
331
332 if(Tnl > 1e-15)
333 {
334 Vector3<S> Sp;
335
336 V = T2[0] - T1[0];
337 Sp[0] = V.dot(Tn);
338
339 V = T2[0] - T1[1];
340 Sp[1] = V.dot(Tn);
341
342 V = T2[0] - T1[2];
343 Sp[2] = V.dot(Tn);
344
345 int point = -1;
346 if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
347 {
348 if(Sp[0] < Sp[1]) point = 0; else point = 1;
349 if(Sp[2] < Sp[point]) point = 2;
350 }
351 else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
352 {
353 if(Sp[0] > Sp[1]) point = 0; else point = 1;
354 if(Sp[2] > Sp[point]) point = 2;
355 }
356
357 if(point >= 0)
358 {
359 shown_disjoint = 1;
360
361 V = T1[point] - T2[0];
362 Z = Tn.cross(Tv[0]);
363 if(V.dot(Z) > 0)
364 {
365 V = T1[point] - T2[1];
366 Z = Tn.cross(Tv[1]);
367 if(V.dot(Z) > 0)
368 {
369 V = T1[point] - T2[2];
370 Z = Tn.cross(Tv[2]);
371 if(V.dot(Z) > 0)
372 {
373 P = T1[point];
374 Q = T1[point] + Tn * (Sp[point] / Tnl);
375 return (P - Q).norm();
376 }
377 }
378 }
379 }
380 }
381
382 // Case 1 can't be shown.
383 // If one of these tests showed the triangles disjoint,
384 // we assume case 3 or 4, otherwise we conclude case 2,
385 // that the triangles overlap.
386
387 if(shown_disjoint)
388 {
389 P = minP;
390 Q = minQ;
391 return sqrt(mindd);
392 }
393 else return 0;
394 }
395
396 //==============================================================================
397 template <typename S>
triDistance(const Vector3<S> & S1,const Vector3<S> & S2,const Vector3<S> & S3,const Vector3<S> & T1,const Vector3<S> & T2,const Vector3<S> & T3,Vector3<S> & P,Vector3<S> & Q)398 S TriangleDistance<S>::triDistance(const Vector3<S>& S1, const Vector3<S>& S2, const Vector3<S>& S3,
399 const Vector3<S>& T1, const Vector3<S>& T2, const Vector3<S>& T3,
400 Vector3<S>& P, Vector3<S>& Q)
401 {
402 Vector3<S> U[3];
403 Vector3<S> T[3];
404 U[0] = S1; U[1] = S2; U[2] = S3;
405 T[0] = T1; T[1] = T2; T[2] = T3;
406
407 return triDistance(U, T, P, Q);
408 }
409
410 //==============================================================================
411 template <typename S>
triDistance(const Vector3<S> T1[3],const Vector3<S> T2[3],const Matrix3<S> & R,const Vector3<S> & Tl,Vector3<S> & P,Vector3<S> & Q)412 S TriangleDistance<S>::triDistance(const Vector3<S> T1[3], const Vector3<S> T2[3],
413 const Matrix3<S>& R, const Vector3<S>& Tl,
414 Vector3<S>& P, Vector3<S>& Q)
415 {
416 Vector3<S> T_transformed[3];
417 T_transformed[0] = R * T2[0] + Tl;
418 T_transformed[1] = R * T2[1] + Tl;
419 T_transformed[2] = R * T2[2] + Tl;
420
421 return triDistance(T1, T_transformed, P, Q);
422 }
423
424 //==============================================================================
425 template <typename S>
triDistance(const Vector3<S> T1[3],const Vector3<S> T2[3],const Transform3<S> & tf,Vector3<S> & P,Vector3<S> & Q)426 S TriangleDistance<S>::triDistance(const Vector3<S> T1[3], const Vector3<S> T2[3],
427 const Transform3<S>& tf,
428 Vector3<S>& P, Vector3<S>& Q)
429 {
430 Vector3<S> T_transformed[3];
431 T_transformed[0] = tf * T2[0];
432 T_transformed[1] = tf * T2[1];
433 T_transformed[2] = tf * T2[2];
434
435 return triDistance(T1, T_transformed, P, Q);
436 }
437
438 //==============================================================================
439 template <typename S>
triDistance(const Vector3<S> & S1,const Vector3<S> & S2,const Vector3<S> & S3,const Vector3<S> & T1,const Vector3<S> & T2,const Vector3<S> & T3,const Matrix3<S> & R,const Vector3<S> & Tl,Vector3<S> & P,Vector3<S> & Q)440 S TriangleDistance<S>::triDistance(const Vector3<S>& S1, const Vector3<S>& S2, const Vector3<S>& S3,
441 const Vector3<S>& T1, const Vector3<S>& T2, const Vector3<S>& T3,
442 const Matrix3<S>& R, const Vector3<S>& Tl,
443 Vector3<S>& P, Vector3<S>& Q)
444 {
445 Vector3<S> T1_transformed = R * T1 + Tl;
446 Vector3<S> T2_transformed = R * T2 + Tl;
447 Vector3<S> T3_transformed = R * T3 + Tl;
448 return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
449 }
450
451 //==============================================================================
452 template <typename S>
triDistance(const Vector3<S> & S1,const Vector3<S> & S2,const Vector3<S> & S3,const Vector3<S> & T1,const Vector3<S> & T2,const Vector3<S> & T3,const Transform3<S> & tf,Vector3<S> & P,Vector3<S> & Q)453 S TriangleDistance<S>::triDistance(const Vector3<S>& S1, const Vector3<S>& S2, const Vector3<S>& S3,
454 const Vector3<S>& T1, const Vector3<S>& T2, const Vector3<S>& T3,
455 const Transform3<S>& tf,
456 Vector3<S>& P, Vector3<S>& Q)
457 {
458 Vector3<S> T1_transformed = tf * T1;
459 Vector3<S> T2_transformed = tf * T2;
460 Vector3<S> T3_transformed = tf * T3;
461 return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
462 }
463
464 } // namespace detail
465 } // namespace fcl
466
467 #endif
468