hpp-fcl
1.4.4
HPP fork of FCL -- The Flexible Collision Library
|
Go to the documentation of this file.
158 GJK(
unsigned int max_iterations_,
FCL_REAL tolerance_) : max_iterations(max_iterations_),
159 tolerance(tolerance_)
173 sv.
w.noalias() = sv.
w0 - sv.
w1;
188 return distance < distance_upper_bound;
213 distance_upper_bound = dup;
224 unsigned int max_iterations;
229 inline void removeVertex(Simplex& simplex);
232 inline void appendVertex(Simplex& simplex,
const Vec3f& v,
bool isNormalized =
false);
235 bool projectLineOrigin(
const Simplex& current, Simplex& next);
238 bool projectTriangleOrigin(
const Simplex& current, Simplex& next);
241 bool projectTetrahedraOrigin(
const Simplex& current, Simplex& next);
245 static const size_t EPA_MAX_FACES = 128;
246 static const size_t EPA_MAX_VERTICES = 64;
247 static const FCL_REAL EPA_EPS = 0.000001;
248 static const size_t EPA_MAX_ITERATIONS = 255;
283 if(face->
l[1]) face->
l[1]->
l[0] = face->
l[0];
284 if(face->
l[0]) face->
l[0]->
l[1] = face->
l[1];
292 fa->
e[ea] = eb; fa->
f[ea] = fb;
293 fb->
e[eb] = ea; fb->
f[eb] = fa;
305 unsigned int max_face_num;
306 unsigned int max_vertex_num;
307 unsigned int max_iterations;
333 EPA(
unsigned int max_face_num_,
unsigned int max_vertex_num_,
unsigned int max_iterations_,
FCL_REAL tolerance_) : max_face_num(max_face_num_),
334 max_vertex_num(max_vertex_num_),
335 max_iterations(max_iterations_),
336 tolerance(tolerance_)
364 SimplexF* findBest();
367 bool expand(
size_t pass,
SimplexV* w, SimplexF* f,
size_t e, SimplexHorizon& horizon);
MinkowskiDiff const * shape
Definition: gjk.h:139
SimplexV * vertex[3]
Definition: gjk.h:258
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized)
the support function for shape
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
Vec3f support0(const Vec3f &d, bool dIsNormalized) const
support function for shape0
Definition: gjk.h:93
class for GJK algorithm
Definition: gjk.h:115
SimplexV * vertex[4]
simplex vertex
Definition: gjk.h:130
bool encloseOrigin()
whether the simplex enclose the origin
size_t pass
Definition: gjk.h:262
class for EPA algorithm
Definition: gjk.h:251
Vec3f n
Definition: gjk.h:256
GJK::Simplex result
Definition: gjk.h:325
Status
Definition: gjk.h:312
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:333
void support(const Vec3f &d, bool dIsNormalized, Vec3f &supp0, Vec3f &supp1) const
support function for the pair of shapes
Definition: gjk.h:105
void set(const ShapeBase *shape0, const ShapeBase *shape1)
SimplexF * cf
Definition: gjk.h:298
SimplexF * f[3]
Definition: gjk.h:259
Vec3f ray
Definition: gjk.h:140
Simplex()
Definition: gjk.h:134
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction)
Definition: gjk.h:122
SimplexF * root
Definition: gjk.h:269
@ NonConvex
Definition: gjk.h:317
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:158
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess)
GJK algorithm, given the initial value guess.
MinkowskiDiff()
Definition: gjk.h:82
Minkowski difference class of two shapes.
Definition: gjk.h:61
void getSupport(const Vec3f &d, bool dIsNormalized, SimplexV &sv) const
apply the support function along a direction, the result is return in sv
Definition: gjk.h:170
double FCL_REAL
Definition: data_types.h:68
@ Inside
Definition: gjk.h:137
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
size_t count
Definition: gjk.h:270
Vec3f getGuessFromSimplex() const
get the guess from current simplex
Definition: traversal_node_setup.h:775
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:290
@ Valid
Definition: gjk.h:137
@ Failed
Definition: gjk.h:313
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk.h:180
Status
Definition: gjk.h:137
SimplexHorizon()
Definition: gjk.h:301
SimplexF * ff
Definition: gjk.h:299
Vec3f w1
Definition: gjk.h:120
void append(SimplexF *face)
Definition: gjk.h:272
@ FallBack
Definition: gjk.h:321
Main namespace.
Definition: AABB.h:43
FCL_REAL distance
Definition: gjk.h:154
size_t nextsv
Definition: gjk.h:330
GetSupportFunction getSupportFunc
Definition: gjk.h:80
void remove(SimplexF *face)
Definition: gjk.h:281
@ Valid
Definition: gjk.h:314
FCL_REAL depth
Definition: gjk.h:327
FCL_REAL d
Definition: gjk.h:257
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:76
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:120
~EPA()
Definition: gjk.h:341
SimplexList hull
Definition: gjk.h:331
SimplexF * fc_store
Definition: gjk.h:329
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
Simplex simplices[2]
Definition: gjk.h:155
SimplexV * sv_store
Definition: gjk.h:328
size_t nf
Definition: gjk.h:300
unsigned char vertex_id_t
Definition: gjk.h:125
SimplexF()
Definition: gjk.h:264
@ Failed
Definition: gjk.h:137
@ InvalidHull
Definition: gjk.h:318
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:211
const ShapeBase * shapes[2]
points to two shapes
Definition: gjk.h:64
@ AccuracyReached
Definition: gjk.h:315
Vec3f support1(const Vec3f &d, bool dIsNormalized) const
support function for shape1
Definition: gjk.h:99
@ Degenerated
Definition: gjk.h:316
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:132
Status evaluate(GJK &gjk, const Vec3f &guess)
Vec3f normal
Definition: gjk.h:326
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:186
size_t e[3]
Definition: gjk.h:261
@ OutOfFaces
Definition: gjk.h:319
SimplexList()
Definition: gjk.h:271
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:54
void(* GetSupportFunction)(const MinkowskiDiff &minkowskiDiff, const Vec3f &dir, bool dirIsNormalized, Vec3f &support0, Vec3f &support1)
Definition: gjk.h:78
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:68
SimplexF * l[2]
Definition: gjk.h:260
@ OutOfVertices
Definition: gjk.h:320
GJK::SimplexV SimplexV
Definition: gjk.h:253
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:72
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:195
SimplexList stock
Definition: gjk.h:331
Status status
Definition: gjk.h:324