93 const Vec3f& dir,
bool dirIsNormalized,
Vec3f& support0,
Vec3f& support1,
97 MinkowskiDiff() : linear_log_convex_threshold (32), getSupportFunc (NULL) {}
110 return getSupport(shapes[0], d, dIsNormalized, hint);
116 return oR1 *
getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1;
122 assert(getSupportFunc != NULL);
123 getSupportFunc(*
this, d, dIsNormalized, supp0, supp1, hint,
const_cast<ShapeData*
>(data));
181 GJK(
unsigned int max_iterations_,
FCL_REAL tolerance_) : max_iterations(max_iterations_),
182 tolerance(tolerance_)
197 shape->
support(d, dIsNormalized, sv.
w0, sv.
w1, hint);
198 sv.
w.noalias() = sv.
w0 - sv.
w1;
213 return distance < distance_upper_bound;
238 distance_upper_bound = dup;
249 unsigned int max_iterations;
254 inline void removeVertex(Simplex& simplex);
257 inline void appendVertex(Simplex& simplex,
const Vec3f& v,
bool isNormalized,
261 bool projectLineOrigin(
const Simplex& current, Simplex& next);
264 bool projectTriangleOrigin(
const Simplex& current, Simplex& next);
267 bool projectTetrahedraOrigin(
const Simplex& current, Simplex& next);
271static const size_t EPA_MAX_FACES = 128;
272static const size_t EPA_MAX_VERTICES = 64;
273static const FCL_REAL EPA_EPS = 0.000001;
274static const size_t EPA_MAX_ITERATIONS = 255;
302 if(root) root->
l[0] = face;
309 if(face->
l[1]) face->
l[1]->
l[0] = face->
l[0];
310 if(face->
l[0]) face->
l[0]->
l[1] = face->
l[1];
311 if(face == root) root = face->
l[1];
318 fa->
e[ea] = eb; fa->
f[ea] = fb;
319 fb->
e[eb] = ea; fb->
f[eb] = fa;
331 unsigned int max_face_num;
332 unsigned int max_vertex_num;
333 unsigned int max_iterations;
341 AccuracyReached = 1 << 1 | Valid ,
342 Degenerated = 1 << 1 | Failed,
343 NonConvex = 2 << 1 | Failed,
344 InvalidHull = 3 << 1 | Failed,
345 OutOfFaces = 4 << 1 | Failed,
346 OutOfVertices = 5 << 1 | Failed,
347 FallBack = 6 << 1 | Failed
359 EPA(
unsigned int max_face_num_,
unsigned int max_vertex_num_,
unsigned int max_iterations_,
FCL_REAL tolerance_) : max_face_num(max_face_num_),
360 max_vertex_num(max_vertex_num_),
361 max_iterations(max_iterations_),
362 tolerance(tolerance_)
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:55
#define HPP_FCL_DLLAPI
Definition: config.hh:64
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 getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
double FCL_REAL
Definition: data_types.h:66
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:73
Main namespace.
Definition: AABB.h:44
FCL_REAL d
Definition: gjk.h:283
Vec3f n
Definition: gjk.h:282
SimplexF * f[3]
Definition: gjk.h:285
size_t pass
Definition: gjk.h:288
SimplexF()
Definition: gjk.h:290
size_t e[3]
Definition: gjk.h:287
SimplexF * l[2]
Definition: gjk.h:286
SimplexF * cf
Definition: gjk.h:324
SimplexF * ff
Definition: gjk.h:325
size_t nf
Definition: gjk.h:326
SimplexHorizon()
Definition: gjk.h:327
SimplexList()
Definition: gjk.h:297
size_t count
Definition: gjk.h:296
void remove(SimplexF *face)
Definition: gjk.h:307
SimplexF * root
Definition: gjk.h:295
void append(SimplexF *face)
Definition: gjk.h:298
class for EPA algorithm
Definition: gjk.h:278
Status status
Definition: gjk.h:350
GJK::SimplexV SimplexV
Definition: gjk.h:279
SimplexF * fc_store
Definition: gjk.h:355
Vec3f normal
Definition: gjk.h:352
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:359
~EPA()
Definition: gjk.h:367
size_t nextsv
Definition: gjk.h:356
Status
Definition: gjk.h:338
Status evaluate(GJK &gjk, const Vec3f &guess)
FCL_REAL depth
Definition: gjk.h:353
SimplexList hull
Definition: gjk.h:357
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:316
GJK::Simplex result
Definition: gjk.h:351
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
SimplexV * sv_store
Definition: gjk.h:354
Vec3f w1
Definition: gjk.h:135
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:135
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction)
Definition: gjk.h:137
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:147
Simplex()
Definition: gjk.h:149
class for GJK algorithm
Definition: gjk.h:131
void getSupport(const Vec3f &d, bool dIsNormalized, SimplexV &sv, support_func_guess_t &hint) const
apply the support function along a direction, the result is return in sv
Definition: gjk.h:194
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
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:220
support_func_guess_t support_hint
Definition: gjk.h:156
MinkowskiDiff const * shape
Definition: gjk.h:154
Status
Definition: gjk.h:152
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:181
FCL_REAL distance
Definition: gjk.h:170
Vec3f ray
Definition: gjk.h:155
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.
unsigned char vertex_id_t
Definition: gjk.h:140
Vec3f getGuessFromSimplex() const
get the guess from current simplex
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk.h:205
bool encloseOrigin()
whether the simplex enclose the origin
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:211
std::vector< int8_t > visited
Definition: gjk.h:67
Minkowski difference class of two shapes.
Definition: gjk.h:62
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:114
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:108
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:80
void support(const Vec3f &d, bool dIsNormalized, Vec3f &supp0, Vec3f &supp1, support_func_guess_t &hint) const
support function for the pair of shapes
Definition: gjk.h:120
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:76
void set(const ShapeBase *shape0, const ShapeBase *shape1, const Transform3f &tf0, const Transform3f &tf1)
Set the two shapes, with a relative transformation.
GetSupportFunction getSupportFunc
Definition: gjk.h:95
int linear_log_convex_threshold
Number of points in a Convex object from which using a logarithmic support function is faster than a ...
Definition: gjk.h:90
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Eigen::Array< FCL_REAL, 1, 2 > inflation
The radius of the sphere swepted volume. The 2 values correspond to the inflation of shape 0 and shap...
Definition: gjk.h:84
MinkowskiDiff()
Definition: gjk.h:97