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);
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 {
186 TriangleP tri(tf_1M2.transform(P1), tf_1M2.transform(P2),
187 tf_1M2.transform(P3));
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);
226 distance = -epa.
depth;
229 assert(distance <= 1e-6);
231 distance = -(std::numeric_limits<FCL_REAL>::max)();
250 assert(distance > 0);
253 assert(
false &&
"should not reach type part.");
254 throw std::logic_error(
"GJKSolver: should not reach this part.");
260 template <
typename S1,
typename S2>
268 shape.
set(&s1, &s2, tf1, tf2);
273 initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
277 enable_cached_guess) {
284 assert(distance * distance < sqrt(eps));
307 Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
323 epa_max_iterations, epa_tolerance);
331 assert(epa.
depth >= -eps);
332 distance = (std::min)(0., -epa.
depth);
338 distance = -(std::numeric_limits<FCL_REAL>::max)();
351 gjk_max_iterations = 128;
352 gjk_tolerance = 1e-6;
353 epa_max_face_num = 128;
354 epa_max_vertex_num = 64;
355 epa_max_iterations = 255;
356 epa_tolerance = 1e-6;
357 enable_cached_guess =
false;
358 cached_guess =
Vec3f(1, 0, 0);
359 support_func_cached_guess = support_func_guess_t::Zero();
360 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
372 cached_guess =
Vec3f(1, 0, 0);
373 support_func_cached_guess = support_func_guess_t::Zero();
374 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
377 epa_max_face_num = 128;
378 epa_max_vertex_num = 64;
379 epa_max_iterations = 255;
380 epa_tolerance = 1e-6;
390 gjk_initial_guess = request.gjk_initial_guess;
392 enable_cached_guess = request.enable_cached_gjk_guess;
393 gjk_variant = request.gjk_variant;
394 gjk_convergence_criterion = request.gjk_convergence_criterion;
395 gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
396 gjk_tolerance = request.gjk_tolerance;
397 gjk_max_iterations = request.gjk_max_iterations;
399 enable_cached_guess) {
400 cached_guess = request.cached_gjk_guess;
401 support_func_cached_guess = request.cached_support_func_guess;
410 cached_guess =
Vec3f(1, 0, 0);
411 support_func_cached_guess = support_func_guess_t::Zero();
412 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
415 epa_max_face_num = 128;
416 epa_max_vertex_num = 64;
417 epa_max_iterations = 255;
418 epa_tolerance = 1e-6;
428 gjk_initial_guess = request.gjk_initial_guess;
430 enable_cached_guess = request.enable_cached_gjk_guess;
431 gjk_variant = request.gjk_variant;
432 gjk_convergence_criterion = request.gjk_convergence_criterion;
433 gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
434 gjk_tolerance = request.gjk_tolerance;
435 gjk_max_iterations = request.gjk_max_iterations;
437 enable_cached_guess) {
438 cached_guess = request.cached_gjk_guess;
439 support_func_cached_guess = request.cached_support_func_guess;
444 distance_upper_bound = (std::max)(
445 0., (std::max)(request.distance_upper_bound, request.security_margin));
459 enable_cached_guess ==
468 gjk_convergence_criterion_type ==
495 HPP_FCL_DEPRECATED_MESSAGE(
"Use gjk_initial_guess instead")
496 mutable
bool enable_cached_guess;
523 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
527 HPP_FCL_DLLAPI
bool GJKSolver::shapeTriangleInteraction(
533 HPP_FCL_DLLAPI
bool GJKSolver::shapeTriangleInteraction(
539 HPP_FCL_DLLAPI
bool GJKSolver::shapeTriangleInteraction(
544 #define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2) \ 546 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2>( \ 547 const S1& s1, const Transform3f& tf1, const S2& s2, \ 548 const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \ 549 Vec3f* contact_points, Vec3f* normal) const 551 #define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \ 552 SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \ 553 SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1) 572 #undef SHAPE_INTERSECT_SPECIALIZATION 573 #undef SHAPE_INTERSECT_SPECIALIZATION_BASE 575 #define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2) \ 577 HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2>( \ 578 const S1& s1, const Transform3f& tf1, const S2& s2, \ 579 const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ 582 #define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \ 583 SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \ 584 SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1) 593 #undef SHAPE_DISTANCE_SPECIALIZATION 594 #undef SHAPE_DISTANCE_SPECIALIZATION_BASE 596 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) 597 #pragma GCC diagnostic push 598 #pragma GCC diagnostic ignored "-Wc99-extensions" 605 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc) \ 608 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2>( \ 609 const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ 610 const Transform3f& tf2, FCL_REAL& distance_lower_bound, \ 611 bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const 612 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \ 613 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc) 614 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \ 615 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc); \ 616 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc) 624 HPP_FCL_DLLAPI
bool GJKSolver::shapeIntersect<Box, Sphere>(
625 const Box& s1,
const Transform3f&
tf1,
const Sphere& s2,
627 bool enable_penetration,
Vec3f* contact_points,
Vec3f* normal)
const;
629 #ifdef IS_DOXYGEN // for doxygen only 634 HPP_FCL_DLLAPI
bool GJKSolver::shapeIntersect<Box, Box>(
635 const Box& s1,
const Transform3f&
tf1,
const Box& s2,
636 const Transform3f&
tf2,
FCL_REAL& distance_lower_bound,
637 bool enable_penetration,
Vec3f* contact_points,
Vec3f* normal)
const;
657 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT 658 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF 659 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR 668 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc) \ 671 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape>( \ 672 const Shape& s, const Transform3f& tf1, const Vec3f& P1, \ 673 const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, \ 674 FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const 680 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE 689 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc) \ 692 bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2>( \ 693 const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ 694 const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ 696 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \ 697 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc) 698 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \ 699 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc); \ 700 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc) 718 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE 719 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF 720 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR 723 #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) 724 #pragma GCC diagnostic pop GJKConvergenceCriterionType gjk_convergence_criterion_type
Relative or absolute.
GJKVariant gjk_variant
Variant to use for the GJK algorithm.
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
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Capsule)
request to the distance computation
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
support_func_guess_t support_hint
GJKVariant
Variant to use for the GJK algorithm.
GJKSolver(const DistanceRequest &request)
Constructor from a DistanceRequest.
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
GJKConvergenceCriterion gjk_convergence_criterion
Criterion used to stop GJK.
bool enable_cached_guess
Whether smart guess can be provided Use gjk_initial_guess instead.
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere, Sphere)
SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere, Sphere)
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
void set(const ShapeBase *shape0, const ShapeBase *shape1)
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,)
Minkowski difference class of two shapes.
#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Vec3f getGuessFromSimplex() const
get the guess from current simplex
request to the collision algorithm
HPP_FCL_DLLAPI 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.
Vec3f ot1
translation from shape1 to shape0 such that .
GJKInitialGuess
Initial guess to use for the GJK algorithm DefaultGuess: Vec3f(1, 0, 0) CachedGuess: previous vector ...
SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision. Inside: GJK converged and the shapes are in collision. Failed: GJK did not converge.
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,)
Triangle stores the points instead of only indices of points.
HPP_FCL_COMPILER_DIAGNOSTIC_POP bool operator!=(const GJKSolver &other) const
Eigen::Vector2i support_func_guess_t
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
Center at zero point sphere.
Vec3f cached_guess
smart guess
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
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool operator==(const GJKSolver &other) const
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
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,)
Status evaluate(GJK &gjk, const Vec3f &guess)
GJKConvergenceCriterion convergence_criterion
void setDistanceEarlyBreak(const FCL_REAL &dup)
Distance threshold for early break. GJK stops when it proved the distance is more than this threshold...
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,)
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS GJKSolver()
Default constructor for GJK algorithm.
#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)...
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Eigen::Array< FCL_REAL, 1, 2 > Array2d
GJKSolver(const CollisionRequest &request)
Constructor from a CollisionRequest.
support_func_guess_t support_func_cached_guess
smart guess for the support function
size_t gjk_max_iterations
maximum number of iterations used for GJK iterations
Matrix3f oR1
rotation from shape1 to shape0 such that .
bool hasPenetrationInformation(const MinkowskiDiff &shape)
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,)
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
GJKConvergenceCriterionType convergence_criterion_type
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
#define HPP_FCL_THROW_PRETTY(message, exception)
FCL_REAL distance_upper_bound
Distance above which the GJK solver stoppes its computations and processes to an early stopping...
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.