hpp-fcl  1.4.4
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 
53  struct GJKSolver
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);
65 
67  shape.set (&s1, &s2, tf1, tf2);
68 
69  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
70  details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
72 
73  Vec3f w0, w1;
74  switch(gjk_status) {
76  if (!enable_penetration && contact_points == NULL && normal == NULL)
77  return true;
78  if (gjk.hasPenetrationInformation(shape)) {
79  gjk.getClosestPoints (shape, w0, w1);
80  distance_lower_bound = gjk.distance;
81  if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
82  if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
83  return true;
84  } else {
86  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
87  if(epa_status & details::EPA::Valid
88  || epa_status == details::EPA::OutOfFaces // Warnings
89  || epa_status == details::EPA::OutOfVertices // Warnings
90  )
91  {
92  epa.getClosestPoints (shape, w0, w1);
93  distance_lower_bound = -epa.depth;
94  if(normal) *normal = tf1.getRotation() * epa.normal;
95  if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
96  return true;
97  }
98  distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
99  // EPA failed but we know there is a collision so we should
100  return true;
101  }
102  break;
103  case details::GJK::Valid:
104  distance_lower_bound = gjk.distance;
105  break;
106  default:
107  ;
108  }
109 
110  return false;
111  }
112 
115  template<typename S>
117  (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
118  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
119  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
120  {
121  bool col;
122  // Express everything in frame 1
123  const Transform3f tf_1M2 (tf1.inverseTimes(tf2));
124  TriangleP tri(
125  tf_1M2.transform (P1),
126  tf_1M2.transform (P2),
127  tf_1M2.transform (P3));
128 
129  Vec3f guess(1, 0, 0);
130  if(enable_cached_guess) guess = cached_guess;
131 
133  shape.set (&s, &tri);
134 
135  details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
136  details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
138 
139  Vec3f w0, w1;
140  switch(gjk_status) {
142  col = true;
143  if (gjk.hasPenetrationInformation(shape)) {
144  gjk.getClosestPoints (shape, w0, w1);
145  distance = gjk.distance;
146  normal = tf1.getRotation() * (w1 - w0).normalized();
147  p1 = p2 = tf1.transform((w0 + w1) / 2);
148  } else {
150  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
151  if(epa_status & details::EPA::Valid
152  || epa_status == details::EPA::OutOfFaces // Warnings
153  || epa_status == details::EPA::OutOfVertices // Warnings
154  )
155  {
156  epa.getClosestPoints (shape, w0, w1);
157  distance = -epa.depth;
158  normal = -epa.normal;
159  p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
160  assert (distance <= 1e-6);
161  } else {
162  distance = -std::numeric_limits<FCL_REAL>::max();
163  gjk.getClosestPoints (shape, w0, w1);
164  p1 = p2 = tf1.transform (w0);
165  }
166  }
167  break;
168  case details::GJK::Valid:
170  col = false;
171 
172  gjk.getClosestPoints (shape, p1, p2);
173  // TODO On degenerated case, the closest point may be wrong
174  // (i.e. an object face normal is colinear to gjk.ray
175  // assert (distance == (w0 - w1).norm());
176  distance = gjk.distance;
177 
178  p1 = tf1.transform (p1);
179  p2 = tf1.transform (p2);
180  assert (distance > 0);
181  break;
182  default:
183  assert (false && "should not reach type part.");
184  return true;
185  }
186  return col;
187  }
188 
190  template<typename S1, typename S2>
191  bool shapeDistance(const S1& s1, const Transform3f& tf1,
192  const S2& s2, const Transform3f& tf2,
193  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
194  Vec3f& normal) const
195  {
196 #ifndef NDEBUG
197  FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
198 #endif
199  Vec3f guess(1, 0, 0);
200  if(enable_cached_guess) guess = cached_guess;
201 
203  shape.set (&s1, &s2, tf1, tf2);
204 
205  details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
206  details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
208 
209  if(gjk_status == details::GJK::Failed)
210  {
211  // TODO: understand why GJK fails between cylinder and box
212  assert (distance * distance < sqrt (eps));
213  Vec3f w0, w1;
214  gjk.getClosestPoints (shape, w0, w1);
215  distance = 0;
216  p1 = p2 = tf1.transform (.5* (w0 + w1));
217  normal = Vec3f (0,0,0);
218  return false;
219  }
220  else if(gjk_status == details::GJK::Valid)
221  {
222  gjk.getClosestPoints (shape, p1, p2);
223  // TODO On degenerated case, the closest point may be wrong
224  // (i.e. an object face normal is colinear to gjk.ray
225  // assert (distance == (w0 - w1).norm());
226  distance = gjk.distance;
227 
228  p1 = tf1.transform (p1);
229  p2 = tf1.transform (p2);
230  return true;
231  }
232  else
233  {
234  assert (gjk_status == details::GJK::Inside);
235  if (gjk.hasPenetrationInformation (shape)) {
236  gjk.getClosestPoints (shape, p1, p2);
237  distance = gjk.distance;
238  // Return contact points in case of collision
239  //p1 = tf1.transform (p1);
240  //p2 = tf1.transform (p2);
241  normal = (tf1.getRotation() * (p2 - p1)).normalized();
242  p1 = p2 = tf1.transform(p1);
243  } else {
246  details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
247  if(epa_status & details::EPA::Valid
248  || epa_status == details::EPA::OutOfFaces // Warnings
249  || epa_status == details::EPA::OutOfVertices // Warnings
250  )
251  {
252  Vec3f w0, w1;
253  epa.getClosestPoints (shape, w0, w1);
254  assert (epa.depth >= -eps);
255  distance = std::min (0., -epa.depth);
256  // TODO should be
257  // normal = tf1.getRotation() * epa.normal;
258  normal = tf2.getRotation() * epa.normal;
259  p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
260  return false;
261  }
262  distance = -std::numeric_limits<FCL_REAL>::max();
263  gjk.getClosestPoints (shape, p1, p2);
264  p1 = p2 = tf1.transform (p1);
265  }
266  return false;
267  }
268  }
269 
272  {
273  gjk_max_iterations = 128;
274  gjk_tolerance = 1e-6;
275  epa_max_face_num = 128;
276  epa_max_vertex_num = 64;
277  epa_max_iterations = 255;
278  epa_tolerance = 1e-6;
279  enable_cached_guess = false;
280  cached_guess = Vec3f(1, 0, 0);
281  }
282 
283  void enableCachedGuess(bool if_enable) const
284  {
285  enable_cached_guess = if_enable;
286  }
287 
288  void setCachedGuess(const Vec3f& guess) const
289  {
290  cached_guess = guess;
291  }
292 
294  {
295  return cached_guess;
296  }
297 
299  unsigned int epa_max_face_num;
300 
302  unsigned int epa_max_vertex_num;
303 
305  unsigned int epa_max_iterations;
306 
309 
312 
315 
317  mutable bool enable_cached_guess;
318 
321  };
322 
323 #pragma GCC diagnostic push
324 #pragma GCC diagnostic ignored "-Wc99-extensions"
325 
328 // param doc is the doxygen detailled description (should be enclosed in /** */
329 // and contain no dot for some obscure reasons).
330 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \
331  \
332  doc \
333  template<> \
334  bool GJKSolver::shapeIntersect<Shape1, Shape2> \
335  (const Shape1& s1, const Transform3f& tf1, \
336  const Shape2& s2, const Transform3f& tf2, \
337  FCL_REAL& distance_lower_bound, bool enable_penetration, \
338  Vec3f* contact_points, Vec3f* normal) const
339 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
340  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
341 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
342  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc); \
343  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc)
344 
346  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,);
347  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
348  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,);
349 
350 #ifdef IS_DOXYGEN // for doxygen only
351 
354  template<> bool GJKSolver::shapeIntersect<Box, Box>
355  (const Box& s1, const Transform3f& tf1,
356  const Box& s2, const Transform3f& tf2,
357  FCL_REAL& distance_lower_bound, bool enable_penetration,
358  Vec3f* contact_points, Vec3f* normal) const;
359 #endif
360  //HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
361  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,);
363 
364  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace,);
365  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane,);
366 
367  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace,);
368  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane,);
369 
370  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace,);
372 
374 
376  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace,);
377 
378 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT
379 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
380 #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
381 
383 
386 
387 // param doc is the doxygen detailled description (should be enclosed in /** */
388 // and contain no dot for some obscure reasons).
389 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \
390  \
391  doc \
392  template<> bool GJKSolver::shapeTriangleInteraction<Shape> \
393  (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \
394  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, \
395  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
396 
398  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace,);
400 
401 #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
402 
404 
407 
408 // param doc is the doxygen detailled description (should be enclosed in /** */
409 // and contain no dot for some obscure reasons).
410 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \
411  \
412  doc \
413  template<> \
414  bool GJKSolver::shapeDistance<Shape1, Shape2> \
415  (const Shape1& s1, const Transform3f& tf1, \
416  const Shape2& s2, const Transform3f& tf2, \
417  FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
418 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc) \
419  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc)
420 #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc) \
421  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc); \
422  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc)
423 
425  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule,);
426  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder,);
428 
431  );
432 
435  );
436 
437 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE
438 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
439 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
440 
442 #pragma GCC diagnostic pop
443 }
444 
445 } // namespace hpp
446 
447 #endif
Definition: gjk.h:137
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
Definition: gjk.h:137
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:305
Status
Definition: gjk.h:312
Main namespace.
Definition: AABB.h:43
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:299
Definition: gjk.h:314
bool enable_cached_guess
Whether smart guess can be provided.
Definition: narrowphase.h:317
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKSolver()
default setting for GJK algorithm
Definition: narrowphase.h:271
void set(const ShapeBase *shape0, const ShapeBase *shape1)
class for GJK algorithm
Definition: gjk.h:115
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,)
Minkowski difference class of two shapes.
Definition: gjk.h:61
class for EPA algorithm
Definition: gjk.h:251
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Vec3f getGuessFromSimplex() const
get the guess from current simplex
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.
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:169
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess)
GJK algorithm, given the initial value guess.
double FCL_REAL
Definition: data_types.h:68
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:302
Status
Definition: gjk.h:137
void enableCachedGuess(bool if_enable) const
Definition: narrowphase.h:283
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,)
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:70
FCL_REAL depth
Definition: gjk.h:327
FCL_REAL distance
Definition: gjk.h:154
FCL_REAL gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:314
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:191
Vec3f cached_guess
smart guess
Definition: narrowphase.h:320
bool shapeTriangleInteraction(const S &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
intersection checking between one shape and a triangle with transformation
Definition: narrowphase.h:117
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:73
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:311
Vec3f normal
Definition: gjk.h:326
Vec3f getCachedGuess() const
Definition: narrowphase.h:293
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:120
void setCachedGuess(const Vec3f &guess) const
Definition: narrowphase.h:288
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:195
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,)
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:308
Definition: gjk.h:137