hpp-fcl
1.4.5
HPP fork of FCL -- The Flexible Collision Library
|
Go to the documentation of this file.
39 #ifndef HPP_FCL_NARROWPHASE_H
40 #define HPP_FCL_NARROWPHASE_H
56 template<
typename S1,
typename S2>
60 bool enable_penetration,
65 if(enable_cached_guess) {
67 support_hint = support_func_cached_guess;
69 support_hint.setZero();
72 shape.
set (&s1, &s2, tf1, tf2);
74 details::GJK gjk((
unsigned int )gjk_max_iterations, gjk_tolerance);
76 if(enable_cached_guess) {
84 if (!enable_penetration && contact_points == NULL && normal == NULL)
89 if(normal) *normal = tf1.
getRotation() * (w0 - w1).normalized();
90 if(contact_points) *contact_points = tf1.
transform((w0 + w1) / 2);
93 details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
101 distance_lower_bound = -epa.
depth;
106 distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
112 distance_lower_bound = gjk.
distance;
124 bool shapeTriangleInteraction
137 Vec3f guess(1, 0, 0);
139 if(enable_cached_guess) {
140 guess = cached_guess;
141 support_hint = support_func_cached_guess;
143 support_hint.setZero();
146 shape.
set (&s, &tri);
148 details::GJK gjk((
unsigned int )gjk_max_iterations, gjk_tolerance);
150 if(enable_cached_guess) {
162 normal = tf1.
getRotation() * (w1 - w0).normalized();
165 details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
178 distance = -std::numeric_limits<FCL_REAL>::max();
199 assert (
false &&
"should not reach type part.");
206 template<
typename S1,
typename S2>
213 FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
215 Vec3f guess(1, 0, 0);
217 if(enable_cached_guess) {
218 guess = cached_guess;
219 support_hint = support_func_cached_guess;
221 support_hint.setZero();
224 shape.
set (&s1, &s2, tf1, tf2);
226 details::GJK gjk((
unsigned int) gjk_max_iterations, gjk_tolerance);
228 if(enable_cached_guess) {
241 normal =
Vec3f (0,0,0);
266 normal = (tf1.
getRotation() * (p2 - p1)).normalized();
270 epa_max_iterations, epa_tolerance);
279 assert (epa.
depth >= -eps);
287 distance = -std::numeric_limits<FCL_REAL>::max();
298 gjk_max_iterations = 128;
299 gjk_tolerance = 1e-6;
300 epa_max_face_num = 128;
301 epa_max_vertex_num = 64;
302 epa_max_iterations = 255;
303 epa_tolerance = 1e-6;
304 enable_cached_guess =
false;
305 cached_guess =
Vec3f(1, 0, 0);
306 support_func_cached_guess = support_func_guess_t::Zero();
311 enable_cached_guess = if_enable;
316 cached_guess = guess;
352 #if __cplusplus < 201103L
353 #pragma GCC diagnostic push
354 #pragma GCC diagnostic ignored "-Wc99-extensions"
361 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \
365 bool GJKSolver::shapeIntersect<Shape1, Shape2> \
366 (const Shape1& s1, const Transform3f& tf1, \
367 const Shape2& s2, const Transform3f& tf2, \
368 FCL_REAL& distance_lower_bound, bool enable_penetration, \
369 Vec3f* contact_points, Vec3f* normal) const
370 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
371 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
372 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
373 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc); \
374 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc)
381 #ifdef IS_DOXYGEN // for doxygen only
385 template<>
bool GJKSolver::shapeIntersect<Box, Box>
386 (
const Box& s1,
const Transform3f& tf1,
387 const Box& s2,
const Transform3f& tf2,
388 FCL_REAL& distance_lower_bound,
bool enable_penetration,
409 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT
410 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
411 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
420 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \
423 template<> bool GJKSolver::shapeTriangleInteraction<Shape> \
424 (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \
425 const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, \
426 Vec3f& p1, Vec3f& p2, Vec3f& normal) const
432 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
441 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \
445 bool GJKSolver::shapeDistance<Shape1, Shape2> \
446 (const Shape1& s1, const Transform3f& tf1, \
447 const Shape2& s2, const Transform3f& tf2, \
448 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
449 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc) \
450 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc)
451 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc) \
452 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc); \
453 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc)
468 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE
469 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
470 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
473 #if __cplusplus < 201103L
474 #pragma GCC diagnostic pop
#define HPP_FCL_DLLAPI
Definition: config.hh:64
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,)
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:74
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,)
class for GJK algorithm
Definition: gjk.h:117
support_func_guess_t support_hint
Definition: gjk.h:143
class for EPA algorithm
Definition: gjk.h:257
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,)
Status
Definition: gjk.h:318
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,)
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Vec3f ray
Definition: gjk.h:142
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:340
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
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:207
Minkowski difference class of two shapes.
Definition: gjk.h:62
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.
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:325
double FCL_REAL
Definition: data_types.h:69
@ Inside
Definition: gjk.h:139
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Vec3f getGuessFromSimplex() const
get the guess from current simplex
@ Valid
Definition: gjk.h:139
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:343
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:53
Status
Definition: gjk.h:139
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:76
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:314
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:334
Main namespace.
Definition: AABB.h:43
FCL_REAL distance
Definition: gjk.h:157
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:328
@ Valid
Definition: gjk.h:320
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:296
FCL_REAL depth
Definition: gjk.h:333
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:331
@ Failed
Definition: gjk.h:139
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,)
Status evaluate(GJK &gjk, const Vec3f &guess)
Vec3f normal
Definition: gjk.h:332
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:70
@ OutOfFaces
Definition: gjk.h:325
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:309
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:337
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:349
@ OutOfVertices
Definition: gjk.h:326
Vec3f cached_guess
smart guess
Definition: narrowphase.h:346
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
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:200
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL) HPP_FCL_DLLAPI
Approximate distance between two kIOS bounding volumes.
Vec3f getCachedGuess() const
Definition: narrowphase.h:319