hpp-fcl  2.4.1
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-2022, 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 #include <iostream>
45 
46 #include <hpp/fcl/narrowphase/gjk.h>
47 #include <hpp/fcl/collision_data.h>
48 
49 namespace hpp {
50 namespace fcl {
51 
54 struct HPP_FCL_DLLAPI GJKSolver {
55  typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;
56 
58  template <typename S1, typename S2>
59  void initialize_gjk(details::GJK& gjk, const details::MinkowskiDiff& shape,
60  const S1& s1, const S2& s2, Vec3f& guess,
61  support_func_guess_t& support_hint) const {
62  switch (gjk_initial_guess) {
63  case GJKInitialGuess::DefaultGuess:
64  guess = Vec3f(1, 0, 0);
65  support_hint.setZero();
66  break;
67  case GJKInitialGuess::CachedGuess:
68  guess = cached_guess;
69  support_hint = support_func_cached_guess;
70  break;
71  case GJKInitialGuess::BoundingVolumeGuess:
72  if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
73  HPP_FCL_THROW_PRETTY(
74  "computeLocalAABB must have been called on the shapes before "
75  "using "
76  "GJKInitialGuess::BoundingVolumeGuess.",
77  std::logic_error);
78  }
79  guess.noalias() = s1.aabb_local.center() -
80  (shape.oR1 * s2.aabb_local.center() + shape.ot1);
81  support_hint.setZero();
82  break;
83  default:
84  HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
85  }
86  // TODO: use gjk_initial_guess instead
87  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
88  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
89  if (enable_cached_guess) {
90  guess = cached_guess;
91  support_hint = support_func_cached_guess;
92  }
93  HPP_FCL_COMPILER_DIAGNOSTIC_POP
94 
95  gjk.setDistanceEarlyBreak(distance_upper_bound);
96 
97  gjk.gjk_variant = gjk_variant;
98  gjk.convergence_criterion = gjk_convergence_criterion;
99  gjk.convergence_criterion_type = gjk_convergence_criterion_type;
100  }
101 
103  template <typename S1, typename S2>
104  bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2,
105  const Transform3f& tf2, FCL_REAL& distance_lower_bound,
106  bool enable_penetration, Vec3f* contact_points,
107  Vec3f* normal) const {
108  details::MinkowskiDiff shape;
109  shape.set(&s1, &s2, tf1, tf2);
110 
111  Vec3f guess;
112  support_func_guess_t support_hint;
113  details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
114  initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
115 
116  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
117  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
118  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
119  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
120  enable_cached_guess) {
121  cached_guess = gjk.getGuessFromSimplex();
122  support_func_cached_guess = gjk.support_hint;
123  }
124  HPP_FCL_COMPILER_DIAGNOSTIC_POP
125 
126  Vec3f w0, w1;
127  switch (gjk_status) {
128  case details::GJK::Inside:
129  if (!enable_penetration && contact_points == NULL && normal == NULL)
130  return true;
131  if (gjk.hasPenetrationInformation(shape)) {
132  gjk.getClosestPoints(shape, w0, w1);
133  distance_lower_bound = gjk.distance;
134  if (normal)
135  (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized();
136  if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
137  return true;
138  } else {
139  details::EPA epa(epa_max_face_num, epa_max_vertex_num,
140  epa_max_iterations, epa_tolerance);
141  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
142  if (epa_status & details::EPA::Valid ||
143  epa_status == details::EPA::OutOfFaces // Warnings
144  || epa_status == details::EPA::OutOfVertices // Warnings
145  ) {
146  epa.getClosestPoints(shape, w0, w1);
147  distance_lower_bound = -epa.depth;
148  if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal;
149  if (contact_points)
150  *contact_points =
151  tf1.transform(w0 - epa.normal * (epa.depth * 0.5));
152  return true;
153  } else if (epa_status == details::EPA::FallBack) {
154  epa.getClosestPoints(shape, w0, w1);
155  distance_lower_bound = -epa.depth; // Should be zero
156  if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal;
157  if (contact_points) *contact_points = tf1.transform(w0);
158  return true;
159  }
160  distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)();
161  // EPA failed but we know there is a collision so we should
162  return true;
163  }
164  break;
165  case details::GJK::Valid:
166  distance_lower_bound = gjk.distance;
167  break;
168  default:;
169  }
170 
171  return false;
172  }
173 
177  template <typename S>
178  bool shapeTriangleInteraction(const S& s, const Transform3f& tf1,
179  const Vec3f& P1, const Vec3f& P2,
180  const Vec3f& P3, const Transform3f& tf2,
181  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
182  Vec3f& normal) const {
183  bool col;
184  // Express everything in frame 1
185  const Transform3f tf_1M2(tf1.inverseTimes(tf2));
186  TriangleP tri(tf_1M2.transform(P1), tf_1M2.transform(P2),
187  tf_1M2.transform(P3));
188 
189  details::MinkowskiDiff shape;
190  shape.set(&s, &tri);
191 
192  Vec3f guess;
193  support_func_guess_t support_hint;
194  details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
195  initialize_gjk(gjk, shape, s, tri, guess, support_hint);
196 
197  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
198 
199  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
200  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
201  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
202  enable_cached_guess) {
203  cached_guess = gjk.getGuessFromSimplex();
204  support_func_cached_guess = gjk.support_hint;
205  }
206  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
207 
208  Vec3f w0, w1;
209  switch (gjk_status) {
210  case details::GJK::Inside:
211  col = true;
212  if (gjk.hasPenetrationInformation(shape)) {
213  gjk.getClosestPoints(shape, w0, w1);
214  distance = gjk.distance;
215  normal.noalias() = tf1.getRotation() * (w0 - w1).normalized();
216  p1 = p2 = tf1.transform((w0 + w1) / 2);
217  } else {
218  details::EPA epa(epa_max_face_num, epa_max_vertex_num,
219  epa_max_iterations, epa_tolerance);
220  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
221  if (epa_status & details::EPA::Valid ||
222  epa_status == details::EPA::OutOfFaces // Warnings
223  || epa_status == details::EPA::OutOfVertices // Warnings
224  ) {
225  epa.getClosestPoints(shape, w0, w1);
226  distance = -epa.depth;
227  normal.noalias() = tf1.getRotation() * epa.normal;
228  p1 = p2 = tf1.transform(w0 - epa.normal * (epa.depth * 0.5));
229  assert(distance <= 1e-6);
230  } else {
231  distance = -(std::numeric_limits<FCL_REAL>::max)();
232  gjk.getClosestPoints(shape, w0, w1);
233  p1 = p2 = tf1.transform(w0);
234  }
235  }
236  break;
237  case details::GJK::Valid:
238  case details::GJK::EarlyStopped:
239  case details::GJK::Failed:
240  col = false;
241 
242  gjk.getClosestPoints(shape, p1, p2);
243  // TODO On degenerated case, the closest point may be wrong
244  // (i.e. an object face normal is colinear to gjk.ray
245  // assert (distance == (w0 - w1).norm());
246  distance = gjk.distance;
247 
248  p1 = tf1.transform(p1);
249  p2 = tf1.transform(p2);
250  assert(distance > 0);
251  break;
252  default:
253  assert(false && "should not reach type part.");
254  throw std::logic_error("GJKSolver: should not reach this part.");
255  }
256  return col;
257  }
258 
260  template <typename S1, typename S2>
261  bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2,
262  const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
263  Vec3f& p2, Vec3f& normal) const {
264 #ifndef NDEBUG
265  FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
266 #endif
267  details::MinkowskiDiff shape;
268  shape.set(&s1, &s2, tf1, tf2);
269 
270  Vec3f guess;
271  support_func_guess_t support_hint;
272  details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
273  initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
274 
275  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
276  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
277  enable_cached_guess) {
278  cached_guess = gjk.getGuessFromSimplex();
279  support_func_cached_guess = gjk.support_hint;
280  }
281 
282  if (gjk_status == details::GJK::Failed) {
283  // TODO: understand why GJK fails between cylinder and box
284  assert(distance * distance < sqrt(eps));
285  Vec3f w0, w1;
286  gjk.getClosestPoints(shape, w0, w1);
287  distance = 0;
288  p1 = tf1.transform(w0);
289  p2 = tf1.transform(w1);
290  normal.setZero();
291  return false;
292  } else if (gjk_status == details::GJK::Valid) {
293  gjk.getClosestPoints(shape, p1, p2);
294  // TODO On degenerated case, the closest point may be wrong
295  // (i.e. an object face normal is colinear to gjk.ray
296  // assert (distance == (w0 - w1).norm());
297  distance = gjk.distance;
298 
299  normal.noalias() = tf1.getRotation() * gjk.ray;
300  normal.normalize();
301  p1 = tf1.transform(p1);
302  p2 = tf1.transform(p2);
303  return true;
304  } else if (gjk_status == details::GJK::EarlyStopped) {
305  distance = gjk.distance;
306  p1 = p2 = normal =
307  Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
308  return true;
309  } else {
310  assert(gjk_status == details::GJK::Inside);
311  if (gjk.hasPenetrationInformation(shape)) {
312  gjk.getClosestPoints(shape, p1, p2);
313  distance = gjk.distance;
314  // Return contact points in case of collision
315  // p1 = tf1.transform (p1);
316  // p2 = tf1.transform (p2);
317  normal.noalias() = tf1.getRotation() * (p1 - p2);
318  normal.normalize();
319  p1 = tf1.transform(p1);
320  p2 = tf1.transform(p2);
321  } else {
322  details::EPA epa(epa_max_face_num, epa_max_vertex_num,
323  epa_max_iterations, epa_tolerance);
324  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
325  if (epa_status & details::EPA::Valid ||
326  epa_status == details::EPA::OutOfFaces // Warnings
327  || epa_status == details::EPA::OutOfVertices // Warnings
328  || epa_status == details::EPA::FallBack) {
329  Vec3f w0, w1;
330  epa.getClosestPoints(shape, w0, w1);
331  assert(epa.depth >= -eps);
332  distance = (std::min)(0., -epa.depth);
333  normal.noalias() = tf1.getRotation() * epa.normal;
334  p1 = tf1.transform(w0);
335  p2 = tf1.transform(w1);
336  return false;
337  }
338  distance = -(std::numeric_limits<FCL_REAL>::max)();
339  gjk.getClosestPoints(shape, p1, p2);
340  p1 = tf1.transform(p1);
341  p2 = tf1.transform(p2);
342  }
343  return false;
344  }
345  }
346 
347  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
348  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
350  GJKSolver() {
351  gjk_max_iterations = 128;
352  gjk_tolerance = 1e-6;
353  epa_max_face_num = 128;
354  epa_max_vertex_num = 64;
355  epa_max_iterations = 255;
356  epa_tolerance = 1e-6;
357  enable_cached_guess = false; // TODO: use gjk_initial_guess instead
358  cached_guess = Vec3f(1, 0, 0);
359  support_func_cached_guess = support_func_guess_t::Zero();
360  distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
361  gjk_initial_guess = GJKInitialGuess::DefaultGuess;
362  gjk_variant = GJKVariant::DefaultGJK;
363  gjk_convergence_criterion = GJKConvergenceCriterion::VDB;
364  gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative;
365  }
366 
371  GJKSolver(const DistanceRequest& request) {
372  cached_guess = Vec3f(1, 0, 0);
373  support_func_cached_guess = support_func_guess_t::Zero();
374  distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
375 
376  // EPS settings
377  epa_max_face_num = 128;
378  epa_max_vertex_num = 64;
379  epa_max_iterations = 255;
380  epa_tolerance = 1e-6;
381 
382  set(request);
383  }
384 
389  void set(const DistanceRequest& request) {
390  gjk_initial_guess = request.gjk_initial_guess;
391  // TODO: use gjk_initial_guess instead
392  enable_cached_guess = request.enable_cached_gjk_guess;
393  gjk_variant = request.gjk_variant;
394  gjk_convergence_criterion = request.gjk_convergence_criterion;
395  gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
396  gjk_tolerance = request.gjk_tolerance;
397  gjk_max_iterations = request.gjk_max_iterations;
398  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
399  enable_cached_guess) {
400  cached_guess = request.cached_gjk_guess;
401  support_func_cached_guess = request.cached_support_func_guess;
402  }
403  }
404 
409  GJKSolver(const CollisionRequest& request) {
410  cached_guess = Vec3f(1, 0, 0);
411  support_func_cached_guess = support_func_guess_t::Zero();
412  distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
413 
414  // EPS settings
415  epa_max_face_num = 128;
416  epa_max_vertex_num = 64;
417  epa_max_iterations = 255;
418  epa_tolerance = 1e-6;
419 
420  set(request);
421  }
422 
427  void set(const CollisionRequest& request) {
428  gjk_initial_guess = request.gjk_initial_guess;
429  // TODO: use gjk_initial_guess instead
430  enable_cached_guess = request.enable_cached_gjk_guess;
431  gjk_variant = request.gjk_variant;
432  gjk_convergence_criterion = request.gjk_convergence_criterion;
433  gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
434  gjk_tolerance = request.gjk_tolerance;
435  gjk_max_iterations = request.gjk_max_iterations;
436  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
437  enable_cached_guess) {
438  cached_guess = request.cached_gjk_guess;
439  support_func_cached_guess = request.cached_support_func_guess;
440  }
441 
442  // The distance upper bound should be at least greater to the requested
443  // security margin. Otherwise, we will likely miss some collisions.
444  distance_upper_bound = (std::max)(
445  0., (std::max)(request.distance_upper_bound, request.security_margin));
446  }
447 
449  GJKSolver(const GJKSolver& other) = default;
450 
451  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
452  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
453  bool operator==(const GJKSolver& other) const {
454  return epa_max_face_num == other.epa_max_face_num &&
455  epa_max_vertex_num == other.epa_max_vertex_num &&
456  epa_max_iterations == other.epa_max_iterations &&
457  epa_tolerance == other.epa_tolerance &&
458  gjk_max_iterations == other.gjk_max_iterations &&
459  enable_cached_guess ==
460  other.enable_cached_guess && // TODO: use gjk_initial_guess
461  // instead
462  cached_guess == other.cached_guess &&
463  support_func_cached_guess == other.support_func_cached_guess &&
464  distance_upper_bound == other.distance_upper_bound &&
465  gjk_initial_guess == other.gjk_initial_guess &&
466  gjk_variant == other.gjk_variant &&
467  gjk_convergence_criterion == other.gjk_convergence_criterion &&
468  gjk_convergence_criterion_type ==
469  other.gjk_convergence_criterion_type;
470  }
471  HPP_FCL_COMPILER_DIAGNOSTIC_POP
472 
473  bool operator!=(const GJKSolver& other) const { return !(*this == other); }
474 
476  unsigned int epa_max_face_num;
477 
479  unsigned int epa_max_vertex_num;
480 
482  unsigned int epa_max_iterations;
483 
485  FCL_REAL epa_tolerance;
486 
488  mutable FCL_REAL gjk_tolerance;
489 
491  mutable size_t gjk_max_iterations;
492 
495  HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead")
496  mutable bool enable_cached_guess;
497 
499  mutable Vec3f cached_guess;
500 
502  mutable GJKInitialGuess gjk_initial_guess;
503 
505  mutable GJKVariant gjk_variant;
506 
508  mutable GJKConvergenceCriterion gjk_convergence_criterion;
509 
511  mutable GJKConvergenceCriterionType gjk_convergence_criterion_type;
512 
514  mutable support_func_guess_t support_func_cached_guess;
515 
520  mutable FCL_REAL distance_upper_bound;
521 
522  public:
523  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
524 };
525 
526 template <>
527 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
528  const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
529  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
530  Vec3f& p2, Vec3f& normal) const;
531 
532 template <>
533 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
534  const Halfspace& s, const Transform3f& tf1, const Vec3f& P1,
535  const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
536  FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
537 
538 template <>
539 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
540  const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
541  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
542  Vec3f& p2, Vec3f& normal) const;
543 
544 #define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2) \
545  template <> \
546  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2>( \
547  const S1& s1, const Transform3f& tf1, const S2& s2, \
548  const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \
549  Vec3f* contact_points, Vec3f* normal) const
550 
551 #define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \
552  SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \
553  SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1)
554 
555 SHAPE_INTERSECT_SPECIALIZATION(Sphere, Capsule);
556 SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere, Sphere);
557 SHAPE_INTERSECT_SPECIALIZATION(Sphere, Box);
558 SHAPE_INTERSECT_SPECIALIZATION(Sphere, Halfspace);
559 SHAPE_INTERSECT_SPECIALIZATION(Sphere, Plane);
560 
561 SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Box);
562 SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Capsule);
563 SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cylinder);
564 SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cone);
565 SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Plane);
566 
567 SHAPE_INTERSECT_SPECIALIZATION(Plane, Box);
568 SHAPE_INTERSECT_SPECIALIZATION(Plane, Capsule);
569 SHAPE_INTERSECT_SPECIALIZATION(Plane, Cylinder);
570 SHAPE_INTERSECT_SPECIALIZATION(Plane, Cone);
571 
572 #undef SHAPE_INTERSECT_SPECIALIZATION
573 #undef SHAPE_INTERSECT_SPECIALIZATION_BASE
574 
575 #define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2) \
576  template <> \
577  HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2>( \
578  const S1& s1, const Transform3f& tf1, const S2& s2, \
579  const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
580  Vec3f& normal) const
581 
582 #define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \
583  SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \
584  SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1)
585 
586 SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule);
587 SHAPE_DISTANCE_SPECIALIZATION(Sphere, Box);
588 SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder);
589 SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere, Sphere);
590 SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule, Capsule);
591 SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP, TriangleP);
592 
593 #undef SHAPE_DISTANCE_SPECIALIZATION
594 #undef SHAPE_DISTANCE_SPECIALIZATION_BASE
595 
596 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
597 #pragma GCC diagnostic push
598 #pragma GCC diagnostic ignored "-Wc99-extensions"
599 #endif
600 
603 // param doc is the doxygen detailled description (should be enclosed in /** */
604 // and contain no dot for some obscure reasons).
605 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc) \
606  \
607  doc template <> \
608  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2>( \
609  const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
610  const Transform3f& tf2, FCL_REAL& distance_lower_bound, \
611  bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const
612 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \
613  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc)
614 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \
615  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc); \
616  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc)
617 
618 HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere, );
619 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule, );
620 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace, );
621 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane, );
622 
623 template <>
624 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Sphere>(
625  const Box& s1, const Transform3f& tf1, const Sphere& s2,
626  const Transform3f& tf2, FCL_REAL& distance_lower_bound,
627  bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const;
628 
629 #ifdef IS_DOXYGEN // for doxygen only
630 
633 template <>
634 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Box>(
635  const Box& s1, const Transform3f& tf1, const Box& s2,
636  const Transform3f& tf2, FCL_REAL& distance_lower_bound,
637  bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const;
638 #endif
639 // HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
640 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace, );
641 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane, );
642 
643 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace, );
644 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane, );
645 
646 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace, );
647 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane, );
648 
649 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace, );
650 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane, );
651 
652 HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace, );
653 
654 HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane, );
655 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace, );
656 
657 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT
658 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
659 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
660 
662 
665 
666 // param doc is the doxygen detailled description (should be enclosed in /** */
667 // and contain no dot for some obscure reasons).
668 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc) \
669  \
670  doc template <> \
671  HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape>( \
672  const Shape& s, const Transform3f& tf1, const Vec3f& P1, \
673  const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, \
674  FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
675 
676 HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere, );
677 HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace, );
678 HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane, );
679 
680 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
681 
683 
686 
687 // param doc is the doxygen detailled description (should be enclosed in /** */
688 // and contain no dot for some obscure reasons).
689 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc) \
690  \
691  doc template <> \
692  bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2>( \
693  const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
694  const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
695  Vec3f& normal) const
696 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \
697  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc)
698 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \
699  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc); \
700  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc)
701 
702 HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box, );
703 HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule, );
704 HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder, );
705 HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere, );
706 
707 HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(
708  Capsule,
710 );
711 
712 HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(
713  TriangleP,
716 );
717 
718 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE
719 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
720 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
721 
723 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
724 #pragma GCC diagnostic pop
725 #endif
726 } // namespace fcl
727 
728 } // namespace hpp
729 
730 #endif
hpp::fcl::GJKSolver::gjk_convergence_criterion
GJKConvergenceCriterion gjk_convergence_criterion
Criterion used to stop GJK.
Definition: narrowphase.h:508
HPP_FCL_DLLAPI
#define HPP_FCL_DLLAPI
Definition: config.hh:88
hpp::fcl::CachedGuess
@ CachedGuess
Definition: data_types.h:80
hpp::fcl::details::GJK::EarlyStopped
@ EarlyStopped
Definition: gjk.h:165
hpp::fcl::GJKConvergenceCriterionType
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:93
hpp::fcl::BoundingVolumeGuess
@ BoundingVolumeGuess
Definition: data_types.h:80
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc)
Definition: narrowphase.h:612
hpp::fcl::Transform3f::inverseTimes
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:170
gjk.h
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:141
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:172
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:698
hpp::fcl::VDB
@ VDB
Definition: data_types.h:89
hpp::fcl::GJKSolver::operator!=
bool operator!=(const GJKSolver &other) const
Definition: narrowphase.h:473
hpp::fcl::GJKSolver::gjk_max_iterations
size_t gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:491
hpp::fcl::details::EPA
class for EPA algorithm
Definition: gjk.h:312
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
Definition: fwd.hh:82
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc)
Definition: narrowphase.h:696
SHAPE_DISTANCE_SPECIALIZATION_BASE
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:575
hpp::fcl::details::EPA::Status
Status
Definition: gjk.h:367
hpp::fcl::GJKSolver::distance_upper_bound
FCL_REAL distance_upper_bound
Distance above which the GJK solver stoppes its computations and processes to an early stopping....
Definition: narrowphase.h:520
hpp::fcl::DefaultGJK
@ DefaultGJK
Definition: data_types.h:83
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: fwd.hh:84
hpp::fcl::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:196
hpp::fcl::GJKConvergenceCriterion
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
Definition: data_types.h:89
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
hpp::fcl::GJKSolver::gjk_initial_guess
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
Definition: narrowphase.h:502
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:168
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:178
hpp::fcl::QueryRequest::gjk_initial_guess
GJKInitialGuess gjk_initial_guess
Definition: collision_data.h:121
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.
SHAPE_INTERSECT_SPECIALIZATION_BASE
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:544
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:614
SHAPE_INTERSECT_SPECIALIZATION
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:551
HPP_FCL_DEPRECATED_MESSAGE
#define HPP_FCL_DEPRECATED_MESSAGE(message)
Definition: deprecated.hh:38
hpp::fcl::details::EPA::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
SHAPE_DISTANCE_SPECIALIZATION
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:582
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:261
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:59
hpp::fcl::GJKSolver::gjk_variant
GJKVariant gjk_variant
Variant to use for the GJK algorithm.
Definition: narrowphase.h:505
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::GJKSolver::epa_max_face_num
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:476
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::details::GJK::Inside
@ Inside
Definition: gjk.h:165
hpp::fcl::details::GJK::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::GJK::getGuessFromSimplex
Vec3f getGuessFromSimplex() const
get the guess from current simplex
hpp::fcl::details::GJK::Valid
@ Valid
Definition: gjk.h:165
hpp::fcl::GJKSolver::set
void set(const DistanceRequest &request)
setter from a DistanceRequest
Definition: narrowphase.h:389
hpp::fcl::GJKSolver::GJKSolver
GJKSolver(const DistanceRequest &request)
Constructor from a DistanceRequest.
Definition: narrowphase.h:371
HPP_FCL_DECLARE_SHAPE_TRIANGLE
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)
Definition: narrowphase.h:668
hpp::fcl::GJKSolver::gjk_convergence_criterion_type
GJKConvergenceCriterionType gjk_convergence_criterion_type
Relative or absolute.
Definition: narrowphase.h:511
hpp::fcl::GJKSolver::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.
Definition: narrowphase.h:496
hpp::fcl::details::GJK::gjk_variant
GJKVariant gjk_variant
Definition: gjk.h:169
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
hpp::fcl::details::GJK::Status
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision....
Definition: gjk.h:165
hpp::fcl::CollisionRequest::security_margin
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.
Definition: collision_data.h:251
hpp::fcl::QueryRequest::gjk_convergence_criterion
GJKConvergenceCriterion gjk_convergence_criterion
convergence criterion used to stop GJK
Definition: collision_data.h:132
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
collision_data.h
HPP_FCL_COMPILER_DIAGNOSTIC_POP
#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
Definition: fwd.hh:83
hpp::fcl::QueryRequest::cached_gjk_guess
Vec3f cached_gjk_guess
the gjk initial guess set by user
Definition: collision_data.h:144
hpp::fcl::GJKSolver::epa_tolerance
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:485
hpp::fcl::details::EPA::FallBack
@ FallBack
Definition: gjk.h:376
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:186
hpp::fcl::QueryRequest::gjk_tolerance
FCL_REAL gjk_tolerance
tolerance for the GJK algorithm
Definition: collision_data.h:138
hpp::fcl::GJKSolver::Array2d
Eigen::Array< FCL_REAL, 1, 2 > Array2d
Definition: narrowphase.h:55
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:479
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
hpp::fcl::GJKSolver::GJKSolver
GJKSolver(const CollisionRequest &request)
Constructor from a CollisionRequest.
Definition: narrowphase.h:409
hpp::fcl::details::EPA::Valid
@ Valid
Definition: gjk.h:369
hpp::fcl::QueryRequest::gjk_variant
GJKVariant gjk_variant
whether to enable the Nesterov accleration of GJK
Definition: collision_data.h:129
hpp::fcl::GJKSolver::GJKSolver
GJKSolver()
Default constructor for GJK algorithm.
Definition: narrowphase.h:350
hpp::fcl::details::EPA::depth
FCL_REAL depth
Definition: gjk.h:382
hpp::fcl::QueryRequest::gjk_convergence_criterion_type
GJKConvergenceCriterionType gjk_convergence_criterion_type
convergence criterion used to stop GJK
Definition: collision_data.h:135
hpp::fcl::Relative
@ Relative
Definition: data_types.h:93
hpp::fcl::QueryRequest::enable_cached_gjk_guess
bool enable_cached_gjk_guess
whether enable gjk initial guess @Deprecated Use gjk_initial_guess instead
Definition: collision_data.h:126
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
hpp::fcl::details::GJK::convergence_criterion
GJKConvergenceCriterion convergence_criterion
Definition: gjk.h:170
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::GJKSolver::epa_max_iterations
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:482
hpp::fcl::details::GJK::Failed
@ Failed
Definition: gjk.h:165
hpp::fcl::GJKInitialGuess
GJKInitialGuess
Initial guess to use for the GJK algorithm DefaultGuess: Vec3f(1, 0, 0) CachedGuess: previous vector ...
Definition: data_types.h:80
hpp::fcl::QueryRequest::cached_support_func_guess
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Definition: collision_data.h:147
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:729
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::details::GJK::setDistanceEarlyBreak
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:245
hpp::fcl::GJKSolver::initialize_gjk
void initialize_gjk(details::GJK &gjk, const details::MinkowskiDiff &shape, const S1 &s1, const S2 &s2, Vec3f &guess, support_func_guess_t &support_hint) const
initialize GJK
Definition: narrowphase.h:59
hpp::fcl::DefaultGuess
@ DefaultGuess
Definition: data_types.h:80
hpp::fcl::QueryRequest::gjk_max_iterations
size_t gjk_max_iterations
maximum iteration for the GJK algorithm
Definition: collision_data.h:141
hpp::fcl::details::EPA::evaluate
Status evaluate(GJK &gjk, const Vec3f &guess)
hpp::fcl::GJKSolver::set
void set(const CollisionRequest &request)
setter from a CollisionRequest
Definition: narrowphase.h:427
HPP_FCL_THROW_PRETTY
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
hpp::fcl::details::EPA::normal
Vec3f normal
Definition: gjk.h:381
hpp::fcl::CollisionRequest::distance_upper_bound
FCL_REAL distance_upper_bound
Distance above which GJK solver makes an early stopping. GJK stops searching for the closest points w...
Definition: collision_data.h:263
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:71
hpp::fcl::details::EPA::OutOfFaces
@ OutOfFaces
Definition: gjk.h:374
hpp::fcl::details::GJK::convergence_criterion_type
GJKConvergenceCriterionType convergence_criterion_type
Definition: gjk.h:171
hpp::fcl::GJKSolver::operator==
bool operator==(const GJKSolver &other) const
Definition: narrowphase.h:453
hpp::fcl::details::MinkowskiDiff::oR1
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:75
hpp::fcl::GJKSolver::gjk_tolerance
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:488
hpp::fcl::GJKVariant
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
hpp::fcl::GJKSolver::support_func_cached_guess
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:514
hpp::fcl::details::EPA::OutOfVertices
@ OutOfVertices
Definition: gjk.h:375
hpp::fcl::GJKSolver::cached_guess
Vec3f cached_guess
smart guess
Definition: narrowphase.h:499
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:104
hpp::fcl::details::MinkowskiDiff::ot1
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:79
hpp::fcl::details::GJK::hasPenetrationInformation
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:230
hpp::fcl::Plane
Infinite plane.
Definition: geometric_shapes.h:810