FCL  0.6.0
Flexible Collision Library
gjk_solver_indep-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, 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 FCL_NARROWPHASE_GJKSOLVERINDEP_INL_H
39 #define FCL_NARROWPHASE_GJKSOLVERINDEP_INL_H
40 
41 #include "fcl/narrowphase/detail/gjk_solver_indep.h"
42 
43 #include <algorithm>
44 
45 #include "fcl/geometry/shape/triangle_p.h"
46 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk.h"
47 #include "fcl/narrowphase/detail/convexity_based_algorithm/epa.h"
48 #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h"
49 #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h"
50 #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h"
51 #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
52 #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h"
53 #include "fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h"
54 #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane.h"
55 
56 namespace fcl
57 {
58 
59 namespace detail
60 {
61 
62 //==============================================================================
63 extern template
64 struct GJKSolver_indep<double>;
65 
66 //==============================================================================
67 template <typename S>
68 template<typename Shape1, typename Shape2>
69 bool GJKSolver_indep<S>::shapeIntersect(const Shape1& s1, const Transform3<S>& tf1,
70  const Shape2& s2, const Transform3<S>& tf2,
71  Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal) const
72 {
73  bool res;
74 
75  if (contact_points || penetration_depth || normal)
76  {
77  std::vector<ContactPoint<S>> contacts;
78 
79  res = shapeIntersect(s1, tf1, s2, tf2, &contacts);
80 
81  if (!contacts.empty())
82  {
83  // Get the deepest contact point
84  const ContactPoint<S>& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth<S>);
85 
86  if (contact_points)
87  *contact_points = maxDepthContact.pos;
88 
89  if (penetration_depth)
90  *penetration_depth = maxDepthContact.penetration_depth;
91 
92  if (normal)
93  *normal = maxDepthContact.normal;
94  }
95  }
96  else
97  {
98  res = shapeIntersect(s1, tf1, s2, tf2, nullptr);
99  }
100 
101  return res;
102 }
103 
104 //==============================================================================
105 template<typename S, typename Shape1, typename Shape2>
106 struct ShapeIntersectIndepImpl
107 {
108  static bool run(
109  const GJKSolver_indep<S>& gjkSolver,
110  const Shape1& s1,
111  const Transform3<S>& tf1,
112  const Shape2& s2,
113  const Transform3<S>& tf2,
114  std::vector<ContactPoint<S>>* contacts)
115  {
116  Vector3<S> guess(1, 0, 0);
117  if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess;
118 
119  detail::MinkowskiDiff<S> shape;
120  shape.shapes[0] = &s1;
121  shape.shapes[1] = &s2;
122  shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear();
123  shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2;
124 
125  detail::GJK<S> gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance);
126  typename detail::GJK<S>::Status gjk_status = gjk.evaluate(shape, -guess);
127  if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex();
128 
129  switch(gjk_status)
130  {
131  case detail::GJK<S>::Inside:
132  {
133  detail::EPA<S> epa(gjkSolver.epa_max_face_num, gjkSolver.epa_max_vertex_num, gjkSolver.epa_max_iterations, gjkSolver.epa_tolerance);
134  typename detail::EPA<S>::Status epa_status = epa.evaluate(gjk, -guess);
135  if(epa_status != detail::EPA<S>::Failed)
136  {
137  Vector3<S> w0 = Vector3<S>::Zero();
138  for(size_t i = 0; i < epa.result.rank; ++i)
139  {
140  w0.noalias() += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
141  }
142  if(contacts)
143  {
144  Vector3<S> normal = epa.normal;
145  Vector3<S> point = tf1 * (w0 - epa.normal*(epa.depth *0.5));
146  S depth = -epa.depth;
147  contacts->emplace_back(normal, point, depth);
148  }
149  return true;
150  }
151  else return false;
152  }
153  break;
154  default:
155  ;
156  }
157 
158  return false;
159  }
160 };
161 
162 //==============================================================================
163 template<typename S>
164 template<typename Shape1, typename Shape2>
165 bool GJKSolver_indep<S>::shapeIntersect(
166  const Shape1& s1,
167  const Transform3<S>& tf1,
168  const Shape2& s2,
169  const Transform3<S>& tf2,
170  std::vector<ContactPoint<S>>* contacts) const
171 {
172  return ShapeIntersectIndepImpl<S, Shape1, Shape2>::run(
173  *this, s1, tf1, s2, tf2, contacts);
174 }
175 
176 // Shape intersect algorithms not using built-in GJK algorithm
177 //
178 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
179 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
180 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
181 // | box | O | | | | | | O | O | |
182 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
183 // | sphere |/////| O | | O | | | O | O | O |
184 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
185 // | ellipsoid |/////|////////| | | | | O | O | |
186 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
187 // | capsule |/////|////////|///////////| | | | O | O | |
188 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
189 // | cone |/////|////////|///////////|/////////| | | O | O | |
190 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
191 // | cylinder |/////|////////|///////////|/////////|//////| | O | O | |
192 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
193 // | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O |
194 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
195 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O |
196 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
197 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
198 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
199 
200 #define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
201  template <typename S>\
202  struct ShapeIntersectIndepImpl<S, SHAPE1<S>, SHAPE2<S>>\
203  {\
204  static bool run(\
205  const GJKSolver_indep<S>& /*gjkSolver*/,\
206  const SHAPE1<S>& s1,\
207  const Transform3<S>& tf1,\
208  const SHAPE2<S>& s2,\
209  const Transform3<S>& tf2,\
210  std::vector<ContactPoint<S>>* contacts)\
211  {\
212  return ALG(s1, tf1, s2, tf2, contacts);\
213  }\
214  };
215 
216 #define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)\
217  template <typename S>\
218  struct ShapeIntersectIndepImpl<S, SHAPE2<S>, SHAPE1<S>>\
219  {\
220  static bool run(\
221  const GJKSolver_indep<S>& /*gjkSolver*/,\
222  const SHAPE2<S>& s1,\
223  const Transform3<S>& tf1,\
224  const SHAPE1<S>& s2,\
225  const Transform3<S>& tf2,\
226  std::vector<ContactPoint<S>>* contacts)\
227  {\
228  const bool res = ALG(s2, tf2, s1, tf1, contacts);\
229  if (contacts) flipNormal(*contacts);\
230  return res;\
231  }\
232  };
233 
234 #define FCL_GJK_INDEP_SHAPE_INTERSECT(SHAPE, ALG)\
235  FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE, SHAPE, ALG)
236 
237 #define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(SHAPE1, SHAPE2, ALG)\
238  FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
239  FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)
240 
241 FCL_GJK_INDEP_SHAPE_INTERSECT(Sphere, detail::sphereSphereIntersect)
242 FCL_GJK_INDEP_SHAPE_INTERSECT(Box, detail::boxBoxIntersect)
243 
244 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect)
245 
246 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
247 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
248 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect)
249 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, detail::capsuleHalfspaceIntersect)
250 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, detail::cylinderHalfspaceIntersect)
251 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, detail::coneHalfspaceIntersect)
252 
253 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Plane, detail::spherePlaneIntersect)
254 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, detail::ellipsoidPlaneIntersect)
255 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Plane, detail::boxPlaneIntersect)
256 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Capsule, Plane, detail::capsulePlaneIntersect)
257 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cylinder, Plane, detail::cylinderPlaneIntersect)
258 FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cone, Plane, detail::conePlaneIntersect)
259 
260 template <typename S>
261 struct ShapeIntersectIndepImpl<S, Halfspace<S>, Halfspace<S>>
262 {
263  static bool run(
264  const GJKSolver_indep<S>& /*gjkSolver*/,
265  const Halfspace<S>& s1,
266  const Transform3<S>& tf1,
267  const Halfspace<S>& s2,
268  const Transform3<S>& tf2,
269  std::vector<ContactPoint<S>>* contacts)
270  {
271  Halfspace<S> s;
272  Vector3<S> p, d;
273  S depth;
274  int ret;
275  return detail::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
276  }
277 };
278 
279 template <typename S>
280 struct ShapeIntersectIndepImpl<S, Plane<S>, Plane<S>>
281 {
282  static bool run(
283  const GJKSolver_indep<S>& /*gjkSolver*/,
284  const Plane<S>& s1,
285  const Transform3<S>& tf1,
286  const Plane<S>& s2,
287  const Transform3<S>& tf2,
288  std::vector<ContactPoint<S>>* contacts)
289  {
290  return detail::planeIntersect(s1, tf1, s2, tf2, contacts);
291  }
292 };
293 
294 template <typename S>
295 struct ShapeIntersectIndepImpl<S, Plane<S>, Halfspace<S>>
296 {
297  static bool run(
298  const GJKSolver_indep<S>& /*gjkSolver*/,
299  const Plane<S>& s1,
300  const Transform3<S>& tf1,
301  const Halfspace<S>& s2,
302  const Transform3<S>& tf2,
303  std::vector<ContactPoint<S>>* contacts)
304  {
305  Plane<S> pl;
306  Vector3<S> p, d;
307  S depth;
308  int ret;
309  return detail::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
310  }
311 };
312 
313 template <typename S>
314 struct ShapeIntersectIndepImpl<S, Halfspace<S>, Plane<S>>
315 {
316  static bool run(
317  const GJKSolver_indep<S>& /*gjkSolver*/,
318  const Halfspace<S>& s1,
319  const Transform3<S>& tf1,
320  const Plane<S>& s2,
321  const Transform3<S>& tf2,
322  std::vector<ContactPoint<S>>* contacts)
323  {
324  Plane<S> pl;
325  Vector3<S> p, d;
326  S depth;
327  int ret;
328  return detail::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
329  }
330 };
331 
332 //==============================================================================
333 template<typename S, typename Shape>
334 struct ShapeTriangleIntersectIndepImpl
335 {
336  static bool run(
337  const GJKSolver_indep<S>& gjkSolver,
338  const Shape& s,
339  const Transform3<S>& tf,
340  const Vector3<S>& P1,
341  const Vector3<S>& P2,
342  const Vector3<S>& P3,
343  Vector3<S>* contact_points,
344  S* penetration_depth,
345  Vector3<S>* normal)
346  {
347  TriangleP<S> tri(P1, P2, P3);
348 
349  Vector3<S> guess(1, 0, 0);
350  if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess;
351 
352  detail::MinkowskiDiff<S> shape;
353  shape.shapes[0] = &s;
354  shape.shapes[1] = &tri;
355  shape.toshape1 = tf.linear();
356  shape.toshape0 = tf.inverse(Eigen::Isometry);
357 
358  detail::GJK<S> gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance);
359  typename detail::GJK<S>::Status gjk_status = gjk.evaluate(shape, -guess);
360  if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex();
361 
362  switch(gjk_status)
363  {
364  case detail::GJK<S>::Inside:
365  {
366  detail::EPA<S> epa(gjkSolver.epa_max_face_num, gjkSolver.epa_max_vertex_num, gjkSolver.epa_max_iterations, gjkSolver.epa_tolerance);
367  typename detail::EPA<S>::Status epa_status = epa.evaluate(gjk, -guess);
368  if(epa_status != detail::EPA<S>::Failed)
369  {
370  Vector3<S> w0 = Vector3<S>::Zero();
371  for(size_t i = 0; i < epa.result.rank; ++i)
372  {
373  w0.noalias() += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
374  }
375  if(penetration_depth) *penetration_depth = -epa.depth;
376  if(normal) *normal = -epa.normal;
377  if(contact_points) (*contact_points).noalias() = tf * (w0 - epa.normal*(epa.depth *0.5));
378  return true;
379  }
380  else return false;
381  }
382  break;
383  default:
384  ;
385  }
386 
387  return false;
388  }
389 };
390 
391 template<typename S>
392 template<typename Shape>
393 bool GJKSolver_indep<S>::shapeTriangleIntersect(
394  const Shape& s,
395  const Transform3<S>& tf,
396  const Vector3<S>& P1,
397  const Vector3<S>& P2,
398  const Vector3<S>& P3,
399  Vector3<S>* contact_points,
400  S* penetration_depth,
401  Vector3<S>* normal) const
402 {
403  return ShapeTriangleIntersectIndepImpl<S, Shape>::run(
404  *this, s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
405 }
406 
407 //==============================================================================
408 template<typename S>
409 struct ShapeTriangleIntersectIndepImpl<S, Sphere<S>>
410 {
411  static bool run(
412  const GJKSolver_indep<S>& /*gjkSolver*/,
413  const Sphere<S>& s,
414  const Transform3<S>& tf,
415  const Vector3<S>& P1,
416  const Vector3<S>& P2,
417  const Vector3<S>& P3,
418  Vector3<S>* contact_points,
419  S* penetration_depth,
420  Vector3<S>* normal)
421  {
422  return detail::sphereTriangleIntersect(
423  s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
424  }
425 };
426 
427 
428 //==============================================================================
429 template<typename S, typename Shape>
430 struct ShapeTransformedTriangleIntersectIndepImpl
431 {
432  static bool run(
433  const GJKSolver_indep<S>& gjkSolver,
434  const Shape& s,
435  const Transform3<S>& tf1,
436  const Vector3<S>& P1,
437  const Vector3<S>& P2,
438  const Vector3<S>& P3,
439  const Transform3<S>& tf2,
440  Vector3<S>* contact_points,
441  S* penetration_depth,
442  Vector3<S>* normal)
443  {
444  TriangleP<S> tri(P1, P2, P3);
445 
446  Vector3<S> guess(1, 0, 0);
447  if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess;
448 
449  detail::MinkowskiDiff<S> shape;
450  shape.shapes[0] = &s;
451  shape.shapes[1] = &tri;
452  shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear();
453  shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2;
454 
455  detail::GJK<S> gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance);
456  typename detail::GJK<S>::Status gjk_status = gjk.evaluate(shape, -guess);
457  if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex();
458 
459  switch(gjk_status)
460  {
461  case detail::GJK<S>::Inside:
462  {
463  detail::EPA<S> epa(gjkSolver.epa_max_face_num, gjkSolver.epa_max_vertex_num, gjkSolver.epa_max_iterations, gjkSolver.epa_tolerance);
464  typename detail::EPA<S>::Status epa_status = epa.evaluate(gjk, -guess);
465  if(epa_status != detail::EPA<S>::Failed)
466  {
467  Vector3<S> w0 = Vector3<S>::Zero();
468  for(size_t i = 0; i < epa.result.rank; ++i)
469  {
470  w0.noalias() += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
471  }
472  if(penetration_depth) *penetration_depth = -epa.depth;
473  if(normal) *normal = -epa.normal;
474  if(contact_points) (*contact_points).noalias() = tf1 * (w0 - epa.normal*(epa.depth *0.5));
475  return true;
476  }
477  else return false;
478  }
479  break;
480  default:
481  ;
482  }
483 
484  return false;
485  }
486 };
487 
488 template<typename S>
489 template<typename Shape>
490 bool GJKSolver_indep<S>::shapeTriangleIntersect(
491  const Shape& s,
492  const Transform3<S>& tf1,
493  const Vector3<S>& P1,
494  const Vector3<S>& P2,
495  const Vector3<S>& P3,
496  const Transform3<S>& tf2,
497  Vector3<S>* contact_points,
498  S* penetration_depth,
499  Vector3<S>* normal) const
500 {
501  return ShapeTransformedTriangleIntersectIndepImpl<S, Shape>::run(
502  *this, s, tf1, P1, P2, P3, tf2,
503  contact_points, penetration_depth, normal);
504 }
505 
506 //==============================================================================
507 template<typename S>
508 struct ShapeTransformedTriangleIntersectIndepImpl<S, Sphere<S>>
509 {
510  static bool run(
511  const GJKSolver_indep<S>& /*gjkSolver*/,
512  const Sphere<S>& s,
513  const Transform3<S>& tf1,
514  const Vector3<S>& P1,
515  const Vector3<S>& P2,
516  const Vector3<S>& P3,
517  const Transform3<S>& tf2,
518  Vector3<S>* contact_points,
519  S* penetration_depth,
520  Vector3<S>* normal)
521  {
522  return detail::sphereTriangleIntersect(
523  s, tf1, tf2 * P1, tf2 * P2, tf2 * P3,
524  contact_points, penetration_depth, normal);
525  }
526 };
527 
528 //==============================================================================
529 template<typename S>
530 struct ShapeTransformedTriangleIntersectIndepImpl<S, Halfspace<S>>
531 {
532  static bool run(
533  const GJKSolver_indep<S>& /*gjkSolver*/,
534  const Halfspace<S>& s,
535  const Transform3<S>& tf1,
536  const Vector3<S>& P1,
537  const Vector3<S>& P2,
538  const Vector3<S>& P3,
539  const Transform3<S>& tf2,
540  Vector3<S>* contact_points,
541  S* penetration_depth,
542  Vector3<S>* normal)
543  {
544  return detail::halfspaceTriangleIntersect(
545  s, tf1, P1, P2, P3, tf2,
546  contact_points, penetration_depth, normal);
547  }
548 };
549 
550 //==============================================================================
551 template<typename S>
552 struct ShapeTransformedTriangleIntersectIndepImpl<S, Plane<S>>
553 {
554  static bool run(
555  const GJKSolver_indep<S>& /*gjkSolver*/,
556  const Plane<S>& s,
557  const Transform3<S>& tf1,
558  const Vector3<S>& P1,
559  const Vector3<S>& P2,
560  const Vector3<S>& P3,
561  const Transform3<S>& tf2,
562  Vector3<S>* contact_points,
563  S* penetration_depth,
564  Vector3<S>* normal)
565  {
566  return detail::planeTriangleIntersect(
567  s, tf1, P1, P2, P3, tf2,
568  contact_points, penetration_depth, normal);
569  }
570 };
571 
572 //==============================================================================
573 template<typename S, typename Shape1, typename Shape2>
574 struct ShapeDistanceIndepImpl
575 {
576  static bool run(
577  const GJKSolver_indep<S>& gjkSolver,
578  const Shape1& s1,
579  const Transform3<S>& tf1,
580  const Shape2& s2,
581  const Transform3<S>& tf2,
582  S* distance,
583  Vector3<S>* p1,
584  Vector3<S>* p2)
585  {
586  Vector3<S> guess(1, 0, 0);
587  if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess;
588 
589  detail::MinkowskiDiff<S> shape;
590  shape.shapes[0] = &s1;
591  shape.shapes[1] = &s2;
592  shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear();
593  shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2;
594 
595  detail::GJK<S> gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance);
596  typename detail::GJK<S>::Status gjk_status = gjk.evaluate(shape, -guess);
597  if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex();
598 
599  if(gjk_status == detail::GJK<S>::Valid)
600  {
601  Vector3<S> w0 = Vector3<S>::Zero();
602  Vector3<S> w1 = Vector3<S>::Zero();
603  for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
604  {
605  S p = gjk.getSimplex()->p[i];
606  w0.noalias() += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
607  w1.noalias() += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
608  }
609 
610  if(distance) *distance = (w0 - w1).norm();
611 
612  if(p1) *p1 = w0;
613  if(p2) (*p2).noalias() = shape.toshape0 * w1;
614 
615  return true;
616  }
617  else
618  {
619  if(distance) *distance = -1;
620  return false;
621  }
622  }
623 };
624 
625 template<typename S>
626 template<typename Shape1, typename Shape2>
627 bool GJKSolver_indep<S>::shapeDistance(
628  const Shape1& s1,
629  const Transform3<S>& tf1,
630  const Shape2& s2,
631  const Transform3<S>& tf2,
632  S* dist,
633  Vector3<S>* p1,
634  Vector3<S>* p2) const
635 {
636  return ShapeDistanceIndepImpl<S, Shape1, Shape2>::run(
637  *this, s1, tf1, s2, tf2, dist, p1, p2);
638 }
639 
640 // Shape distance algorithms not using built-in GJK algorithm
641 //
642 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
643 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
644 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
645 // | box | | | | | | | | | |
646 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
647 // | sphere |/////| O | | O | | | | | O |
648 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
649 // | ellipsoid |/////|////////| | | | | | | |
650 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
651 // | capsule |/////|////////|///////////| O | | | | | |
652 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
653 // | cone |/////|////////|///////////|/////////| | | | | |
654 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
655 // | cylinder |/////|////////|///////////|/////////|//////| | | | |
656 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
657 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
658 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
659 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
660 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
661 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
662 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
663 
664 //==============================================================================
665 template<typename S>
666 struct ShapeDistanceIndepImpl<S, Sphere<S>, Capsule<S>>
667 {
668  static bool run(
669  const GJKSolver_indep<S>& /*gjkSolver*/,
670  const Sphere<S>& s1,
671  const Transform3<S>& tf1,
672  const Capsule<S>& s2,
673  const Transform3<S>& tf2,
674  S* dist,
675  Vector3<S>* p1,
676  Vector3<S>* p2)
677  {
678  return detail::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
679  }
680 };
681 
682 //==============================================================================
683 template<typename S>
684 struct ShapeDistanceIndepImpl<S, Capsule<S>, Sphere<S>>
685 {
686  static bool run(
687  const GJKSolver_indep<S>& /*gjkSolver*/,
688  const Capsule<S>& s1,
689  const Transform3<S>& tf1,
690  const Sphere<S>& s2,
691  const Transform3<S>& tf2,
692  S* dist,
693  Vector3<S>* p1,
694  Vector3<S>* p2)
695  {
696  return detail::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
697  }
698 };
699 
700 //==============================================================================
701 template<typename S>
702 struct ShapeDistanceIndepImpl<S, Sphere<S>, Sphere<S>>
703 {
704  static bool run(
705  const GJKSolver_indep<S>& /*gjkSolver*/,
706  const Sphere<S>& s1,
707  const Transform3<S>& tf1,
708  const Sphere<S>& s2,
709  const Transform3<S>& tf2,
710  S* dist,
711  Vector3<S>* p1,
712  Vector3<S>* p2)
713  {
714  return detail::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
715  }
716 };
717 
718 //==============================================================================
719 template<typename S>
720 struct ShapeDistanceIndepImpl<S, Capsule<S>, Capsule<S>>
721 {
722  static bool run(
723  const GJKSolver_indep<S>& /*gjkSolver*/,
724  const Capsule<S>& s1,
725  const Transform3<S>& tf1,
726  const Capsule<S>& s2,
727  const Transform3<S>& tf2,
728  S* dist,
729  Vector3<S>* p1,
730  Vector3<S>* p2)
731  {
732  return detail::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
733  }
734 };
735 
736 //==============================================================================
737 template<typename S, typename Shape>
738 struct ShapeTriangleDistanceIndepImpl
739 {
740  static bool run(
741  const GJKSolver_indep<S>& gjkSolver,
742  const Shape& s,
743  const Transform3<S>& tf,
744  const Vector3<S>& P1,
745  const Vector3<S>& P2,
746  const Vector3<S>& P3,
747  S* distance,
748  Vector3<S>* p1,
749  Vector3<S>* p2)
750  {
751  TriangleP<S> tri(P1, P2, P3);
752  Vector3<S> guess(1, 0, 0);
753  if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess;
754 
755  detail::MinkowskiDiff<S> shape;
756  shape.shapes[0] = &s;
757  shape.shapes[1] = &tri;
758  shape.toshape1 = tf.linear();
759  shape.toshape0 = tf.inverse(Eigen::Isometry);
760 
761  detail::GJK<S> gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance);
762  typename detail::GJK<S>::Status gjk_status = gjk.evaluate(shape, -guess);
763  if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex();
764 
765  if(gjk_status == detail::GJK<S>::Valid)
766  {
767  Vector3<S> w0 = Vector3<S>::Zero();
768  Vector3<S> w1 = Vector3<S>::Zero();
769  for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
770  {
771  S p = gjk.getSimplex()->p[i];
772  w0.noalias() += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
773  w1.noalias() += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
774  }
775 
776  if(distance) *distance = (w0 - w1).norm();
777  if(p1) *p1 = w0;
778  if(p2) (*p2).noalias() = shape.toshape0 * w1;
779  return true;
780  }
781  else
782  {
783  if(distance) *distance = -1;
784  return false;
785  }
786  }
787 };
788 
789 //==============================================================================
790 template<typename S>
791 template<typename Shape>
792 bool GJKSolver_indep<S>::shapeTriangleDistance(
793  const Shape& s,
794  const Transform3<S>& tf,
795  const Vector3<S>& P1,
796  const Vector3<S>& P2,
797  const Vector3<S>& P3,
798  S* dist,
799  Vector3<S>* p1,
800  Vector3<S>* p2) const
801 {
802  return ShapeTriangleDistanceIndepImpl<S, Shape>::run(
803  *this, s, tf, P1, P2, P3, dist, p1, p2);
804 }
805 
806 //==============================================================================
807 template<typename S>
808 struct ShapeTriangleDistanceIndepImpl<S, Sphere<S>>
809 {
810  static bool run(
811  const GJKSolver_indep<S>& /*gjkSolver*/,
812  const Sphere<S>& s,
813  const Transform3<S>& tf,
814  const Vector3<S>& P1,
815  const Vector3<S>& P2,
816  const Vector3<S>& P3,
817  S* dist,
818  Vector3<S>* p1,
819  Vector3<S>* p2)
820  {
821  return detail::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
822  }
823 };
824 
825 //==============================================================================
826 template<typename S, typename Shape>
827 struct ShapeTransformedTriangleDistanceIndepImpl
828 {
829  static bool run(
830  const GJKSolver_indep<S>& gjkSolver,
831  const Shape& s,
832  const Transform3<S>& tf1,
833  const Vector3<S>& P1,
834  const Vector3<S>& P2,
835  const Vector3<S>& P3,
836  const Transform3<S>& tf2,
837  S* distance,
838  Vector3<S>* p1,
839  Vector3<S>* p2)
840  {
841  TriangleP<S> tri(P1, P2, P3);
842  Vector3<S> guess(1, 0, 0);
843  if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess;
844 
845  detail::MinkowskiDiff<S> shape;
846  shape.shapes[0] = &s;
847  shape.shapes[1] = &tri;
848  shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear();
849  shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2;
850 
851  detail::GJK<S> gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance);
852  typename detail::GJK<S>::Status gjk_status = gjk.evaluate(shape, -guess);
853  if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex();
854 
855  if(gjk_status == detail::GJK<S>::Valid)
856  {
857  Vector3<S> w0 = Vector3<S>::Zero();
858  Vector3<S> w1 = Vector3<S>::Zero();
859  for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
860  {
861  S p = gjk.getSimplex()->p[i];
862  w0.noalias() += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
863  w1.noalias() += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
864  }
865 
866  if(distance) *distance = (w0 - w1).norm();
867  if(p1) *p1 = w0;
868  if(p2) (*p2).noalias() = shape.toshape0 * w1;
869  return true;
870  }
871  else
872  {
873  if(distance) *distance = -1;
874  return false;
875  }
876  }
877 };
878 
879 //==============================================================================
880 template<typename S>
881 template<typename Shape>
882 bool GJKSolver_indep<S>::shapeTriangleDistance(
883  const Shape& s,
884  const Transform3<S>& tf1,
885  const Vector3<S>& P1,
886  const Vector3<S>& P2,
887  const Vector3<S>& P3,
888  const Transform3<S>& tf2,
889  S* dist,
890  Vector3<S>* p1,
891  Vector3<S>* p2) const
892 {
893  return ShapeTransformedTriangleDistanceIndepImpl<S, Shape>::run(
894  *this, s, tf1, P1, P2, P3, tf2, dist, p1, p2);
895 }
896 
897 //==============================================================================
898 template<typename S>
899 struct ShapeTransformedTriangleDistanceIndepImpl<S, Sphere<S>>
900 {
901  static bool run(
902  const GJKSolver_indep<S>& /*gjkSolver*/,
903  const Sphere<S>& s,
904  const Transform3<S>& tf1,
905  const Vector3<S>& P1,
906  const Vector3<S>& P2,
907  const Vector3<S>& P3,
908  const Transform3<S>& tf2,
909  S* dist,
910  Vector3<S>* p1,
911  Vector3<S>* p2)
912  {
913  return detail::sphereTriangleDistance(
914  s, tf1, P1, P2, P3, tf2, dist, p1, p2);
915  }
916 };
917 
918 //==============================================================================
919 template <typename S>
920 GJKSolver_indep<S>::GJKSolver_indep()
921 {
922  gjk_max_iterations = 128;
923  gjk_tolerance = 1e-6;
924  epa_max_face_num = 128;
925  epa_max_vertex_num = 64;
926  epa_max_iterations = 255;
927  epa_tolerance = 1e-6;
928  enable_cached_guess = false;
929  cached_guess = Vector3<S>(1, 0, 0);
930 }
931 
932 //==============================================================================
933 template <typename S>
934 void GJKSolver_indep<S>::enableCachedGuess(bool if_enable) const
935 {
936  enable_cached_guess = if_enable;
937 }
938 
939 //==============================================================================
940 template <typename S>
941 void GJKSolver_indep<S>::setCachedGuess(const Vector3<S>& guess) const
942 {
943  cached_guess = guess;
944 }
945 
946 //==============================================================================
947 template <typename S>
948 Vector3<S> GJKSolver_indep<S>::getCachedGuess() const
949 {
950  return cached_guess;
951 }
952 
953 } // namespace detail
954 } // namespace fcl
955 
956 #endif
fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: triangle_p.h:48
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d; Po...
Definition: halfspace.h:55
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: minkowski_diff.h:58
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
fcl::detail::GJKSolver_indep::shapeTriangleDistance
bool shapeTriangleDistance(const Shape &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *distance=nullptr, Vector3< S > *p1=nullptr, Vector3< S > *p2=nullptr) const
distance computation between one shape and a triangle
Definition: gjk_solver_indep-inl.h:792
fcl::detail::GJKSolver_indep::cached_guess
Vector3< S > cached_guess
smart guess
Definition: gjk_solver_indep.h:171
fcl::detail::GJKSolver_indep::gjk_max_iterations
S gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: gjk_solver_indep.h:165
fcl::detail::EPA
class for EPA algorithm
Definition: epa.h:57
fcl::detail::GJKSolver_indep::shapeDistance
bool shapeDistance(const Shape1 &s1, const Transform3< S > &tf1, const Shape2 &s2, const Transform3< S > &tf2, S *distance=nullptr, Vector3< S > *p1=nullptr, Vector3< S > *p2=nullptr) const
distance computation between two shapes
Definition: gjk_solver_indep-inl.h:627
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:48
fcl::detail::GJKSolver_indep::gjk_tolerance
S gjk_tolerance
the threshold used in GJK to stop iteration
Definition: gjk_solver_indep.h:162
fcl::detail::GJKSolver_indep::GJKSolver_indep
GJKSolver_indep()
default setting for GJK algorithm
Definition: gjk_solver_indep-inl.h:920
fcl::detail::MinkowskiDiff::shapes
const ShapeBase< S > * shapes[2]
points to two shapes
Definition: minkowski_diff.h:61
fcl::detail::ShapeTriangleDistanceIndepImpl
Definition: gjk_solver_indep-inl.h:738
fcl::detail::MinkowskiDiff::support
Vector3< S > support(const Vector3< S > &d) const
support function for the pair of shapes
Definition: minkowski_diff-inl.h:231
fcl::detail::GJKSolver_indep::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided.
Definition: gjk_solver_indep.h:168
fcl::detail::GJK::getGuessFromSimplex
Vector3< S > getGuessFromSimplex() const
get the guess from current simplex
Definition: gjk-inl.h:75
fcl::detail::ShapeTransformedTriangleDistanceIndepImpl
Definition: gjk_solver_indep-inl.h:827
fcl::detail::MinkowskiDiff::toshape0
Transform3< S > toshape0
transform from shape1 to shape0
Definition: minkowski_diff.h:67
fcl::Capsule
Center at zero point capsule.
Definition: capsule.h:48
fcl::detail::ShapeDistanceIndepImpl
Definition: gjk_solver_indep-inl.h:574
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:48
fcl::detail::ShapeIntersectIndepImpl
Definition: gjk_solver_indep-inl.h:106
fcl::detail::GJKSolver_indep::epa_max_iterations
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: gjk_solver_indep.h:156
fcl::detail::MinkowskiDiff::toshape1
Matrix3< S > toshape1
rotation from shape0 to shape1
Definition: minkowski_diff.h:64
fcl::detail::GJKSolver_indep::epa_max_face_num
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: gjk_solver_indep.h:150
fcl::Plane
Infinite plane.
Definition: plane.h:48
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:48
fcl::detail::GJK::evaluate
Status evaluate(const MinkowskiDiff< S > &shape_, const Vector3< S > &guess)
GJK algorithm, given the initial value guess.
Definition: gjk-inl.h:82
fcl::Cone
Center at zero cone.
Definition: cone.h:48
fcl::detail::GJKSolver_indep::epa_max_vertex_num
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: gjk_solver_indep.h:153
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::detail::GJK::getSimplex
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk-inl.h:302
fcl::detail::GJKSolver_indep
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: gjk_solver_indep.h:53
fcl::detail::GJK
class for GJK algorithm
Definition: gjk.h:52
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:48
fcl::detail::GJKSolver_indep::epa_tolerance
S epa_tolerance
the threshold used in EPA to stop iteration
Definition: gjk_solver_indep.h:159
fcl::detail::GJKSolver_indep::shapeIntersect
FCL_DEPRECATED bool shapeIntersect(const Shape1 &s1, const Transform3< S > &tf1, const Shape2 &s2, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) const
intersection checking between two shapes
fcl::detail::ShapeTransformedTriangleIntersectIndepImpl
Definition: gjk_solver_indep-inl.h:430
fcl::detail::ShapeTriangleIntersectIndepImpl
Definition: gjk_solver_indep-inl.h:334
fcl::detail::GJKSolver_indep::shapeTriangleIntersect
bool shapeTriangleIntersect(const Shape &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, Vector3< S > *contact_points=nullptr, S *penetration_depth=nullptr, Vector3< S > *normal=nullptr) const
intersection checking between one shape and a triangle
Definition: gjk_solver_indep-inl.h:393