hpp-fcl: include/hpp/fcl/narrowphase/narrowphase.h Source File

hpp-fcl 1.7.7 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
45 #include <hpp/fcl/narrowphase/gjk.h>
46
47 namespace hpp
48 {
49 namespace fcl
50 {
51
53 struct HPP_FCL_DLLAPI GJKSolver
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
71 details::MinkowskiDiff shape;
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) {
86 case details::GJK::Inside:
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
148 details::MinkowskiDiff shape;
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) {
163 case details::GJK::Inside:
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:
191 case details::GJK::Failed:
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
229 details::MinkowskiDiff shape;
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
309 GJKSolver()
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
333 Vec3f getCachedGuess() const
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
348 FCL_REAL epa_tolerance;
349
351 FCL_REAL gjk_tolerance;
352
354 FCL_REAL gjk_max_iterations;
355
357 mutable bool enable_cached_guess;
358
360 mutable Vec3f cached_guess;
361
363 mutable support_func_guess_t support_func_cached_guess;
364
367 mutable FCL_REAL distance_upper_bound;
368
369 public:
370
371 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
372 };
373
374 template<>
375 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction
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<>
381 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction
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<>
387 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction
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);
406 SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere,Sphere);
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
417 SHAPE_INTERSECT_SPECIALIZATION(Plane,Box);
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
452
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
470 HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,);
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
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,);
495 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane,);
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,);
504 HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane,);
505
506 HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace,);
507
508 HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane,);
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
530 HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,);
531 HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace,);
532 HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane,);
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
557 HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,);
558 HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule,);
559 HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder,);
560 HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,);
561
562 HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Capsule,
564 );
565
566 HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(TriangleP,
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
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:103
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:193
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:250
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:306
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:454
hpp::fcl::Plane
Infinite plane.
Definition: geometric_shapes.h:524
hpp::fcl::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:150
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:124
hpp::fcl::Transform3f::inverseTimes
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:193
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:173
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:76
gjk.h
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::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:71
hpp
Main namespace.
Definition: AABB.h:44
HPP_FCL_DECLARE_SHAPE_TRIANGLE
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)
Definition: narrowphase.h:522
SHAPE_INTERSECT_SPECIALIZATION
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:401
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:553
SHAPE_DISTANCE_SPECIALIZATION_BASE
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:425
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:466
SHAPE_INTERSECT_SPECIALIZATION_BASE
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:392
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc)
Definition: narrowphase.h:464
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc)
Definition: narrowphase.h:551
SHAPE_DISTANCE_SPECIALIZATION
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:432
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::GJKSolver::enableCachedGuess
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:323
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:339
hpp::fcl::GJKSolver::gjk_max_iterations
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:354
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:57
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:342
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:213
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:128
hpp::fcl::GJKSolver::epa_max_iterations
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:345
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:367
hpp::fcl::GJKSolver::getCachedGuess
Vec3f getCachedGuess() const
Definition: narrowphase.h:333
hpp::fcl::GJKSolver::gjk_tolerance
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:351
hpp::fcl::GJKSolver::cached_guess
Vec3f cached_guess
smart guess
Definition: narrowphase.h:360
hpp::fcl::GJKSolver::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:357
hpp::fcl::GJKSolver::epa_tolerance
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:348
hpp::fcl::GJKSolver::setCachedGuess
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:328
hpp::fcl::GJKSolver::GJKSolver
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:309
hpp::fcl::GJKSolver::support_func_cached_guess
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:363
hpp::fcl::details::EPA
class for EPA algorithm
Definition: gjk.h:278
hpp::fcl::details::EPA::normal
Vec3f normal
Definition: gjk.h:352
hpp::fcl::details::EPA::Status
Status
Definition: gjk.h:338
hpp::fcl::details::EPA::Valid
@ Valid
Definition: gjk.h:340
hpp::fcl::details::EPA::OutOfVertices
@ OutOfVertices
Definition: gjk.h:346
hpp::fcl::details::EPA::OutOfFaces
@ OutOfFaces
Definition: gjk.h:345
hpp::fcl::details::EPA::evaluate
Status evaluate(GJK &gjk, const Vec3f &guess)
hpp::fcl::details::EPA::depth
FCL_REAL depth
Definition: gjk.h:353
hpp::fcl::details::EPA::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:131
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:236
hpp::fcl::details::GJK::hasPenetrationInformation
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:220
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:156
hpp::fcl::details::GJK::Status
Status
Definition: gjk.h:152
hpp::fcl::details::GJK::Valid
@ Valid
Definition: gjk.h:152
hpp::fcl::details::GJK::Failed
@ Failed
Definition: gjk.h:152
hpp::fcl::details::GJK::Inside
@ Inside
Definition: gjk.h:152
hpp::fcl::details::GJK::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:170
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:155
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::details::GJK::getGuessFromSimplex
Vec3f getGuessFromSimplex() const
get the guess from current simplex
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:62
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)