39 #ifndef HPP_FCL_NARROWPHASE_H 40 #define HPP_FCL_NARROWPHASE_H 56 template<
typename S1,
typename S2>
60 bool enable_penetration,
67 shape.
set (&s1, &s2, tf1, tf2);
76 if (!enable_penetration && contact_points == NULL && normal == NULL)
81 if(normal) *normal = tf1.
getRotation() * (w0 - w1).normalized();
82 if(contact_points) *contact_points = tf1.
transform((w0 + w1) / 2);
93 distance_lower_bound = -epa.
depth;
98 distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
104 distance_lower_bound = gjk.
distance;
125 tf_1M2.transform (P1),
126 tf_1M2.transform (P2),
127 tf_1M2.transform (P3));
129 Vec3f guess(1, 0, 0);
133 shape.
set (&s, &tri);
146 normal = tf1.
getRotation() * (w1 - w0).normalized();
157 distance = -epa.
depth;
160 assert (distance <= 1e-6);
162 distance = -std::numeric_limits<FCL_REAL>::max();
180 assert (distance > 0);
183 assert (
false &&
"should not reach type part.");
190 template<
typename S1,
typename S2>
197 FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
199 Vec3f guess(1, 0, 0);
203 shape.
set (&s1, &s2, tf1, tf2);
212 assert (distance * distance < sqrt (eps));
217 normal =
Vec3f (0,0,0);
241 normal = (tf1.
getRotation() * (p2 - p1)).normalized();
254 assert (epa.
depth >= -eps);
255 distance = std::min (0., -epa.
depth);
262 distance = -std::numeric_limits<FCL_REAL>::max();
323 #pragma GCC diagnostic push 324 #pragma GCC diagnostic ignored "-Wc99-extensions" 330 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \ 334 bool GJKSolver::shapeIntersect<Shape1, Shape2> \ 335 (const Shape1& s1, const Transform3f& tf1, \ 336 const Shape2& s2, const Transform3f& tf2, \ 337 FCL_REAL& distance_lower_bound, bool enable_penetration, \ 338 Vec3f* contact_points, Vec3f* normal) const 339 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \ 340 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc) 341 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \ 342 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc); \ 343 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc) 350 #ifdef IS_DOXYGEN // for doxygen only 354 template<>
bool GJKSolver::shapeIntersect<Box, Box>
355 (
const Box& s1,
const Transform3f& tf1,
356 const Box& s2,
const Transform3f& tf2,
357 FCL_REAL& distance_lower_bound,
bool enable_penetration,
378 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT 379 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF 380 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR 389 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \ 392 template<> bool GJKSolver::shapeTriangleInteraction<Shape> \ 393 (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \ 394 const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, \ 395 Vec3f& p1, Vec3f& p2, Vec3f& normal) const 401 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE 410 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \ 414 bool GJKSolver::shapeDistance<Shape1, Shape2> \ 415 (const Shape1& s1, const Transform3f& tf1, \ 416 const Shape2& s2, const Transform3f& tf2, \ 417 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const 418 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc) \ 419 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc) 420 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc) \ 421 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc); \ 422 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc) 437 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE 438 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF 439 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR 442 #pragma GCC diagnostic pop
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:305
Status
Definition: gjk.h:312
Main namespace.
Definition: AABB.h:43
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:299
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:317
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:271
void set(const ShapeBase *shape0, const ShapeBase *shape1)
class for GJK algorithm
Definition: gjk.h:115
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,)
Minkowski difference class of two shapes.
Definition: gjk.h:61
class for EPA algorithm
Definition: gjk.h:251
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Vec3f getGuessFromSimplex() const
get the guess from current simplex
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.
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess)
GJK algorithm, given the initial value guess.
double FCL_REAL
Definition: data_types.h:68
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:302
Status
Definition: gjk.h:137
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:283
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,)
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:70
FCL_REAL depth
Definition: gjk.h:327
FCL_REAL distance
Definition: gjk.h:154
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:314
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:191
Vec3f cached_guess
smart guess
Definition: narrowphase.h:320
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:117
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
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,)
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,)
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:73
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:311
Vec3f normal
Definition: gjk.h:326
Vec3f getCachedGuess() const
Definition: narrowphase.h:293
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:288
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:195
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,)
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:308