hpp-fcl 2.2.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
gjk.h
Go to the documentation of this file.
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
38#ifndef HPP_FCL_GJK_H
39#define HPP_FCL_GJK_H
40
41#include <vector>
42
45
46namespace hpp {
47namespace fcl {
48
49namespace details {
50
53Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
54 int& hint);
55
60 typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;
61
63 const ShapeBase* shapes[2];
64
65 struct ShapeData {
66 std::vector<int8_t> visited;
67 };
68
71 ShapeData data[2];
72
76
80
85
91
96
97 typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
98 const Vec3f& dir, bool dirIsNormalized,
99 Vec3f& support0, Vec3f& support1,
101 ShapeData data[2]);
102 GetSupportFunction getSupportFunc;
103
105 : linear_log_convex_threshold(32),
106 normalize_support_direction(false),
107 getSupportFunc(NULL) {}
108
111 void set(const ShapeBase* shape0, const ShapeBase* shape1);
112
114 void set(const ShapeBase* shape0, const ShapeBase* shape1,
115 const Transform3f& tf0, const Transform3f& tf1);
116
118 inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const {
119 return getSupport(shapes[0], d, dIsNormalized, hint);
120 }
121
123 inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const {
124 return oR1 *
125 getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) +
126 ot1;
127 }
128
130 inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0,
131 Vec3f& supp1, support_func_guess_t& hint) const {
132 assert(getSupportFunc != NULL);
133 getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint,
134 const_cast<ShapeData*>(data));
135 }
136};
137
148 };
149
150 typedef unsigned char vertex_id_t;
151
154 SimplexV* vertex[4];
157
159 };
160
165 enum Status { Valid, Inside, Failed };
166
187 Simplex simplices[2];
188
196 GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
197 : max_iterations(max_iterations_), tolerance(tolerance_) {
198 initialize();
199 }
200
202
205 const MinkowskiDiff& shape, const Vec3f& guess,
206 const support_func_guess_t& supportHint = support_func_guess_t::Zero());
207
210 inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv,
211 support_func_guess_t& hint) const {
212 shape->support(d, dIsNormalized, sv.w0, sv.w1, hint);
213 sv.w = sv.w0 - sv.w1;
214 }
215
218
221 inline Simplex* getSimplex() const { return simplex; }
222
224 bool hasClosestPoints() { return distance < distance_upper_bound; }
225
231 return distance > -shape.inflation.sum();
232 }
233
236 bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
237
240
246 distance_upper_bound = dup;
247 }
248
251 bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
252 const FCL_REAL& omega);
253
255 inline size_t getIterations() { return iterations; }
256
258 inline FCL_REAL getTolerance() { return tolerance; }
259
260 private:
261 SimplexV store_v[4];
262 SimplexV* free_v[4];
263 vertex_id_t nfree;
264 vertex_id_t current;
265 Simplex* simplex;
266 Status status;
267
268 unsigned int max_iterations;
269 FCL_REAL tolerance;
270 FCL_REAL distance_upper_bound;
271 size_t iterations;
272
274 inline void removeVertex(Simplex& simplex);
275
277 inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
279
293 // of the simplex are added. To know more about these, visit
294 // https://caseymuratori.com/blog_0003.
295 bool projectLineOrigin(const Simplex& current, Simplex& next);
296
299 bool projectTriangleOrigin(const Simplex& current, Simplex& next);
300
303 bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
304};
305
306static const size_t EPA_MAX_FACES = 128;
307static const size_t EPA_MAX_VERTICES = 64;
308static const FCL_REAL EPA_EPS = 0.000001;
309static const size_t EPA_MAX_ITERATIONS = 255;
310
317 SimplexV* vertex[3]; // a face has three vertices
318 SimplexF* f[3]; // a face has three adjacent faces
319 SimplexF* l[2]; // the pre and post faces in the list
320 size_t e[3];
321 size_t pass;
322
323 SimplexF() : n(Vec3f::Zero()){};
324 };
325
328 size_t count;
329 SimplexList() : root(NULL), count(0) {}
330 void append(SimplexF* face) {
331 face->l[0] = NULL;
332 face->l[1] = root;
333 if (root) root->l[0] = face;
334 root = face;
335 ++count;
336 }
337
338 void remove(SimplexF* 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];
342 --count;
343 }
344 };
345
346 static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) {
347 fa->e[ea] = eb;
348 fa->f[ea] = fb;
349 fb->e[eb] = ea;
350 fb->f[eb] = fa;
351 }
352
354 SimplexF* cf; // current face in the horizon
355 SimplexF* ff; // first face in the horizon
356 size_t nf; // number of faces in the horizon
357 SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
358 };
359
360 private:
361 unsigned int max_face_num;
362 unsigned int max_vertex_num;
363 unsigned int max_iterations;
364 FCL_REAL tolerance;
365
366 public:
367 enum Status {
368 Failed = 0,
369 Valid = 1,
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
377 };
378
385 size_t nextsv;
387
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_) {
394 initialize();
395 }
396
398 delete[] sv_store;
399 delete[] fc_store;
400 }
401
403
407 Status evaluate(GJK& gjk, const Vec3f& guess);
408
411 bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
412
413 private:
414 bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
415
416 SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced);
417
419 SimplexF* findBest();
420
422 bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e,
423 SimplexHorizon& horizon);
424};
425
426} // namespace details
427
428} // namespace fcl
429
430} // namespace hpp
431
432#endif
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:51
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
#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:68
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:93
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
Definition: data_types.h:89
double FCL_REAL
Definition: data_types.h:65
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
Main namespace.
Definition: broadphase_bruteforce.h:44
FCL_REAL d
Definition: gjk.h:316
Vec3f n
Definition: gjk.h:315
SimplexF * f[3]
Definition: gjk.h:318
size_t pass
Definition: gjk.h:321
SimplexF()
Definition: gjk.h:323
size_t e[3]
Definition: gjk.h:320
SimplexF * l[2]
Definition: gjk.h:319
SimplexF * cf
Definition: gjk.h:354
SimplexF * ff
Definition: gjk.h:355
size_t nf
Definition: gjk.h:356
SimplexHorizon()
Definition: gjk.h:357
SimplexList()
Definition: gjk.h:329
size_t count
Definition: gjk.h:328
void remove(SimplexF *face)
Definition: gjk.h:338
SimplexF * root
Definition: gjk.h:327
void append(SimplexF *face)
Definition: gjk.h:330
class for EPA algorithm
Definition: gjk.h:312
Status status
Definition: gjk.h:379
GJK::SimplexV SimplexV
Definition: gjk.h:313
SimplexF * fc_store
Definition: gjk.h:384
Vec3f normal
Definition: gjk.h:381
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:388
~EPA()
Definition: gjk.h:397
size_t nextsv
Definition: gjk.h:385
Status
Definition: gjk.h:367
Status evaluate(GJK &gjk, const Vec3f &guess)
FCL_REAL depth
Definition: gjk.h:382
SimplexList hull
Definition: gjk.h:386
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:346
GJK::Simplex result
Definition: gjk.h:380
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
SimplexV * sv_store
Definition: gjk.h:383
Vec3f w1
Definition: gjk.h:144
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:144
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction)
Definition: gjk.h:147
Definition: gjk.h:152
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:156
Simplex()
Definition: gjk.h:158
class for GJK algorithm
Definition: gjk.h:141
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
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
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:230
support_func_guess_t support_hint
Definition: gjk.h:172
MinkowskiDiff const * shape
Definition: gjk.h:167
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision....
Definition: gjk.h:165
FCL_REAL getTolerance()
Get GJK tolerance.
Definition: gjk.h:258
size_t getIterations()
Get GJK number of iterations.
Definition: gjk.h:255
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKConvergenceCriterion convergence_criterion
Definition: gjk.h:170
bool checkConvergence(const Vec3f &w, const FCL_REAL &rl, FCL_REAL &alpha, const FCL_REAL &omega)
Convergence check used to stop GJK when shapes are not in collision.
GJKConvergenceCriterionType convergence_criterion_type
Definition: gjk.h:171
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:196
FCL_REAL distance
Definition: gjk.h:186
Vec3f ray
Definition: gjk.h:168
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.
GJKVariant gjk_variant
Definition: gjk.h:169
unsigned char vertex_id_t
Definition: gjk.h:150
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:221
bool encloseOrigin()
whether the simplex enclose the origin
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:224
std::vector< int8_t > visited
Definition: gjk.h:66
Minkowski difference class of two shapes.
Definition: gjk.h:59
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:123
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:118
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:79
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
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:75
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:102
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)
MinkowskiDiff()
Definition: gjk.h:104
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
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::Array< FCL_REAL, 1, 2 > Array2d
Definition: gjk.h:60