Go to the documentation of this file.
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;
95 gjk.setDistanceEarlyBreak(distance_upper_bound);
97 gjk.gjk_variant = gjk_variant;
98 gjk.convergence_criterion = gjk_convergence_criterion;
99 gjk.convergence_criterion_type = gjk_convergence_criterion_type;
103 template <
typename S1,
typename S2>
106 bool enable_penetration,
Vec3f* contact_points,
107 Vec3f* normal)
const {
114 initialize_gjk(
gjk, shape, s1, s2, guess, support_hint);
120 enable_cached_guess) {
121 cached_guess =
gjk.getGuessFromSimplex();
122 support_func_cached_guess =
gjk.support_hint;
127 switch (gjk_status) {
129 if (!enable_penetration && contact_points == NULL && normal == NULL)
131 if (
gjk.hasPenetrationInformation(shape)) {
132 gjk.getClosestPoints(shape, w0, w1);
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);
148 if (normal) (*normal).noalias() =
tf1.getRotation() * epa.
normal;
156 if (normal) (*normal).noalias() =
tf1.getRotation() * epa.
normal;
157 if (contact_points) *contact_points =
tf1.transform(w0);
177 template <
typename S>
182 Vec3f& normal)
const {
195 initialize_gjk(
gjk, shape, s, tri, guess, support_hint);
202 enable_cached_guess) {
203 cached_guess =
gjk.getGuessFromSimplex();
204 support_func_cached_guess =
gjk.support_hint;
209 switch (gjk_status) {
212 if (
gjk.hasPenetrationInformation(shape)) {
213 gjk.getClosestPoints(shape, w0, w1);
215 normal.noalias() =
tf1.getRotation() * (w0 - w1).normalized();
216 p1 = p2 =
tf1.transform((w0 + w1) / 2);
219 epa_max_iterations, epa_tolerance);
227 normal.noalias() =
tf1.getRotation() * epa.
normal;
231 distance = -(std::numeric_limits<FCL_REAL>::max)();
232 gjk.getClosestPoints(shape, w0, w1);
233 p1 = p2 =
tf1.transform(w0);
242 gjk.getClosestPoints(shape,
p1, p2);
249 p2 =
tf1.transform(p2);
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>
273 initialize_gjk(
gjk, shape, s1, s2, guess, support_hint);
277 enable_cached_guess) {
278 cached_guess =
gjk.getGuessFromSimplex();
279 support_func_cached_guess =
gjk.support_hint;
286 gjk.getClosestPoints(shape, w0, w1);
288 p1 =
tf1.transform(w0);
289 p2 =
tf1.transform(w1);
293 gjk.getClosestPoints(shape,
p1, p2);
299 normal.noalias() =
tf1.getRotation() *
gjk.ray;
302 p2 =
tf1.transform(p2);
307 Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
311 if (
gjk.hasPenetrationInformation(shape)) {
312 gjk.getClosestPoints(shape,
p1, p2);
317 normal.noalias() =
tf1.getRotation() * (
p1 - p2);
320 p2 =
tf1.transform(p2);
323 epa_max_iterations, epa_tolerance);
333 normal.noalias() =
tf1.getRotation() * epa.
normal;
334 p1 =
tf1.transform(w0);
335 p2 =
tf1.transform(w1);
338 distance = -(std::numeric_limits<FCL_REAL>::max)();
339 gjk.getClosestPoints(shape,
p1, p2);
341 p2 =
tf1.transform(p2);
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;
399 enable_cached_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;
437 enable_cached_guess) {
444 distance_upper_bound = (std::max)(
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,
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
GJKConvergenceCriterion gjk_convergence_criterion
Criterion used to stop GJK.
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,)
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,)
SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere, Sphere)
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Capsule)
#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
size_t gjk_max_iterations
maximum number of iterations used for GJK iterations
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,)
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,)
FCL_REAL distance_upper_bound
Distance above which the GJK solver stoppes its computations and processes to an early stopping....
Center at zero point sphere.
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
void set(const ShapeBase *shape0, const ShapeBase *shape1)
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
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
GJKInitialGuess gjk_initial_guess
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.
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
Minkowski difference class of two shapes.
GJKVariant gjk_variant
Variant to use for the GJK algorithm.
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool operator==(const GJKSolver &other) const
void set(const DistanceRequest &request)
setter from a DistanceRequest
GJKSolver(const DistanceRequest &request)
Constructor from a DistanceRequest.
GJKConvergenceCriterionType gjk_convergence_criterion_type
Relative or absolute.
bool enable_cached_guess
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision....
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.
GJKConvergenceCriterion gjk_convergence_criterion
convergence criterion used to stop GJK
Eigen::Vector2i support_func_guess_t
Vec3f cached_gjk_guess
the gjk initial guess set by user
#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS GJKSolver()
Default constructor for GJK algorithm.
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
request to the collision algorithm
FCL_REAL gjk_tolerance
tolerance for the GJK algorithm
Eigen::Array< FCL_REAL, 1, 2 > Array2d
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
GJKSolver(const CollisionRequest &request)
Constructor from a CollisionRequest.
GJKVariant gjk_variant
whether to enable the Nesterov accleration of GJK
GJKConvergenceCriterionType gjk_convergence_criterion_type
convergence criterion used to stop GJK
SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
#define HPP_FCL_THROW_PRETTY(message, exception)
bool enable_cached_gjk_guess
whether enable gjk initial guess @Deprecated Use gjk_initial_guess instead
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
GJKInitialGuess
Initial guess to use for the GJK algorithm DefaultGuess: Vec3f(1, 0, 0) CachedGuess: previous vector ...
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
request to the distance computation
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
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,)
size_t gjk_max_iterations
maximum iteration for the GJK algorithm
void set(const CollisionRequest &request)
setter from a CollisionRequest
Status evaluate(GJK &gjk, const Vec3f &guess)
#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere, Sphere)
FCL_REAL distance_upper_bound
Distance above which GJK solver makes an early stopping. GJK stops searching for the closest points w...
Triangle stores the points instead of only indices of points.
Matrix3f oR1
rotation from shape1 to shape0 such that .
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
GJKVariant
Variant to use for the GJK algorithm.
support_func_guess_t support_func_cached_guess
smart guess for the support function
Vec3f cached_guess
smart 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
Vec3f ot1
translation from shape1 to shape0 such that .
HPP_FCL_COMPILER_DIAGNOSTIC_POP bool operator!=(const GJKSolver &other) const
hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14