intersect.cpp
Go to the documentation of this file.
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 
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 void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
61  const Vec3f& B, Vec3f& VEC, Vec3f& X,
62  Vec3f& Y) {
63  Vec3f T;
64  FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
65  Vec3f TMP;
66 
67  T = Q - P;
68  A_dot_A = A.dot(A);
69  B_dot_B = B.dot(B);
70  A_dot_B = A.dot(B);
71  A_dot_T = A.dot(T);
72  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  FCL_REAL denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B;
83 
84  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  if ((t < 0) || std::isnan(t))
89  t = 0;
90  else if (t > 1)
91  t = 1;
92 
93  // find u for point on ray Q,B closest to point at t
94 
95  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  if ((u <= 0) || std::isnan(u)) {
102  Y = Q;
103 
104  t = A_dot_T / A_dot_A;
105 
106  if ((t <= 0) || std::isnan(t)) {
107  X = P;
108  VEC = Q - P;
109  } else if (t >= 1) {
110  X = P + A;
111  VEC = Q - X;
112  } else {
113  X = P + A * t;
114  TMP = T.cross(A);
115  VEC = A.cross(TMP);
116  }
117  } else if (u >= 1) {
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  X = P;
124  VEC = Y - P;
125  } else if (t >= 1) {
126  X = P + A;
127  VEC = Y - X;
128  } else {
129  X = P + A * t;
130  T = Y - P;
131  TMP = T.cross(A);
132  VEC = A.cross(TMP);
133  }
134  } else {
135  Y = Q + B * u;
136 
137  if ((t <= 0) || std::isnan(t)) {
138  X = P;
139  TMP = T.cross(B);
140  VEC = B.cross(TMP);
141  } else if (t >= 1) {
142  X = P + A;
143  T = Q - X;
144  TMP = T.cross(B);
145  VEC = B.cross(TMP);
146  } else {
147  X = P + A * t;
148  VEC = A.cross(B);
149  if (VEC.dot(T) < 0) {
150  VEC = VEC * (-1);
151  }
152  }
153  }
154 }
155 
156 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  Vec3f Sv[3];
161  Vec3f Tv[3];
162  Vec3f VEC;
163 
164  Sv[0] = S[1] - S[0];
165  Sv[1] = S[2] - S[1];
166  Sv[2] = S[0] - S[2];
167 
168  Tv[0] = T[1] - T[0];
169  Tv[1] = T[2] - T[1];
170  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  Vec3f V, Z, minP, minQ;
181  FCL_REAL mindd;
182  int shown_disjoint = 0;
183 
184  mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high
185 
186  for (int i = 0; i < 3; ++i) {
187  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  segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
191 
192  V = Q - P;
193  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  if (dd <= mindd) {
199  minP = P;
200  minQ = Q;
201  mindd = dd;
202 
203  Z = S[(i + 2) % 3] - P;
204  FCL_REAL a = Z.dot(VEC);
205  Z = T[(j + 2) % 3] - Q;
206  FCL_REAL b = Z.dot(VEC);
207 
208  if ((a <= 0) && (b >= 0)) return dd;
209 
210  FCL_REAL p = V.dot(VEC);
211 
212  if (a < 0) a = 0;
213  if (b > 0) b = 0;
214  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  Vec3f Sn;
236  FCL_REAL Snl;
237 
238  Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle
239  Snl = Sn.dot(Sn); // Compute square of length of normal
240 
241  // If cross product is long enough,
242 
243  if (Snl > 1e-15) {
244  // Get projection lengths of T points
245 
246  Vec3f Tp;
247 
248  V = S[0] - T[0];
249  Tp[0] = V.dot(Sn);
250 
251  V = S[0] - T[1];
252  Tp[1] = V.dot(Sn);
253 
254  V = S[0] - T[2];
255  Tp[2] = V.dot(Sn);
256 
257  // If Sn is a separating direction,
258  // find point with smallest projection
259 
260  int point = -1;
261  if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) {
262  if (Tp[0] < Tp[1])
263  point = 0;
264  else
265  point = 1;
266  if (Tp[2] < Tp[point]) point = 2;
267  } else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) {
268  if (Tp[0] > Tp[1])
269  point = 0;
270  else
271  point = 1;
272  if (Tp[2] > Tp[point]) point = 2;
273  }
274 
275  // If Sn is a separating direction,
276 
277  if (point >= 0) {
278  shown_disjoint = 1;
279 
280  // Test whether the point found, when projected onto the
281  // other triangle, lies within the face.
282 
283  V = T[point] - S[0];
284  Z = Sn.cross(Sv[0]);
285  if (V.dot(Z) > 0) {
286  V = T[point] - S[1];
287  Z = Sn.cross(Sv[1]);
288  if (V.dot(Z) > 0) {
289  V = T[point] - S[2];
290  Z = Sn.cross(Sv[2]);
291  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  P = T[point] + Sn * (Tp[point] / Snl);
295  Q = T[point];
296  return (P - Q).squaredNorm();
297  }
298  }
299  }
300  }
301  }
302 
303  Vec3f Tn;
304  FCL_REAL Tnl;
305 
306  Tn = Tv[0].cross(Tv[1]);
307  Tnl = Tn.dot(Tn);
308 
309  if (Tnl > 1e-15) {
310  Vec3f Sp;
311 
312  V = T[0] - S[0];
313  Sp[0] = V.dot(Tn);
314 
315  V = T[0] - S[1];
316  Sp[1] = V.dot(Tn);
317 
318  V = T[0] - S[2];
319  Sp[2] = V.dot(Tn);
320 
321  int point = -1;
322  if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) {
323  if (Sp[0] < Sp[1])
324  point = 0;
325  else
326  point = 1;
327  if (Sp[2] < Sp[point]) point = 2;
328  } else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) {
329  if (Sp[0] > Sp[1])
330  point = 0;
331  else
332  point = 1;
333  if (Sp[2] > Sp[point]) point = 2;
334  }
335 
336  if (point >= 0) {
337  shown_disjoint = 1;
338 
339  V = S[point] - T[0];
340  Z = Tn.cross(Tv[0]);
341  if (V.dot(Z) > 0) {
342  V = S[point] - T[1];
343  Z = Tn.cross(Tv[1]);
344  if (V.dot(Z) > 0) {
345  V = S[point] - T[2];
346  Z = Tn.cross(Tv[2]);
347  if (V.dot(Z) > 0) {
348  P = S[point];
349  Q = S[point] + Tn * (Sp[point] / Tnl);
350  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  if (shown_disjoint) {
363  P = minP;
364  Q = minQ;
365  return mindd;
366  } else
367  return 0;
368 }
369 
370 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  Vec3f S[3];
375  Vec3f T[3];
376  S[0] = S1;
377  S[1] = S2;
378  S[2] = S3;
379  T[0] = T1;
380  T[1] = T2;
381  T[2] = T3;
382 
383  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 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  Vec3f T1_transformed = R * T1 + Tl;
414  Vec3f T2_transformed = R * T2 + Tl;
415  Vec3f T3_transformed = R * T3 + Tl;
416  return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
417  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 Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b,
433  const Vec3f& p) {
434  ProjectResult res;
435 
436  const Vec3f d = b - a;
437  const FCL_REAL l = d.squaredNorm();
438 
439  if (l > 0) {
440  const FCL_REAL t = (p - a).dot(d);
441  res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
442  res.parameterization[0] = 1 - res.parameterization[1];
443  if (t >= l) {
444  res.sqr_distance = (p - b).squaredNorm();
445  res.encode = 2; /* 0x10 */
446  } else if (t <= 0) {
447  res.sqr_distance = (p - a).squaredNorm();
448  res.encode = 1; /* 0x01 */
449  } else {
450  res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm();
451  res.encode = 3; /* 0x00 */
452  }
453  }
454 
455  return res;
456 }
457 
458 Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
459  const Vec3f& c,
460  const Vec3f& p) {
461  ProjectResult res;
462 
463  static const size_t nexti[3] = {1, 2, 0};
464  const Vec3f* vt[] = {&a, &b, &c};
465  const Vec3f dl[] = {a - b, b - c, c - a};
466  const Vec3f& n = dl[0].cross(dl[1]);
467  const FCL_REAL l = n.squaredNorm();
468 
469  if (l > 0) {
470  FCL_REAL mindist = -1;
471  for (size_t i = 0; i < 3; ++i) {
472  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  size_t j = nexti[i];
477  ProjectResult res_line = projectLine(*vt[i], *vt[j], p);
478 
479  if (mindist < 0 || res_line.sqr_distance < mindist) {
480  mindist = res_line.sqr_distance;
481  res.encode =
482  static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) +
483  ((res_line.encode & 2) ? 1 << j : 0));
484  res.parameterization[i] = res_line.parameterization[0];
485  res.parameterization[j] = res_line.parameterization[1];
486  res.parameterization[nexti[j]] = 0;
487  }
488  }
489  }
490 
491  if (mindist < 0) // the origin project is within the triangle
492  {
493  FCL_REAL d = (a - p).dot(n);
494  FCL_REAL s = sqrt(l);
495  Vec3f p_to_project = n * (d / l);
496  mindist = p_to_project.squaredNorm();
497  res.encode = 7; // m = 0x111
498  res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s;
499  res.parameterization[1] = dl[2].cross(c - p - p_to_project).norm() / s;
500  res.parameterization[2] =
501  1 - res.parameterization[0] - res.parameterization[1];
502  }
503 
504  res.sqr_distance = mindist;
505  }
506 
507  return res;
508 }
509 
510 Project::ProjectResult Project::projectTetrahedra(const Vec3f& a,
511  const Vec3f& b,
512  const Vec3f& c,
513  const Vec3f& d,
514  const Vec3f& p) {
515  ProjectResult res;
516 
517  static const size_t nexti[] = {1, 2, 0};
518  const Vec3f* vt[] = {&a, &b, &c, &d};
519  const Vec3f dl[3] = {a - d, b - d, c - d};
520  FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
521  bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0;
522  if (ng &&
523  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  FCL_REAL mindist = -1;
529 
530  for (size_t i = 0; i < 3; ++i) {
531  size_t j = nexti[i];
532  FCL_REAL s = vl * (d - p).dot(dl[i].cross(dl[j]));
533  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  ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p);
537  if (mindist < 0 || res_triangle.sqr_distance < mindist) {
538  mindist = res_triangle.sqr_distance;
539  res.encode =
540  static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) +
541  (res_triangle.encode & 2 ? 1 << j : 0) +
542  (res_triangle.encode & 4 ? 8 : 0));
543  res.parameterization[i] = res_triangle.parameterization[0];
544  res.parameterization[j] = res_triangle.parameterization[1];
545  res.parameterization[nexti[j]] = 0;
546  res.parameterization[3] = res_triangle.parameterization[2];
547  }
548  }
549  }
550 
551  if (mindist < 0) {
552  mindist = 0;
553  res.encode = 15;
554  res.parameterization[0] = triple(c - p, b - p, d - p) / vl;
555  res.parameterization[1] = triple(a - p, c - p, d - p) / vl;
556  res.parameterization[2] = triple(b - p, a - p, d - p) / vl;
557  res.parameterization[3] =
558  1 - (res.parameterization[0] + res.parameterization[1] +
559  res.parameterization[2]);
560  }
561 
562  res.sqr_distance = mindist;
563  } else if (!ng) {
564  res = projectTriangle(a, b, c, p);
565  res.parameterization[3] = 0;
566  }
567  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 Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a,
597  const Vec3f& b,
598  const Vec3f& c) {
599  ProjectResult res;
600 
601  static const size_t nexti[3] = {1, 2, 0};
602  const Vec3f* vt[] = {&a, &b, &c};
603  const Vec3f dl[] = {a - b, b - c, c - a};
604  const Vec3f& n = dl[0].cross(dl[1]);
605  const FCL_REAL l = n.squaredNorm();
606 
607  if (l > 0) {
608  FCL_REAL mindist = -1;
609  for (size_t i = 0; i < 3; ++i) {
610  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  if (mindist < 0) // the origin project is within the triangle
630  {
631  FCL_REAL d = a.dot(n);
632  FCL_REAL s = sqrt(l);
633  Vec3f o_to_project = n * (d / l);
634  mindist = o_to_project.squaredNorm();
635  res.encode = 7; // m = 0x111
636  res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s;
637  res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s;
638  res.parameterization[2] =
639  1 - res.parameterization[0] - res.parameterization[1];
640  }
641 
642  res.sqr_distance = mindist;
643  }
644 
645  return res;
646 }
647 
648 Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a,
649  const Vec3f& b,
650  const Vec3f& c,
651  const Vec3f& d) {
652  ProjectResult res;
653 
654  static const size_t nexti[] = {1, 2, 0};
655  const Vec3f* vt[] = {&a, &b, &c, &d};
656  const Vec3f dl[3] = {a - d, b - d, c - d};
657  FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
658  bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0;
659  if (ng &&
660  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  FCL_REAL mindist = -1;
666 
667  for (size_t i = 0; i < 3; ++i) {
668  size_t j = nexti[i];
669  FCL_REAL s = vl * d.dot(dl[i].cross(dl[j]));
670  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  if (mindist < 0) {
689  mindist = 0;
690  res.encode = 15;
691  res.parameterization[0] = triple(c, b, d) / vl;
692  res.parameterization[1] = triple(a, c, d) / vl;
693  res.parameterization[2] = triple(b, a, d) / vl;
694  res.parameterization[3] =
695  1 - (res.parameterization[0] + res.parameterization[1] +
696  res.parameterization[2]);
697  }
698 
699  res.sqr_distance = mindist;
700  } else if (!ng) {
701  res = projectTriangleOrigin(a, b, c);
702  res.parameterization[3] = 0;
703  }
704  return res;
705 }
706 
707 } // namespace fcl
708 
709 } // namespace hpp
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
V
V
B
B
R
R
a
list a
res
res
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
c
c
P
P
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
t
tuple t
hpp::fcl::triple
static Derived::Scalar triple(const Eigen::MatrixBase< Derived > &x, const Eigen::MatrixBase< Derived > &y, const Eigen::MatrixBase< Derived > &z)
Definition: tools.h:53
generate_distance_plot.X
X
Definition: generate_distance_plot.py:11
A
A
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
Y
Y
intersect.h
tools.h
X


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14