hpp-fcl  1.6.0
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  *
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  */
36 
39 #ifndef HPP_FCL_NARROWPHASE_H
40 #define HPP_FCL_NARROWPHASE_H
41 
42 #include <hpp/fcl/narrowphase/gjk.h>
43 
44 namespace hpp
45 {
46 namespace fcl
47 {
48 
50  struct HPP_FCL_DLLAPI GJKSolver
51  {
53  template<typename S1, typename S2>
54  bool shapeIntersect(const S1& s1, const Transform3f& tf1,
55  const S2& s2, const Transform3f& tf2,
56  FCL_REAL& distance_lower_bound,
57  bool enable_penetration,
58  Vec3f* contact_points, Vec3f* normal) const
59  {
60  Vec3f guess(1, 0, 0);
61  support_func_guess_t support_hint;
62  if(enable_cached_guess) {
63  guess = cached_guess;
64  support_hint = support_func_cached_guess;
65  } else
66  support_hint.setZero();
67 
68  details::MinkowskiDiff shape;
69  shape.set (&s1, &s2, tf1, tf2);
70 
71  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
72  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
73  if(enable_cached_guess) {
74  cached_guess = gjk.getGuessFromSimplex();
75  support_func_cached_guess = gjk.support_hint;
76  }
77 
78  Vec3f w0, w1;
79  switch(gjk_status) {
80  case details::GJK::Inside:
81  if (!enable_penetration && contact_points == NULL && normal == NULL)
82  return true;
83  if (gjk.hasPenetrationInformation(shape)) {
84  gjk.getClosestPoints (shape, w0, w1);
85  distance_lower_bound = gjk.distance;
86  if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
87  if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
88  return true;
89  } else {
90  details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
91  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
92  if(epa_status & details::EPA::Valid
93  || epa_status == details::EPA::OutOfFaces // Warnings
94  || epa_status == details::EPA::OutOfVertices // Warnings
95  )
96  {
97  epa.getClosestPoints (shape, w0, w1);
98  distance_lower_bound = -epa.depth;
99  if(normal) *normal = tf1.getRotation() * epa.normal;
100  if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
101  return true;
102  }
103  distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)();
104  // EPA failed but we know there is a collision so we should
105  return true;
106  }
107  break;
108  case details::GJK::Valid:
109  distance_lower_bound = gjk.distance;
110  break;
111  default:
112  ;
113  }
114 
115  return false;
116  }
117 
120  template<typename S>
121  bool shapeTriangleInteraction
122  (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
123  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
124  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
125  {
126  bool col;
127  // Express everything in frame 1
128  const Transform3f tf_1M2 (tf1.inverseTimes(tf2));
129  TriangleP tri(
130  tf_1M2.transform (P1),
131  tf_1M2.transform (P2),
132  tf_1M2.transform (P3));
133 
134  Vec3f guess(1, 0, 0);
135  support_func_guess_t support_hint;
136  if(enable_cached_guess) {
137  guess = cached_guess;
138  support_hint = support_func_cached_guess;
139  } else
140  support_hint.setZero();
141 
142  details::MinkowskiDiff shape;
143  shape.set (&s, &tri);
144 
145  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
146  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
147  if(enable_cached_guess) {
148  cached_guess = gjk.getGuessFromSimplex();
149  support_func_cached_guess = gjk.support_hint;
150  }
151 
152  Vec3f w0, w1;
153  switch(gjk_status) {
154  case details::GJK::Inside:
155  col = true;
156  if (gjk.hasPenetrationInformation(shape)) {
157  gjk.getClosestPoints (shape, w0, w1);
158  distance = gjk.distance;
159  normal = tf1.getRotation() * (w0 - w1).normalized();
160  p1 = p2 = tf1.transform((w0 + w1) / 2);
161  } else {
162  details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
163  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
164  if(epa_status & details::EPA::Valid
165  || epa_status == details::EPA::OutOfFaces // Warnings
166  || epa_status == details::EPA::OutOfVertices // Warnings
167  )
168  {
169  epa.getClosestPoints (shape, w0, w1);
170  distance = -epa.depth;
171  normal = tf1.getRotation() * epa.normal;
172  p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
173  assert (distance <= 1e-6);
174  } else {
175  distance = -(std::numeric_limits<FCL_REAL>::max)();
176  gjk.getClosestPoints (shape, w0, w1);
177  p1 = p2 = tf1.transform (w0);
178  }
179  }
180  break;
181  case details::GJK::Valid:
182  case details::GJK::Failed:
183  col = false;
184 
185  gjk.getClosestPoints (shape, p1, p2);
186  // TODO On degenerated case, the closest point may be wrong
187  // (i.e. an object face normal is colinear to gjk.ray
188  // assert (distance == (w0 - w1).norm());
189  distance = gjk.distance;
190 
191  p1 = tf1.transform (p1);
192  p2 = tf1.transform (p2);
193  assert (distance > 0);
194  break;
195  default:
196  assert (false && "should not reach type part.");
197  return true;
198  }
199  return col;
200  }
201 
203  template<typename S1, typename S2>
204  bool shapeDistance(const S1& s1, const Transform3f& tf1,
205  const S2& s2, const Transform3f& tf2,
206  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
207  Vec3f& normal) const
208  {
209 #ifndef NDEBUG
210  FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
211 #endif
212  Vec3f guess(1, 0, 0);
213  support_func_guess_t support_hint;
214  if(enable_cached_guess) {
215  guess = cached_guess;
216  support_hint = support_func_cached_guess;
217  } else
218  support_hint.setZero();
219 
220  details::MinkowskiDiff shape;
221  shape.set (&s1, &s2, tf1, tf2);
222 
223  details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
224  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
225  if(enable_cached_guess) {
226  cached_guess = gjk.getGuessFromSimplex();
227  support_func_cached_guess = gjk.support_hint;
228  }
229 
230  if(gjk_status == details::GJK::Failed)
231  {
232  // TODO: understand why GJK fails between cylinder and box
233  assert (distance * distance < sqrt (eps));
234  Vec3f w0, w1;
235  gjk.getClosestPoints (shape, w0, w1);
236  distance = 0;
237  p1 = tf1.transform (w0);
238  p2 = tf1.transform (w1);
239  normal = Vec3f (0,0,0);
240  return false;
241  }
242  else if(gjk_status == details::GJK::Valid)
243  {
244  gjk.getClosestPoints (shape, p1, p2);
245  // TODO On degenerated case, the closest point may be wrong
246  // (i.e. an object face normal is colinear to gjk.ray
247  // assert (distance == (w0 - w1).norm());
248  distance = gjk.distance;
249 
250  normal = (tf1.getRotation() * gjk.ray).normalized();
251  p1 = tf1.transform (p1);
252  p2 = tf1.transform (p2);
253  return true;
254  }
255  else
256  {
257  assert (gjk_status == details::GJK::Inside);
258  if (gjk.hasPenetrationInformation (shape)) {
259  gjk.getClosestPoints (shape, p1, p2);
260  distance = gjk.distance;
261  // Return contact points in case of collision
262  //p1 = tf1.transform (p1);
263  //p2 = tf1.transform (p2);
264  normal = (tf1.getRotation() * (p1 - p2)).normalized();
265  p1 = tf1.transform(p1);
266  p2 = tf1.transform(p2);
267  } else {
268  details::EPA epa(epa_max_face_num, epa_max_vertex_num,
269  epa_max_iterations, epa_tolerance);
270  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
271  if(epa_status & details::EPA::Valid
272  || epa_status == details::EPA::OutOfFaces // Warnings
273  || epa_status == details::EPA::OutOfVertices // Warnings
274  )
275  {
276  Vec3f w0, w1;
277  epa.getClosestPoints (shape, w0, w1);
278  assert (epa.depth >= -eps);
279  distance = (std::min) (0., -epa.depth);
280  normal = tf1.getRotation() * epa.normal;
281  p1 = tf1.transform(w0);
282  p2 = tf1.transform(w1);
283  return false;
284  }
285  distance = -(std::numeric_limits<FCL_REAL>::max)();
286  gjk.getClosestPoints (shape, p1, p2);
287  p1 = tf1.transform(p1);
288  p2 = tf1.transform(p2);
289  }
290  return false;
291  }
292  }
293 
295  GJKSolver()
296  {
297  gjk_max_iterations = 128;
298  gjk_tolerance = 1e-6;
299  epa_max_face_num = 128;
300  epa_max_vertex_num = 64;
301  epa_max_iterations = 255;
302  epa_tolerance = 1e-6;
303  enable_cached_guess = false;
304  cached_guess = Vec3f(1, 0, 0);
305  support_func_cached_guess = support_func_guess_t::Zero();
306  }
307 
308  void enableCachedGuess(bool if_enable) const
309  {
310  enable_cached_guess = if_enable;
311  }
312 
313  void setCachedGuess(const Vec3f& guess) const
314  {
315  cached_guess = guess;
316  }
317 
318  Vec3f getCachedGuess() const
319  {
320  return cached_guess;
321  }
322 
324  unsigned int epa_max_face_num;
325 
327  unsigned int epa_max_vertex_num;
328 
330  unsigned int epa_max_iterations;
331 
333  FCL_REAL epa_tolerance;
334 
336  FCL_REAL gjk_tolerance;
337 
339  FCL_REAL gjk_max_iterations;
340 
342  mutable bool enable_cached_guess;
343 
345  mutable Vec3f cached_guess;
346 
348  mutable support_func_guess_t support_func_cached_guess;
349  };
350 
351  template<>
352  HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction
353  (const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
354  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
355  Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
356 
357  template<>
358  HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction
359  (const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
360  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
361  Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
362 
363  template<>
364  HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction
365  (const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
366  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
367  Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
368 
369 #define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2) \
370 template<> \
371 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2> \
372  (const S1 &s1, const Transform3f& tf1, \
373  const S2 &s2, const Transform3f& tf2, \
374  FCL_REAL& distance_lower_bound, \
375  bool, \
376  Vec3f* contact_points, Vec3f* normal) const
377 
378 #define SHAPE_INTERSECT_SPECIALIZATION(S1,S2) \
379  SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2); \
380  SHAPE_INTERSECT_SPECIALIZATION_BASE(S2,S1)
381 
382  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Capsule);
383  SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere,Sphere);
384  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Box);
385  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Halfspace);
386  SHAPE_INTERSECT_SPECIALIZATION(Sphere,Plane);
387 
388  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Box);
389  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Capsule);
390  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Cylinder);
391  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Cone);
392  SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Plane);
393 
394  SHAPE_INTERSECT_SPECIALIZATION(Plane,Box);
395  SHAPE_INTERSECT_SPECIALIZATION(Plane,Capsule);
396  SHAPE_INTERSECT_SPECIALIZATION(Plane,Cylinder);
397  SHAPE_INTERSECT_SPECIALIZATION(Plane,Cone);
398 
399 #undef SHAPE_INTERSECT_SPECIALIZATION
400 #undef SHAPE_INTERSECT_SPECIALIZATION_BASE
401 
402 #define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2) \
403 template<> \
404 HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2> \
405  (const S1& s1, const Transform3f& tf1, \
406  const S2& s2, const Transform3f& tf2, \
407  FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
408 
409 #define SHAPE_DISTANCE_SPECIALIZATION(S1,S2) \
410  SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2); \
411  SHAPE_DISTANCE_SPECIALIZATION_BASE(S2,S1)
412 
413  SHAPE_DISTANCE_SPECIALIZATION(Sphere,Capsule);
414  SHAPE_DISTANCE_SPECIALIZATION(Sphere,Box);
415  SHAPE_DISTANCE_SPECIALIZATION(Sphere,Cylinder);
416  SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere,Sphere);
417  SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule,Capsule);
418  SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP,TriangleP);
419 
420 #undef SHAPE_DISTANCE_SPECIALIZATION
421 #undef SHAPE_DISTANCE_SPECIALIZATION_BASE
422 
423 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
424 #pragma GCC diagnostic push
425 #pragma GCC diagnostic ignored "-Wc99-extensions"
426 #endif
427 
430 // param doc is the doxygen detailled description (should be enclosed in /** */
431 // and contain no dot for some obscure reasons).
432 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \
433  \
434  doc \
435  template<> \
436  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2> \
437  (const Shape1& s1, const Transform3f& tf1, \
438  const Shape2& s2, const Transform3f& tf2, \
439  FCL_REAL& distance_lower_bound, bool enable_penetration, \
440  Vec3f* contact_points, Vec3f* normal) const
441 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
442  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
443 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
444  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc); \
445  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc)
446 
447  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,);
448  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,);
449  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
450  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,);
451 
452  template<>
453  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Sphere>
454  (const Box& s1, const Transform3f& tf1,
455  const Sphere& s2, const Transform3f& tf2,
456  FCL_REAL& distance_lower_bound, bool enable_penetration,
457  Vec3f* contact_points, Vec3f* normal) const;
458 
459 #ifdef IS_DOXYGEN // for doxygen only
460 
463  template<>
464  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Box>
465  (const Box& s1, const Transform3f& tf1,
466  const Box& s2, const Transform3f& tf2,
467  FCL_REAL& distance_lower_bound, bool enable_penetration,
468  Vec3f* contact_points, Vec3f* normal) const;
469 #endif
470  //HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
471  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,);
472  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane,);
473 
474  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace,);
475  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane,);
476 
477  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace,);
478  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane,);
479 
480  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace,);
481  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane,);
482 
483  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace,);
484 
485  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane,);
486  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace,);
487 
488 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT
489 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
490 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
491 
493 
496 
497 // param doc is the doxygen detailled description (should be enclosed in /** */
498 // and contain no dot for some obscure reasons).
499 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \
500  \
501  doc \
502  template<> HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape> \
503  (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \
504  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, \
505  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
506 
507  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,);
508  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace,);
509  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane,);
510 
511 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
512 
514 
517 
518 // param doc is the doxygen detailled description (should be enclosed in /** */
519 // and contain no dot for some obscure reasons).
520 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \
521  \
522  doc \
523  template<> \
524  bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2> \
525  (const Shape1& s1, const Transform3f& tf1, \
526  const Shape2& s2, const Transform3f& tf2, \
527  FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
528 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc) \
529  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc)
530 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc) \
531  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc); \
532  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc)
533 
534  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,);
535  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule,);
536  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder,);
537  HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,);
538 
539  HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Capsule,
541  );
542 
543  HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(TriangleP,
545  );
546 
547 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE
548 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
549 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
550 
552 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
553 #pragma GCC diagnostic pop
554 #endif
555 }
556 
557 } // namespace hpp
558 
559 #endif
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc)
Definition: narrowphase.h:441
hpp::fcl::GJKSolver::GJKSolver
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:295
gjk.h
hpp::fcl::details::GJK::Status
Status
Definition: gjk.h:152
hpp::fcl::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:123
hpp::fcl::details::GJK::evaluate
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.
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:530
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:244
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:87
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:62
hpp::fcl::GJKSolver::shapeDistance
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:204
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc)
Definition: narrowphase.h:528
hpp::fcl::GJKSolver::setCachedGuess
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:313
SHAPE_DISTANCE_SPECIALIZATION_BASE
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:402
hpp::fcl::details::GJK::Valid
@ Valid
Definition: gjk.h:152
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:119
hpp::fcl::details::EPA::evaluate
Status evaluate(GJK &gjk, const Vec3f &guess)
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:168
hpp::fcl::details::EPA::Status
Status
Definition: gjk.h:338
hpp::fcl::details::EPA::Valid
@ Valid
Definition: gjk.h:340
hpp::fcl::details::EPA::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::EPA
class for EPA algorithm
Definition: gjk.h:278
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:156
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:358
SHAPE_INTERSECT_SPECIALIZATION_BASE
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:369
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:443
SHAPE_INTERSECT_SPECIALIZATION
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:378
hpp::fcl::GJKSolver::cached_guess
Vec3f cached_guess
smart guess
Definition: narrowphase.h:345
SHAPE_DISTANCE_SPECIALIZATION
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:409
hpp::fcl::GJKSolver::shapeIntersect
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:54
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
hpp::fcl::GJKSolver::getCachedGuess
Vec3f getCachedGuess() const
Definition: narrowphase.h:318
hpp::fcl::GJKSolver::enableCachedGuess
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:308
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:155
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:155
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:200
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
HPP_FCL_DECLARE_SHAPE_TRIANGLE
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)
Definition: narrowphase.h:499
hpp::fcl::GJKSolver::shapeTriangleInteraction
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:122
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:69
hpp::fcl::GJKSolver::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:342
hpp::fcl::Transform3f::inverseTimes
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:188
hpp::fcl::details::EPA::OutOfFaces
@ OutOfFaces
Definition: gjk.h:345
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::details::GJK::hasPenetrationInformation
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:220
hpp::fcl::GJKSolver::gjk_tolerance
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:336
hpp::fcl::GJKSolver::epa_max_face_num
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:324
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:131
hpp::fcl::details::GJK::Inside
@ Inside
Definition: gjk.h:152
hpp::fcl::GJKSolver::gjk_max_iterations
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:339
hpp::fcl::Plane
Infinite plane.
Definition: geometric_shapes.h:406
hpp::fcl::GJKSolver::epa_tolerance
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:333
hpp::fcl::details::EPA::depth
FCL_REAL depth
Definition: gjk.h:353
hpp::fcl::GJKSolver::support_func_cached_guess
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:348
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::details::GJK::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::GJK::Failed
@ Failed
Definition: gjk.h:152
hpp::fcl::details::EPA::OutOfVertices
@ OutOfVertices
Definition: gjk.h:346
hpp::fcl::GJKSolver::epa_max_iterations
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:330
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:51
hpp::fcl::GJKSolver::epa_max_vertex_num
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:327
hpp::fcl::details::GJK::getGuessFromSimplex
Vec3f getGuessFromSimplex() const
get the guess from current simplex
hpp::fcl::details::EPA::normal
Vec3f normal
Definition: gjk.h:352
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:71
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:170