details.h
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  * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
38 #ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H
39 #define HPP_FCL_SRC_NARROWPHASE_DETAILS_H
40 
41 #include <hpp/fcl/internal/traversal_node_setup.h>
42 #include <hpp/fcl/narrowphase/narrowphase.h>
43 
44 namespace hpp {
45 namespace fcl {
46 namespace details {
47 // Compute the point on a line segment that is the closest point on the
48 // segment to to another point. The code is inspired by the explanation
49 // given by Dan Sunday's page:
50 // http://geomalgorithms.com/a02-_lines.html
51 static inline void lineSegmentPointClosestToPoint(const Vec3f& p,
52  const Vec3f& s1,
53  const Vec3f& s2, Vec3f& sp) {
54  Vec3f v = s2 - s1;
55  Vec3f w = p - s1;
56 
57  FCL_REAL c1 = w.dot(v);
58  FCL_REAL c2 = v.dot(v);
59 
60  if (c1 <= 0) {
61  sp = s1;
62  } else if (c2 <= c1) {
63  sp = s2;
64  } else {
65  FCL_REAL b = c1 / c2;
66  Vec3f Pb = s1 + v * b;
67  sp = Pb;
68  }
69 }
70 
71 inline bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
72  const Capsule& s2, const Transform3f& tf2,
73  FCL_REAL& distance, Vec3f* contact_points,
74  Vec3f* normal_) {
75  Vec3f pos1(
76  tf2.transform(Vec3f(0., 0., s2.halfLength))); // from distance function
77  Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength)));
78  Vec3f s_c = tf1.getTranslation();
79 
80  Vec3f segment_point;
81 
82  lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point);
83  Vec3f diff = s_c - segment_point;
84 
85  FCL_REAL diffN = diff.norm();
86  distance = diffN - s1.radius - s2.radius;
87 
88  if (distance > 0) return false;
89 
90  // Vec3f normal (-diff.normalized());
91 
92  if (normal_) *normal_ = -diff / diffN;
93 
94  if (contact_points) {
95  *contact_points = segment_point + diff * s2.radius;
96  }
97 
98  return true;
99 }
100 
101 inline bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
102  const Capsule& s2, const Transform3f& tf2,
103  FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
104  Vec3f& normal) {
105  Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength)));
106  Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength)));
107  Vec3f s_c = tf1.getTranslation();
108 
109  Vec3f segment_point;
110 
111  lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point);
112  normal = segment_point - s_c;
113  FCL_REAL norm(normal.norm());
114  dist = norm - s1.radius - s2.radius;
115 
116  static const FCL_REAL eps(std::numeric_limits<FCL_REAL>::epsilon());
117  if (norm > eps) {
118  normal.normalize();
119  } else {
120  normal << 1, 0, 0;
121  }
122  p1 = s_c + normal * s1.radius;
123  p2 = segment_point - normal * s2.radius;
124 
125  if (dist <= 0) {
126  p1 = p2 = .5 * (p1 + p2);
127  return false;
128  }
129  return true;
130 }
131 
132 inline bool sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
133  const Cylinder& s2, const Transform3f& tf2,
134  FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
135  Vec3f& normal) {
136  static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
137  FCL_REAL r1(s1.radius);
138  FCL_REAL r2(s2.radius);
139  FCL_REAL lz2(s2.halfLength);
140  // boundaries of the cylinder axis
141  Vec3f A(tf2.transform(Vec3f(0, 0, -lz2)));
142  Vec3f B(tf2.transform(Vec3f(0, 0, lz2)));
143  // Position of the center of the sphere
144  Vec3f S(tf1.getTranslation());
145  // axis of the cylinder
146  Vec3f u(tf2.getRotation().col(2));
149  assert((B - A - (s2.halfLength * 2) * u).norm() < eps);
150  Vec3f AS(S - A);
151  // abscissa of S on cylinder axis with A as the origin
152  FCL_REAL s(u.dot(AS));
153  Vec3f P(A + s * u);
154  Vec3f PS(S - P);
155  FCL_REAL dPS = PS.norm();
156  // Normal to cylinder axis such that plane (A, u, v) contains sphere
157  // center
158  Vec3f v(0, 0, 0);
159  if (dPS > eps) {
160  // S is not on cylinder axis
161  v = (1 / dPS) * PS;
162  }
163  if (s <= 0) {
164  if (dPS <= r2) {
165  // closest point on cylinder is on cylinder disc basis
166  dist = -s - r1;
167  p1 = S + r1 * u;
168  p2 = A + dPS * v;
169  normal = u;
170  } else {
171  // closest point on cylinder is on cylinder circle basis
172  p2 = A + r2 * v;
173  Vec3f Sp2(p2 - S);
174  FCL_REAL dSp2 = Sp2.norm();
175  if (dSp2 > eps) {
176  normal = (1 / dSp2) * Sp2;
177  p1 = S + r1 * normal;
178  dist = dSp2 - r1;
179  assert(fabs(dist) - (p1 - p2).norm() < eps);
180  } else {
181  // Center of sphere is on cylinder boundary
182  normal = .5 * (A + B) - p2;
183  normal.normalize();
184  p1 = p2;
185  dist = -r1;
186  }
187  }
188  } else if (s <= (s2.halfLength * 2)) {
189  // 0 < s <= s2.lz
190  normal = -v;
191  dist = dPS - r1 - r2;
192  if (dPS <= r2) {
193  // Sphere center is inside cylinder
194  p1 = p2 = S;
195  } else {
196  p2 = P + r2 * v;
197  p1 = S - r1 * v;
198  }
199  } else {
200  // lz < s
201  if (dPS <= r2) {
202  // closest point on cylinder is on cylinder disc basis
203  dist = s - (s2.halfLength * 2) - r1;
204  p1 = S - r1 * u;
205  p2 = B + dPS * v;
206  normal = -u;
207  } else {
208  // closest point on cylinder is on cylinder circle basis
209  p2 = B + r2 * v;
210  Vec3f Sp2(p2 - S);
211  FCL_REAL dSp2 = Sp2.norm();
212  if (dSp2 > eps) {
213  normal = (1 / dSp2) * Sp2;
214  p1 = S + r1 * normal;
215  dist = dSp2 - r1;
216  assert(fabs(dist) - (p1 - p2).norm() < eps);
217  } else {
218  // Center of sphere is on cylinder boundary
219  normal = .5 * (A + B) - p2;
220  normal.normalize();
221  p1 = p2;
222  dist = -r1;
223  }
224  }
225  }
226  if (dist < 0) {
227  p1 = p2 = .5 * (p1 + p2);
228  }
229  return (dist > 0);
230 }
231 
232 inline bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
233  const Sphere& s2, const Transform3f& tf2,
234  FCL_REAL& distance, Vec3f* contact_points,
235  Vec3f* normal) {
236  const Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
237  FCL_REAL len = diff.norm();
238  distance = len - s1.radius - s2.radius;
239  if (distance > 0) return false;
240 
241  // If the centers of two sphere are at the same position, the normal is (0, 0,
242  // 0). Otherwise, normal is pointing from center of object 1 to center of
243  // object 2
244  if (normal) {
245  if (len > 0)
246  *normal = diff / len;
247  else
248  *normal = diff;
249  }
250 
251  if (contact_points)
252  *contact_points =
253  tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius);
254 
255  return true;
256 }
257 
258 inline bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
259  const Sphere& s2, const Transform3f& tf2,
260  FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
261  Vec3f& normal) {
262  const Vec3f& o1 = tf1.getTranslation();
263  const Vec3f& o2 = tf2.getTranslation();
264  Vec3f diff = o1 - o2;
265  FCL_REAL len = diff.norm();
266  normal = -diff / len;
267  dist = len - s1.radius - s2.radius;
268 
269  p1.noalias() = o1 + normal * s1.radius;
270  p2.noalias() = o2 - normal * s2.radius;
271 
272  return (dist >= 0);
273 }
274 
276 inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,
277  const Vec3f& p, Vec3f& nearest) {
278  Vec3f diff = p - from;
279  Vec3f v = to - from;
280  FCL_REAL t = v.dot(diff);
281 
282  if (t > 0) {
283  FCL_REAL dotVV = v.squaredNorm();
284  if (t < dotVV) {
285  t /= dotVV;
286  diff -= v * t;
287  } else {
288  t = 1;
289  diff -= v;
290  }
291  } else
292  t = 0;
293 
294  nearest.noalias() = from + v * t;
295  return diff.squaredNorm();
296 }
297 
299 inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
300  const Vec3f& normal, const Vec3f& p) {
301  Vec3f edge1(p2 - p1);
302  Vec3f edge2(p3 - p2);
303  Vec3f edge3(p1 - p3);
304 
305  Vec3f p1_to_p(p - p1);
306  Vec3f p2_to_p(p - p2);
307  Vec3f p3_to_p(p - p3);
308 
309  Vec3f edge1_normal(edge1.cross(normal));
310  Vec3f edge2_normal(edge2.cross(normal));
311  Vec3f edge3_normal(edge3.cross(normal));
312 
313  FCL_REAL r1, r2, r3;
314  r1 = edge1_normal.dot(p1_to_p);
315  r2 = edge2_normal.dot(p2_to_p);
316  r3 = edge3_normal.dot(p3_to_p);
317  if ((r1 > 0 && r2 > 0 && r3 > 0) || (r1 <= 0 && r2 <= 0 && r3 <= 0))
318  return true;
319  return false;
320 }
321 
322 // Intersection between sphere and triangle
323 // Sphere is in position tf1, Triangle is expressed in global frame
324 inline bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf1,
325  const Vec3f& P1, const Vec3f& P2,
326  const Vec3f& P3, FCL_REAL& distance,
327  Vec3f& p1, Vec3f& p2, Vec3f& normal_) {
328  Vec3f normal = (P2 - P1).cross(P3 - P1);
329  normal.normalize();
330  const Vec3f& center = tf1.getTranslation();
331  const FCL_REAL& radius = s.radius;
332  assert(radius >= 0);
333  Vec3f p1_to_center = center - P1;
334  FCL_REAL distance_from_plane = p1_to_center.dot(normal);
335  Vec3f closest_point(
336  Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
337  FCL_REAL min_distance_sqr, distance_sqr;
338 
339  if (distance_from_plane < 0) {
340  distance_from_plane *= -1;
341  normal *= -1;
342  }
343 
344  if (projectInTriangle(P1, P2, P3, normal, center)) {
345  closest_point = center - normal * distance_from_plane;
346  min_distance_sqr = distance_from_plane;
347  } else {
348  // Compute distance to each each and take minimal distance
349  Vec3f nearest_on_edge;
350  min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point);
351 
352  distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
353  if (distance_sqr < min_distance_sqr) {
354  min_distance_sqr = distance_sqr;
355  closest_point = nearest_on_edge;
356  }
357  distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
358  if (distance_sqr < min_distance_sqr) {
359  min_distance_sqr = distance_sqr;
360  closest_point = nearest_on_edge;
361  }
362  }
363 
364  if (min_distance_sqr < radius * radius) {
365  // Collision
366  normal_ = (closest_point - center).normalized();
367  p1 = p2 = closest_point;
368  distance = sqrt(min_distance_sqr) - radius;
369  assert(distance < 0);
370  return true;
371  } else {
372  normal_ = (closest_point - center).normalized();
373  p1 = center + normal_ * radius;
374  p2 = closest_point;
375  distance = sqrt(min_distance_sqr) - radius;
376  assert(distance >= 0);
377  return false;
378  }
379 }
380 
381 inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
382  const Vec3f& P1, const Vec3f& P2,
383  const Vec3f& P3, FCL_REAL* dist) {
384  // from geometric tools, very different from the collision code.
385 
386  const Vec3f& center = tf.getTranslation();
387  FCL_REAL radius = sp.radius;
388  Vec3f diff = P1 - center;
389  Vec3f edge0 = P2 - P1;
390  Vec3f edge1 = P3 - P1;
391  FCL_REAL a00 = edge0.squaredNorm();
392  FCL_REAL a01 = edge0.dot(edge1);
393  FCL_REAL a11 = edge1.squaredNorm();
394  FCL_REAL b0 = diff.dot(edge0);
395  FCL_REAL b1 = diff.dot(edge1);
396  FCL_REAL c = diff.squaredNorm();
397  FCL_REAL det = fabs(a00 * a11 - a01 * a01);
398  FCL_REAL s = a01 * b1 - a11 * b0;
399  FCL_REAL t = a01 * b0 - a00 * b1;
400 
401  FCL_REAL sqr_dist;
402 
403  if (s + t <= det) {
404  if (s < 0) {
405  if (t < 0) // region 4
406  {
407  if (b0 < 0) {
408  t = 0;
409  if (-b0 >= a00) {
410  s = 1;
411  sqr_dist = a00 + 2 * b0 + c;
412  } else {
413  s = -b0 / a00;
414  sqr_dist = b0 * s + c;
415  }
416  } else {
417  s = 0;
418  if (b1 >= 0) {
419  t = 0;
420  sqr_dist = c;
421  } else if (-b1 >= a11) {
422  t = 1;
423  sqr_dist = a11 + 2 * b1 + c;
424  } else {
425  t = -b1 / a11;
426  sqr_dist = b1 * t + c;
427  }
428  }
429  } else // region 3
430  {
431  s = 0;
432  if (b1 >= 0) {
433  t = 0;
434  sqr_dist = c;
435  } else if (-b1 >= a11) {
436  t = 1;
437  sqr_dist = a11 + 2 * b1 + c;
438  } else {
439  t = -b1 / a11;
440  sqr_dist = b1 * t + c;
441  }
442  }
443  } else if (t < 0) // region 5
444  {
445  t = 0;
446  if (b0 >= 0) {
447  s = 0;
448  sqr_dist = c;
449  } else if (-b0 >= a00) {
450  s = 1;
451  sqr_dist = a00 + 2 * b0 + c;
452  } else {
453  s = -b0 / a00;
454  sqr_dist = b0 * s + c;
455  }
456  } else // region 0
457  {
458  // minimum at interior point
459  FCL_REAL inv_det = (1) / det;
460  s *= inv_det;
461  t *= inv_det;
462  sqr_dist = s * (a00 * s + a01 * t + 2 * b0) +
463  t * (a01 * s + a11 * t + 2 * b1) + c;
464  }
465  } else {
466  FCL_REAL tmp0, tmp1, numer, denom;
467 
468  if (s < 0) // region 2
469  {
470  tmp0 = a01 + b0;
471  tmp1 = a11 + b1;
472  if (tmp1 > tmp0) {
473  numer = tmp1 - tmp0;
474  denom = a00 - 2 * a01 + a11;
475  if (numer >= denom) {
476  s = 1;
477  t = 0;
478  sqr_dist = a00 + 2 * b0 + c;
479  } else {
480  s = numer / denom;
481  t = 1 - s;
482  sqr_dist = s * (a00 * s + a01 * t + 2 * b0) +
483  t * (a01 * s + a11 * t + 2 * b1) + c;
484  }
485  } else {
486  s = 0;
487  if (tmp1 <= 0) {
488  t = 1;
489  sqr_dist = a11 + 2 * b1 + c;
490  } else if (b1 >= 0) {
491  t = 0;
492  sqr_dist = c;
493  } else {
494  t = -b1 / a11;
495  sqr_dist = b1 * t + c;
496  }
497  }
498  } else if (t < 0) // region 6
499  {
500  tmp0 = a01 + b1;
501  tmp1 = a00 + b0;
502  if (tmp1 > tmp0) {
503  numer = tmp1 - tmp0;
504  denom = a00 - 2 * a01 + a11;
505  if (numer >= denom) {
506  t = 1;
507  s = 0;
508  sqr_dist = a11 + 2 * b1 + c;
509  } else {
510  t = numer / denom;
511  s = 1 - t;
512  sqr_dist = s * (a00 * s + a01 * t + 2 * b0) +
513  t * (a01 * s + a11 * t + 2 * b1) + c;
514  }
515  } else {
516  t = 0;
517  if (tmp1 <= 0) {
518  s = 1;
519  sqr_dist = a00 + 2 * b0 + c;
520  } else if (b0 >= 0) {
521  s = 0;
522  sqr_dist = c;
523  } else {
524  s = -b0 / a00;
525  sqr_dist = b0 * s + c;
526  }
527  }
528  } else // region 1
529  {
530  numer = a11 + b1 - a01 - b0;
531  if (numer <= 0) {
532  s = 0;
533  t = 1;
534  sqr_dist = a11 + 2 * b1 + c;
535  } else {
536  denom = a00 - 2 * a01 + a11;
537  if (numer >= denom) {
538  s = 1;
539  t = 0;
540  sqr_dist = a00 + 2 * b0 + c;
541  } else {
542  s = numer / denom;
543  t = 1 - s;
544  sqr_dist = s * (a00 * s + a01 * t + 2 * b0) +
545  t * (a01 * s + a11 * t + 2 * b1) + c;
546  }
547  }
548  }
549  }
550 
551  // Account for numerical round-off error.
552  if (sqr_dist < 0) sqr_dist = 0;
553 
554  if (sqr_dist > radius * radius) {
555  if (dist) *dist = std::sqrt(sqr_dist) - radius;
556  return true;
557  } else {
558  if (dist) *dist = -1;
559  return false;
560  }
561 }
562 
563 inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
564  const Vec3f& P1, const Vec3f& P2,
565  const Vec3f& P3, FCL_REAL* dist, Vec3f* p1,
566  Vec3f* p2) {
567  if (p1 || p2) {
568  const Vec3f& o = tf.getTranslation();
569  Project::ProjectResult result;
570  result = Project::projectTriangle(P1, P2, P3, o);
571  if (result.sqr_distance > sp.radius * sp.radius) {
572  if (dist) *dist = std::sqrt(result.sqr_distance) - sp.radius;
573  Vec3f project_p = P1 * result.parameterization[0] +
574  P2 * result.parameterization[1] +
575  P3 * result.parameterization[2];
576  Vec3f dir = o - project_p;
577  dir.normalize();
578  if (p1) {
579  *p1 = o - dir * sp.radius;
580  }
581  if (p2) *p2 = project_p;
582  return true;
583  } else
584  return false;
585  } else {
586  return sphereTriangleDistance(sp, tf, P1, P2, P3, dist);
587  }
588 }
589 
590 inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1,
591  const Vec3f& P1, const Vec3f& P2,
592  const Vec3f& P3, const Transform3f& tf2,
593  FCL_REAL* dist, Vec3f* p1, Vec3f* p2) {
594  bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1),
595  tf2.transform(P2),
596  tf2.transform(P3), dist, p1, p2);
597  return res;
598 }
599 
600 struct HPP_FCL_LOCAL ContactPoint {
601  Vec3f normal;
602  Vec3f point;
603  FCL_REAL depth;
604  ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d)
605  : normal(n), point(p), depth(d) {}
606 };
607 
608 static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
609  const Vec3f& pb, const Vec3f& ub,
610  FCL_REAL* alpha, FCL_REAL* beta) {
611  Vec3f p = pb - pa;
612  FCL_REAL uaub = ua.dot(ub);
613  FCL_REAL q1 = ua.dot(p);
614  FCL_REAL q2 = -ub.dot(p);
615  FCL_REAL d = 1 - uaub * uaub;
616  if (d <= (FCL_REAL)(0.0001f)) {
617  *alpha = 0;
618  *beta = 0;
619  } else {
620  d = 1 / d;
621  *alpha = (q1 + uaub * q2) * d;
622  *beta = (uaub * q1 + q2) * d;
623  }
624 }
625 
626 // find all the intersection points between the 2D rectangle with vertices
627 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
628 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
629 //
630 // the intersection points are returned as x,y pairs in the 'ret' array.
631 // the number of intersection points is returned by the function (this will
632 // be in the range 0 to 8).
633 static unsigned int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8],
634  FCL_REAL ret[16]) {
635  // q (and r) contain nq (and nr) coordinate points for the current (and
636  // chopped) polygons
637  unsigned int nq = 4, nr = 0;
638  FCL_REAL buffer[16];
639  FCL_REAL* q = p;
640  FCL_REAL* r = ret;
641  for (int dir = 0; dir <= 1; ++dir) {
642  // direction notation: xy[0] = x axis, xy[1] = y axis
643  for (int sign = -1; sign <= 1; sign += 2) {
644  // chop q along the line xy[dir] = sign*h[dir]
645  FCL_REAL* pq = q;
646  FCL_REAL* pr = r;
647  nr = 0;
648  for (unsigned int i = nq; i > 0; --i) {
649  // go through all points in q and all lines between adjacent points
650  if (sign * pq[dir] < h[dir]) {
651  // this point is inside the chopping line
652  pr[0] = pq[0];
653  pr[1] = pq[1];
654  pr += 2;
655  nr++;
656  if (nr & 8) {
657  q = r;
658  goto done;
659  }
660  }
661  FCL_REAL* nextq = (i > 1) ? pq + 2 : q;
662  if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir])) {
663  // this line crosses the chopping line
664  pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) /
665  (nextq[dir] - pq[dir]) *
666  (sign * h[dir] - pq[dir]);
667  pr[dir] = sign * h[dir];
668  pr += 2;
669  nr++;
670  if (nr & 8) {
671  q = r;
672  goto done;
673  }
674  }
675  pq += 2;
676  }
677  q = r;
678  r = (q == ret) ? buffer : ret;
679  nq = nr;
680  }
681  }
682 
683 done:
684  if (q != ret) memcpy(ret, q, nr * 2 * sizeof(FCL_REAL));
685  return nr;
686 }
687 
688 // given n points in the plane (array p, of size 2*n), generate m points that
689 // best represent the whole set. the definition of 'best' here is not
690 // predetermined - the idea is to select points that give good box-box
691 // collision detection behavior. the chosen point indexes are returned in the
692 // array iret (of size m). 'i0' is always the first entry in the array.
693 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
694 // in the range [0..n-1].
695 static inline void cullPoints2(unsigned int n, FCL_REAL p[], unsigned int m,
696  unsigned int i0, unsigned int iret[]) {
697  // compute the centroid of the polygon in cx,cy
698  FCL_REAL a, cx, cy, q;
699  switch (n) {
700  case 1:
701  cx = p[0];
702  cy = p[1];
703  break;
704  case 2:
705  cx = 0.5 * (p[0] + p[2]);
706  cy = 0.5 * (p[1] + p[3]);
707  break;
708  default:
709  a = 0;
710  cx = 0;
711  cy = 0;
712  assert(n > 0 && "n should be positive");
713  for (int i = 0; i < (int)n - 1; ++i) {
714  q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
715  a += q;
716  cx += q * (p[i * 2] + p[i * 2 + 2]);
717  cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
718  }
719  q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
720  if (std::abs(a + q) > std::numeric_limits<FCL_REAL>::epsilon())
721  a = 1 / (3 * (a + q));
722  else
723  a = 1e18f;
724 
725  cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
726  cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
727  }
728 
729  // compute the angle of each point w.r.t. the centroid
730  FCL_REAL A[8];
731  for (unsigned int i = 0; i < n; ++i)
732  A[i] = atan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
733 
734  // search for points that have angles closest to A[i0] + i*(2*pi/m).
735  int avail[] = {1, 1, 1, 1, 1, 1, 1, 1};
736  avail[i0] = 0;
737  iret[0] = i0;
738  iret++;
739  const double pi = boost::math::constants::pi<FCL_REAL>();
740  for (unsigned int j = 1; j < m; ++j) {
741  a = j * (2 * pi / m) + A[i0];
742  if (a > pi) a -= 2 * pi;
743  FCL_REAL maxdiff = 1e9, diff;
744 
745  *iret = i0; // iret is not allowed to keep this value, but it sometimes
746  // does, when diff=#QNAN0
747  for (unsigned int i = 0; i < n; ++i) {
748  if (avail[i]) {
749  diff = std::abs(A[i] - a);
750  if (diff > pi) diff = 2 * pi - diff;
751  if (diff < maxdiff) {
752  maxdiff = diff;
753  *iret = i;
754  }
755  }
756  }
757  avail[*iret] = 0;
758  iret++;
759  }
760 }
761 
762 inline unsigned int boxBox2(const Vec3f& halfSide1, const Matrix3f& R1,
763  const Vec3f& T1, const Vec3f& halfSide2,
764  const Matrix3f& R2, const Vec3f& T2, Vec3f& normal,
765  FCL_REAL* depth, int* return_code,
766  unsigned int maxc,
767  std::vector<ContactPoint>& contacts) {
768  const FCL_REAL fudge_factor = FCL_REAL(1.05);
769  Vec3f normalC;
770  FCL_REAL s, s2, l;
771  int invert_normal, code;
772 
773  Vec3f p(T2 -
774  T1); // get vector from centers of box 1 to box 2, relative to box 1
775  Vec3f pp(R1.transpose() * p); // get pp = p relative to body 1
776 
777  // get side lengths / 2
778  const Vec3f& A(halfSide1);
779  const Vec3f& B(halfSide2);
780 
781  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
782  Matrix3f R(R1.transpose() * R2);
783  Matrix3f Q(R.cwiseAbs());
784 
785  // for all 15 possible separating axes:
786  // * see if the axis separates the boxes. if so, return 0.
787  // * find the depth of the penetration along the separating axis (s2)
788  // * if this is the largest depth so far, record it.
789  // the normal vector will be set to the separating axis with the smallest
790  // depth. note: normalR is set to point to a column of R1 or R2 if that is
791  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
792  // set to a vector relative to body 1. invert_normal is 1 if the sign of
793  // the normal should be flipped.
794 
795  int best_col_id = -1;
796  const Matrix3f* normalR = 0;
797  FCL_REAL tmp = 0;
798 
799  s = -(std::numeric_limits<FCL_REAL>::max)();
800  invert_normal = 0;
801  code = 0;
802 
803  // separating axis = u1, u2, u3
804  tmp = pp[0];
805  s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
806  if (s2 > 0) {
807  *return_code = 0;
808  return 0;
809  }
810  if (s2 > s) {
811  s = s2;
812  best_col_id = 0;
813  normalR = &R1;
814  invert_normal = (tmp < 0);
815  code = 1;
816  }
817 
818  tmp = pp[1];
819  s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
820  if (s2 > 0) {
821  *return_code = 0;
822  return 0;
823  }
824  if (s2 > s) {
825  s = s2;
826  best_col_id = 1;
827  normalR = &R1;
828  invert_normal = (tmp < 0);
829  code = 2;
830  }
831 
832  tmp = pp[2];
833  s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
834  if (s2 > 0) {
835  *return_code = 0;
836  return 0;
837  }
838  if (s2 > s) {
839  s = s2;
840  best_col_id = 2;
841  normalR = &R1;
842  invert_normal = (tmp < 0);
843  code = 3;
844  }
845 
846  // separating axis = v1, v2, v3
847  tmp = R2.col(0).dot(p);
848  s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]);
849  if (s2 > 0) {
850  *return_code = 0;
851  return 0;
852  }
853  if (s2 > s) {
854  s = s2;
855  best_col_id = 0;
856  normalR = &R2;
857  invert_normal = (tmp < 0);
858  code = 4;
859  }
860 
861  tmp = R2.col(1).dot(p);
862  s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]);
863  if (s2 > 0) {
864  *return_code = 0;
865  return 0;
866  }
867  if (s2 > s) {
868  s = s2;
869  best_col_id = 1;
870  normalR = &R2;
871  invert_normal = (tmp < 0);
872  code = 5;
873  }
874 
875  tmp = R2.col(2).dot(p);
876  s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]);
877  if (s2 > 0) {
878  *return_code = 0;
879  return 0;
880  }
881  if (s2 > s) {
882  s = s2;
883  best_col_id = 2;
884  normalR = &R2;
885  invert_normal = (tmp < 0);
886  code = 6;
887  }
888 
889  FCL_REAL fudge2(1.0e-6);
890  Q.array() += fudge2;
891 
892  Vec3f n;
893  static const FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
894 
895  // separating axis = u1 x (v1,v2,v3)
896  tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
897  s2 = std::abs(tmp) -
898  (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
899  if (s2 > 0) {
900  *return_code = 0;
901  return 0;
902  }
903  n = Vec3f(0, -R(2, 0), R(1, 0));
904  l = n.norm();
905  if (l > eps) {
906  s2 /= l;
907  if (s2 * fudge_factor > s) {
908  s = s2;
909  best_col_id = -1;
910  normalC = n / l;
911  invert_normal = (tmp < 0);
912  code = 7;
913  }
914  }
915 
916  tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
917  s2 = std::abs(tmp) -
918  (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
919  if (s2 > 0) {
920  *return_code = 0;
921  return 0;
922  }
923  n = Vec3f(0, -R(2, 1), R(1, 1));
924  l = n.norm();
925  if (l > eps) {
926  s2 /= l;
927  if (s2 * fudge_factor > s) {
928  s = s2;
929  best_col_id = -1;
930  normalC = n / l;
931  invert_normal = (tmp < 0);
932  code = 8;
933  }
934  }
935 
936  tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
937  s2 = std::abs(tmp) -
938  (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
939  if (s2 > 0) {
940  *return_code = 0;
941  return 0;
942  }
943  n = Vec3f(0, -R(2, 2), R(1, 2));
944  l = n.norm();
945  if (l > eps) {
946  s2 /= l;
947  if (s2 * fudge_factor > s) {
948  s = s2;
949  best_col_id = -1;
950  normalC = n / l;
951  invert_normal = (tmp < 0);
952  code = 9;
953  }
954  }
955 
956  // separating axis = u2 x (v1,v2,v3)
957  tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
958  s2 = std::abs(tmp) -
959  (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
960  if (s2 > 0) {
961  *return_code = 0;
962  return 0;
963  }
964  n = Vec3f(R(2, 0), 0, -R(0, 0));
965  l = n.norm();
966  if (l > eps) {
967  s2 /= l;
968  if (s2 * fudge_factor > s) {
969  s = s2;
970  best_col_id = -1;
971  normalC = n / l;
972  invert_normal = (tmp < 0);
973  code = 10;
974  }
975  }
976 
977  tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
978  s2 = std::abs(tmp) -
979  (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
980  if (s2 > 0) {
981  *return_code = 0;
982  return 0;
983  }
984  n = Vec3f(R(2, 1), 0, -R(0, 1));
985  l = n.norm();
986  if (l > eps) {
987  s2 /= l;
988  if (s2 * fudge_factor > s) {
989  s = s2;
990  best_col_id = -1;
991  normalC = n / l;
992  invert_normal = (tmp < 0);
993  code = 11;
994  }
995  }
996 
997  tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
998  s2 = std::abs(tmp) -
999  (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
1000  if (s2 > 0) {
1001  *return_code = 0;
1002  return 0;
1003  }
1004  n = Vec3f(R(2, 2), 0, -R(0, 2));
1005  l = n.norm();
1006  if (l > eps) {
1007  s2 /= l;
1008  if (s2 * fudge_factor > s) {
1009  s = s2;
1010  best_col_id = -1;
1011  normalC = n / l;
1012  invert_normal = (tmp < 0);
1013  code = 12;
1014  }
1015  }
1016 
1017  // separating axis = u3 x (v1,v2,v3)
1018  tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
1019  s2 = std::abs(tmp) -
1020  (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
1021  if (s2 > 0) {
1022  *return_code = 0;
1023  return 0;
1024  }
1025  n = Vec3f(-R(1, 0), R(0, 0), 0);
1026  l = n.norm();
1027  if (l > eps) {
1028  s2 /= l;
1029  if (s2 * fudge_factor > s) {
1030  s = s2;
1031  best_col_id = -1;
1032  normalC = n / l;
1033  invert_normal = (tmp < 0);
1034  code = 13;
1035  }
1036  }
1037 
1038  tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
1039  s2 = std::abs(tmp) -
1040  (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
1041  if (s2 > 0) {
1042  *return_code = 0;
1043  return 0;
1044  }
1045  n = Vec3f(-R(1, 1), R(0, 1), 0);
1046  l = n.norm();
1047  if (l > eps) {
1048  s2 /= l;
1049  if (s2 * fudge_factor > s) {
1050  s = s2;
1051  best_col_id = -1;
1052  normalC = n / l;
1053  invert_normal = (tmp < 0);
1054  code = 14;
1055  }
1056  }
1057 
1058  tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
1059  s2 = std::abs(tmp) -
1060  (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
1061  if (s2 > 0) {
1062  *return_code = 0;
1063  return 0;
1064  }
1065  n = Vec3f(-R(1, 2), R(0, 2), 0);
1066  l = n.norm();
1067  if (l > eps) {
1068  s2 /= l;
1069  if (s2 * fudge_factor > s) {
1070  s = s2;
1071  best_col_id = -1;
1072  normalC = n / l;
1073  invert_normal = (tmp < 0);
1074  code = 15;
1075  }
1076  }
1077 
1078  if (!code) {
1079  *return_code = code;
1080  return 0;
1081  }
1082 
1083  // if we get to this point, the boxes interpenetrate. compute the normal
1084  // in global coordinates.
1085  if (best_col_id != -1)
1086  normal = normalR->col(best_col_id);
1087  else
1088  normal.noalias() = R1 * normalC;
1089 
1090  if (invert_normal) normal = -normal;
1091 
1092  *depth = -s; // s is negative when the boxes are in collision
1093 
1094  // compute contact point(s)
1095 
1096  if (code > 6) {
1097  // an edge from box 1 touches an edge from box 2.
1098  // find a point pa on the intersecting edge of box 1
1099  Vec3f pa(T1);
1100  FCL_REAL sign;
1101 
1102  for (int j = 0; j < 3; ++j) {
1103  sign = (R1.col(j).dot(normal) > 0) ? 1 : -1;
1104  pa += R1.col(j) * (A[j] * sign);
1105  }
1106 
1107  // find a point pb on the intersecting edge of box 2
1108  Vec3f pb(T2);
1109 
1110  for (int j = 0; j < 3; ++j) {
1111  sign = (R2.col(j).dot(normal) > 0) ? -1 : 1;
1112  pb += R2.col(j) * (B[j] * sign);
1113  }
1114 
1115  FCL_REAL alpha, beta;
1116  Vec3f ua(R1.col((code - 7) / 3));
1117  Vec3f ub(R2.col((code - 7) % 3));
1118 
1119  lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
1120  pa += ua * alpha;
1121  pb += ub * beta;
1122 
1123  // Vec3f pointInWorld((pa + pb) * 0.5);
1124  // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
1125  contacts.push_back(ContactPoint(-normal, pb, -*depth));
1126  *return_code = code;
1127 
1128  return 1;
1129  }
1130 
1131  // okay, we have a face-something intersection (because the separating
1132  // axis is perpendicular to a face). define face 'a' to be the reference
1133  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
1134  // the incident face (the closest face of the other box).
1135 
1136  const Matrix3f *Ra, *Rb;
1137  const Vec3f *pa, *pb, *Sa, *Sb;
1138 
1139  if (code <= 3) {
1140  Ra = &R1;
1141  Rb = &R2;
1142  pa = &T1;
1143  pb = &T2;
1144  Sa = &A;
1145  Sb = &B;
1146  } else {
1147  Ra = &R2;
1148  Rb = &R1;
1149  pa = &T2;
1150  pb = &T1;
1151  Sa = &B;
1152  Sb = &A;
1153  }
1154 
1155  // nr = normal vector of reference face dotted with axes of incident box.
1156  // anr = absolute values of nr.
1157  Vec3f normal2, nr, anr;
1158  if (code <= 3)
1159  normal2 = normal;
1160  else
1161  normal2 = -normal;
1162 
1163  nr.noalias() = Rb->transpose() * normal2;
1164  anr = nr.cwiseAbs();
1165 
1166  // find the largest compontent of anr: this corresponds to the normal
1167  // for the indident face. the other axis numbers of the indicent face
1168  // are stored in a1,a2.
1169  int lanr, a1, a2;
1170  if (anr[1] > anr[0]) {
1171  if (anr[1] > anr[2]) {
1172  a1 = 0;
1173  lanr = 1;
1174  a2 = 2;
1175  } else {
1176  a1 = 0;
1177  a2 = 1;
1178  lanr = 2;
1179  }
1180  } else {
1181  if (anr[0] > anr[2]) {
1182  lanr = 0;
1183  a1 = 1;
1184  a2 = 2;
1185  } else {
1186  a1 = 0;
1187  a2 = 1;
1188  lanr = 2;
1189  }
1190  }
1191 
1192  // compute center point of incident face, in reference-face coordinates
1193  Vec3f center;
1194  if (nr[lanr] < 0)
1195  center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
1196  else
1197  center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
1198 
1199  // find the normal and non-normal axis numbers of the reference box
1200  int codeN, code1, code2;
1201  if (code <= 3)
1202  codeN = code - 1;
1203  else
1204  codeN = code - 4;
1205 
1206  if (codeN == 0) {
1207  code1 = 1;
1208  code2 = 2;
1209  } else if (codeN == 1) {
1210  code1 = 0;
1211  code2 = 2;
1212  } else {
1213  code1 = 0;
1214  code2 = 1;
1215  }
1216 
1217  // find the four corners of the incident face, in reference-face coordinates
1218  FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
1219  FCL_REAL c1, c2, m11, m12, m21, m22;
1220  c1 = Ra->col(code1).dot(center);
1221  c2 = Ra->col(code2).dot(center);
1222  // optimize this? - we have already computed this data above, but it is not
1223  // stored in an easy-to-index format. for now it's quicker just to recompute
1224  // the four dot products.
1225  Vec3f tempRac = Ra->col(code1);
1226  m11 = Rb->col(a1).dot(tempRac);
1227  m12 = Rb->col(a2).dot(tempRac);
1228  tempRac = Ra->col(code2);
1229  m21 = Rb->col(a1).dot(tempRac);
1230  m22 = Rb->col(a2).dot(tempRac);
1231 
1232  FCL_REAL k1 = m11 * (*Sb)[a1];
1233  FCL_REAL k2 = m21 * (*Sb)[a1];
1234  FCL_REAL k3 = m12 * (*Sb)[a2];
1235  FCL_REAL k4 = m22 * (*Sb)[a2];
1236  quad[0] = c1 - k1 - k3;
1237  quad[1] = c2 - k2 - k4;
1238  quad[2] = c1 - k1 + k3;
1239  quad[3] = c2 - k2 + k4;
1240  quad[4] = c1 + k1 + k3;
1241  quad[5] = c2 + k2 + k4;
1242  quad[6] = c1 + k1 - k3;
1243  quad[7] = c2 + k2 - k4;
1244 
1245  // find the size of the reference face
1246  FCL_REAL rect[2];
1247  rect[0] = (*Sa)[code1];
1248  rect[1] = (*Sa)[code2];
1249 
1250  // intersect the incident and reference faces
1251  FCL_REAL ret[16];
1252  const unsigned int n_intersect = intersectRectQuad2(rect, quad, ret);
1253  if (n_intersect < 1) {
1254  *return_code = code;
1255  return 0;
1256  } // this should never happen
1257 
1258  // convert the intersection points into reference-face coordinates,
1259  // and compute the contact position and depth for each point. only keep
1260  // those points that have a positive (penetrating) depth. delete points in
1261  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
1262  Vec3f points[8]; // penetrating contact points
1263  FCL_REAL dep[8]; // depths for those points
1264  FCL_REAL det1 = 1.f / (m11 * m22 - m12 * m21);
1265  m11 *= det1;
1266  m12 *= det1;
1267  m21 *= det1;
1268  m22 *= det1;
1269  unsigned int cnum = 0; // number of penetrating contact points found
1270  for (unsigned int j = 0; j < n_intersect; ++j) {
1271  FCL_REAL k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
1272  FCL_REAL k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
1273  points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2;
1274  dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
1275  if (dep[cnum] >= 0) {
1276  ret[cnum * 2] = ret[j * 2];
1277  ret[cnum * 2 + 1] = ret[j * 2 + 1];
1278  cnum++;
1279  }
1280  }
1281  if (cnum < 1) {
1282  *return_code = code;
1283  return 0;
1284  } // this should never happen
1285 
1286  // we can't generate more contacts than we actually have
1287  if (maxc > cnum) maxc = cnum;
1288  if (maxc < 1) maxc = 1;
1289 
1290  if (cnum <= maxc) {
1291  if (code < 4) {
1292  // we have less contacts than we need, so we use them all
1293  for (unsigned int j = 0; j < cnum; ++j) {
1294  Vec3f pointInWorld = points[j] + (*pa);
1295  contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
1296  }
1297  } else {
1298  // we have less contacts than we need, so we use them all
1299  for (unsigned int j = 0; j < cnum; ++j) {
1300  Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j];
1301  contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
1302  }
1303  }
1304  } else {
1305  // we have more contacts than are wanted, some of them must be culled.
1306  // find the deepest point, it is always the first contact.
1307  unsigned int i1 = 0;
1308  FCL_REAL maxdepth = dep[0];
1309  for (unsigned int i = 1; i < cnum; ++i) {
1310  if (dep[i] > maxdepth) {
1311  maxdepth = dep[i];
1312  i1 = i;
1313  }
1314  }
1315 
1316  unsigned int iret[8];
1317  cullPoints2(cnum, ret, maxc, i1, iret);
1318 
1319  for (unsigned int j = 0; j < maxc; ++j) {
1320  Vec3f posInWorld = points[iret[j]] + (*pa);
1321  if (code < 4)
1322  contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]]));
1323  else
1324  contacts.push_back(ContactPoint(
1325  -normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
1326  }
1327  cnum = maxc;
1328  }
1329 
1330  *return_code = code;
1331  return cnum;
1332 }
1333 
1334 inline bool compareContactPoints(const ContactPoint& c1,
1335  const ContactPoint& c2) {
1336  return c1.depth < c2.depth;
1337 } // Accending order, assuming penetration depth is a negative number!
1338 
1339 inline bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
1340  const Box& s2, const Transform3f& tf2,
1341  Vec3f* contact_points, FCL_REAL* penetration_depth_,
1342  Vec3f* normal_) {
1343  std::vector<ContactPoint> contacts;
1344  int return_code;
1345  Vec3f normal;
1346  FCL_REAL depth;
1347  /* int cnum = */ boxBox2(s1.halfSide, tf1.getRotation(), tf1.getTranslation(),
1348  s2.halfSide, tf2.getRotation(), tf2.getTranslation(),
1349  normal, &depth, &return_code, 4, contacts);
1350 
1351  if (normal_) *normal_ = normal;
1352  if (penetration_depth_) *penetration_depth_ = depth;
1353 
1354  if (contact_points) {
1355  if (contacts.size() > 0) {
1356  std::sort(contacts.begin(), contacts.end(), compareContactPoints);
1357  *contact_points = contacts[0].point;
1358  if (penetration_depth_) *penetration_depth_ = -contacts[0].depth;
1359  }
1360  }
1361 
1362  return return_code != 0;
1363 }
1364 
1365 template <typename T>
1366 inline T halfspaceIntersectTolerance() {
1367  return 0;
1368 }
1369 
1370 template <>
1371 inline float halfspaceIntersectTolerance() {
1372  return 0.0001f;
1373 }
1374 
1375 template <>
1376 inline double halfspaceIntersectTolerance() {
1377  return 0.0000001;
1378 }
1379 
1380 inline bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1,
1381  const Halfspace& s2,
1382  const Transform3f& tf2, FCL_REAL& distance,
1383  Vec3f& p1, Vec3f& p2, Vec3f& normal) {
1384  Halfspace new_s2 = transform(s2, tf2);
1385  const Vec3f& center = tf1.getTranslation();
1386  distance = new_s2.signedDistance(center) - s1.radius;
1387  if (distance <= 0) {
1388  normal = -new_s2.n; // pointing from s1 to s2
1389  p1 = p2 = center - new_s2.n * s1.radius - (distance * 0.5) * new_s2.n;
1390  return true;
1391  } else {
1392  p1 = center - s1.radius * new_s2.n;
1393  p2 = p1 - distance * new_s2.n;
1394  return false;
1395  }
1396 }
1397 
1404 inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
1405  const Halfspace& s2, const Transform3f& tf2) {
1406  Halfspace new_s2 = transform(s2, tf2);
1407 
1408  const Matrix3f& R = tf1.getRotation();
1409  const Vec3f& T = tf1.getTranslation();
1410 
1411  Vec3f Q(R.transpose() * new_s2.n);
1412 
1413  FCL_REAL depth =
1414  Q.cwiseProduct(s1.halfSide).lpNorm<1>() - new_s2.signedDistance(T);
1415  return (depth >= 0);
1416 }
1417 
1418 inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
1419  const Halfspace& s2, const Transform3f& tf2,
1420  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1421  Vec3f& normal) {
1422  Halfspace new_s2 = transform(s2, tf2);
1423 
1424  const Matrix3f& R = tf1.getRotation();
1425  const Vec3f& T = tf1.getTranslation();
1426 
1427  // Q: plane normal expressed in box frame
1428  const Vec3f Q(R.transpose() * new_s2.n);
1429  // A: scalar products of each side with normal
1430  const Vec3f A(Q.cwiseProduct(s1.halfSide));
1431 
1432  distance = new_s2.signedDistance(T) - A.lpNorm<1>();
1433  if (distance > 0) {
1434  p1.noalias() = T + R * (A.array() > 0).select(s1.halfSide, -s1.halfSide);
1435  p2.noalias() = p1 - distance * new_s2.n;
1436  return false;
1437  }
1438 
1440  Vec3f p(T);
1441  int sign = 0;
1442 
1443  if (std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
1444  std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) {
1445  sign = (A[0] > 0) ? -1 : 1;
1446  p += R.col(0) * (s1.halfSide[0] * sign);
1447  } else if (std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
1448  std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) {
1449  sign = (A[1] > 0) ? -1 : 1;
1450  p += R.col(1) * (s1.halfSide[1] * sign);
1451  } else if (std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
1452  std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) {
1453  sign = (A[2] > 0) ? -1 : 1;
1454  p += R.col(2) * (s1.halfSide[2] * sign);
1455  } else {
1456  p.noalias() += R * (A.array() > 0).select(-s1.halfSide, s1.halfSide);
1457  }
1458 
1460  normal = -new_s2.n;
1461  p1 = p2 = p - new_s2.n * (distance * 0.5);
1462 
1463  return true;
1464 }
1465 
1466 inline bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1,
1467  const Halfspace& s2,
1468  const Transform3f& tf2,
1469  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1470  Vec3f& normal) {
1471  Halfspace new_s2 = transform(s2, tf2);
1472 
1473  const Matrix3f& R = tf1.getRotation();
1474  const Vec3f& T = tf1.getTranslation();
1475 
1476  Vec3f dir_z = R.col(2);
1477 
1478  FCL_REAL cosa = dir_z.dot(new_s2.n);
1479  if (std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>()) {
1480  // Capsule parallel to plane
1481  FCL_REAL signed_dist = new_s2.signedDistance(T);
1482  distance = signed_dist - s1.radius;
1483  if (distance > 0) {
1484  p1 = T - s1.radius * new_s2.n;
1485  p2 = p1 - distance * new_s2.n;
1486  return false;
1487  }
1488 
1489  normal = -new_s2.n;
1490  p1 = p2 = T + new_s2.n * (-0.5 * distance - s1.radius);
1491  return true;
1492  } else {
1493  int sign = (cosa > 0) ? -1 : 1;
1494  // closest capsule vertex to halfspace if no collision,
1495  // or deeper inside halspace if collision
1496  Vec3f p = T + dir_z * (s1.halfLength * sign);
1497 
1498  FCL_REAL signed_dist = new_s2.signedDistance(p);
1499  distance = signed_dist - s1.radius;
1500  if (distance > 0) {
1501  p1 = T - s1.radius * new_s2.n;
1502  p2 = p1 - distance * new_s2.n;
1503  return false;
1504  }
1505  normal = -new_s2.n;
1506  // deepest point
1507  Vec3f c = p - new_s2.n * s1.radius;
1508  p1 = p2 = c - (0.5 * distance) * new_s2.n;
1509  return true;
1510  }
1511 }
1512 
1513 inline bool cylinderHalfspaceIntersect(const Cylinder& s1,
1514  const Transform3f& tf1,
1515  const Halfspace& s2,
1516  const Transform3f& tf2,
1517  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1518  Vec3f& normal) {
1519  Halfspace new_s2 = transform(s2, tf2);
1520 
1521  const Matrix3f& R = tf1.getRotation();
1522  const Vec3f& T = tf1.getTranslation();
1523 
1524  Vec3f dir_z = R.col(2);
1525  FCL_REAL cosa = dir_z.dot(new_s2.n);
1526 
1527  if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) {
1528  FCL_REAL signed_dist = new_s2.signedDistance(T);
1529  distance = signed_dist - s1.radius;
1530  if (distance > 0) {
1531  // TODO: compute closest points
1532  p1 = p2 = Vec3f(0, 0, 0);
1533  return false;
1534  }
1535 
1536  normal = -new_s2.n;
1537  p1 = p2 = T - (0.5 * distance + s1.radius) * new_s2.n;
1538  return true;
1539  } else {
1540  Vec3f C = dir_z * cosa - new_s2.n;
1541  if (std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
1542  std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>())
1543  C = Vec3f(0, 0, 0);
1544  else {
1545  FCL_REAL s = C.norm();
1546  s = s1.radius / s;
1547  C *= s;
1548  }
1549 
1550  int sign = (cosa > 0) ? -1 : 1;
1551  // deepest point
1552  Vec3f p = T + dir_z * (s1.halfLength * sign) + C;
1553  distance = new_s2.signedDistance(p);
1554  if (distance > 0) {
1555  // TODO: compute closest points
1556  p1 = p2 = Vec3f(0, 0, 0);
1557  return false;
1558  } else {
1559  normal = -new_s2.n;
1560  p1 = p2 = p - (0.5 * distance) * new_s2.n;
1561  return true;
1562  }
1563  }
1564 }
1565 
1566 inline bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1,
1567  const Halfspace& s2, const Transform3f& tf2,
1568  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1569  Vec3f& normal) {
1570  Halfspace new_s2 = transform(s2, tf2);
1571 
1572  const Matrix3f& R = tf1.getRotation();
1573  const Vec3f& T = tf1.getTranslation();
1574 
1575  Vec3f dir_z = R.col(2);
1576  FCL_REAL cosa = dir_z.dot(new_s2.n);
1577 
1578  if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) {
1579  FCL_REAL signed_dist = new_s2.signedDistance(T);
1580  distance = signed_dist - s1.radius;
1581  if (distance > 0) {
1582  p1 = p2 = Vec3f(0, 0, 0);
1583  return false;
1584  } else {
1585  normal = -new_s2.n;
1586  p1 = p2 =
1587  T - dir_z * (s1.halfLength) - new_s2.n * (0.5 * distance + s1.radius);
1588  return true;
1589  }
1590  } else {
1591  Vec3f C = dir_z * cosa - new_s2.n;
1592  if (std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
1593  std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>())
1594  C = Vec3f(0, 0, 0);
1595  else {
1596  FCL_REAL s = C.norm();
1597  s = s1.radius / s;
1598  C *= s;
1599  }
1600 
1601  Vec3f a1 = T + dir_z * (s1.halfLength);
1602  Vec3f a2 = T - dir_z * (s1.halfLength) + C;
1603 
1604  FCL_REAL d1 = new_s2.signedDistance(a1);
1605  FCL_REAL d2 = new_s2.signedDistance(a2);
1606 
1607  if (d1 > 0 && d2 > 0)
1608  return false;
1609  else {
1610  distance = std::min(d1, d2);
1611  normal = -new_s2.n;
1612  p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n;
1613  return true;
1614  }
1615  }
1616 }
1617 
1618 inline bool convexHalfspaceIntersect(
1619  const ConvexBase& s1, const Transform3f& tf1, const Halfspace& s2,
1620  const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth,
1621  Vec3f* normal) {
1622  Halfspace new_s2 = transform(s2, tf2);
1623 
1624  Vec3f v;
1625  FCL_REAL depth = (std::numeric_limits<FCL_REAL>::max)();
1626 
1627  for (unsigned int i = 0; i < s1.num_points; ++i) {
1628  Vec3f p = tf1.transform(s1.points[i]);
1629 
1630  FCL_REAL d = new_s2.signedDistance(p);
1631  if (d < depth) {
1632  depth = d;
1633  v = p;
1634  }
1635  }
1636 
1637  if (depth <= 0) {
1638  if (contact_points) *contact_points = v - new_s2.n * (0.5 * depth);
1639  if (penetration_depth) *penetration_depth = depth;
1640  if (normal) *normal = -new_s2.n;
1641  return true;
1642  } else
1643  return false;
1644 }
1645 
1646 // Intersection between half-space and triangle
1647 // Half-space is in pose tf1,
1648 // Triangle is in pose tf2
1649 // Computes distance and closest points (p1, p2) if no collision,
1650 // contact point (p1 = p2), normal and penetration depth (-distance)
1651 // if collision
1652 inline bool halfspaceTriangleIntersect(const Halfspace& s1,
1653  const Transform3f& tf1, const Vec3f& P1,
1654  const Vec3f& P2, const Vec3f& P3,
1655  const Transform3f& tf2,
1656  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1657  Vec3f& normal) {
1658  Halfspace new_s1 = transform(s1, tf1);
1659 
1660  Vec3f v = tf2.transform(P1);
1661  FCL_REAL depth = new_s1.signedDistance(v);
1662 
1663  Vec3f p = tf2.transform(P2);
1664  FCL_REAL d = new_s1.signedDistance(p);
1665  if (d < depth) {
1666  depth = d;
1667  v = p;
1668  }
1669 
1670  p = tf2.transform(P3);
1671  d = new_s1.signedDistance(p);
1672  if (d < depth) {
1673  depth = d;
1674  v = p;
1675  }
1676  // v is the vertex with minimal projection abscissa (depth) on normal to
1677  // plane,
1678  distance = depth;
1679  if (depth <= 0) {
1680  normal = new_s1.n;
1681  p1 = p2 = v - (0.5 * depth) * new_s1.n;
1682  return true;
1683  } else {
1684  p1 = v - depth * new_s1.n;
1685  p2 = v;
1686  return false;
1687  }
1688 }
1689 
1700 inline bool planeHalfspaceIntersect(const Plane& s1, const Transform3f& tf1,
1701  const Halfspace& s2, const Transform3f& tf2,
1702  Plane& pl, Vec3f& p, Vec3f& d,
1703  FCL_REAL& penetration_depth, int& ret) {
1704  Plane new_s1 = transform(s1, tf1);
1705  Halfspace new_s2 = transform(s2, tf2);
1706 
1707  ret = 0;
1708 
1709  penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
1710  Vec3f dir = (new_s1.n).cross(new_s2.n);
1711  FCL_REAL dir_norm = dir.squaredNorm();
1712  if (dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
1713  {
1714  if ((new_s1.n).dot(new_s2.n) > 0) {
1715  penetration_depth = new_s2.d - new_s1.d;
1716  if (penetration_depth < 0)
1717  return false;
1718  else {
1719  ret = 1;
1720  pl = new_s1;
1721  return true;
1722  }
1723  } else {
1724  penetration_depth = -(new_s1.d + new_s2.d);
1725  if (penetration_depth < 0)
1726  return false;
1727  else {
1728  ret = 2;
1729  pl = new_s1;
1730  return true;
1731  }
1732  }
1733  }
1734 
1735  Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
1736  Vec3f origin = n.cross(dir);
1737  origin *= (1.0 / dir_norm);
1738 
1739  p = origin;
1740  d = dir;
1741  ret = 3;
1742 
1743  return true;
1744 }
1745 
1756 inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1,
1757  const Halfspace& s2, const Transform3f& tf2,
1758  Vec3f& p, Vec3f& d, Halfspace& s,
1759  FCL_REAL& penetration_depth, int& ret) {
1760  Halfspace new_s1 = transform(s1, tf1);
1761  Halfspace new_s2 = transform(s2, tf2);
1762 
1763  ret = 0;
1764 
1765  penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
1766  Vec3f dir = (new_s1.n).cross(new_s2.n);
1767  FCL_REAL dir_norm = dir.squaredNorm();
1768  if (dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
1769  {
1770  if ((new_s1.n).dot(new_s2.n) > 0) {
1771  if (new_s1.d < new_s2.d) // s1 is inside s2
1772  {
1773  ret = 1;
1774  s = new_s1;
1775  } else // s2 is inside s1
1776  {
1777  ret = 2;
1778  s = new_s2;
1779  }
1780  return true;
1781  } else {
1782  penetration_depth = -(new_s1.d + new_s2.d);
1783  if (penetration_depth < 0) // not collision
1784  return false;
1785  else // in each other
1786  {
1787  ret = 3;
1788  return true;
1789  }
1790  }
1791  }
1792 
1793  Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
1794  Vec3f origin = n.cross(dir);
1795  origin *= (1.0 / dir_norm);
1796 
1797  p = origin;
1798  d = dir;
1799  ret = 4;
1800 
1801  return true;
1802 }
1803 
1808 inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
1809  const ShapeBase& s, const Transform3f& tf2,
1810  FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
1811  Vec3f& normal) {
1812  Vec3f n_w = tf1.getRotation() * h.n;
1813  Vec3f n_2(tf2.getRotation().transpose() * n_w);
1814  int hint = 0;
1815  p2 = getSupport(&s, -n_2, true, hint);
1816  p2 = tf2.transform(p2);
1817 
1818  dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d;
1819  p1 = p2 - dist * n_w;
1820  normal = n_w;
1821 
1822  return dist <= 0;
1823 }
1824 
1825 template <typename T>
1826 inline T planeIntersectTolerance() {
1827  return 0;
1828 }
1829 
1830 template <>
1831 inline double planeIntersectTolerance<double>() {
1832  return 0.0000001;
1833 }
1834 
1835 template <>
1836 float inline planeIntersectTolerance<float>() {
1837  return 0.0001f;
1838 }
1839 
1840 inline bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1,
1841  const Plane& s2, const Transform3f& tf2,
1842  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1843  Vec3f& normal)
1844 // Vec3f& contact_points, FCL_REAL& penetration_depth, Vec3f* normal)
1845 {
1846  Plane new_s2 = transform(s2, tf2);
1847 
1848  const Vec3f& center = tf1.getTranslation();
1849  FCL_REAL signed_dist = new_s2.signedDistance(center);
1850  distance = std::abs(signed_dist) - s1.radius;
1851  if (distance <= 0) {
1852  if (signed_dist > 0)
1853  normal = -new_s2.n;
1854  else
1855  normal = new_s2.n;
1856  p1 = p2 = center - new_s2.n * signed_dist;
1857  return true;
1858  } else {
1859  if (signed_dist > 0) {
1860  p1 = center - s1.radius * new_s2.n;
1861  p2 = center - signed_dist * new_s2.n;
1862  } else {
1863  p1 = center + s1.radius * new_s2.n;
1864  p2 = center + signed_dist * new_s2.n;
1865  }
1866  return false;
1867  }
1868 }
1869 
1879 inline bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
1880  const Plane& s2, const Transform3f& tf2,
1881  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1882  Vec3f& normal)
1883 // Vec3f* contact_points,
1884 // FCL_REAL* penetration_depth, Vec3f* normal)
1885 {
1886  static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
1887  const Plane new_s2 = transform(s2, tf2);
1888 
1889  const Matrix3f& R = tf1.getRotation();
1890  const Vec3f& T = tf1.getTranslation();
1891 
1892  const Vec3f Q(R.transpose() * new_s2.n);
1893  const Vec3f A(Q.cwiseProduct(s1.halfSide));
1894 
1895  const FCL_REAL signed_dist = new_s2.signedDistance(T);
1896  distance = std::abs(signed_dist) - A.lpNorm<1>();
1897  if (distance > 0) {
1898  // Is the box above or below the plane
1899  const bool positive = signed_dist > 0;
1900  // Set p1 at the center of the box
1901  p1 = T;
1902  for (Vec3f::Index i = 0; i < 3; ++i) {
1903  // scalar product between box axis and plane normal
1904  FCL_REAL alpha((positive ? 1 : -1) * R.col(i).dot(new_s2.n));
1905  if (alpha > eps) {
1906  p1 -= R.col(i) * s1.halfSide[i];
1907  } else if (alpha < -eps) {
1908  p1 += R.col(i) * s1.halfSide[i];
1909  }
1910  }
1911  p2 = p1 - (positive ? distance : -distance) * new_s2.n;
1912  assert(new_s2.distance(p2) < 3 * eps);
1913  return false;
1914  }
1915 
1916  // find the deepest point
1917  Vec3f p = T;
1918 
1919  // when center is on the positive side of the plane, use a, b, c
1920  // make (R^T n) (a v1 + b v2 + c v3) the minimum
1921  // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum
1922  int sign = (signed_dist > 0) ? 1 : -1;
1923 
1924  if (std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() ||
1925  std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>()) {
1926  int sign2 = (A[0] > 0) ? -sign : sign;
1927  p.noalias() += R.col(0) * (s1.halfSide[0] * sign2);
1928  } else if (std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
1929  std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>()) {
1930  int sign2 = (A[1] > 0) ? -sign : sign;
1931  p.noalias() += R.col(1) * (s1.halfSide[1] * sign2);
1932  } else if (std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
1933  std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>()) {
1934  int sign2 = (A[2] > 0) ? -sign : sign;
1935  p.noalias() += R.col(2) * (s1.halfSide[2] * sign2);
1936  } else {
1937  Vec3f tmp(sign * R * s1.halfSide);
1938  p.noalias() += (A.array() > 0).select(-tmp, tmp);
1939  }
1940 
1941  // compute the contact point by project the deepest point onto the plane
1942  if (signed_dist > 0)
1943  normal = -new_s2.n;
1944  else
1945  normal = new_s2.n;
1946  p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p);
1947 
1948  return true;
1949 }
1950 
1957 inline bool boxSphereDistance(const Box& b, const Transform3f& tfb,
1958  const Sphere& s, const Transform3f& tfs,
1959  FCL_REAL& dist, Vec3f& pb, Vec3f& ps,
1960  Vec3f& normal) {
1961  const Vec3f& os = tfs.getTranslation();
1962  const Vec3f& ob = tfb.getTranslation();
1963  const Matrix3f& Rb = tfb.getRotation();
1964 
1965  pb = ob;
1966 
1967  bool outside = false;
1968  const Vec3f os_in_b_frame(Rb.transpose() * (os - ob));
1969  int axis = -1;
1970  FCL_REAL min_d = (std::numeric_limits<FCL_REAL>::max)();
1971  for (int i = 0; i < 3; ++i) {
1972  FCL_REAL facedist;
1973  if (os_in_b_frame(i) < -b.halfSide(i)) { // outside
1974  pb.noalias() -= b.halfSide(i) * Rb.col(i);
1975  outside = true;
1976  } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside
1977  pb.noalias() += b.halfSide(i) * Rb.col(i);
1978  outside = true;
1979  } else {
1980  pb.noalias() += os_in_b_frame(i) * Rb.col(i);
1981  if (!outside &&
1982  (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) {
1983  axis = i;
1984  min_d = facedist;
1985  }
1986  }
1987  }
1988  normal = pb - os;
1989  FCL_REAL pdist = normal.norm();
1990  if (outside) { // pb is on the box
1991  dist = pdist - s.radius;
1992  normal /= -pdist;
1993  } else { // pb is inside the box
1994  if (os_in_b_frame(axis) >= 0)
1995  normal = Rb.col(axis);
1996  else
1997  normal = -Rb.col(axis);
1998  dist = -min_d - s.radius;
1999  }
2000  if (!outside || dist <= 0) {
2001  ps = pb;
2002  return true;
2003  } else {
2004  ps = os - s.radius * normal;
2005  return false;
2006  }
2007 }
2008 
2009 inline bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
2010  const Plane& s2, const Transform3f& tf2,
2011  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2012  Vec3f& normal) {
2013  Plane new_s2 = transform(s2, tf2);
2014 
2015  // position orientation of capsule
2016  const Matrix3f& R1 = tf1.getRotation();
2017  const Vec3f& T1 = tf1.getTranslation();
2018 
2019  Vec3f dir_z = R1.col(2);
2020 
2021  // ends of capsule inner segment
2022  Vec3f a1 = T1 + dir_z * s1.halfLength;
2023  Vec3f a2 = T1 - dir_z * s1.halfLength;
2024 
2025  FCL_REAL d1 = new_s2.signedDistance(a1);
2026  FCL_REAL d2 = new_s2.signedDistance(a2);
2027 
2028  FCL_REAL abs_d1 = std::abs(d1);
2029  FCL_REAL abs_d2 = std::abs(d2);
2030 
2031  // two end points on different side of the plane
2032  // the contact point is the intersect of axis with the plane
2033  // the normal is the direction to avoid intersection
2034  // the depth is the minimum distance to resolve the collision
2035  if (d1 * d2 < -planeIntersectTolerance<FCL_REAL>()) {
2036  if (abs_d1 < abs_d2) {
2037  distance = -abs_d1 - s1.radius;
2038  p1 = p2 =
2039  a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
2040  if (d1 < 0)
2041  normal = -new_s2.n;
2042  else
2043  normal = new_s2.n;
2044  } else {
2045  distance = -abs_d2 - s1.radius;
2046  p1 = p2 =
2047  a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
2048  if (d2 < 0)
2049  normal = -new_s2.n;
2050  else
2051  normal = new_s2.n;
2052  }
2053  assert(!p1.hasNaN() && !p2.hasNaN());
2054  return true;
2055  }
2056 
2057  if (abs_d1 > s1.radius && abs_d2 > s1.radius) {
2058  // Here both capsule ends are on the same side of the plane
2059  if (d1 > 0)
2060  normal = new_s2.n;
2061  else
2062  normal = -new_s2.n;
2063  if (abs_d1 < abs_d2) {
2064  distance = abs_d1 - s1.radius;
2065  p1 = a1 - s1.radius * normal;
2066  p2 = p1 - distance * normal;
2067  } else {
2068  distance = abs_d2 - s1.radius;
2069  p1 = a2 - s1.radius * normal;
2070  p2 = p1 - distance * normal;
2071  }
2072  assert(!p1.hasNaN() && !p2.hasNaN());
2073  return false;
2074  } else {
2075  // Both capsule ends are on the same side of the plane, but one
2076  // is closer than the capsule radius, hence collision
2077  distance = std::min(abs_d1, abs_d2) - s1.radius;
2078 
2079  if (abs_d1 <= s1.radius && abs_d2 <= s1.radius) {
2080  Vec3f c1 = a1 - new_s2.n * d1;
2081  Vec3f c2 = a2 - new_s2.n * d2;
2082  p1 = p2 = (c1 + c2) * 0.5;
2083  } else if (abs_d1 <= s1.radius) {
2084  Vec3f c = a1 - new_s2.n * d1;
2085  p1 = p2 = c;
2086  } else if (abs_d2 <= s1.radius) {
2087  Vec3f c = a2 - new_s2.n * d2;
2088  p1 = p2 = c;
2089  } else {
2090  assert(false);
2091  }
2092 
2093  if (d1 < 0)
2094  normal = new_s2.n;
2095  else
2096  normal = -new_s2.n;
2097  assert(!p1.hasNaN() && !p2.hasNaN());
2098  return true;
2099  }
2100  assert(false);
2101 }
2102 
2109 inline bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
2110  const Plane& s2, const Transform3f& tf2,
2111  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2112  Vec3f& normal) {
2113  Plane new_s2 = transform(s2, tf2);
2114 
2115  const Matrix3f& R = tf1.getRotation();
2116  const Vec3f& T = tf1.getTranslation();
2117 
2118  Vec3f dir_z = R.col(2);
2119  FCL_REAL cosa = dir_z.dot(new_s2.n);
2120 
2121  if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) {
2122  FCL_REAL d = new_s2.signedDistance(T);
2123  distance = std::abs(d) - s1.radius;
2124  if (distance > 0)
2125  return false;
2126  else {
2127  if (d < 0)
2128  normal = new_s2.n;
2129  else
2130  normal = -new_s2.n;
2131  p1 = p2 = T - new_s2.n * d;
2132  return true;
2133  }
2134  } else {
2135  Vec3f C = dir_z * cosa - new_s2.n;
2136  if (std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() ||
2137  std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
2138  C = Vec3f(0, 0, 0);
2139  else {
2140  FCL_REAL s = C.norm();
2141  s = s1.radius / s;
2142  C *= s;
2143  }
2144 
2145  Vec3f a1 = T + dir_z * (s1.halfLength);
2146  Vec3f a2 = T - dir_z * (s1.halfLength);
2147 
2148  Vec3f c1, c2;
2149  if (cosa > 0) {
2150  c1 = a1 - C;
2151  c2 = a2 + C;
2152  } else {
2153  c1 = a1 + C;
2154  c2 = a2 - C;
2155  }
2156 
2157  FCL_REAL d1 = new_s2.signedDistance(c1);
2158  FCL_REAL d2 = new_s2.signedDistance(c2);
2159 
2160  if (d1 * d2 <= 0) {
2161  FCL_REAL abs_d1 = std::abs(d1);
2162  FCL_REAL abs_d2 = std::abs(d2);
2163 
2164  if (abs_d1 > abs_d2) {
2165  distance = -abs_d2;
2166  p1 = p2 = c2 - new_s2.n * d2;
2167  if (d2 < 0)
2168  normal = -new_s2.n;
2169  else
2170  normal = new_s2.n;
2171  } else {
2172  distance = -abs_d1;
2173  p1 = p2 = c1 - new_s2.n * d1;
2174  if (d1 < 0)
2175  normal = -new_s2.n;
2176  else
2177  normal = new_s2.n;
2178  }
2179  return true;
2180  } else
2181  return false;
2182  }
2183 }
2184 
2185 inline bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
2186  const Plane& s2, const Transform3f& tf2,
2187  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2188  Vec3f& normal) {
2189  Plane new_s2 = transform(s2, tf2);
2190 
2191  const Matrix3f& R = tf1.getRotation();
2192  const Vec3f& T = tf1.getTranslation();
2193 
2194  Vec3f dir_z = R.col(2);
2195  FCL_REAL cosa = dir_z.dot(new_s2.n);
2196 
2197  if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) {
2198  FCL_REAL d = new_s2.signedDistance(T);
2199  distance = std::abs(d) - s1.radius;
2200  if (distance > 0) {
2201  p1 = p2 = Vec3f(0, 0, 0);
2202  return false;
2203  } else {
2204  if (d < 0)
2205  normal = new_s2.n;
2206  else
2207  normal = -new_s2.n;
2208  p1 = p2 = T - dir_z * (s1.halfLength) +
2209  dir_z * (-distance / s1.radius * s1.halfLength) - new_s2.n * d;
2210  return true;
2211  }
2212  } else {
2213  Vec3f C = dir_z * cosa - new_s2.n;
2214  if (std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() ||
2215  std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
2216  C = Vec3f(0, 0, 0);
2217  else {
2218  FCL_REAL s = C.norm();
2219  s = s1.radius / s;
2220  C *= s;
2221  }
2222 
2223  Vec3f c[3];
2224  c[0] = T + dir_z * (s1.halfLength);
2225  c[1] = T - dir_z * (s1.halfLength) + C;
2226  c[2] = T - dir_z * (s1.halfLength) - C;
2227 
2228  FCL_REAL d[3];
2229  d[0] = new_s2.signedDistance(c[0]);
2230  d[1] = new_s2.signedDistance(c[1]);
2231  d[2] = new_s2.signedDistance(c[2]);
2232 
2233  if ((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) ||
2234  (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
2235  return false;
2236  else {
2237  bool positive[3];
2238  for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] >= 0);
2239 
2240  int n_positive = 0;
2241  FCL_REAL d_positive = 0, d_negative = 0;
2242  for (std::size_t i = 0; i < 3; ++i) {
2243  if (positive[i]) {
2244  n_positive++;
2245  if (d_positive <= d[i]) d_positive = d[i];
2246  } else {
2247  if (d_negative <= -d[i]) d_negative = -d[i];
2248  }
2249  }
2250 
2251  distance = -std::min(d_positive, d_negative);
2252  if (d_positive > d_negative)
2253  normal = -new_s2.n;
2254  else
2255  normal = new_s2.n;
2256  Vec3f p[2];
2257  Vec3f q;
2258 
2259  FCL_REAL p_d[2];
2260  FCL_REAL q_d(0);
2261 
2262  if (n_positive == 2) {
2263  for (std::size_t i = 0, j = 0; i < 3; ++i) {
2264  if (positive[i]) {
2265  p[j] = c[i];
2266  p_d[j] = d[i];
2267  j++;
2268  } else {
2269  q = c[i];
2270  q_d = d[i];
2271  }
2272  }
2273 
2274  Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
2275  Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
2276  p1 = p2 = (t1 + t2) * 0.5;
2277  } else {
2278  for (std::size_t i = 0, j = 0; i < 3; ++i) {
2279  if (!positive[i]) {
2280  p[j] = c[i];
2281  p_d[j] = d[i];
2282  j++;
2283  } else {
2284  q = c[i];
2285  q_d = d[i];
2286  }
2287  }
2288 
2289  Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
2290  Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
2291  p1 = p2 = (t1 + t2) * 0.5;
2292  }
2293  }
2294  return true;
2295  }
2296 }
2297 
2298 inline bool convexPlaneIntersect(const ConvexBase& s1, const Transform3f& tf1,
2299  const Plane& s2, const Transform3f& tf2,
2300  Vec3f* contact_points,
2301  FCL_REAL* penetration_depth, Vec3f* normal) {
2302  Plane new_s2 = transform(s2, tf2);
2303 
2304  Vec3f v_min, v_max;
2305  FCL_REAL d_min = (std::numeric_limits<FCL_REAL>::max)(),
2306  d_max = -(std::numeric_limits<FCL_REAL>::max)();
2307 
2308  for (unsigned int i = 0; i < s1.num_points; ++i) {
2309  Vec3f p = tf1.transform(s1.points[i]);
2310 
2311  FCL_REAL d = new_s2.signedDistance(p);
2312 
2313  if (d < d_min) {
2314  d_min = d;
2315  v_min = p;
2316  }
2317  if (d > d_max) {
2318  d_max = d;
2319  v_max = p;
2320  }
2321  }
2322 
2323  if (d_min * d_max > 0)
2324  return false;
2325  else {
2326  if (d_min + d_max > 0) {
2327  if (penetration_depth) *penetration_depth = -d_min;
2328  if (normal) *normal = -new_s2.n;
2329  if (contact_points) *contact_points = v_min - new_s2.n * d_min;
2330  } else {
2331  if (penetration_depth) *penetration_depth = d_max;
2332  if (normal) *normal = new_s2.n;
2333  if (contact_points) *contact_points = v_max - new_s2.n * d_max;
2334  }
2335  return true;
2336  }
2337 }
2338 
2339 inline bool planeTriangleIntersect(const Plane& s1, const Transform3f& tf1,
2340  const Vec3f& P1, const Vec3f& P2,
2341  const Vec3f& P3, const Transform3f& tf2,
2342  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2343  Vec3f& normal) {
2344  Plane new_s1 = transform(s1, tf1);
2345 
2346  Vec3f c[3];
2347  c[0] = tf2.transform(P1);
2348  c[1] = tf2.transform(P2);
2349  c[2] = tf2.transform(P3);
2350 
2351  FCL_REAL d[3];
2352  d[0] = new_s1.signedDistance(c[0]);
2353  d[1] = new_s1.signedDistance(c[1]);
2354  d[2] = new_s1.signedDistance(c[2]);
2355  int imin;
2356  if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0) {
2357  if (d[0] < d[1])
2358  if (d[0] < d[2]) {
2359  imin = 0;
2360  } else { // d [2] <= d[0] < d [1]
2361  imin = 2;
2362  }
2363  else { // d[1] <= d[0]
2364  if (d[2] < d[1]) {
2365  imin = 2;
2366  } else { // d[1] <= d[2]
2367  imin = 1;
2368  }
2369  }
2370  distance = d[imin];
2371  p2 = c[imin];
2372  p1 = c[imin] - d[imin] * new_s1.n;
2373  return false;
2374  }
2375  if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0) {
2376  if (d[0] > d[1])
2377  if (d[0] > d[2]) {
2378  imin = 0;
2379  } else { // d [2] >= d[0] > d [1]
2380  imin = 2;
2381  }
2382  else { // d[1] >= d[0]
2383  if (d[2] > d[1]) {
2384  imin = 2;
2385  } else { // d[1] >= d[2]
2386  imin = 1;
2387  }
2388  }
2389  distance = -d[imin];
2390  p2 = c[imin];
2391  p1 = c[imin] - d[imin] * new_s1.n;
2392  return false;
2393  }
2394  bool positive[3];
2395  for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] > 0);
2396 
2397  int n_positive = 0;
2398  FCL_REAL d_positive = 0, d_negative = 0;
2399  for (std::size_t i = 0; i < 3; ++i) {
2400  if (positive[i]) {
2401  n_positive++;
2402  if (d_positive <= d[i]) d_positive = d[i];
2403  } else {
2404  if (d_negative <= -d[i]) d_negative = -d[i];
2405  }
2406  }
2407 
2408  distance = -std::min(d_positive, d_negative);
2409  if (d_positive > d_negative) {
2410  normal = new_s1.n;
2411  } else {
2412  normal = -new_s1.n;
2413  }
2414  Vec3f p[2];
2415  Vec3f q;
2416 
2417  FCL_REAL p_d[2];
2418  FCL_REAL q_d(0);
2419 
2420  if (n_positive == 2) {
2421  for (std::size_t i = 0, j = 0; i < 3; ++i) {
2422  if (positive[i]) {
2423  p[j] = c[i];
2424  p_d[j] = d[i];
2425  j++;
2426  } else {
2427  q = c[i];
2428  q_d = d[i];
2429  }
2430  }
2431 
2432  Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
2433  Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
2434  p1 = p2 = (t1 + t2) * 0.5;
2435  } else {
2436  for (std::size_t i = 0, j = 0; i < 3; ++i) {
2437  if (!positive[i]) {
2438  p[j] = c[i];
2439  p_d[j] = d[i];
2440  j++;
2441  } else {
2442  q = c[i];
2443  q_d = d[i];
2444  }
2445  }
2446 
2447  Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
2448  Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
2449  p1 = p2 = (t1 + t2) * 0.5;
2450  }
2451  return true;
2452 }
2453 
2454 inline bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3f& tf1,
2455  const Plane& s2, const Transform3f& tf2,
2456  Plane& pl, Vec3f& p, Vec3f& d,
2457  FCL_REAL& penetration_depth, int& ret) {
2458  return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth,
2459  ret);
2460 }
2461 
2462 inline bool planeIntersect(const Plane& s1, const Transform3f& tf1,
2463  const Plane& s2, const Transform3f& tf2,
2464  Vec3f* /*contact_points*/,
2465  FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) {
2466  Plane new_s1 = transform(s1, tf1);
2467  Plane new_s2 = transform(s2, tf2);
2468 
2469  FCL_REAL a = (new_s1.n).dot(new_s2.n);
2470  if (a == 1 && new_s1.d != new_s2.d) return false;
2471  if (a == -1 && new_s1.d != -new_s2.d) return false;
2472 
2473  return true;
2474 }
2475 
2477 inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
2478  const Vec3f& P3, const Vec3f& Q1,
2479  const Vec3f& Q2, const Vec3f& Q3,
2480  Vec3f& normal) {
2481  Vec3f u((P2 - P1).cross(P3 - P1));
2482  normal = u.normalized();
2483  FCL_REAL depth1((P1 - Q1).dot(normal));
2484  FCL_REAL depth2((P1 - Q2).dot(normal));
2485  FCL_REAL depth3((P1 - Q3).dot(normal));
2486  return std::max(depth1, std::max(depth2, depth3));
2487 }
2488 
2489 // Compute penetration distance and normal of two triangles in collision
2490 // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
2491 // minimal distance (Q1, Q2, Q3) should be translated along the normal so
2492 // that the triangles are collision free.
2493 //
2494 // Note that we compute here an upper bound of the penetration distance,
2495 // not the exact value.
2496 inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
2497  const Vec3f& P3, const Vec3f& Q1,
2498  const Vec3f& Q2, const Vec3f& Q3,
2499  const Transform3f& tf1,
2500  const Transform3f& tf2, Vec3f& normal) {
2501  Vec3f globalP1(tf1.transform(P1));
2502  Vec3f globalP2(tf1.transform(P2));
2503  Vec3f globalP3(tf1.transform(P3));
2504  Vec3f globalQ1(tf2.transform(Q1));
2505  Vec3f globalQ2(tf2.transform(Q2));
2506  Vec3f globalQ3(tf2.transform(Q3));
2507  return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2,
2508  globalQ3, normal);
2509 }
2510 } // namespace details
2511 } // namespace fcl
2512 } // namespace hpp
2513 
2514 #endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H
hpp::fcl::details::planeIntersect
bool planeIntersect(const Plane &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Vec3f *, FCL_REAL *, Vec3f *)
Definition: details.h:2462
hpp::fcl::details::capsulePlaneIntersect
bool capsulePlaneIntersect(const Capsule &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2009
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: shape/geometric_shapes.h:148
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
traversal_node_setup.h
axis
axis
hpp::fcl::details::sphereCapsuleDistance
bool sphereCapsuleDistance(const Sphere &s1, const Transform3f &tf1, const Capsule &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:101
gjk.P1
tuple P1
Definition: test/scripts/gjk.py:22
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:8
hpp::fcl::Halfspace::n
Vec3f n
Plane normal.
Definition: shape/geometric_shapes.h:787
q
q
octree.p1
tuple p1
Definition: octree.py:55
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:433
hpp::fcl::details::halfspaceTriangleIntersect
bool halfspaceTriangleIntersect(const Halfspace &s1, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1652
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::Halfspace::signedDistance
FCL_REAL signedDistance(const Vec3f &p) const
Definition: shape/geometric_shapes.h:757
t
t
hpp::fcl::details::convexPlaneIntersect
bool convexPlaneIntersect(const ConvexBase &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Vec3f *contact_points, FCL_REAL *penetration_depth, Vec3f *normal)
Definition: details.h:2298
hpp::fcl::details::conePlaneIntersect
bool conePlaneIntersect(const Cone &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2185
epsilon
static FCL_REAL epsilon
Definition: simple.cpp:12
hpp::fcl::details::halfspacePlaneIntersect
bool halfspacePlaneIntersect(const Halfspace &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Plane &pl, Vec3f &p, Vec3f &d, FCL_REAL &penetration_depth, int &ret)
Definition: details.h:2454
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:37
gjk.Q2
tuple Q2
Definition: test/scripts/gjk.py:26
hpp::fcl::details::cylinderHalfspaceIntersect
bool cylinderHalfspaceIntersect(const Cylinder &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1513
hpp::fcl::details::sphereTriangleDistance
bool sphereTriangleDistance(const Sphere &sp, const Transform3f &tf, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, FCL_REAL *dist)
Definition: details.h:381
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::details::boxPlaneIntersect
bool boxPlaneIntersect(const Box &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + ...
Definition: details.h:1879
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: shape/geometric_shapes.h:729
hpp::fcl::details::getSupport
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
Definition: src/narrowphase/gjk.cpp:260
hpp::fcl::details::sphereCapsuleIntersect
bool sphereCapsuleIntersect(const Sphere &s1, const Transform3f &tf1, const Capsule &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f *contact_points, Vec3f *normal_)
Definition: details.h:71
eps
const FCL_REAL eps
Definition: obb.cpp:93
hpp::fcl::details::boxHalfspaceIntersect
bool boxHalfspaceIntersect(const Box &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2)
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) <= d so (R^T n) (a v1 +...
Definition: details.h:1404
gjk.P3
tuple P3
Definition: test/scripts/gjk.py:24
hpp::fcl::details::cylinderPlaneIntersect
bool cylinderPlaneIntersect(const Cylinder &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
cylinder-plane intersect n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d need one point to b...
Definition: details.h:2109
c
c
hpp::fcl::details::compareContactPoints
bool compareContactPoints(const ContactPoint &c1, const ContactPoint &c2)
Definition: details.h:1334
hpp::fcl::details::sphereHalfspaceIntersect
bool sphereHalfspaceIntersect(const Sphere &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1380
P
P
hpp::fcl::details::halfspaceIntersectTolerance
T halfspaceIntersectTolerance()
Definition: details.h:1366
geometric_shapes.d1
float d1
Definition: scripts/geometric_shapes.py:31
hpp::fcl::details::sphereCylinderDistance
bool sphereCylinderDistance(const Sphere &s1, const Transform3f &tf1, const Cylinder &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:132
obb.v
list v
Definition: obb.py:45
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape/geometric_shapes.h:51
hpp::fcl::details::intersectRectQuad2
static unsigned int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
Definition: details.h:633
hpp::fcl::ConvexBase::num_points
unsigned int num_points
Definition: shape/geometric_shapes.h:624
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
hpp::fcl::details::planeIntersectTolerance< double >
double planeIntersectTolerance< double >()
Definition: details.h:1831
hpp::fcl::details::coneHalfspaceIntersect
bool coneHalfspaceIntersect(const Cone &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1566
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: shape/geometric_shapes.h:427
hpp::fcl::details::lineClosestApproach
static void lineClosestApproach(const Vec3f &pa, const Vec3f &ua, const Vec3f &pb, const Vec3f &ub, FCL_REAL *alpha, FCL_REAL *beta)
Definition: details.h:608
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::details::planeIntersectTolerance
T planeIntersectTolerance()
Definition: details.h:1826
hpp::fcl::Plane::distance
FCL_REAL distance(const Vec3f &p) const
Definition: shape/geometric_shapes.h:839
hpp::fcl::details::cullPoints2
static void cullPoints2(unsigned int n, FCL_REAL p[], unsigned int m, unsigned int i0, unsigned int iret[])
Definition: details.h:695
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::details::lineSegmentPointClosestToPoint
static void lineSegmentPointClosestToPoint(const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp)
Definition: details.h:51
hpp::fcl::details::halfspaceIntersect
bool halfspaceIntersect(const Halfspace &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Vec3f &p, Vec3f &d, Halfspace &s, FCL_REAL &penetration_depth, int &ret)
Definition: details.h:1756
hpp::fcl::details::capsuleHalfspaceIntersect
bool capsuleHalfspaceIntersect(const Capsule &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1466
hpp::fcl::details::sphereSphereDistance
bool sphereSphereDistance(const Sphere &s1, const Transform3f &tf1, const Sphere &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:258
gjk.Q3
tuple Q3
Definition: test/scripts/gjk.py:27
hpp::fcl::details::sphereSphereIntersect
bool sphereSphereIntersect(const Sphere &s1, const Transform3f &tf1, const Sphere &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f *contact_points, Vec3f *normal)
Definition: details.h:232
hpp::fcl::Transform3f::getTranslation
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
R
R
hpp::fcl::Halfspace::d
FCL_REAL d
Plane offset.
Definition: shape/geometric_shapes.h:790
hpp::fcl::Plane::d
FCL_REAL d
Plane offset.
Definition: shape/geometric_shapes.h:851
hpp::fcl::Sphere::radius
FCL_REAL radius
Radius of the sphere.
Definition: shape/geometric_shapes.h:206
hpp::fcl::details::ContactPoint::ContactPoint
ContactPoint(const Vec3f &n, const Vec3f &p, FCL_REAL d)
Definition: details.h:604
hpp::fcl::details::projectInTriangle
bool projectInTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &normal, const Vec3f &p)
Whether a point&#39;s projection is in a triangle.
Definition: details.h:299
generate_distance_plot.m
float m
Definition: generate_distance_plot.py:7
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: shape/geometric_shapes.h:522
hpp::fcl::Plane::n
Vec3f n
Plane normal.
Definition: shape/geometric_shapes.h:848
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB...
Definition: shape/geometric_shapes.h:333
gjk.Q1
tuple Q1
Definition: test/scripts/gjk.py:25
hpp::fcl::details::halfspaceDistance
bool halfspaceDistance(const Halfspace &h, const Transform3f &tf1, const ShapeBase &s, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1808
hpp::fcl::detail::select
size_t select(const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2 ...
Definition: hierarchy_tree-inl.h:952
q2
q2
hpp::fcl::details::spherePlaneIntersect
bool spherePlaneIntersect(const Sphere &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1840
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: shape/geometric_shapes.h:581
B
B
r2
r2
res
res
gjk.P2
tuple P2
Definition: test/scripts/gjk.py:23
hpp::fcl::details::planeHalfspaceIntersect
bool planeHalfspaceIntersect(const Plane &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Plane &pl, Vec3f &p, Vec3f &d, FCL_REAL &penetration_depth, int &ret)
return whether plane collides with halfspace if the separation plane of the halfspace is parallel wit...
Definition: details.h:1700
hpp::fcl::details::planeIntersectTolerance< float >
float planeIntersectTolerance< float >()
Definition: details.h:1836
hpp::fcl::details::convexHalfspaceIntersect
bool convexHalfspaceIntersect(const ConvexBase &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Vec3f *contact_points, FCL_REAL *penetration_depth, Vec3f *normal)
Definition: details.h:1618
hpp::fcl::Plane::signedDistance
FCL_REAL signedDistance(const Vec3f &p) const
Definition: shape/geometric_shapes.h:837
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:528
c2
c2
a
list a
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::details::sphereTriangleIntersect
bool sphereTriangleIntersect(const Sphere &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal_)
Definition: details.h:324
A
A
hpp::fcl::details::planeTriangleIntersect
bool planeTriangleIntersect(const Plane &s1, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2339
hpp::fcl::details::boxSphereDistance
bool boxSphereDistance(const Box &b, const Transform3f &tfb, const Sphere &s, const Transform3f &tfs, FCL_REAL &dist, Vec3f &pb, Vec3f &ps, Vec3f &normal)
Definition: details.h:1957
hpp::fcl::details::boxBox2
unsigned int boxBox2(const Vec3f &halfSide1, const Matrix3f &R1, const Vec3f &T1, const Vec3f &halfSide2, const Matrix3f &R2, const Vec3f &T2, Vec3f &normal, FCL_REAL *depth, int *return_code, unsigned int maxc, std::vector< ContactPoint > &contacts)
Definition: details.h:762
hpp::fcl::transform
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
Definition: geometric_shapes_utility.cpp:248
hpp::fcl::details::segmentSqrDistance
FCL_REAL segmentSqrDistance(const Vec3f &from, const Vec3f &to, const Vec3f &p, Vec3f &nearest)
the minimum distance from a point to a line
Definition: details.h:276
hpp::fcl::details::computePenetration
FCL_REAL computePenetration(const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Vec3f &Q1, const Vec3f &Q2, const Vec3f &Q3, Vec3f &normal)
See the prototype below.
Definition: details.h:2477
hpp::fcl::details::boxBoxIntersect
bool boxBoxIntersect(const Box &s1, const Transform3f &tf1, const Box &s2, const Transform3f &tf2, Vec3f *contact_points, FCL_REAL *penetration_depth_, Vec3f *normal_)
Definition: details.h:1339
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
hpp::fcl::Capsule::radius
FCL_REAL radius
Radius of capsule.
Definition: shape/geometric_shapes.h:346
hpp::fcl::Capsule::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:352
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28
hpp::fcl::ConvexBase::points
Vec3f * points
An array of the points of the polygon.
Definition: shape/geometric_shapes.h:623

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00