hpp-fcl  1.7.8
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 
41 #include <vector>
42 
44 #include <hpp/fcl/math/transform.h>
45 
46 namespace hpp
47 {
48 namespace fcl
49 {
50 
51 namespace details
52 {
53 
56 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint);
57 
62 {
64  const ShapeBase* shapes[2];
65 
66  struct ShapeData {
67  std::vector<int8_t> visited;
68  };
69 
72  ShapeData data[2];
73 
77 
81 
84  Eigen::Array<FCL_REAL, 1, 2> inflation;
85 
91 
92  typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
93  const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
94  support_func_guess_t& hint, ShapeData data[2]);
95  GetSupportFunction getSupportFunc;
96 
97  MinkowskiDiff() : linear_log_convex_threshold (32), getSupportFunc (NULL) {}
98 
101  void set (const ShapeBase* shape0, const ShapeBase* shape1);
102 
104  void set (const ShapeBase* shape0, const ShapeBase* shape1,
105  const Transform3f& tf0, const Transform3f& tf1);
106 
108  inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const
109  {
110  return getSupport(shapes[0], d, dIsNormalized, hint);
111  }
112 
114  inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const
115  {
116  return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1;
117  }
118 
120  inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, support_func_guess_t& hint) const
121  {
122  assert(getSupportFunc != NULL);
123  getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, const_cast<ShapeData*>(data));
124  }
125 };
126 
131 {
133  {
135  Vec3f w0, w1;
138  };
139 
140  typedef unsigned char vertex_id_t;
141 
143  {
145  SimplexV* vertex[4];
147  vertex_id_t rank;
148 
149  Simplex() {}
150  };
151 
152  enum Status {Valid, Inside, Failed};
153 
171  Simplex simplices[2];
172 
173 
181  GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_),
182  tolerance(tolerance_)
183  {
184  initialize();
185  }
186 
187  void initialize();
188 
190  Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess,
191  const support_func_guess_t& supportHint = support_func_guess_t::Zero());
192 
194  inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv,
195  support_func_guess_t& hint) const
196  {
197  shape->support(d, dIsNormalized, sv.w0, sv.w1, hint);
198  sv.w.noalias() = sv.w0 - sv.w1;
199  }
200 
202  bool encloseOrigin();
203 
205  inline Simplex* getSimplex() const
206  {
207  return simplex;
208  }
209 
212  {
213  return distance < distance_upper_bound;
214  }
215 
221  {
222  return distance > - shape.inflation.sum();
223  }
224 
227  bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
228 
230  Vec3f getGuessFromSimplex() const;
231 
236  void setDistanceEarlyBreak (const FCL_REAL& dup)
237  {
238  distance_upper_bound = dup;
239  }
240 
241 private:
242  SimplexV store_v[4];
243  SimplexV* free_v[4];
244  vertex_id_t nfree;
245  vertex_id_t current;
246  Simplex* simplex;
247  Status status;
248 
249  unsigned int max_iterations;
250  FCL_REAL tolerance;
251  FCL_REAL distance_upper_bound;
252 
254  inline void removeVertex(Simplex& simplex);
255 
257  inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
258  support_func_guess_t& hint);
259 
261  bool projectLineOrigin(const Simplex& current, Simplex& next);
262 
264  bool projectTriangleOrigin(const Simplex& current, Simplex& next);
265 
267  bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
268 };
269 
270 
271 static const size_t EPA_MAX_FACES = 128;
272 static const size_t EPA_MAX_VERTICES = 64;
273 static const FCL_REAL EPA_EPS = 0.000001;
274 static const size_t EPA_MAX_ITERATIONS = 255;
275 
278 {
281  {
284  SimplexV* vertex[3]; // a face has three vertices
285  SimplexF* f[3]; // a face has three adjacent faces
286  SimplexF* l[2]; // the pre and post faces in the list
287  size_t e[3];
288  size_t pass;
289 
290  SimplexF () : n(Vec3f::Zero()) {};
291  };
292 
294  {
296  size_t count;
297  SimplexList() : root(NULL), count(0) {}
298  void append(SimplexF* face)
299  {
300  face->l[0] = NULL;
301  face->l[1] = root;
302  if(root) root->l[0] = face;
303  root = face;
304  ++count;
305  }
306 
307  void remove(SimplexF* face)
308  {
309  if(face->l[1]) face->l[1]->l[0] = face->l[0];
310  if(face->l[0]) face->l[0]->l[1] = face->l[1];
311  if(face == root) root = face->l[1];
312  --count;
313  }
314  };
315 
316  static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
317  {
318  fa->e[ea] = eb; fa->f[ea] = fb;
319  fb->e[eb] = ea; fb->f[eb] = fa;
320  }
321 
323  {
324  SimplexF* cf; // current face in the horizon
325  SimplexF* ff; // first face in the horizon
326  size_t nf; // number of faces in the horizon
327  SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
328  };
329 
330 private:
331  unsigned int max_face_num;
332  unsigned int max_vertex_num;
333  unsigned int max_iterations;
334  FCL_REAL tolerance;
335 
336 public:
337 
338  enum Status {
339  Failed = 0,
340  Valid = 1,
341  AccuracyReached = 1 << 1 | Valid ,
342  Degenerated = 1 << 1 | Failed,
343  NonConvex = 2 << 1 | Failed,
344  InvalidHull = 3 << 1 | Failed,
345  OutOfFaces = 4 << 1 | Failed,
346  OutOfVertices = 5 << 1 | Failed,
347  FallBack = 6 << 1 | Failed
348  };
349 
354  SimplexV* sv_store;
356  size_t nextsv;
358 
359  EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_),
360  max_vertex_num(max_vertex_num_),
361  max_iterations(max_iterations_),
362  tolerance(tolerance_)
363  {
364  initialize();
365  }
366 
368  {
369  delete [] sv_store;
370  delete [] fc_store;
371  }
372 
373  void initialize();
374 
378  Status evaluate(GJK& gjk, const Vec3f& guess);
379 
382  bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
383 
384 private:
385  bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
386 
387  SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced);
388 
390  SimplexF* findBest();
391 
393  bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
394 };
395 
396 
397 } // details
398 
399 
400 
401 }
402 
403 
404 } // namespace hpp
405 
406 #endif
SimplexList stock
Definition: gjk.h:357
Simple transform class used locally by InterpMotion.
Definition: transform.h:58
Definition: gjk.h:152
MinkowskiDiff const * shape
Definition: gjk.h:154
support_func_guess_t support_hint
Definition: gjk.h:156
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:84
GJK::Simplex result
Definition: gjk.h:351
Status
Definition: gjk.h:338
MinkowskiDiff()
Definition: gjk.h:97
std::vector< int8_t > visited
Definition: gjk.h:67
Main namespace.
Definition: AABB.h:43
size_t pass
Definition: gjk.h:288
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:108
Vec3f n
Definition: gjk.h:282
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:408
Simplex()
Definition: gjk.h:149
SimplexF * f[3]
Definition: gjk.h:285
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
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:359
void append(SimplexF *face)
Definition: gjk.h:298
class for GJK algorithm
Definition: gjk.h:130
SimplexF * cf
Definition: gjk.h:324
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:114
Vec3f ray
Definition: gjk.h:155
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction) ...
Definition: gjk.h:137
Minkowski difference class of two shapes.
Definition: gjk.h:61
class for EPA algorithm
Definition: gjk.h:277
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:316
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:54
SimplexF * ff
Definition: gjk.h:325
Vec3f w1
Definition: gjk.h:135
GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
Definition: gjk.h:181
SimplexHorizon()
Definition: gjk.h:327
double FCL_REAL
Definition: data_types.h:66
GetSupportFunction getSupportFunc
Definition: gjk.h:95
Definition: gjk.h:142
Definition: traversal_node_setup.h:784
size_t nextsv
Definition: gjk.h:356
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:80
SimplexF * root
Definition: gjk.h:295
Status
Definition: gjk.h:152
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk.h:205
SimplexList()
Definition: gjk.h:297
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:71
FCL_REAL depth
Definition: gjk.h:353
FCL_REAL distance
Definition: gjk.h:170
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:135
~EPA()
Definition: gjk.h:367
unsigned char vertex_id_t
Definition: gjk.h:140
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:120
FCL_REAL d
Definition: gjk.h:283
SimplexV * sv_store
Definition: gjk.h:354
size_t nf
Definition: gjk.h:326
SimplexF()
Definition: gjk.h:290
SimplexF * fc_store
Definition: gjk.h:355
size_t count
Definition: gjk.h:296
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:236
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
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:147
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:211
Vec3f normal
Definition: gjk.h:352
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:76
size_t e[3]
Definition: gjk.h:287
SimplexF * l[2]
Definition: gjk.h:286
Status status
Definition: gjk.h:350
GJK::SimplexV SimplexV
Definition: gjk.h:279
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:220
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:194
#define HPP_FCL_DLLAPI
Definition: config.hh:64