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

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