40 #ifndef HPP_FCL_NARROWPHASE_H
41 #define HPP_FCL_NARROWPHASE_H
55 typedef Eigen::Array<FCL_REAL, 1, 2>
Array2d;
58 template <
typename S1,
typename S2>
60 const S1& s1,
const S2& s2,
Vec3f& guess,
62 switch (gjk_initial_guess) {
64 guess =
Vec3f(1, 0, 0);
65 support_hint.setZero();
69 support_hint = support_func_cached_guess;
72 if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
74 "computeLocalAABB must have been called on the shapes before "
76 "GJKInitialGuess::BoundingVolumeGuess.",
79 guess.noalias() = s1.aabb_local.center() -
80 (shape.
oR1 * s2.aabb_local.center() + shape.
ot1);
81 support_hint.setZero();
89 if (enable_cached_guess) {
91 support_hint = support_func_cached_guess;
103 template <
typename S1,
typename S2>
106 bool enable_penetration,
Vec3f* contact_points,
107 Vec3f* normal)
const {
109 shape.
set(&s1, &s2, tf1, tf2);
113 details::GJK gjk((
unsigned int)gjk_max_iterations, gjk_tolerance);
114 initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
120 enable_cached_guess) {
127 switch (gjk_status) {
129 if (!enable_penetration && contact_points == NULL && normal == NULL)
133 distance_lower_bound = gjk.
distance;
135 (*normal).noalias() = tf1.
getRotation() * (w0 - w1).normalized();
136 if (contact_points) *contact_points = tf1.
transform((w0 + w1) / 2);
140 epa_max_iterations, epa_tolerance);
147 distance_lower_bound = -epa.
depth;
155 distance_lower_bound = -epa.
depth;
157 if (contact_points) *contact_points = tf1.
transform(w0);
160 distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)();
166 distance_lower_bound = gjk.
distance;
177 template <
typename S>
182 Vec3f& normal)
const {
194 details::GJK gjk((
unsigned int)gjk_max_iterations, gjk_tolerance);
195 initialize_gjk(gjk, shape, s, tri, guess, support_hint);
202 enable_cached_guess) {
209 switch (gjk_status) {
215 normal.noalias() = tf1.
getRotation() * (w0 - w1).normalized();
219 epa_max_iterations, epa_tolerance);
231 distance = -(std::numeric_limits<FCL_REAL>::max)();
252 assert(
false &&
"should not reach type part.");
259 template <
typename S1,
typename S2>
264 FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
267 shape.
set(&s1, &s2, tf1, tf2);
271 details::GJK gjk((
unsigned int)gjk_max_iterations, gjk_tolerance);
272 initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
276 enable_cached_guess) {
317 epa_max_iterations, epa_tolerance);
325 assert(epa.
depth >= -eps);
332 distance = -(std::numeric_limits<FCL_REAL>::max)();
345 gjk_max_iterations = 128;
346 gjk_tolerance = 1e-6;
347 epa_max_face_num = 128;
348 epa_max_vertex_num = 64;
349 epa_max_iterations = 255;
350 epa_tolerance = 1e-6;
351 enable_cached_guess =
false;
352 cached_guess =
Vec3f(1, 0, 0);
353 support_func_cached_guess = support_func_guess_t::Zero();
354 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
366 cached_guess =
Vec3f(1, 0, 0);
367 support_func_cached_guess = support_func_guess_t::Zero();
368 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
371 epa_max_face_num = 128;
372 epa_max_vertex_num = 64;
373 epa_max_iterations = 255;
374 epa_tolerance = 1e-6;
393 enable_cached_guess) {
404 cached_guess =
Vec3f(1, 0, 0);
405 support_func_cached_guess = support_func_guess_t::Zero();
406 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
409 epa_max_face_num = 128;
410 epa_max_vertex_num = 64;
411 epa_max_iterations = 255;
412 epa_tolerance = 1e-6;
431 enable_cached_guess) {
438 distance_upper_bound = (std::max)(
453 enable_cached_guess ==
462 gjk_convergence_criterion_type ==
490 mutable
bool enable_cached_guess;
517 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
538 #define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2) \
540 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2>( \
541 const S1& s1, const Transform3f& tf1, const S2& s2, \
542 const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \
543 Vec3f* contact_points, Vec3f* normal) const
545 #define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \
546 SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \
547 SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1)
566 #undef SHAPE_INTERSECT_SPECIALIZATION
567 #undef SHAPE_INTERSECT_SPECIALIZATION_BASE
569 #define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2) \
571 HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2>( \
572 const S1& s1, const Transform3f& tf1, const S2& s2, \
573 const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
576 #define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \
577 SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \
578 SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1)
587 #undef SHAPE_DISTANCE_SPECIALIZATION
588 #undef SHAPE_DISTANCE_SPECIALIZATION_BASE
590 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
591 #pragma GCC diagnostic push
592 #pragma GCC diagnostic ignored "-Wc99-extensions"
599 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc) \
602 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2>( \
603 const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
604 const Transform3f& tf2, FCL_REAL& distance_lower_bound, \
605 bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const
606 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \
607 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc)
608 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \
609 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc); \
610 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc)
621 bool enable_penetration,
Vec3f* contact_points,
Vec3f* normal)
const;
631 bool enable_penetration,
Vec3f* contact_points,
Vec3f* normal)
const;
651 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT
652 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
653 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
662 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc) \
665 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape>( \
666 const Shape& s, const Transform3f& tf1, const Vec3f& P1, \
667 const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, \
668 FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
674 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
683 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc) \
686 bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2>( \
687 const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
688 const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
690 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \
691 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc)
692 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \
693 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc); \
694 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc)
712 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE
713 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
714 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
717 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
718 #pragma GCC diagnostic pop
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:125
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:333
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:414
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:501
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
Infinite plane.
Definition: geometric_shapes.h:810
Center at zero point sphere.
Definition: geometric_shapes.h:196
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:71
#define HPP_FCL_DLLAPI
Definition: config.hh:64
#define HPP_FCL_DEPRECATED_MESSAGE(message)
Definition: deprecated.hh:38
#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: fwd.hh:84
#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
Definition: fwd.hh:82
#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
Definition: fwd.hh:83
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
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.
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
@ DefaultGJK
Definition: data_types.h:83
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:93
@ Relative
Definition: data_types.h:93
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
Definition: data_types.h:89
@ VDB
Definition: data_types.h:89
GJKInitialGuess
Initial guess to use for the GJK algorithm DefaultGuess: Vec3f(1, 0, 0) CachedGuess: previous vector ...
Definition: data_types.h:80
@ BoundingVolumeGuess
Definition: data_types.h:80
@ CachedGuess
Definition: data_types.h:80
@ DefaultGuess
Definition: data_types.h:80
double FCL_REAL
Definition: data_types.h:65
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
Main namespace.
Definition: broadphase_bruteforce.h:44
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)
Definition: narrowphase.h:662
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:545
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:692
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:569
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:608
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:538
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc)
Definition: narrowphase.h:606
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc)
Definition: narrowphase.h:690
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:576
request to the collision algorithm
Definition: collision_data.h:235
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
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.
Definition: collision_data.h:251
request to the distance computation
Definition: collision_data.h:392
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:470
GJKConvergenceCriterion gjk_convergence_criterion
Criterion used to stop GJK.
Definition: narrowphase.h:502
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
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:473
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:260
void set(const DistanceRequest &request)
setter from a DistanceRequest
Definition: narrowphase.h:383
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
GJKVariant gjk_variant
Variant to use for the GJK algorithm.
Definition: narrowphase.h:499
GJKSolver(const GJKSolver &other)=default
Copy constructor.
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
Definition: narrowphase.h:496
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:476
Eigen::Array< FCL_REAL, 1, 2 > Array2d
Definition: narrowphase.h:55
bool operator!=(const GJKSolver &other) const
Definition: narrowphase.h:467
bool operator==(const GJKSolver &other) const
Definition: narrowphase.h:447
FCL_REAL distance_upper_bound
Distance above which the GJK solver stoppes its computations and processes to an early stopping....
Definition: narrowphase.h:514
GJKSolver(const CollisionRequest &request)
Constructor from a CollisionRequest.
Definition: narrowphase.h:403
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:482
Vec3f cached_guess
smart guess
Definition: narrowphase.h:493
GJKConvergenceCriterionType gjk_convergence_criterion_type
Relative or absolute.
Definition: narrowphase.h:505
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
bool enable_cached_guess
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.
Definition: narrowphase.h:490
GJKSolver(const DistanceRequest &request)
Constructor from a DistanceRequest.
Definition: narrowphase.h:365
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:479
GJKSolver()
Default constructor for GJK algorithm.
Definition: narrowphase.h:344
void set(const CollisionRequest &request)
setter from a CollisionRequest
Definition: narrowphase.h:421
size_t gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:485
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:508
GJKVariant gjk_variant
whether to enable the Nesterov accleration of GJK
Definition: collision_data.h:129
Vec3f cached_gjk_guess
the gjk initial guess set by user
Definition: collision_data.h:144
GJKInitialGuess gjk_initial_guess
Definition: collision_data.h:121
GJKConvergenceCriterion gjk_convergence_criterion
convergence criterion used to stop GJK
Definition: collision_data.h:132
size_t gjk_max_iterations
maximum iteration for the GJK algorithm
Definition: collision_data.h:141
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Definition: collision_data.h:147
FCL_REAL gjk_tolerance
tolerance for the GJK algorithm
Definition: collision_data.h:138
bool enable_cached_gjk_guess
whether enable gjk initial guess @Deprecated Use gjk_initial_guess instead
Definition: collision_data.h:126
GJKConvergenceCriterionType gjk_convergence_criterion_type
convergence criterion used to stop GJK
Definition: collision_data.h:135
class for EPA algorithm
Definition: gjk.h:312
Vec3f normal
Definition: gjk.h:381
Status
Definition: gjk.h:367
@ Valid
Definition: gjk.h:369
@ FallBack
Definition: gjk.h:376
@ OutOfVertices
Definition: gjk.h:375
@ OutOfFaces
Definition: gjk.h:374
Status evaluate(GJK &gjk, const Vec3f &guess)
FCL_REAL depth
Definition: gjk.h:382
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
class for GJK algorithm
Definition: gjk.h:141
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
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:230
support_func_guess_t support_hint
Definition: gjk.h:172
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision....
Definition: gjk.h:165
@ Valid
Definition: gjk.h:165
@ Failed
Definition: gjk.h:165
@ Inside
Definition: gjk.h:165
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKConvergenceCriterion convergence_criterion
Definition: gjk.h:170
GJKConvergenceCriterionType convergence_criterion_type
Definition: gjk.h:171
FCL_REAL distance
Definition: gjk.h:186
Vec3f ray
Definition: gjk.h:168
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.
GJKVariant gjk_variant
Definition: gjk.h:169
Vec3f getGuessFromSimplex() const
get the guess from current simplex
Minkowski difference class of two shapes.
Definition: gjk.h:59
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:79
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:75
void set(const ShapeBase *shape0, const ShapeBase *shape1)