hpp-fcl  1.7.8
HPP fork of FCL -- The Flexible Collision Library
narrowphase.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  * Copyright (c) 2021, INRIA
9  * All rights reserved.
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Open Source Robotics Foundation nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  */
37 
40 #ifndef HPP_FCL_NARROWPHASE_H
41 #define HPP_FCL_NARROWPHASE_H
42 
43 #include <limits>
44 
46 
47 namespace hpp
48 {
49 namespace fcl
50 {
51 
54  {
56  template<typename S1, typename S2>
57  bool shapeIntersect(const S1& s1, const Transform3f& tf1,
58  const S2& s2, const Transform3f& tf2,
59  FCL_REAL& distance_lower_bound,
60  bool enable_penetration,
61  Vec3f* contact_points, Vec3f* normal) const
62  {
63  Vec3f guess(1, 0, 0);
64  support_func_guess_t support_hint;
65  if(enable_cached_guess) {
66  guess = cached_guess;
67  support_hint = support_func_cached_guess;
68  } else
69  support_hint.setZero();
70 
72  shape.set (&s1, &s2, tf1, tf2);
73 
74  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
75 
76  gjk.setDistanceEarlyBreak(distance_upper_bound);
77 
78  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
79  if(enable_cached_guess) {
80  cached_guess = gjk.getGuessFromSimplex();
81  support_func_cached_guess = gjk.support_hint;
82  }
83 
84  Vec3f w0, w1;
85  switch(gjk_status) {
87  if (!enable_penetration && contact_points == NULL && normal == NULL)
88  return true;
89  if (gjk.hasPenetrationInformation(shape)) {
90  gjk.getClosestPoints (shape, w0, w1);
91  distance_lower_bound = gjk.distance;
92  if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
93  if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
94  return true;
95  } else {
96  details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
97  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
98  if(epa_status & details::EPA::Valid
99  || epa_status == details::EPA::OutOfFaces // Warnings
100  || epa_status == details::EPA::OutOfVertices // Warnings
101  )
102  {
103  epa.getClosestPoints (shape, w0, w1);
104  distance_lower_bound = -epa.depth;
105  if(normal) (*normal).noalias() = tf1.getRotation() * epa.normal;
106  if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
107  return true;
108  }
109  distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)();
110  // EPA failed but we know there is a collision so we should
111  return true;
112  }
113  break;
114  case details::GJK::Valid:
115  distance_lower_bound = gjk.distance;
116  break;
117  default:
118  ;
119  }
120 
121  return false;
122  }
123 
126  template<typename S>
127  bool shapeTriangleInteraction
128  (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
129  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
130  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
131  {
132  bool col;
133  // Express everything in frame 1
134  const Transform3f tf_1M2 (tf1.inverseTimes(tf2));
135  TriangleP tri(
136  tf_1M2.transform (P1),
137  tf_1M2.transform (P2),
138  tf_1M2.transform (P3));
139 
140  Vec3f guess(1, 0, 0);
141  support_func_guess_t support_hint;
142  if(enable_cached_guess) {
143  guess = cached_guess;
144  support_hint = support_func_cached_guess;
145  } else
146  support_hint.setZero();
147 
149  shape.set (&s, &tri);
150 
151  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
152 
153  gjk.setDistanceEarlyBreak(distance_upper_bound);
154 
155  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
156  if(enable_cached_guess) {
157  cached_guess = gjk.getGuessFromSimplex();
158  support_func_cached_guess = gjk.support_hint;
159  }
160 
161  Vec3f w0, w1;
162  switch(gjk_status) {
164  col = true;
165  if (gjk.hasPenetrationInformation(shape)) {
166  gjk.getClosestPoints (shape, w0, w1);
167  distance = gjk.distance;
168  normal = tf1.getRotation() * (w0 - w1).normalized();
169  p1 = p2 = tf1.transform((w0 + w1) / 2);
170  } else {
171  details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
172  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
173  if(epa_status & details::EPA::Valid
174  || epa_status == details::EPA::OutOfFaces // Warnings
175  || epa_status == details::EPA::OutOfVertices // Warnings
176  )
177  {
178  epa.getClosestPoints (shape, w0, w1);
179  distance = -epa.depth;
180  normal.noalias() = tf1.getRotation() * epa.normal;
181  p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
182  assert (distance <= 1e-6);
183  } else {
184  distance = -(std::numeric_limits<FCL_REAL>::max)();
185  gjk.getClosestPoints (shape, w0, w1);
186  p1 = p2 = tf1.transform (w0);
187  }
188  }
189  break;
190  case details::GJK::Valid:
192  col = false;
193 
194  gjk.getClosestPoints (shape, p1, p2);
195  // TODO On degenerated case, the closest point may be wrong
196  // (i.e. an object face normal is colinear to gjk.ray
197  // assert (distance == (w0 - w1).norm());
198  distance = gjk.distance;
199 
200  p1 = tf1.transform (p1);
201  p2 = tf1.transform (p2);
202  assert (distance > 0);
203  break;
204  default:
205  assert (false && "should not reach type part.");
206  return true;
207  }
208  return col;
209  }
210 
212  template<typename S1, typename S2>
213  bool shapeDistance(const S1& s1, const Transform3f& tf1,
214  const S2& s2, const Transform3f& tf2,
215  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
216  Vec3f& normal) const
217  {
218 #ifndef NDEBUG
219  FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
220 #endif
221  Vec3f guess(1, 0, 0);
222  support_func_guess_t support_hint;
223  if(enable_cached_guess) {
224  guess = cached_guess;
225  support_hint = support_func_cached_guess;
226  } else
227  support_hint.setZero();
228 
230  shape.set (&s1, &s2, tf1, tf2);
231 
232  details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
233 
234  gjk.setDistanceEarlyBreak(distance_upper_bound);
235 
236  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
237  if(enable_cached_guess) {
238  cached_guess = gjk.getGuessFromSimplex();
239  support_func_cached_guess = gjk.support_hint;
240  }
241 
242  if(gjk_status == details::GJK::Failed)
243  {
244  // TODO: understand why GJK fails between cylinder and box
245  assert (distance * distance < sqrt (eps));
246  Vec3f w0, w1;
247  gjk.getClosestPoints (shape, w0, w1);
248  distance = 0;
249  p1 = tf1.transform (w0);
250  p2 = tf1.transform (w1);
251  normal.setZero();
252  return false;
253  }
254  else if(gjk_status == details::GJK::Valid)
255  {
256  gjk.getClosestPoints (shape, p1, p2);
257  // TODO On degenerated case, the closest point may be wrong
258  // (i.e. an object face normal is colinear to gjk.ray
259  // assert (distance == (w0 - w1).norm());
260  distance = gjk.distance;
261 
262  normal.noalias() = tf1.getRotation() * gjk.ray;
263  normal.normalize();
264  p1 = tf1.transform (p1);
265  p2 = tf1.transform (p2);
266  return true;
267  }
268  else
269  {
270  assert (gjk_status == details::GJK::Inside);
271  if (gjk.hasPenetrationInformation (shape)) {
272  gjk.getClosestPoints (shape, p1, p2);
273  distance = gjk.distance;
274  // Return contact points in case of collision
275  //p1 = tf1.transform (p1);
276  //p2 = tf1.transform (p2);
277  normal.noalias() = tf1.getRotation() * (p1 - p2);
278  normal.normalize();
279  p1 = tf1.transform(p1);
280  p2 = tf1.transform(p2);
281  } else {
282  details::EPA epa(epa_max_face_num, epa_max_vertex_num,
283  epa_max_iterations, epa_tolerance);
284  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
285  if(epa_status & details::EPA::Valid
286  || epa_status == details::EPA::OutOfFaces // Warnings
287  || epa_status == details::EPA::OutOfVertices // Warnings
288  )
289  {
290  Vec3f w0, w1;
291  epa.getClosestPoints (shape, w0, w1);
292  assert (epa.depth >= -eps);
293  distance = (std::min) (0., -epa.depth);
294  normal.noalias() = tf1.getRotation() * epa.normal;
295  p1 = tf1.transform(w0);
296  p2 = tf1.transform(w1);
297  return false;
298  }
299  distance = -(std::numeric_limits<FCL_REAL>::max)();
300  gjk.getClosestPoints (shape, p1, p2);
301  p1 = tf1.transform(p1);
302  p2 = tf1.transform(p2);
303  }
304  return false;
305  }
306  }
307 
310  {
311  gjk_max_iterations = 128;
312  gjk_tolerance = 1e-6;
313  epa_max_face_num = 128;
314  epa_max_vertex_num = 64;
315  epa_max_iterations = 255;
316  epa_tolerance = 1e-6;
317  enable_cached_guess = false;
318  cached_guess = Vec3f(1, 0, 0);
319  support_func_cached_guess = support_func_guess_t::Zero();
320  distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
321  }
322 
323  void enableCachedGuess(bool if_enable) const
324  {
325  enable_cached_guess = if_enable;
326  }
327 
328  void setCachedGuess(const Vec3f& guess) const
329  {
330  cached_guess = guess;
331  }
332 
334  {
335  return cached_guess;
336  }
337 
339  unsigned int epa_max_face_num;
340 
342  unsigned int epa_max_vertex_num;
343 
345  unsigned int epa_max_iterations;
346 
349 
352 
355 
357  mutable bool enable_cached_guess;
358 
361 
364 
368 
369  public:
370 
371  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
372  };
373 
374  template<>
376  (const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
377  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
378  Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
379 
380  template<>
382  (const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
383  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
384  Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
385 
386  template<>
388  (const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
389  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
390  Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
391 
392 #define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2) \
393 template<> \
394 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2> \
395  (const S1 &s1, const Transform3f& tf1, \
396  const S2 &s2, const Transform3f& tf2, \
397  FCL_REAL& distance_lower_bound, \
398  bool, \
399  Vec3f* contact_points, Vec3f* normal) const
400 
401 #define SHAPE_INTERSECT_SPECIALIZATION(S1,S2) \
402  SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2); \
403  SHAPE_INTERSECT_SPECIALIZATION_BASE(S2,S1)
404 
405  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Capsule);
407  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Box);
408  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Halfspace);
409  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Plane);
410 
411  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Box);
412  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Capsule);
413  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Cylinder);
414  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Cone);
415  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Plane);
416 
418  SHAPE_INTERSECT_SPECIALIZATION(Plane,Capsule);
419  SHAPE_INTERSECT_SPECIALIZATION(Plane,Cylinder);
420  SHAPE_INTERSECT_SPECIALIZATION(Plane,Cone);
421 
422 #undef SHAPE_INTERSECT_SPECIALIZATION
423 #undef SHAPE_INTERSECT_SPECIALIZATION_BASE
424 
425 #define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2) \
426 template<> \
427 HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2> \
428  (const S1& s1, const Transform3f& tf1, \
429  const S2& s2, const Transform3f& tf2, \
430  FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
431 
432 #define SHAPE_DISTANCE_SPECIALIZATION(S1,S2) \
433  SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2); \
434  SHAPE_DISTANCE_SPECIALIZATION_BASE(S2,S1)
435 
436  SHAPE_DISTANCE_SPECIALIZATION(Sphere,Capsule);
437  SHAPE_DISTANCE_SPECIALIZATION(Sphere,Box);
438  SHAPE_DISTANCE_SPECIALIZATION(Sphere,Cylinder);
439  SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere,Sphere);
440  SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule,Capsule);
441  SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP,TriangleP);
442 
443 #undef SHAPE_DISTANCE_SPECIALIZATION
444 #undef SHAPE_DISTANCE_SPECIALIZATION_BASE
445 
446 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
447 #pragma GCC diagnostic push
448 #pragma GCC diagnostic ignored "-Wc99-extensions"
449 #endif
450 
453 // param doc is the doxygen detailled description (should be enclosed in /** */
454 // and contain no dot for some obscure reasons).
455 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \
456  \
457  doc \
458  template<> \
459  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2> \
460  (const Shape1& s1, const Transform3f& tf1, \
461  const Shape2& s2, const Transform3f& tf2, \
462  FCL_REAL& distance_lower_bound, bool enable_penetration, \
463  Vec3f* contact_points, Vec3f* normal) const
464 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
465  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
466 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
467  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc); \
468  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc)
469 
471  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,);
472  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
473  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,);
474 
475  template<>
476  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Sphere>
477  (const Box& s1, const Transform3f& tf1,
478  const Sphere& s2, const Transform3f& tf2,
479  FCL_REAL& distance_lower_bound, bool enable_penetration,
480  Vec3f* contact_points, Vec3f* normal) const;
481 
482 #ifdef IS_DOXYGEN // for doxygen only
483 
486  template<>
487  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Box>
488  (const Box& s1, const Transform3f& tf1,
489  const Box& s2, const Transform3f& tf2,
490  FCL_REAL& distance_lower_bound, bool enable_penetration,
491  Vec3f* contact_points, Vec3f* normal) const;
492 #endif
493  //HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
494  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,);
496 
497  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace,);
498  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane,);
499 
500  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace,);
501  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane,);
502 
503  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace,);
505 
507 
509  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace,);
510 
511 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT
512 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
513 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
514 
516 
519 
520 // param doc is the doxygen detailled description (should be enclosed in /** */
521 // and contain no dot for some obscure reasons).
522 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \
523  \
524  doc \
525  template<> HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape> \
526  (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \
527  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, \
528  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
529 
531  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace,);
533 
534 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
535 
537 
540 
541 // param doc is the doxygen detailled description (should be enclosed in /** */
542 // and contain no dot for some obscure reasons).
543 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \
544  \
545  doc \
546  template<> \
547  bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2> \
548  (const Shape1& s1, const Transform3f& tf1, \
549  const Shape2& s2, const Transform3f& tf2, \
550  FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
551 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc) \
552  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc)
553 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc) \
554  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc); \
555  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc)
556 
558  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule,);
559  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder,);
561 
564  );
565 
568  );
569 
570 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE
571 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
572 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
573 
575 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
576 #pragma GCC diagnostic pop
577 #endif
578 }
579 
580 } // namespace hpp
581 
582 #endif
Definition: gjk.h:152
Simple transform class used locally by InterpMotion.
Definition: transform.h:58
Definition: gjk.h:152
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:345
support_func_guess_t support_hint
Definition: gjk.h:156
Status
Definition: gjk.h:338
Main namespace.
Definition: AABB.h:43
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:339
Definition: gjk.h:340
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:357
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc)
Definition: narrowphase.h:551
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:453
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:432
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:309
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Infinite plane.
Definition: geometric_shapes.h:523
class for GJK algorithm
Definition: gjk.h:130
Vec3f ray
Definition: gjk.h:155
Minkowski difference class of two shapes.
Definition: gjk.h:61
class for EPA algorithm
Definition: gjk.h:277
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess, const support_func_guess_t &supportHint=support_func_guess_t::Zero())
GJK algorithm, given the initial value guess.
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Vec3f getGuessFromSimplex() const
get the guess from current simplex
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:425
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.
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:173
double FCL_REAL
Definition: data_types.h:66
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:342
Status
Definition: gjk.h:152
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:323
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:75
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:71
FCL_REAL depth
Definition: gjk.h:353
FCL_REAL distance
Definition: gjk.h:170
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:392
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:354
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:193
bool shapeDistance(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
distance computation between two shapes
Definition: narrowphase.h:213
Center at zero point sphere.
Definition: geometric_shapes.h:149
Vec3f cached_guess
smart guess
Definition: narrowphase.h:360
bool shapeTriangleInteraction(const S &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
intersection checking between one shape and a triangle with transformation
Definition: narrowphase.h:128
Status evaluate(GJK &gjk, const Vec3f &guess)
bool shapeIntersect(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance_lower_bound, bool enable_penetration, Vec3f *contact_points, Vec3f *normal) const
intersection checking between two shapes
Definition: narrowphase.h:57
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc)
Definition: narrowphase.h:464
void setDistanceEarlyBreak(const FCL_REAL &dup)
Distance threshold for early break. GJK stops when it proved the distance is more than this threshold...
Definition: gjk.h:236
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:466
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:401
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:553
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:53
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:351
Vec3f normal
Definition: gjk.h:352
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:363
Vec3f getCachedGuess() const
Definition: narrowphase.h:333
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:124
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:328
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:220
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:348
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)
Definition: narrowphase.h:522
#define HPP_FCL_DLLAPI
Definition: config.hh:64
FCL_REAL distance_upper_bound
Distance above which the GJK solver stoppes its computations and processes to an early stopping...
Definition: narrowphase.h:367
Definition: gjk.h:152