hpp-fcl  1.4.5
HPP fork of FCL -- The Flexible Collision Library
narrowphase.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  * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
39 #ifndef HPP_FCL_NARROWPHASE_H
40 #define HPP_FCL_NARROWPHASE_H
41 
43 
44 namespace hpp
45 {
46 namespace fcl
47 {
48 
49 
50 
51 
54  {
56  template<typename S1, typename S2>
57  bool shapeIntersect(const S1& s1, const Transform3f& tf1,
58  const S2& s2, const Transform3f& tf2,
59  FCL_REAL& distance_lower_bound,
60  bool enable_penetration,
61  Vec3f* contact_points, Vec3f* normal) const
62  {
63  Vec3f guess(1, 0, 0);
64  support_func_guess_t support_hint;
65  if(enable_cached_guess) {
66  guess = cached_guess;
67  support_hint = support_func_cached_guess;
68  } else
69  support_hint.setZero();
70 
72  shape.set (&s1, &s2, tf1, tf2);
73 
74  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
75  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
76  if(enable_cached_guess) {
77  cached_guess = gjk.getGuessFromSimplex();
78  support_func_cached_guess = gjk.support_hint;
79  }
80 
81  Vec3f w0, w1;
82  switch(gjk_status) {
84  if (!enable_penetration && contact_points == NULL && normal == NULL)
85  return true;
86  if (gjk.hasPenetrationInformation(shape)) {
87  gjk.getClosestPoints (shape, w0, w1);
88  distance_lower_bound = gjk.distance;
89  if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
90  if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
91  return true;
92  } else {
93  details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
94  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
95  if(epa_status & details::EPA::Valid
96  || epa_status == details::EPA::OutOfFaces // Warnings
97  || epa_status == details::EPA::OutOfVertices // Warnings
98  )
99  {
100  epa.getClosestPoints (shape, w0, w1);
101  distance_lower_bound = -epa.depth;
102  if(normal) *normal = tf1.getRotation() * epa.normal;
103  if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
104  return true;
105  }
106  distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
107  // EPA failed but we know there is a collision so we should
108  return true;
109  }
110  break;
111  case details::GJK::Valid:
112  distance_lower_bound = gjk.distance;
113  break;
114  default:
115  ;
116  }
117 
118  return false;
119  }
120 
123  template<typename S>
124  bool shapeTriangleInteraction
125  (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
126  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
127  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
128  {
129  bool col;
130  // Express everything in frame 1
131  const Transform3f tf_1M2 (tf1.inverseTimes(tf2));
132  TriangleP tri(
133  tf_1M2.transform (P1),
134  tf_1M2.transform (P2),
135  tf_1M2.transform (P3));
136 
137  Vec3f guess(1, 0, 0);
138  support_func_guess_t support_hint;
139  if(enable_cached_guess) {
140  guess = cached_guess;
141  support_hint = support_func_cached_guess;
142  } else
143  support_hint.setZero();
144 
146  shape.set (&s, &tri);
147 
148  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
149  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
150  if(enable_cached_guess) {
151  cached_guess = gjk.getGuessFromSimplex();
152  support_func_cached_guess = gjk.support_hint;
153  }
154 
155  Vec3f w0, w1;
156  switch(gjk_status) {
158  col = true;
159  if (gjk.hasPenetrationInformation(shape)) {
160  gjk.getClosestPoints (shape, w0, w1);
161  distance = gjk.distance;
162  normal = tf1.getRotation() * (w1 - w0).normalized();
163  p1 = p2 = tf1.transform((w0 + w1) / 2);
164  } else {
165  details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
166  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
167  if(epa_status & details::EPA::Valid
168  || epa_status == details::EPA::OutOfFaces // Warnings
169  || epa_status == details::EPA::OutOfVertices // Warnings
170  )
171  {
172  epa.getClosestPoints (shape, w0, w1);
173  distance = -epa.depth;
174  normal = -epa.normal;
175  p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
176  assert (distance <= 1e-6);
177  } else {
178  distance = -std::numeric_limits<FCL_REAL>::max();
179  gjk.getClosestPoints (shape, w0, w1);
180  p1 = p2 = tf1.transform (w0);
181  }
182  }
183  break;
184  case details::GJK::Valid:
186  col = false;
187 
188  gjk.getClosestPoints (shape, p1, p2);
189  // TODO On degenerated case, the closest point may be wrong
190  // (i.e. an object face normal is colinear to gjk.ray
191  // assert (distance == (w0 - w1).norm());
192  distance = gjk.distance;
193 
194  p1 = tf1.transform (p1);
195  p2 = tf1.transform (p2);
196  assert (distance > 0);
197  break;
198  default:
199  assert (false && "should not reach type part.");
200  return true;
201  }
202  return col;
203  }
204 
206  template<typename S1, typename S2>
207  bool shapeDistance(const S1& s1, const Transform3f& tf1,
208  const S2& s2, const Transform3f& tf2,
209  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
210  Vec3f& normal) const
211  {
212 #ifndef NDEBUG
213  FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
214 #endif
215  Vec3f guess(1, 0, 0);
216  support_func_guess_t support_hint;
217  if(enable_cached_guess) {
218  guess = cached_guess;
219  support_hint = support_func_cached_guess;
220  } else
221  support_hint.setZero();
222 
224  shape.set (&s1, &s2, tf1, tf2);
225 
226  details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
227  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
228  if(enable_cached_guess) {
229  cached_guess = gjk.getGuessFromSimplex();
230  support_func_cached_guess = gjk.support_hint;
231  }
232 
233  if(gjk_status == details::GJK::Failed)
234  {
235  // TODO: understand why GJK fails between cylinder and box
236  assert (distance * distance < sqrt (eps));
237  Vec3f w0, w1;
238  gjk.getClosestPoints (shape, w0, w1);
239  distance = 0;
240  p1 = p2 = tf1.transform (.5* (w0 + w1));
241  normal = Vec3f (0,0,0);
242  return false;
243  }
244  else if(gjk_status == details::GJK::Valid)
245  {
246  gjk.getClosestPoints (shape, p1, p2);
247  // TODO On degenerated case, the closest point may be wrong
248  // (i.e. an object face normal is colinear to gjk.ray
249  // assert (distance == (w0 - w1).norm());
250  distance = gjk.distance;
251 
252  normal = (tf1.getRotation() * gjk.ray).normalized();
253  p1 = tf1.transform (p1);
254  p2 = tf1.transform (p2);
255  return true;
256  }
257  else
258  {
259  assert (gjk_status == details::GJK::Inside);
260  if (gjk.hasPenetrationInformation (shape)) {
261  gjk.getClosestPoints (shape, p1, p2);
262  distance = gjk.distance;
263  // Return contact points in case of collision
264  //p1 = tf1.transform (p1);
265  //p2 = tf1.transform (p2);
266  normal = (tf1.getRotation() * (p2 - p1)).normalized();
267  p1 = p2 = tf1.transform(p1);
268  } else {
269  details::EPA epa(epa_max_face_num, epa_max_vertex_num,
270  epa_max_iterations, epa_tolerance);
271  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
272  if(epa_status & details::EPA::Valid
273  || epa_status == details::EPA::OutOfFaces // Warnings
274  || epa_status == details::EPA::OutOfVertices // Warnings
275  )
276  {
277  Vec3f w0, w1;
278  epa.getClosestPoints (shape, w0, w1);
279  assert (epa.depth >= -eps);
280  distance = std::min (0., -epa.depth);
281  // TODO should be
282  // normal = tf1.getRotation() * epa.normal;
283  normal = tf2.getRotation() * epa.normal;
284  p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
285  return false;
286  }
287  distance = -std::numeric_limits<FCL_REAL>::max();
288  gjk.getClosestPoints (shape, p1, p2);
289  p1 = p2 = tf1.transform (p1);
290  }
291  return false;
292  }
293  }
294 
297  {
298  gjk_max_iterations = 128;
299  gjk_tolerance = 1e-6;
300  epa_max_face_num = 128;
301  epa_max_vertex_num = 64;
302  epa_max_iterations = 255;
303  epa_tolerance = 1e-6;
304  enable_cached_guess = false;
305  cached_guess = Vec3f(1, 0, 0);
306  support_func_cached_guess = support_func_guess_t::Zero();
307  }
308 
309  void enableCachedGuess(bool if_enable) const
310  {
311  enable_cached_guess = if_enable;
312  }
313 
314  void setCachedGuess(const Vec3f& guess) const
315  {
316  cached_guess = guess;
317  }
318 
320  {
321  return cached_guess;
322  }
323 
325  unsigned int epa_max_face_num;
326 
328  unsigned int epa_max_vertex_num;
329 
331  unsigned int epa_max_iterations;
332 
335 
338 
341 
343  mutable bool enable_cached_guess;
344 
347 
350  };
351 
352 #if __cplusplus < 201103L
353 #pragma GCC diagnostic push
354 #pragma GCC diagnostic ignored "-Wc99-extensions"
355 #endif
356 
359 // param doc is the doxygen detailled description (should be enclosed in /** */
360 // and contain no dot for some obscure reasons).
361 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \
362  \
363  doc \
364  template<> \
365  bool GJKSolver::shapeIntersect<Shape1, Shape2> \
366  (const Shape1& s1, const Transform3f& tf1, \
367  const Shape2& s2, const Transform3f& tf2, \
368  FCL_REAL& distance_lower_bound, bool enable_penetration, \
369  Vec3f* contact_points, Vec3f* normal) const
370 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
371  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
372 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
373  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc); \
374  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc)
375 
377  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,);
378  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
379  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,);
380 
381 #ifdef IS_DOXYGEN // for doxygen only
382 
385  template<> bool GJKSolver::shapeIntersect<Box, Box>
386  (const Box& s1, const Transform3f& tf1,
387  const Box& s2, const Transform3f& tf2,
388  FCL_REAL& distance_lower_bound, bool enable_penetration,
389  Vec3f* contact_points, Vec3f* normal) const;
390 #endif
391  //HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
392  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,);
394 
395  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace,);
396  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane,);
397 
398  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace,);
399  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane,);
400 
401  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace,);
403 
405 
407  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace,);
408 
409 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT
410 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
411 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
412 
414 
417 
418 // param doc is the doxygen detailled description (should be enclosed in /** */
419 // and contain no dot for some obscure reasons).
420 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \
421  \
422  doc \
423  template<> bool GJKSolver::shapeTriangleInteraction<Shape> \
424  (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \
425  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, \
426  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
427 
429  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace,);
431 
432 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
433 
435 
438 
439 // param doc is the doxygen detailled description (should be enclosed in /** */
440 // and contain no dot for some obscure reasons).
441 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \
442  \
443  doc \
444  template<> \
445  bool GJKSolver::shapeDistance<Shape1, Shape2> \
446  (const Shape1& s1, const Transform3f& tf1, \
447  const Shape2& s2, const Transform3f& tf2, \
448  FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
449 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc) \
450  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc)
451 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc) \
452  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc); \
453  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc)
454 
456  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule,);
457  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder,);
459 
462  );
463 
466  );
467 
468 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE
469 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
470 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
471 
473 #if __cplusplus < 201103L
474 #pragma GCC diagnostic pop
475 #endif
476 }
477 
478 } // namespace hpp
479 
480 #endif
Definition: gjk.h:139
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
Definition: gjk.h:139
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:331
support_func_guess_t support_hint
Definition: gjk.h:143
Status
Definition: gjk.h:318
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.
Main namespace.
Definition: AABB.h:43
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:325
Definition: gjk.h:320
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:343
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:296
void set(const ShapeBase *shape0, const ShapeBase *shape1)
class for GJK algorithm
Definition: gjk.h:117
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,)
Vec3f ray
Definition: gjk.h:142
Minkowski difference class of two shapes.
Definition: gjk.h:62
class for EPA algorithm
Definition: gjk.h:257
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.
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Vec3f getGuessFromSimplex() const
get the guess from current simplex
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:169
double FCL_REAL
Definition: data_types.h:69
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:328
Status
Definition: gjk.h:139
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:309
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,)
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:70
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
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:340
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:189
bool shapeDistance(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
distance computation between two shapes
Definition: narrowphase.h:207
Vec3f cached_guess
smart guess
Definition: narrowphase.h:346
Status evaluate(GJK &gjk, const Vec3f &guess)
bool shapeIntersect(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance_lower_bound, bool enable_penetration, Vec3f *contact_points, Vec3f *normal) const
intersection checking between two shapes
Definition: narrowphase.h:57
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,)
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,)
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:53
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:74
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:337
Vec3f normal
Definition: gjk.h:332
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:349
Vec3f getCachedGuess() const
Definition: narrowphase.h:319
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:120
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:314
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:200
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,)
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:334
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Definition: gjk.h:139