hpp-fcl  1.4.5
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 
54 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint);
55 
63 {
65  const ShapeBase* shapes[2];
66 
70 
74 
77  Eigen::Array<FCL_REAL, 1, 2> inflation;
78 
79  typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
80  const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
81  support_func_guess_t& hint);
82  GetSupportFunction getSupportFunc;
83 
84  MinkowskiDiff() : getSupportFunc (NULL) {}
85 
88  void set (const ShapeBase* shape0, const ShapeBase* shape1);
89 
91  void set (const ShapeBase* shape0, const ShapeBase* shape1,
92  const Transform3f& tf0, const Transform3f& tf1);
93 
95  inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const
96  {
97  return getSupport(shapes[0], d, dIsNormalized, hint);
98  }
99 
101  inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const
102  {
103  return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1;
104  }
105 
107  inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, support_func_guess_t& hint) const
108  {
109  assert(getSupportFunc != NULL);
110  getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint);
111  }
112 };
113 
118 {
120  {
122  Vec3f w0, w1;
125  };
126 
127  typedef unsigned char vertex_id_t;
128 
130  {
132  SimplexV* vertex[4];
135 
136  Simplex() {}
137  };
138 
139  enum Status {Valid, Inside, Failed};
140 
158  Simplex simplices[2];
159 
160 
161  GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_),
162  tolerance(tolerance_)
163  {
164  initialize();
165  }
166 
167  void initialize();
168 
170  Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess,
171  const support_func_guess_t& supportHint = support_func_guess_t::Zero());
172 
174  inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv,
175  support_func_guess_t& hint) const
176  {
177  shape->support(d, dIsNormalized, sv.w0, sv.w1, hint);
178  sv.w.noalias() = sv.w0 - sv.w1;
179  }
180 
182  bool encloseOrigin();
183 
185  inline Simplex* getSimplex() const
186  {
187  return simplex;
188  }
189 
192  {
193  return distance < distance_upper_bound;
194  }
195 
201  {
202  return distance > - shape.inflation.sum();
203  }
204 
207  bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
208 
210  Vec3f getGuessFromSimplex() const;
211 
216  void setDistanceEarlyBreak (const FCL_REAL& dup)
217  {
218  distance_upper_bound = dup;
219  }
220 
221 private:
222  SimplexV store_v[4];
223  SimplexV* free_v[4];
224  vertex_id_t nfree;
225  vertex_id_t current;
226  Simplex* simplex;
227  Status status;
228 
229  unsigned int max_iterations;
230  FCL_REAL tolerance;
231  FCL_REAL distance_upper_bound;
232 
234  inline void removeVertex(Simplex& simplex);
235 
237  inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
238  support_func_guess_t& hint);
239 
241  bool projectLineOrigin(const Simplex& current, Simplex& next);
242 
244  bool projectTriangleOrigin(const Simplex& current, Simplex& next);
245 
247  bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
248 };
249 
250 
251 static const size_t EPA_MAX_FACES = 128;
252 static const size_t EPA_MAX_VERTICES = 64;
253 static const FCL_REAL EPA_EPS = 0.000001;
254 static const size_t EPA_MAX_ITERATIONS = 255;
255 
258 {
261  {
264  SimplexV* vertex[3]; // a face has three vertices
265  SimplexF* f[3]; // a face has three adjacent faces
266  SimplexF* l[2]; // the pre and post faces in the list
267  size_t e[3];
268  size_t pass;
269 
270  SimplexF () : n(Vec3f::Zero()) {};
271  };
272 
274  {
276  size_t count;
277  SimplexList() : root(NULL), count(0) {}
278  void append(SimplexF* face)
279  {
280  face->l[0] = NULL;
281  face->l[1] = root;
282  if(root) root->l[0] = face;
283  root = face;
284  ++count;
285  }
286 
287  void remove(SimplexF* face)
288  {
289  if(face->l[1]) face->l[1]->l[0] = face->l[0];
290  if(face->l[0]) face->l[0]->l[1] = face->l[1];
291  if(face == root) root = face->l[1];
292  --count;
293  }
294  };
295 
296  static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
297  {
298  fa->e[ea] = eb; fa->f[ea] = fb;
299  fb->e[eb] = ea; fb->f[eb] = fa;
300  }
301 
303  {
304  SimplexF* cf; // current face in the horizon
305  SimplexF* ff; // first face in the horizon
306  size_t nf; // number of faces in the horizon
307  SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
308  };
309 
310 private:
311  unsigned int max_face_num;
312  unsigned int max_vertex_num;
313  unsigned int max_iterations;
314  FCL_REAL tolerance;
315 
316 public:
317 
318  enum Status {
319  Failed = 0,
320  Valid = 1,
321  AccuracyReached = 1 << 1 | Valid ,
322  Degenerated = 1 << 1 | Failed,
323  NonConvex = 2 << 1 | Failed,
324  InvalidHull = 3 << 1 | Failed,
325  OutOfFaces = 4 << 1 | Failed,
326  OutOfVertices = 5 << 1 | Failed,
327  FallBack = 6 << 1 | Failed
328  };
329 
336  size_t nextsv;
338 
339  EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_),
340  max_vertex_num(max_vertex_num_),
341  max_iterations(max_iterations_),
342  tolerance(tolerance_)
343  {
344  initialize();
345  }
346 
348  {
349  delete [] sv_store;
350  delete [] fc_store;
351  }
352 
353  void initialize();
354 
358  Status evaluate(GJK& gjk, const Vec3f& guess);
359 
362  bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
363 
364 private:
365  bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
366 
367  SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced);
368 
370  SimplexF* findBest();
371 
373  bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
374 };
375 
376 
377 } // details
378 
379 
380 
381 }
382 
383 
384 } // namespace hpp
385 
386 #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:174
HPP_FCL_DLLAPI
#define HPP_FCL_DLLAPI
Definition: config.hh:64
hpp::fcl::details::GJK::shape
MinkowskiDiff const * shape
Definition: gjk.h:141
hpp::fcl::details::EPA::SimplexList
Definition: gjk.h:273
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:74
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:107
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:117
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:143
hpp::fcl::details::EPA::SimplexF::pass
size_t pass
Definition: gjk.h:268
hpp::fcl::details::EPA
class for EPA algorithm
Definition: gjk.h:257
hpp::fcl::details::EPA::SimplexF::n
Vec3f n
Definition: gjk.h:262
hpp::fcl::details::EPA::result
GJK::Simplex result
Definition: gjk.h:331
hpp::fcl::details::EPA::Status
Status
Definition: gjk.h:318
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:339
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:407
hpp::fcl::details::EPA::SimplexHorizon::cf
SimplexF * cf
Definition: gjk.h:304
hpp::fcl::details::EPA::SimplexF::f
SimplexF * f[3]
Definition: gjk.h:265
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:142
hpp::fcl::details::GJK::Simplex::Simplex
Simplex()
Definition: gjk.h:136
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:124
hpp::fcl::details::getSupport
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
hpp::fcl::details::EPA::SimplexList::root
SimplexF * root
Definition: gjk.h:275
hpp::fcl::details::GJK::GJK
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:161
hpp::fcl::details::MinkowskiDiff::MinkowskiDiff
MinkowskiDiff()
Definition: gjk.h:84
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:62
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:69
hpp::fcl::details::EPA::SimplexList::count
size_t count
Definition: gjk.h:276
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:296
hpp::fcl::details::GJK::Valid
@ Valid
Definition: gjk.h:139
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:185
hpp::fcl::details::GJK::Simplex
Definition: gjk.h:129
hpp::fcl::details::GJK::Status
Status
Definition: gjk.h:139
hpp::fcl::details::EPA::SimplexHorizon::SimplexHorizon
SimplexHorizon()
Definition: gjk.h:307
hpp::fcl::details::EPA::SimplexHorizon::ff
SimplexF * ff
Definition: gjk.h:305
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:76
hpp::fcl::details::GJK::SimplexV::w1
Vec3f w1
Definition: gjk.h:122
hpp::fcl::details::EPA::SimplexList::append
void append(SimplexF *face)
Definition: gjk.h:278
hpp::fcl::details::EPA::SimplexHorizon
Definition: gjk.h:302
hpp::fcl::details::EPA::SimplexF
Definition: gjk.h:260
hpp
Main namespace.
Definition: AABB.h:43
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:157
hpp::fcl::details::EPA::nextsv
size_t nextsv
Definition: gjk.h:336
hpp::fcl::details::MinkowskiDiff::getSupportFunc
GetSupportFunction getSupportFunc
Definition: gjk.h:82
hpp::fcl::details::EPA::SimplexList::remove
void remove(SimplexF *face)
Definition: gjk.h:287
hpp::fcl::details::EPA::depth
FCL_REAL depth
Definition: gjk.h:333
hpp::fcl::details::EPA::SimplexF::d
FCL_REAL d
Definition: gjk.h:263
hpp::fcl::details::MinkowskiDiff::support1
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:101
hpp::fcl::details::MinkowskiDiff::support0
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:95
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:77
hpp::fcl::details::GJK::SimplexV::w0
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:122
hpp::fcl::details::EPA::~EPA
~EPA()
Definition: gjk.h:347
hpp::fcl::details::EPA::fc_store
SimplexF * fc_store
Definition: gjk.h:335
hpp::fcl::details::GJK::SimplexV
Definition: gjk.h:119
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:75
hpp::fcl::details::EPA::sv_store
SimplexV * sv_store
Definition: gjk.h:334
hpp::fcl::details::EPA::SimplexHorizon::nf
size_t nf
Definition: gjk.h:306
hpp::fcl::details::GJK::vertex_id_t
unsigned char vertex_id_t
Definition: gjk.h:127
hpp::fcl::details::EPA::SimplexF::SimplexF
SimplexF()
Definition: gjk.h:270
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
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:216
geometric_shapes.h
hpp::fcl::details::GJK::Simplex::rank
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:134
transform.h
hpp::fcl::details::EPA::normal
Vec3f normal
Definition: gjk.h:332
hpp::fcl::details::GJK::hasClosestPoints
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:191
hpp::fcl::details::EPA::SimplexF::e
size_t e[3]
Definition: gjk.h:267
hpp::fcl::details::EPA::SimplexList::SimplexList
SimplexList()
Definition: gjk.h:277
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:54
hpp::fcl::details::MinkowskiDiff::oR1
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:69
hpp::fcl::details::EPA::SimplexF::l
SimplexF * l[2]
Definition: gjk.h:266
hpp::fcl::details::EPA::SimplexV
GJK::SimplexV SimplexV
Definition: gjk.h:259
hpp::fcl::details::MinkowskiDiff::ot1
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:73
hpp::fcl::details::GJK::hasPenetrationInformation
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:200
hpp::fcl::distance
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL) HPP_FCL_DLLAPI
Approximate distance between two kIOS bounding volumes.
hpp::fcl::details::EPA::stock
SimplexList stock
Definition: gjk.h:337
hpp::fcl::details::EPA::status
Status status
Definition: gjk.h:330