hpp-fcl  1.4.4
HPP fork of FCL -- The Flexible Collision Library
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 
42 #include <hpp/fcl/math/transform.h>
43 
44 namespace hpp
45 {
46 namespace fcl
47 {
48 
49 namespace details
50 {
51 
53 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized);
54 
62 {
64  const ShapeBase* shapes[2];
65 
69 
73 
76  Eigen::Array<FCL_REAL, 1, 2> inflation;
77 
78  typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
79  const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1);
81 
83 
86  void set (const ShapeBase* shape0, const ShapeBase* shape1);
87 
89  void set (const ShapeBase* shape0, const ShapeBase* shape1,
90  const Transform3f& tf0, const Transform3f& tf1);
91 
93  inline Vec3f support0(const Vec3f& d, bool dIsNormalized) const
94  {
95  return getSupport(shapes[0], d, dIsNormalized);
96  }
97 
99  inline Vec3f support1(const Vec3f& d, bool dIsNormalized) const
100  {
101  return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized) + ot1;
102  }
103 
105  inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1) const
106  {
107  assert(getSupportFunc != NULL);
108  getSupportFunc(*this, d, dIsNormalized, supp0, supp1);
109  }
110 };
111 
115 struct GJK
116 {
117  struct SimplexV
118  {
123  };
124 
125  typedef unsigned char vertex_id_t;
126 
127  struct Simplex
128  {
133 
134  Simplex() {}
135  };
136 
138 
156 
157 
158  GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_),
159  tolerance(tolerance_)
160  {
161  initialize();
162  }
163 
164  void initialize();
165 
167  Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess);
168 
170  inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const
171  {
172  shape->support(d, dIsNormalized, sv.w0, sv.w1);
173  sv.w.noalias() = sv.w0 - sv.w1;
174  }
175 
177  bool encloseOrigin();
178 
180  inline Simplex* getSimplex() const
181  {
182  return simplex;
183  }
184 
187  {
188  return distance < distance_upper_bound;
189  }
190 
196  {
197  return distance > - shape.inflation.sum();
198  }
199 
202  bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
203 
205  Vec3f getGuessFromSimplex() const;
206 
211  void setDistanceEarlyBreak (const FCL_REAL& dup)
212  {
213  distance_upper_bound = dup;
214  }
215 
216 private:
217  SimplexV store_v[4];
218  SimplexV* free_v[4];
219  vertex_id_t nfree;
220  vertex_id_t current;
221  Simplex* simplex;
222  Status status;
223 
224  unsigned int max_iterations;
225  FCL_REAL tolerance;
226  FCL_REAL distance_upper_bound;
227 
229  inline void removeVertex(Simplex& simplex);
230 
232  inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized = false);
233 
235  bool projectLineOrigin(const Simplex& current, Simplex& next);
236 
238  bool projectTriangleOrigin(const Simplex& current, Simplex& next);
239 
241  bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
242 };
243 
244 
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;
249 
251 struct EPA
252 {
254  struct SimplexF
255  {
258  SimplexV* vertex[3]; // a face has three vertices
259  SimplexF* f[3]; // a face has three adjacent faces
260  SimplexF* l[2]; // the pre and post faces in the list
261  size_t e[3];
262  size_t pass;
263 
264  SimplexF () : n(Vec3f::Zero()) {};
265  };
266 
267  struct SimplexList
268  {
270  size_t count;
271  SimplexList() : root(NULL), count(0) {}
272  void append(SimplexF* face)
273  {
274  face->l[0] = NULL;
275  face->l[1] = root;
276  if(root) root->l[0] = face;
277  root = face;
278  ++count;
279  }
280 
281  void remove(SimplexF* face)
282  {
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];
285  if(face == root) root = face->l[1];
286  --count;
287  }
288  };
289 
290  static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
291  {
292  fa->e[ea] = eb; fa->f[ea] = fb;
293  fb->e[eb] = ea; fb->f[eb] = fa;
294  }
295 
297  {
298  SimplexF* cf; // current face in the horizon
299  SimplexF* ff; // first face in the horizon
300  size_t nf; // number of faces in the horizon
301  SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
302  };
303 
304 private:
305  unsigned int max_face_num;
306  unsigned int max_vertex_num;
307  unsigned int max_iterations;
308  FCL_REAL tolerance;
309 
310 public:
311 
312  enum Status {
313  Failed = 0,
314  Valid = 1,
315  AccuracyReached = 1 << 1 | Valid ,
316  Degenerated = 1 << 1 | Failed,
317  NonConvex = 2 << 1 | Failed,
318  InvalidHull = 3 << 1 | Failed,
319  OutOfFaces = 4 << 1 | Failed,
320  OutOfVertices = 5 << 1 | Failed,
321  FallBack = 6 << 1 | Failed
322  };
323 
330  size_t nextsv;
332 
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_)
337  {
338  initialize();
339  }
340 
342  {
343  delete [] sv_store;
344  delete [] fc_store;
345  }
346 
347  void initialize();
348 
352  Status evaluate(GJK& gjk, const Vec3f& guess);
353 
356  bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
357 
358 private:
359  bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
360 
361  SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced);
362 
364  SimplexF* findBest();
365 
367  bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
368 };
369 
370 
371 } // details
372 
373 
374 
375 }
376 
377 
378 } // namespace hpp
379 
380 #endif
hpp::fcl::details::GJK::shape
MinkowskiDiff const * shape
Definition: gjk.h:139
hpp::fcl::details::EPA::SimplexF::vertex
SimplexV * vertex[3]
Definition: gjk.h:258
hpp::fcl::details::EPA::SimplexList
Definition: gjk.h:267
hpp::fcl::details::getSupport
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized)
the support function for shape
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
hpp::fcl::details::MinkowskiDiff::support0
Vec3f support0(const Vec3f &d, bool dIsNormalized) const
support function for shape0
Definition: gjk.h:93
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:115
hpp::fcl::details::GJK::Simplex::vertex
SimplexV * vertex[4]
simplex vertex
Definition: gjk.h:130
hpp::fcl::details::GJK::encloseOrigin
bool encloseOrigin()
whether the simplex enclose the origin
hpp::fcl::details::EPA::SimplexF::pass
size_t pass
Definition: gjk.h:262
hpp::fcl::details::EPA
class for EPA algorithm
Definition: gjk.h:251
hpp::fcl::details::EPA::SimplexF::n
Vec3f n
Definition: gjk.h:256
hpp::fcl::details::EPA::result
GJK::Simplex result
Definition: gjk.h:325
hpp::fcl::details::EPA::Status
Status
Definition: gjk.h:312
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:333
hpp::fcl::details::MinkowskiDiff::support
void support(const Vec3f &d, bool dIsNormalized, Vec3f &supp0, Vec3f &supp1) const
support function for the pair of shapes
Definition: gjk.h:105
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
hpp::fcl::details::EPA::SimplexHorizon::cf
SimplexF * cf
Definition: gjk.h:298
hpp::fcl::details::EPA::SimplexF::f
SimplexF * f[3]
Definition: gjk.h:259
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:140
hpp::fcl::details::GJK::Simplex::Simplex
Simplex()
Definition: gjk.h:134
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:122
hpp::fcl::details::EPA::SimplexList::root
SimplexF * root
Definition: gjk.h:269
hpp::fcl::details::EPA::NonConvex
@ NonConvex
Definition: gjk.h:317
hpp::fcl::details::EPA::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::GJK::GJK
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:158
hpp::fcl::details::GJK::evaluate
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess)
GJK algorithm, given the initial value guess.
hpp::fcl::details::MinkowskiDiff::MinkowskiDiff
MinkowskiDiff()
Definition: gjk.h:82
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:61
hpp::fcl::details::GJK::getSupport
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
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:68
hpp::fcl::details::GJK::Inside
@ Inside
Definition: gjk.h:137
hpp::fcl::details::GJK::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::EPA::SimplexList::count
size_t count
Definition: gjk.h:270
hpp::fcl::details::GJK::getGuessFromSimplex
Vec3f getGuessFromSimplex() const
get the guess from current simplex
details
Definition: traversal_node_setup.h:775
hpp::fcl::details::EPA::bind
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:290
hpp::fcl::details::GJK::Valid
@ Valid
Definition: gjk.h:137
hpp::fcl::details::EPA::Failed
@ Failed
Definition: gjk.h:313
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:180
hpp::fcl::details::GJK::Simplex
Definition: gjk.h:127
hpp::fcl::details::GJK::Status
Status
Definition: gjk.h:137
hpp::fcl::details::EPA::SimplexHorizon::SimplexHorizon
SimplexHorizon()
Definition: gjk.h:301
hpp::fcl::details::EPA::SimplexHorizon::ff
SimplexF * ff
Definition: gjk.h:299
hpp::fcl::details::GJK::SimplexV::w1
Vec3f w1
Definition: gjk.h:120
hpp::fcl::details::EPA::SimplexList::append
void append(SimplexF *face)
Definition: gjk.h:272
hpp::fcl::details::EPA::SimplexHorizon
Definition: gjk.h:296
hpp::fcl::details::EPA::SimplexF
Definition: gjk.h:254
hpp::fcl::details::EPA::FallBack
@ FallBack
Definition: gjk.h:321
hpp
Main namespace.
Definition: AABB.h:43
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:154
hpp::fcl::details::EPA::nextsv
size_t nextsv
Definition: gjk.h:330
hpp::fcl::details::MinkowskiDiff::getSupportFunc
GetSupportFunction getSupportFunc
Definition: gjk.h:80
hpp::fcl::details::EPA::SimplexList::remove
void remove(SimplexF *face)
Definition: gjk.h:281
hpp::fcl::details::EPA::Valid
@ Valid
Definition: gjk.h:314
hpp::fcl::details::EPA::depth
FCL_REAL depth
Definition: gjk.h:327
hpp::fcl::details::EPA::SimplexF::d
FCL_REAL d
Definition: gjk.h:257
hpp::fcl::details::MinkowskiDiff::inflation
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
hpp::fcl::details::GJK::SimplexV::w0
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:120
hpp::fcl::details::EPA::~EPA
~EPA()
Definition: gjk.h:341
hpp::fcl::details::EPA::hull
SimplexList hull
Definition: gjk.h:331
hpp::fcl::details::EPA::fc_store
SimplexF * fc_store
Definition: gjk.h:329
hpp::fcl::details::GJK::SimplexV
Definition: gjk.h:117
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
hpp::fcl::details::GJK::simplices
Simplex simplices[2]
Definition: gjk.h:155
hpp::fcl::details::EPA::sv_store
SimplexV * sv_store
Definition: gjk.h:328
hpp::fcl::details::EPA::SimplexHorizon::nf
size_t nf
Definition: gjk.h:300
hpp::fcl::details::GJK::vertex_id_t
unsigned char vertex_id_t
Definition: gjk.h:125
hpp::fcl::details::EPA::initialize
void initialize()
hpp::fcl::details::EPA::SimplexF::SimplexF
SimplexF()
Definition: gjk.h:264
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
hpp::fcl::details::GJK::Failed
@ Failed
Definition: gjk.h:137
hpp::fcl::details::EPA::InvalidHull
@ InvalidHull
Definition: gjk.h:318
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:211
geometric_shapes.h
hpp::fcl::details::MinkowskiDiff::shapes
const ShapeBase * shapes[2]
points to two shapes
Definition: gjk.h:64
hpp::fcl::details::EPA::AccuracyReached
@ AccuracyReached
Definition: gjk.h:315
hpp::fcl::details::MinkowskiDiff::support1
Vec3f support1(const Vec3f &d, bool dIsNormalized) const
support function for shape1
Definition: gjk.h:99
hpp::fcl::details::EPA::Degenerated
@ Degenerated
Definition: gjk.h:316
hpp::fcl::details::GJK::Simplex::rank
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:132
hpp::fcl::details::EPA::evaluate
Status evaluate(GJK &gjk, const Vec3f &guess)
transform.h
hpp::fcl::details::EPA::normal
Vec3f normal
Definition: gjk.h:326
hpp::fcl::details::GJK::hasClosestPoints
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:186
hpp::fcl::details::EPA::SimplexF::e
size_t e[3]
Definition: gjk.h:261
hpp::fcl::details::EPA::OutOfFaces
@ OutOfFaces
Definition: gjk.h:319
hpp::fcl::details::EPA::SimplexList::SimplexList
SimplexList()
Definition: gjk.h:271
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:54
hpp::fcl::details::MinkowskiDiff::GetSupportFunction
void(* GetSupportFunction)(const MinkowskiDiff &minkowskiDiff, const Vec3f &dir, bool dirIsNormalized, Vec3f &support0, Vec3f &support1)
Definition: gjk.h:78
hpp::fcl::details::MinkowskiDiff::oR1
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:68
hpp::fcl::details::EPA::SimplexF::l
SimplexF * l[2]
Definition: gjk.h:260
hpp::fcl::details::EPA::OutOfVertices
@ OutOfVertices
Definition: gjk.h:320
hpp::fcl::details::EPA::SimplexV
GJK::SimplexV SimplexV
Definition: gjk.h:253
hpp::fcl::details::MinkowskiDiff::ot1
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:72
hpp::fcl::details::GJK::hasPenetrationInformation
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:195
hpp::fcl::details::EPA::stock
SimplexList stock
Definition: gjk.h:331
hpp::fcl::details::GJK::initialize
void initialize()
hpp::fcl::details::EPA::status
Status status
Definition: gjk.h:324