hpp-fcl: include/hpp/fcl/narrowphase/gjk.h Source File

hpp-fcl 1.7.2 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
43 #include <hpp/fcl/shape/geometric_shapes.h>
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
61 struct HPP_FCL_DLLAPI MinkowskiDiff
62 {
64 const ShapeBase* shapes[2];
65
66 struct ShapeData {
67 std::vector<int8_t> visited;
68 };
69
72 ShapeData data[2];
73
76 Matrix3f oR1;
77
80 Vec3f ot1;
81
84 Eigen::Array<FCL_REAL, 1, 2> inflation;
85
90 int linear_log_convex_threshold;
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
130 struct HPP_FCL_DLLAPI GJK
131 {
132 struct HPP_FCL_DLLAPI SimplexV
133 {
135 Vec3f w0, w1;
137 Vec3f w;
138 };
139
140 typedef unsigned char vertex_id_t;
141
142 struct HPP_FCL_DLLAPI Simplex
143 {
145 SimplexV* vertex[4];
147 vertex_id_t rank;
148
149 Simplex() {}
150 };
151
152 enum Status {Valid, Inside, Failed};
153
154 MinkowskiDiff const* shape;
155 Vec3f ray;
156 support_func_guess_t support_hint;
170 FCL_REAL distance;
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
211 bool hasClosestPoints ()
212 {
213 return distance < distance_upper_bound;
214 }
215
220 bool hasPenetrationInformation (const MinkowskiDiff& shape)
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
277 struct HPP_FCL_DLLAPI EPA
278 {
279 typedef GJK::SimplexV SimplexV;
280 struct HPP_FCL_DLLAPI SimplexF
281 {
282 Vec3f n;
283 FCL_REAL d;
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
293 struct HPP_FCL_DLLAPI SimplexList
294 {
295 SimplexF* root;
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
322 struct HPP_FCL_DLLAPI SimplexHorizon
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
350 Status status;
351 GJK::Simplex result;
352 Vec3f normal;
353 FCL_REAL depth;
354 SimplexV* sv_store;
355 SimplexF* fc_store;
356 size_t nextsv;
357 SimplexList hull, stock;
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
367 ~EPA()
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
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:55
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
geometric_shapes.h
hpp::fcl::distance
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.
details
Definition: traversal_node_setup.h:785
hpp::fcl::details::getSupport
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:71
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::details::EPA::SimplexF
Definition: gjk.h:281
hpp::fcl::details::EPA::SimplexF::d
FCL_REAL d
Definition: gjk.h:283
hpp::fcl::details::EPA::SimplexF::n
Vec3f n
Definition: gjk.h:282
hpp::fcl::details::EPA::SimplexF::f
SimplexF * f[3]
Definition: gjk.h:285
hpp::fcl::details::EPA::SimplexF::pass
size_t pass
Definition: gjk.h:288
hpp::fcl::details::EPA::SimplexF::SimplexF
SimplexF()
Definition: gjk.h:290
hpp::fcl::details::EPA::SimplexF::e
size_t e[3]
Definition: gjk.h:287
hpp::fcl::details::EPA::SimplexF::l
SimplexF * l[2]
Definition: gjk.h:286
hpp::fcl::details::EPA::SimplexHorizon
Definition: gjk.h:323
hpp::fcl::details::EPA::SimplexHorizon::cf
SimplexF * cf
Definition: gjk.h:324
hpp::fcl::details::EPA::SimplexHorizon::ff
SimplexF * ff
Definition: gjk.h:325
hpp::fcl::details::EPA::SimplexHorizon::nf
size_t nf
Definition: gjk.h:326
hpp::fcl::details::EPA::SimplexHorizon::SimplexHorizon
SimplexHorizon()
Definition: gjk.h:327
hpp::fcl::details::EPA::SimplexList
Definition: gjk.h:294
hpp::fcl::details::EPA::SimplexList::SimplexList
SimplexList()
Definition: gjk.h:297
hpp::fcl::details::EPA::SimplexList::count
size_t count
Definition: gjk.h:296
hpp::fcl::details::EPA::SimplexList::remove
void remove(SimplexF *face)
Definition: gjk.h:307
hpp::fcl::details::EPA::SimplexList::root
SimplexF * root
Definition: gjk.h:295
hpp::fcl::details::EPA::SimplexList::append
void append(SimplexF *face)
Definition: gjk.h:298
hpp::fcl::details::EPA
class for EPA algorithm
Definition: gjk.h:278
hpp::fcl::details::EPA::status
Status status
Definition: gjk.h:350
hpp::fcl::details::EPA::SimplexV
GJK::SimplexV SimplexV
Definition: gjk.h:279
hpp::fcl::details::EPA::fc_store
SimplexF * fc_store
Definition: gjk.h:355
hpp::fcl::details::EPA::normal
Vec3f normal
Definition: gjk.h:352
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:359
hpp::fcl::details::EPA::~EPA
~EPA()
Definition: gjk.h:367
hpp::fcl::details::EPA::nextsv
size_t nextsv
Definition: gjk.h:356
hpp::fcl::details::EPA::Status
Status
Definition: gjk.h:338
hpp::fcl::details::EPA::initialize
void initialize()
hpp::fcl::details::EPA::evaluate
Status evaluate(GJK &gjk, const Vec3f &guess)
hpp::fcl::details::EPA::depth
FCL_REAL depth
Definition: gjk.h:353
hpp::fcl::details::EPA::hull
SimplexList hull
Definition: gjk.h:357
hpp::fcl::details::EPA::bind
static void bind(SimplexF *fa, size_t ea, SimplexF *fb, size_t eb)
Definition: gjk.h:316
hpp::fcl::details::EPA::result
GJK::Simplex result
Definition: gjk.h:351
hpp::fcl::details::EPA::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
hpp::fcl::details::EPA::sv_store
SimplexV * sv_store
Definition: gjk.h:354
hpp::fcl::details::GJK::Simplex
Definition: gjk.h:143
hpp::fcl::details::GJK::Simplex::rank
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:147
hpp::fcl::details::GJK::Simplex::Simplex
Simplex()
Definition: gjk.h:149
hpp::fcl::details::GJK::SimplexV
Definition: gjk.h:133
hpp::fcl::details::GJK::SimplexV::w1
Vec3f w1
Definition: gjk.h:135
hpp::fcl::details::GJK::SimplexV::w0
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:135
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:137
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:131
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:205
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:194
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:236
hpp::fcl::details::GJK::hasPenetrationInformation
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:220
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:156
hpp::fcl::details::GJK::shape
MinkowskiDiff const * shape
Definition: gjk.h:154
hpp::fcl::details::GJK::Status
Status
Definition: gjk.h:152
hpp::fcl::details::GJK::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:181
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:170
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:155
hpp::fcl::details::GJK::evaluate
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess, const support_func_guess_t &supportHint=support_func_guess_t::Zero())
GJK algorithm, given the initial value guess.
hpp::fcl::details::GJK::vertex_id_t
unsigned char vertex_id_t
Definition: gjk.h:140
hpp::fcl::details::GJK::getGuessFromSimplex
Vec3f getGuessFromSimplex() const
get the guess from current simplex
hpp::fcl::details::GJK::initialize
void initialize()
hpp::fcl::details::GJK::encloseOrigin
bool encloseOrigin()
whether the simplex enclose the origin
hpp::fcl::details::GJK::hasClosestPoints
bool hasClosestPoints()
Tells whether the closest points are available.
Definition: gjk.h:211
hpp::fcl::details::MinkowskiDiff::ShapeData
Definition: gjk.h:66
hpp::fcl::details::MinkowskiDiff::ShapeData::visited
std::vector< int8_t > visited
Definition: gjk.h:67
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:62
hpp::fcl::details::MinkowskiDiff::support1
Vec3f support1(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape1
Definition: gjk.h:114
hpp::fcl::details::MinkowskiDiff::support0
Vec3f support0(const Vec3f &d, bool dIsNormalized, int &hint) const
support function for shape0
Definition: gjk.h:108
hpp::fcl::details::MinkowskiDiff::ot1
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:80
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:120
hpp::fcl::details::MinkowskiDiff::oR1
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:76
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1, const Transform3f &tf0, const Transform3f &tf1)
Set the two shapes, with a relative transformation.
hpp::fcl::details::MinkowskiDiff::getSupportFunc
GetSupportFunction getSupportFunc
Definition: gjk.h:95
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::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
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:84
hpp::fcl::details::MinkowskiDiff::MinkowskiDiff
MinkowskiDiff()
Definition: gjk.h:97
transform.h
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:408