40 #ifndef HPP_FCL_NARROWPHASE_H 41 #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);
79 if(enable_cached_guess) {
87 if (!enable_penetration && contact_points == NULL && normal == NULL)
92 if(normal) *normal = tf1.
getRotation() * (w0 - w1).normalized();
93 if(contact_points) *contact_points = tf1.
transform((w0 + w1) / 2);
96 details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
104 distance_lower_bound = -epa.
depth;
109 distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)();
115 distance_lower_bound = gjk.
distance;
127 bool shapeTriangleInteraction
136 tf_1M2.transform (P1),
137 tf_1M2.transform (P2),
138 tf_1M2.transform (P3));
140 Vec3f guess(1, 0, 0);
142 if(enable_cached_guess) {
143 guess = cached_guess;
144 support_hint = support_func_cached_guess;
146 support_hint.setZero();
149 shape.
set (&s, &tri);
151 details::GJK gjk((
unsigned int )gjk_max_iterations, gjk_tolerance);
156 if(enable_cached_guess) {
168 normal = tf1.
getRotation() * (w0 - w1).normalized();
171 details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
179 distance = -epa.
depth;
182 assert (distance <= 1e-6);
184 distance = -(std::numeric_limits<FCL_REAL>::max)();
202 assert (distance > 0);
205 assert (
false &&
"should not reach type part.");
212 template<
typename S1,
typename S2>
219 FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
221 Vec3f guess(1, 0, 0);
223 if(enable_cached_guess) {
224 guess = cached_guess;
225 support_hint = support_func_cached_guess;
227 support_hint.setZero();
230 shape.
set (&s1, &s2, tf1, tf2);
232 details::GJK gjk((
unsigned int) gjk_max_iterations, gjk_tolerance);
237 if(enable_cached_guess) {
245 assert (distance * distance < sqrt (eps));
283 epa_max_iterations, epa_tolerance);
292 assert (epa.
depth >= -eps);
293 distance = (std::min) (0., -epa.
depth);
299 distance = -(std::numeric_limits<FCL_REAL>::max)();
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)();
325 enable_cached_guess = if_enable;
330 cached_guess = guess;
371 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
392 #define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2) \ 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, \ 399 Vec3f* contact_points, Vec3f* normal) const 401 #define SHAPE_INTERSECT_SPECIALIZATION(S1,S2) \ 402 SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2); \ 403 SHAPE_INTERSECT_SPECIALIZATION_BASE(S2,S1) 422 #undef SHAPE_INTERSECT_SPECIALIZATION 423 #undef SHAPE_INTERSECT_SPECIALIZATION_BASE 425 #define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2) \ 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 432 #define SHAPE_DISTANCE_SPECIALIZATION(S1,S2) \ 433 SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2); \ 434 SHAPE_DISTANCE_SPECIALIZATION_BASE(S2,S1) 443 #undef SHAPE_DISTANCE_SPECIALIZATION 444 #undef SHAPE_DISTANCE_SPECIALIZATION_BASE 446 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) 447 #pragma GCC diagnostic push 448 #pragma GCC diagnostic ignored "-Wc99-extensions" 455 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \ 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) 477 (
const Box& s1,
const Transform3f& tf1,
478 const Sphere& s2,
const Transform3f& tf2,
479 FCL_REAL& distance_lower_bound,
bool enable_penetration,
482 #ifdef IS_DOXYGEN // for doxygen only 488 (
const Box& s1,
const Transform3f& tf1,
489 const Box& s2,
const Transform3f& tf2,
490 FCL_REAL& distance_lower_bound,
bool enable_penetration,
511 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT 512 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF 513 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR 522 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,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 534 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE 543 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \ 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) 570 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE 571 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF 572 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR 575 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) 576 #pragma GCC diagnostic pop
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:345
support_func_guess_t support_hint
Definition: gjk.h:156
Status
Definition: gjk.h:338
Main namespace.
Definition: AABB.h:43
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:339
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:357
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc)
Definition: narrowphase.h:551
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:453
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:432
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:309
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Infinite plane.
Definition: geometric_shapes.h:523
class for GJK algorithm
Definition: gjk.h:130
Vec3f ray
Definition: gjk.h:155
Minkowski difference class of two shapes.
Definition: gjk.h:61
class for EPA algorithm
Definition: gjk.h:277
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.
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Vec3f getGuessFromSimplex() const
get the guess from current simplex
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:425
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.
double FCL_REAL
Definition: data_types.h:66
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:342
Status
Definition: gjk.h:152
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:323
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:75
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:71
FCL_REAL depth
Definition: gjk.h:353
FCL_REAL distance
Definition: gjk.h:170
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:392
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:354
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
Center at zero point sphere.
Definition: geometric_shapes.h:149
Vec3f cached_guess
smart guess
Definition: narrowphase.h:360
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
Status evaluate(GJK &gjk, const Vec3f &guess)
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
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc)
Definition: narrowphase.h:464
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
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:466
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:401
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:553
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:53
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:351
Vec3f normal
Definition: gjk.h:352
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:363
Vec3f getCachedGuess() const
Definition: narrowphase.h:333
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:328
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:220
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:348
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)
Definition: narrowphase.h:522
#define HPP_FCL_DLLAPI
Definition: config.hh:64
FCL_REAL distance_upper_bound
Distance above which the GJK solver stoppes its computations and processes to an early stopping...
Definition: narrowphase.h:367