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 
44 #include <hpp/fcl/math/transform.h>
45 
46 namespace hpp {
47 namespace fcl {
48 
49 namespace details {
50 
53 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
54  int& hint);
55 
59 struct HPP_FCL_DLLAPI MinkowskiDiff {
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 
72 
76 
80 
85 
91 
96 
97  typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
98  const Vec3f& dir, bool dirIsNormalized,
99  Vec3f& support0, Vec3f& support1,
100  support_func_guess_t& hint,
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 
141 struct HPP_FCL_DLLAPI GJK {
142  struct HPP_FCL_DLLAPI SimplexV {
144  Vec3f w0, w1;
148  };
149 
150  typedef unsigned char vertex_id_t;
151 
152  struct HPP_FCL_DLLAPI Simplex {
154  SimplexV* vertex[4];
157 
158  Simplex() {}
159  };
160 
165  enum Status { Valid, Inside, Failed, EarlyStopped };
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 
201  void initialize();
202 
204  Status evaluate(
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 
217  bool encloseOrigin();
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 
239  Vec3f getGuessFromSimplex() const;
240 
245  void setDistanceEarlyBreak(const FCL_REAL& dup) {
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];
267 
268  unsigned int max_iterations;
271  size_t iterations;
272 
274  inline void removeVertex(Simplex& simplex);
275 
277  inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
278  support_func_guess_t& hint);
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 
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;
310 
312 struct HPP_FCL_DLLAPI EPA {
314  struct HPP_FCL_DLLAPI SimplexF {
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 
326  struct HPP_FCL_DLLAPI SimplexList {
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 
353  struct HPP_FCL_DLLAPI SimplexHorizon {
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;
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 
397  ~EPA() {
398  delete[] sv_store;
399  delete[] fc_store;
400  }
401 
402  void initialize();
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
hpp::fcl::details::GJK::getSupport
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
hpp::fcl::details::GJK::shape
MinkowskiDiff const * shape
Definition: gjk.h:167
hpp::fcl::details::EPA::SimplexList
Definition: gjk.h:326
hpp::fcl::GJKConvergenceCriterionType
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:93
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::details::MinkowskiDiff::support
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
hpp::fcl::details::details::getClosestPoints
bool getClosestPoints(const GJK::Simplex &simplex, Vec3f &w0, Vec3f &w1)
Definition: src/narrowphase/gjk.cpp:532
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:141
hpp::fcl::details::EPA::max_face_num
unsigned int max_face_num
Definition: gjk.h:361
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:172
hpp::fcl::details::EPA::SimplexF::pass
size_t pass
Definition: gjk.h:321
hpp::fcl::details::EPA
class for EPA algorithm
Definition: gjk.h:312
hpp::fcl::details::EPA::SimplexF::n
Vec3f n
Definition: gjk.h:315
hpp::fcl::details::EPA::result
GJK::Simplex result
Definition: gjk.h:380
hpp::fcl::details::EPA::Status
Status
Definition: gjk.h:367
hpp::fcl::details::GJK::getIterations
size_t getIterations()
Get GJK number of iterations.
Definition: gjk.h:255
hpp::fcl::details::EPA::tolerance
FCL_REAL tolerance
Definition: gjk.h:364
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
data
data
hpp::fcl::details::EPA::EPA
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:388
hpp::fcl::GJKConvergenceCriterion
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
Definition: data_types.h:89
initialize
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
hpp::fcl::details::GJK::getTolerance
FCL_REAL getTolerance()
Get GJK tolerance.
Definition: gjk.h:258
hpp::fcl::details::EPA_MAX_FACES
static const size_t EPA_MAX_FACES
Definition: gjk.h:306
hpp::fcl::details::EPA::SimplexHorizon::cf
SimplexF * cf
Definition: gjk.h:354
hpp::fcl::details::EPA::SimplexF::f
SimplexF * f[3]
Definition: gjk.h:318
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:168
hpp::fcl::details::GJK::Simplex::Simplex
Simplex()
Definition: gjk.h:158
hpp::fcl::distance
HPP_FCL_DLLAPI 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.
Definition: kIOS.cpp:181
hpp::fcl::details::EPA_MAX_VERTICES
static const size_t EPA_MAX_VERTICES
Definition: gjk.h:307
hpp::fcl::details::GJK::status
Status status
Definition: gjk.h:266
hpp::fcl::details::GJK::SimplexV::w
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction)
Definition: gjk.h:147
hpp::fcl::details::getSupport
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
Definition: src/narrowphase/gjk.cpp:260
hpp::fcl::details::EPA::SimplexList::root
SimplexF * root
Definition: gjk.h:327
hpp::fcl::details::MinkowskiDiff::ShapeData
Definition: gjk.h:65
hpp::fcl::details::GJK::GJK
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:196
hpp::fcl::details::EPA::max_vertex_num
unsigned int max_vertex_num
Definition: gjk.h:362
hpp::fcl::details::MinkowskiDiff::MinkowskiDiff
MinkowskiDiff()
Definition: gjk.h:104
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:59
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::details::EPA::SimplexList::count
size_t count
Definition: gjk.h:328
details
Definition: traversal_node_setup.h:792
hpp::fcl::details::EPA::bind
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:346
hpp::fcl::details::GJK::Valid
@ Valid
Definition: gjk.h:165
hpp::fcl::details::GJK::getSimplex
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk.h:221
hpp::fcl::details::GJK::gjk_variant
GJKVariant gjk_variant
Definition: gjk.h:169
hpp::fcl::details::GJK::Simplex
Definition: gjk.h:152
hpp::fcl::details::GJK::Status
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision....
Definition: gjk.h:165
hpp::fcl::details::MinkowskiDiff::linear_log_convex_threshold
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
hpp::fcl::details::GJK::distance_upper_bound
FCL_REAL distance_upper_bound
Definition: gjk.h:270
hpp::fcl::details::EPA::SimplexHorizon::SimplexHorizon
SimplexHorizon()
Definition: gjk.h:357
hpp::fcl::details::EPA::SimplexHorizon::ff
SimplexF * ff
Definition: gjk.h:355
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
hpp::fcl::details::GJK::SimplexV::w1
Vec3f w1
Definition: gjk.h:144
hpp::fcl::details::EPA::SimplexList::append
void append(SimplexF *face)
Definition: gjk.h:330
hpp::fcl::details::EPA::SimplexHorizon
Definition: gjk.h:353
hpp::fcl::details::EPA::SimplexF
Definition: gjk.h:314
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:186
hpp::fcl::details::EPA::nextsv
size_t nextsv
Definition: gjk.h:385
hpp::fcl::details::MinkowskiDiff::getSupportFunc
GetSupportFunction getSupportFunc
Definition: gjk.h:102
hpp::fcl::details::EPA::SimplexList::remove
void remove(SimplexF *face)
Definition: gjk.h:338
hpp::fcl::details::GJK::current
vertex_id_t current
Definition: gjk.h:264
hpp::fcl::details::GJK::tolerance
FCL_REAL tolerance
Definition: gjk.h:269
hpp::fcl::details::EPA_MAX_ITERATIONS
static const size_t EPA_MAX_ITERATIONS
Definition: gjk.h:309
hpp::fcl::details::EPA::depth
FCL_REAL depth
Definition: gjk.h:382
hpp::fcl::details::EPA::SimplexF::d
FCL_REAL d
Definition: gjk.h:316
hpp::fcl::details::MinkowskiDiff::support1
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:123
hpp::fcl::details::MinkowskiDiff::support0
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:118
hpp::fcl::details::GJK::max_iterations
unsigned int max_iterations
Definition: gjk.h:268
hpp::fcl::details::GJK::SimplexV::w0
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:144
hpp::fcl::details::EPA::~EPA
~EPA()
Definition: gjk.h:397
hpp::fcl::details::EPA_EPS
static const FCL_REAL EPA_EPS
Definition: gjk.h:308
hpp::fcl::details::GJK::iterations
size_t iterations
Definition: gjk.h:271
hpp::fcl::details::EPA::fc_store
SimplexF * fc_store
Definition: gjk.h:384
hpp::fcl::details::GJK::SimplexV
Definition: gjk.h:142
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::details::EPA::sv_store
SimplexV * sv_store
Definition: gjk.h:383
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::details::EPA::SimplexHorizon::nf
size_t nf
Definition: gjk.h:356
hpp::fcl::details::GJK::vertex_id_t
unsigned char vertex_id_t
Definition: gjk.h:150
hpp::fcl::details::GJK::convergence_criterion
GJKConvergenceCriterion convergence_criterion
Definition: gjk.h:170
hpp::fcl::details::EPA::SimplexF::SimplexF
SimplexF()
Definition: gjk.h:323
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::details::GJK::setDistanceEarlyBreak
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
hpp::fcl::details::GJK::Simplex::rank
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:156
hpp::fcl::details::GJK::simplex
Simplex * simplex
Definition: gjk.h:265
transform.h
hpp::fcl::details::GJK::nfree
vertex_id_t nfree
Definition: gjk.h:263
hpp::fcl::details::EPA::normal
Vec3f normal
Definition: gjk.h:381
hpp::fcl::details::MinkowskiDiff::Array2d
Eigen::Array< FCL_REAL, 1, 2 > Array2d
Definition: gjk.h:60
hpp::fcl::details::GJK::hasClosestPoints
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:224
hpp::fcl::details::EPA::SimplexF::e
size_t e[3]
Definition: gjk.h:320
hpp::fcl::details::MinkowskiDiff::normalize_support_direction
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
geometric_shapes.h
hpp::fcl::details::GJK::convergence_criterion_type
GJKConvergenceCriterionType convergence_criterion_type
Definition: gjk.h:171
hpp::fcl::details::EPA::SimplexList::SimplexList
SimplexList()
Definition: gjk.h:329
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape/geometric_shapes.h:51
hpp::fcl::details::EPA::max_iterations
unsigned int max_iterations
Definition: gjk.h:363
hpp::fcl::details::MinkowskiDiff::oR1
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:75
hpp::fcl::details::EPA::SimplexF::l
SimplexF * l[2]
Definition: gjk.h:319
hpp::fcl::GJKVariant
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
hpp::fcl::details::EPA::SimplexV
GJK::SimplexV SimplexV
Definition: gjk.h:313
hpp::fcl::details::MinkowskiDiff::ot1
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:79
hpp::fcl::details::GJK::hasPenetrationInformation
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:230
obb.v
list v
Definition: obb.py:48
hpp::fcl::details::MinkowskiDiff::ShapeData::visited
std::vector< int8_t > visited
Definition: gjk.h:66
hpp::fcl::details::EPA::stock
SimplexList stock
Definition: gjk.h:386
hpp::fcl::details::EPA::status
Status status
Definition: gjk.h:379
gjk
Definition: doc/gjk.py:1
hpp::fcl::details::MinkowskiDiff::inflation
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


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13