FCL  0.6.0
Flexible Collision Library
triangle_distance-inl.h
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 
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 TriangleDistance<double>;
52 
53 //==============================================================================
54 template <typename S>
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>
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>
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>
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>
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>
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>
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
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
static void 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)
Returns closest points between an segment pair. The first segment is P + t * A The second segment is ...
Definition: triangle_distance-inl.h:55
static S triDistance(const Vector3< S > T1[3], const Vector3< S > T2[3], Vector3< S > &P, Vector3< S > &Q)
Compute the closest points on two triangles given their absolute coordinate, and returns the distance...
Definition: triangle_distance-inl.h:171