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