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];
134  vertex_id_t rank;
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 
334  SimplexV* sv_store;
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
SimplexList stock
Definition: gjk.h:337
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
Definition: gjk.h:139
MinkowskiDiff const * shape
Definition: gjk.h:141
support_func_guess_t support_hint
Definition: gjk.h:143
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
GJK::Simplex result
Definition: gjk.h:331
Status
Definition: gjk.h:318
MinkowskiDiff()
Definition: gjk.h:84
Main namespace.
Definition: AABB.h:43
size_t pass
Definition: gjk.h:268
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:95
Vec3f n
Definition: gjk.h:262
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
Simplex()
Definition: gjk.h:136
SimplexF * f[3]
Definition: gjk.h:265
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:75
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:339
void append(SimplexF *face)
Definition: gjk.h:278
class for GJK algorithm
Definition: gjk.h:117
SimplexF * cf
Definition: gjk.h:304
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:101
Vec3f ray
Definition: gjk.h:142
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction) ...
Definition: gjk.h:124
Minkowski difference class of two shapes.
Definition: gjk.h:62
class for EPA algorithm
Definition: gjk.h:257
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:296
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:54
SimplexF * ff
Definition: gjk.h:305
Vec3f w1
Definition: gjk.h:122
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:161
SimplexHorizon()
Definition: gjk.h:307
double FCL_REAL
Definition: data_types.h:69
GetSupportFunction getSupportFunc
Definition: gjk.h:82
Definition: gjk.h:129
Definition: traversal_node_setup.h:775
size_t nextsv
Definition: gjk.h:336
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:73
SimplexF * root
Definition: gjk.h:275
Status
Definition: gjk.h:139
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk.h:185
SimplexList()
Definition: gjk.h:277
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:76
FCL_REAL depth
Definition: gjk.h:333
FCL_REAL distance
Definition: gjk.h:157
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:122
~EPA()
Definition: gjk.h:347
unsigned char vertex_id_t
Definition: gjk.h:127
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
FCL_REAL d
Definition: gjk.h:263
SimplexV * sv_store
Definition: gjk.h:334
size_t nf
Definition: gjk.h:306
SimplexF()
Definition: gjk.h:270
SimplexF * fc_store
Definition: gjk.h:335
size_t count
Definition: gjk.h:276
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
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:134
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:74
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:191
Vec3f normal
Definition: gjk.h:332
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:69
size_t e[3]
Definition: gjk.h:267
SimplexF * l[2]
Definition: gjk.h:266
Status status
Definition: gjk.h:330
GJK::SimplexV SimplexV
Definition: gjk.h:259
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:200
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
#define HPP_FCL_DLLAPI
Definition: config.hh:64