60 typedef Eigen::Array<FCL_REAL, 1, 2>
Array2d;
98 const Vec3f& dir,
bool dirIsNormalized,
105 : linear_log_convex_threshold(32),
106 normalize_support_direction(false),
107 getSupportFunc(NULL) {}
119 return getSupport(shapes[0], d, dIsNormalized, hint);
125 getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) +
132 assert(getSupportFunc != NULL);
133 getSupportFunc(*
this, d, dIsNormalized, supp0, supp1, hint,
134 const_cast<ShapeData*>(data));
197 : max_iterations(max_iterations_), tolerance(tolerance_) {
212 shape->
support(d, dIsNormalized, sv.
w0, sv.
w1, hint);
213 sv.
w = sv.
w0 - sv.
w1;
217 bool encloseOrigin();
231 return distance > -shape.
inflation.sum();
239 Vec3f getGuessFromSimplex()
const;
246 distance_upper_bound = dup;
268 unsigned int max_iterations;
274 inline void removeVertex(
Simplex& simplex);
277 inline void appendVertex(
Simplex& simplex,
const Vec3f& v,
bool isNormalized,
299 bool projectTriangleOrigin(
const Simplex& current,
Simplex& next);
303 bool projectTetrahedraOrigin(
const Simplex& current,
Simplex& next);
306 static const size_t EPA_MAX_FACES = 128;
307 static const size_t EPA_MAX_VERTICES = 64;
308 static const FCL_REAL EPA_EPS = 0.000001;
309 static const size_t EPA_MAX_ITERATIONS = 255;
333 if (root) root->
l[0] = face;
339 if (face->l[1]) face->
l[1]->
l[0] = face->
l[0];
340 if (face->l[0]) face->
l[0]->
l[1] = face->
l[1];
341 if (face == root) root = face->
l[1];
361 unsigned int max_face_num;
362 unsigned int max_vertex_num;
363 unsigned int max_iterations;
370 AccuracyReached = 1 << 1 | Valid,
371 Degenerated = 1 << 1 | Failed,
372 NonConvex = 2 << 1 | Failed,
373 InvalidHull = 3 << 1 | Failed,
374 OutOfFaces = 4 << 1 | Failed,
375 OutOfVertices = 5 << 1 | Failed,
376 FallBack = 6 << 1 | Failed
388 EPA(
unsigned int max_face_num_,
unsigned int max_vertex_num_,
389 unsigned int max_iterations_,
FCL_REAL tolerance_)
390 : max_face_num(max_face_num_),
391 max_vertex_num(max_vertex_num_),
392 max_iterations(max_iterations_),
393 tolerance(tolerance_) {
416 SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex,
bool forced);
422 bool expand(
size_t pass, SimplexV* w,
SimplexF* f,
size_t e,
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:93
SimplexList stock
Definition: gjk.h:386
MinkowskiDiff const * shape
Definition: gjk.h:167
support_func_guess_t support_hint
Definition: gjk.h:172
GJK::Simplex result
Definition: gjk.h:380
Status
Definition: gjk.h:367
MinkowskiDiff()
Definition: gjk.h:104
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
std::vector< int8_t > visited
Definition: gjk.h:66
Main namespace.
Definition: broadphase_bruteforce.h:44
size_t pass
Definition: gjk.h:321
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:118
FCL_REAL getTolerance()
Get GJK tolerance.
Definition: gjk.h:258
Vec3f n
Definition: gjk.h:315
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Definition: traversal_node_setup.h:423
Simplex()
Definition: gjk.h:158
SimplexF * f[3]
Definition: gjk.h:318
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
size_t getIterations()
Get GJK number of iterations.
Definition: gjk.h:255
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:388
void append(SimplexF *face)
Definition: gjk.h:330
class for GJK algorithm
Definition: gjk.h:141
SimplexF * cf
Definition: gjk.h:354
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:123
Vec3f ray
Definition: gjk.h:168
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction) ...
Definition: gjk.h:147
Minkowski difference class of two shapes.
Definition: gjk.h:59
class for EPA algorithm
Definition: gjk.h:312
Eigen::Array< FCL_REAL, 1, 2 > Array2d
Definition: gjk.h:60
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:346
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:51
SimplexF * ff
Definition: gjk.h:355
Vec3f w1
Definition: gjk.h:144
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:196
SimplexHorizon()
Definition: gjk.h:357
double FCL_REAL
Definition: data_types.h:65
GetSupportFunction getSupportFunc
Definition: gjk.h:102
Definition: traversal_node_setup.h:792
size_t nextsv
Definition: gjk.h:385
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:79
SimplexF * root
Definition: gjk.h:327
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.
Definition: gjk.h:165
Array2d inflation
The radius of the sphere swepted volume. The 2 values correspond to the inflation of shape 0 and shap...
Definition: gjk.h:84
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk.h:221
GJKVariant gjk_variant
Definition: gjk.h:169
SimplexList()
Definition: gjk.h:329
bool normalize_support_direction
Wether or not to use the normalize heuristic in the GJK Nesterov acceleration. This setting is only a...
Definition: gjk.h:95
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
FCL_REAL depth
Definition: gjk.h:382
FCL_REAL distance
Definition: gjk.h:186
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:144
~EPA()
Definition: gjk.h:397
unsigned char vertex_id_t
Definition: gjk.h:150
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:130
FCL_REAL d
Definition: gjk.h:316
SimplexV * sv_store
Definition: gjk.h:383
size_t nf
Definition: gjk.h:356
SimplexF()
Definition: gjk.h:323
SimplexF * fc_store
Definition: gjk.h:384
GJKConvergenceCriterion convergence_criterion
Definition: gjk.h:170
size_t count
Definition: gjk.h:328
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
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
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:156
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:224
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)...
Definition: data_types.h:89
Vec3f normal
Definition: gjk.h:381
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:75
size_t e[3]
Definition: gjk.h:320
SimplexF * l[2]
Definition: gjk.h:319
Status status
Definition: gjk.h:379
GJK::SimplexV SimplexV
Definition: gjk.h:313
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:230
GJKConvergenceCriterionType convergence_criterion_type
Definition: gjk.h:171
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:210
#define HPP_FCL_DLLAPI
Definition: config.hh:64