FCL  0.6.0
Flexible Collision Library
gjk_solver_libccd-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_GJKSOLVERLIBCCD_INL_H
39 #define FCL_NARROWPHASE_GJKSOLVERLIBCCD_INL_H
40 
41 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
42 
43 #include <algorithm>
44 
45 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h"
46 #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h"
47 #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h"
48 #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h"
49 #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
50 #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h"
51 #include "fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h"
52 #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane.h"
53 
54 namespace fcl
55 {
56 
57 namespace detail
58 {
59 
60 //==============================================================================
61 extern template
62 struct GJKSolver_libccd<double>;
63 
64 //==============================================================================
65 template<typename S>
66 template<typename Shape1, typename Shape2>
67 bool GJKSolver_libccd<S>::shapeIntersect(
68  const Shape1& s1, const Transform3<S>& tf1,
69  const Shape2& s2, const Transform3<S>& tf2,
70  Vector3<S>* contact_points,
71  S* penetration_depth,
72  Vector3<S>* normal) const
73 {
74  bool res;
75 
76  if (contact_points || penetration_depth || normal)
77  {
78  std::vector<ContactPoint<S>> contacts;
79 
80  res = shapeIntersect(s1, tf1, s2, tf2, &contacts);
81 
82  if (!contacts.empty())
83  {
84  // Get the deepest contact point
85  const ContactPoint<S>& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth<S>);
86 
87  if (contact_points)
88  *contact_points = maxDepthContact.pos;
89 
90  if (penetration_depth)
91  *penetration_depth = maxDepthContact.penetration_depth;
92 
93  if (normal)
94  *normal = maxDepthContact.normal;
95  }
96  }
97  else
98  {
99  res = shapeIntersect(s1, tf1, s2, tf2, nullptr);
100  }
101 
102  return res;
103 }
104 
105 //==============================================================================
106 template<typename S, typename Shape1, typename Shape2>
107 struct ShapeIntersectLibccdImpl
108 {
109  static bool run(
110  const GJKSolver_libccd<S>& gjkSolver,
111  const Shape1& s1, const Transform3<S>& tf1,
112  const Shape2& s2, const Transform3<S>& tf2,
113  std::vector<ContactPoint<S>>* contacts)
114  {
115  void* o1 = detail::GJKInitializer<S, Shape1>::createGJKObject(s1, tf1);
116  void* o2 = detail::GJKInitializer<S, Shape2>::createGJKObject(s2, tf2);
117 
118  bool res;
119 
120  if(contacts)
121  {
122  Vector3<S> normal;
123  Vector3<S> point;
124  S depth;
125  res = detail::GJKCollide<S>(
126  o1,
127  detail::GJKInitializer<S, Shape1>::getSupportFunction(),
128  detail::GJKInitializer<S, Shape1>::getCenterFunction(),
129  o2, detail::GJKInitializer<S, Shape2>::getSupportFunction(),
130  detail::GJKInitializer<S, Shape2>::getCenterFunction(),
131  gjkSolver.max_collision_iterations,
132  gjkSolver.collision_tolerance,
133  &point,
134  &depth,
135  &normal);
136  contacts->emplace_back(normal, point, depth);
137  }
138  else
139  {
140  res = detail::GJKCollide<S>(
141  o1,
142  detail::GJKInitializer<S, Shape1>::getSupportFunction(),
143  detail::GJKInitializer<S, Shape1>::getCenterFunction(),
144  o2,
145  detail::GJKInitializer<S, Shape2>::getSupportFunction(),
146  detail::GJKInitializer<S, Shape2>::getCenterFunction(),
147  gjkSolver.max_collision_iterations,
148  gjkSolver.collision_tolerance,
149  nullptr,
150  nullptr,
151  nullptr);
152  }
153 
154  detail::GJKInitializer<S, Shape1>::deleteGJKObject(o1);
155  detail::GJKInitializer<S, Shape2>::deleteGJKObject(o2);
156 
157  return res;
158  }
159 };
160 
161 //==============================================================================
162 template<typename S>
163 template<typename Shape1, typename Shape2>
164 bool GJKSolver_libccd<S>::shapeIntersect(
165  const Shape1& s1, const Transform3<S>& tf1,
166  const Shape2& s2, const Transform3<S>& tf2,
167  std::vector<ContactPoint<S>>* contacts) const
168 {
169  return ShapeIntersectLibccdImpl<S, Shape1, Shape2>::run(
170  *this, s1, tf1, s2, tf2, contacts);
171 }
172 
173 // Shape intersect algorithms not using libccd
174 //
175 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
176 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
177 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
178 // | box | O | | | | | | O | O | |
179 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
180 // | sphere |/////| O | | O | | | O | O | O |
181 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
182 // | ellipsoid |/////|////////| | | | | O | O | TODO |
183 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
184 // | capsule |/////|////////|///////////| | | | O | O | |
185 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
186 // | cone |/////|////////|///////////|/////////| | | O | O | |
187 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
188 // | cylinder |/////|////////|///////////|/////////|//////| | O | O | |
189 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
190 // | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O |
191 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
192 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O |
193 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
194 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
195 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
196 
197 #define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
198  template <typename S>\
199  struct ShapeIntersectLibccdImpl<S, SHAPE1<S>, SHAPE2<S>>\
200  {\
201  static bool run(\
202  const GJKSolver_libccd<S>& /*gjkSolver*/,\
203  const SHAPE1<S>& s1,\
204  const Transform3<S>& tf1,\
205  const SHAPE2<S>& s2,\
206  const Transform3<S>& tf2,\
207  std::vector<ContactPoint<S>>* contacts)\
208  {\
209  return ALG(s1, tf1, s2, tf2, contacts);\
210  }\
211  };
212 
213 #define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)\
214  template <typename S>\
215  struct ShapeIntersectLibccdImpl<S, SHAPE2<S>, SHAPE1<S>>\
216  {\
217  static bool run(\
218  const GJKSolver_libccd<S>& /*gjkSolver*/,\
219  const SHAPE2<S>& s1,\
220  const Transform3<S>& tf1,\
221  const SHAPE1<S>& s2,\
222  const Transform3<S>& tf2,\
223  std::vector<ContactPoint<S>>* contacts)\
224  {\
225  const bool res = ALG(s2, tf2, s1, tf1, contacts);\
226  if (contacts) flipNormal(*contacts);\
227  return res;\
228  }\
229  };
230 
231 #define FCL_GJK_LIBCCD_SHAPE_INTERSECT(SHAPE, ALG)\
232  FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE, SHAPE, ALG)
233 
234 #define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(SHAPE1, SHAPE2, ALG)\
235  FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
236  FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)
237 
238 FCL_GJK_LIBCCD_SHAPE_INTERSECT(Sphere, detail::sphereSphereIntersect)
239 FCL_GJK_LIBCCD_SHAPE_INTERSECT(Box, detail::boxBoxIntersect)
240 
241 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect)
242 
243 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
244 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
245 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect)
246 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, detail::capsuleHalfspaceIntersect)
247 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, detail::cylinderHalfspaceIntersect)
248 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, detail::coneHalfspaceIntersect)
249 
250 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Plane, detail::spherePlaneIntersect)
251 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, detail::ellipsoidPlaneIntersect)
252 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Plane, detail::boxPlaneIntersect)
253 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Capsule, Plane, detail::capsulePlaneIntersect)
254 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cylinder, Plane, detail::cylinderPlaneIntersect)
255 FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cone, Plane, detail::conePlaneIntersect)
256 
257 template <typename S>
258 struct ShapeIntersectLibccdImpl<S, Halfspace<S>, Halfspace<S>>
259 {
260  static bool run(
261  const GJKSolver_libccd<S>& /*gjkSolver*/,
262  const Halfspace<S>& s1,
263  const Transform3<S>& tf1,
264  const Halfspace<S>& s2,
265  const Transform3<S>& tf2,
266  std::vector<ContactPoint<S>>* contacts)
267  {
268  Halfspace<S> s;
269  Vector3<S> p, d;
270  S depth;
271  int ret;
272  return detail::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
273  }
274 };
275 
276 template <typename S>
277 struct ShapeIntersectLibccdImpl<S, Plane<S>, Plane<S>>
278 {
279  static bool run(
280  const GJKSolver_libccd<S>& /*gjkSolver*/,
281  const Plane<S>& s1,
282  const Transform3<S>& tf1,
283  const Plane<S>& s2,
284  const Transform3<S>& tf2,
285  std::vector<ContactPoint<S>>* contacts)
286  {
287  return detail::planeIntersect(s1, tf1, s2, tf2, contacts);
288  }
289 };
290 
291 template <typename S>
292 struct ShapeIntersectLibccdImpl<S, Plane<S>, Halfspace<S>>
293 {
294  static bool run(
295  const GJKSolver_libccd<S>& /*gjkSolver*/,
296  const Plane<S>& s1,
297  const Transform3<S>& tf1,
298  const Halfspace<S>& s2,
299  const Transform3<S>& tf2,
300  std::vector<ContactPoint<S>>* contacts)
301  {
302  Plane<S> pl;
303  Vector3<S> p, d;
304  S depth;
305  int ret;
306  return detail::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
307  }
308 };
309 
310 template <typename S>
311 struct ShapeIntersectLibccdImpl<S, Halfspace<S>, Plane<S>>
312 {
313  static bool run(
314  const GJKSolver_libccd<S>& /*gjkSolver*/,
315  const Halfspace<S>& s1,
316  const Transform3<S>& tf1,
317  const Plane<S>& s2,
318  const Transform3<S>& tf2,
319  std::vector<ContactPoint<S>>* contacts)
320  {
321  Plane<S> pl;
322  Vector3<S> p, d;
323  S depth;
324  int ret;
325  return detail::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
326  }
327 };
328 
329 //==============================================================================
330 template<typename S, typename Shape>
331 struct ShapeTriangleIntersectLibccdImpl
332 {
333  static bool run(
334  const GJKSolver_libccd<S>& gjkSolver,
335  const Shape& s,
336  const Transform3<S>& tf,
337  const Vector3<S>& P1,
338  const Vector3<S>& P2,
339  const Vector3<S>& P3,
340  Vector3<S>* contact_points,
341  S* penetration_depth,
342  Vector3<S>* normal)
343  {
344  void* o1 = detail::GJKInitializer<S, Shape>::createGJKObject(s, tf);
345  void* o2 = detail::triCreateGJKObject(P1, P2, P3);
346 
347  bool res = detail::GJKCollide<S>(
348  o1,
349  detail::GJKInitializer<S, Shape>::getSupportFunction(),
350  detail::GJKInitializer<S, Shape>::getCenterFunction(),
351  o2,
352  detail::triGetSupportFunction(),
353  detail::triGetCenterFunction(),
354  gjkSolver.max_collision_iterations,
355  gjkSolver.collision_tolerance,
356  contact_points,
357  penetration_depth,
358  normal);
359 
360  detail::GJKInitializer<S, Shape>::deleteGJKObject(o1);
361  detail::triDeleteGJKObject(o2);
362 
363  return res;
364  }
365 };
366 
367 template<typename S>
368 template<typename Shape>
369 bool GJKSolver_libccd<S>::shapeTriangleIntersect(
370  const Shape& s,
371  const Transform3<S>& tf,
372  const Vector3<S>& P1,
373  const Vector3<S>& P2,
374  const Vector3<S>& P3,
375  Vector3<S>* contact_points,
376  S* penetration_depth,
377  Vector3<S>* normal) const
378 {
379  return ShapeTriangleIntersectLibccdImpl<S, Shape>::run(
380  *this, s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
381 }
382 
383 //==============================================================================
384 template<typename S>
385 struct ShapeTriangleIntersectLibccdImpl<S, Sphere<S>>
386 {
387  static bool run(
388  const GJKSolver_libccd<S>& /*gjkSolver*/,
389  const Sphere<S>& s,
390  const Transform3<S>& tf,
391  const Vector3<S>& P1,
392  const Vector3<S>& P2,
393  const Vector3<S>& P3,
394  Vector3<S>* contact_points,
395  S* penetration_depth,
396  Vector3<S>* normal)
397  {
398  return detail::sphereTriangleIntersect(
399  s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
400  }
401 };
402 
403 //==============================================================================
404 template<typename S, typename Shape>
405 struct ShapeTransformedTriangleIntersectLibccdImpl
406 {
407  static bool run(
408  const GJKSolver_libccd<S>& gjkSolver,
409  const Shape& s,
410  const Transform3<S>& tf1,
411  const Vector3<S>& P1,
412  const Vector3<S>& P2,
413  const Vector3<S>& P3,
414  const Transform3<S>& tf2,
415  Vector3<S>* contact_points,
416  S* penetration_depth,
417  Vector3<S>* normal)
418  {
419  void* o1 = detail::GJKInitializer<S, Shape>::createGJKObject(s, tf1);
420  void* o2 = detail::triCreateGJKObject(P1, P2, P3, tf2);
421 
422  bool res = detail::GJKCollide<S>(
423  o1,
424  detail::GJKInitializer<S, Shape>::getSupportFunction(),
425  detail::GJKInitializer<S, Shape>::getCenterFunction(),
426  o2,
427  detail::triGetSupportFunction(),
428  detail::triGetCenterFunction(),
429  gjkSolver.max_collision_iterations,
430  gjkSolver.collision_tolerance,
431  contact_points,
432  penetration_depth,
433  normal);
434 
435  detail::GJKInitializer<S, Shape>::deleteGJKObject(o1);
436  detail::triDeleteGJKObject(o2);
437 
438  return res;
439  }
440 };
441 
442 template<typename S>
443 template<typename Shape>
444 bool GJKSolver_libccd<S>::shapeTriangleIntersect(
445  const Shape& s,
446  const Transform3<S>& tf1,
447  const Vector3<S>& P1,
448  const Vector3<S>& P2,
449  const Vector3<S>& P3,
450  const Transform3<S>& tf2,
451  Vector3<S>* contact_points,
452  S* penetration_depth,
453  Vector3<S>* normal) const
454 {
455  return ShapeTransformedTriangleIntersectLibccdImpl<S, Shape>::run(
456  *this, s, tf1, P1, P2, P3, tf2,
457  contact_points, penetration_depth, normal);
458 }
459 
460 //==============================================================================
461 template<typename S>
462 struct ShapeTransformedTriangleIntersectLibccdImpl<S, Sphere<S>>
463 {
464  static bool run(
465  const GJKSolver_libccd<S>& /*gjkSolver*/,
466  const Sphere<S>& s,
467  const Transform3<S>& tf1,
468  const Vector3<S>& P1,
469  const Vector3<S>& P2,
470  const Vector3<S>& P3,
471  const Transform3<S>& tf2,
472  Vector3<S>* contact_points,
473  S* penetration_depth,
474  Vector3<S>* normal)
475  {
476  return detail::sphereTriangleIntersect(
477  s, tf1, tf2 * P1, tf2 * P2, tf2 * P3,
478  contact_points, penetration_depth, normal);
479  }
480 };
481 
482 //==============================================================================
483 template<typename S>
484 struct ShapeTransformedTriangleIntersectLibccdImpl<S, Halfspace<S>>
485 {
486  static bool run(
487  const GJKSolver_libccd<S>& /*gjkSolver*/,
488  const Halfspace<S>& s,
489  const Transform3<S>& tf1,
490  const Vector3<S>& P1,
491  const Vector3<S>& P2,
492  const Vector3<S>& P3,
493  const Transform3<S>& tf2,
494  Vector3<S>* contact_points,
495  S* penetration_depth,
496  Vector3<S>* normal)
497  {
498  return detail::halfspaceTriangleIntersect(
499  s, tf1, P1, P2, P3, tf2,
500  contact_points, penetration_depth, normal);
501  }
502 };
503 
504 //==============================================================================
505 template<typename S>
506 struct ShapeTransformedTriangleIntersectLibccdImpl<S, Plane<S>>
507 {
508  static bool run(
509  const GJKSolver_libccd<S>& /*gjkSolver*/,
510  const Plane<S>& s,
511  const Transform3<S>& tf1,
512  const Vector3<S>& P1,
513  const Vector3<S>& P2,
514  const Vector3<S>& P3,
515  const Transform3<S>& tf2,
516  Vector3<S>* contact_points,
517  S* penetration_depth,
518  Vector3<S>* normal)
519  {
520  return detail::planeTriangleIntersect(
521  s, tf1, P1, P2, P3, tf2,
522  contact_points, penetration_depth, normal);
523  }
524 };
525 
526 //==============================================================================
527 template<typename S, typename Shape1, typename Shape2>
528 struct ShapeDistanceLibccdImpl
529 {
530  static bool run(
531  const GJKSolver_libccd<S>& gjkSolver,
532  const Shape1& s1,
533  const Transform3<S>& tf1,
534  const Shape2& s2,
535  const Transform3<S>& tf2,
536  S* dist,
537  Vector3<S>* p1,
538  Vector3<S>* p2)
539  {
540  void* o1 = detail::GJKInitializer<S, Shape1>::createGJKObject(s1, tf1);
541  void* o2 = detail::GJKInitializer<S, Shape2>::createGJKObject(s2, tf2);
542 
543  bool res = detail::GJKDistance(
544  o1,
545  detail::GJKInitializer<S, Shape1>::getSupportFunction(),
546  o2,
547  detail::GJKInitializer<S, Shape2>::getSupportFunction(),
548  gjkSolver.max_distance_iterations,
549  gjkSolver.distance_tolerance,
550  dist,
551  p1,
552  p2);
553 
554  if (p1)
555  (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1;
556 
557  if (p2)
558  (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2;
559 
560  detail::GJKInitializer<S, Shape1>::deleteGJKObject(o1);
561  detail::GJKInitializer<S, Shape2>::deleteGJKObject(o2);
562 
563  return res;
564  }
565 };
566 
567 template<typename S>
568 template<typename Shape1, typename Shape2>
569 bool GJKSolver_libccd<S>::shapeDistance(
570  const Shape1& s1,
571  const Transform3<S>& tf1,
572  const Shape2& s2,
573  const Transform3<S>& tf2,
574  S* dist,
575  Vector3<S>* p1,
576  Vector3<S>* p2) const
577 {
578  return ShapeDistanceLibccdImpl<S, Shape1, Shape2>::run(
579  *this, s1, tf1, s2, tf2, dist, p1, p2);
580 }
581 
582 // Shape distance algorithms not using libccd
583 //
584 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
585 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
586 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
587 // | box | | | | | | | | | |
588 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
589 // | sphere |/////| O | | O | | | | | O |
590 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
591 // | ellipsoid |/////|////////| | | | | | | |
592 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
593 // | capsule |/////|////////|///////////| O | | | | | |
594 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
595 // | cone |/////|////////|///////////|/////////| | | | | |
596 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
597 // | cylinder |/////|////////|///////////|/////////|//////| | | | |
598 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
599 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
600 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
601 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
602 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
603 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
604 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
605 
606 //==============================================================================
607 template<typename S>
608 struct ShapeDistanceLibccdImpl<S, Sphere<S>, Capsule<S>>
609 {
610  static bool run(
611  const GJKSolver_libccd<S>& /*gjkSolver*/,
612  const Sphere<S>& s1,
613  const Transform3<S>& tf1,
614  const Capsule<S>& s2,
615  const Transform3<S>& tf2,
616  S* dist,
617  Vector3<S>* p1,
618  Vector3<S>* p2)
619  {
620  return detail::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
621  }
622 };
623 
624 //==============================================================================
625 template<typename S>
626 struct ShapeDistanceLibccdImpl<S, Capsule<S>, Sphere<S>>
627 {
628  static bool run(
629  const GJKSolver_libccd<S>& /*gjkSolver*/,
630  const Capsule<S>& s1,
631  const Transform3<S>& tf1,
632  const Sphere<S>& s2,
633  const Transform3<S>& tf2,
634  S* dist,
635  Vector3<S>* p1,
636  Vector3<S>* p2)
637  {
638  return detail::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
639  }
640 };
641 
642 //==============================================================================
643 template<typename S>
644 struct ShapeDistanceLibccdImpl<S, Sphere<S>, Sphere<S>>
645 {
646  static bool run(
647  const GJKSolver_libccd<S>& /*gjkSolver*/,
648  const Sphere<S>& s1,
649  const Transform3<S>& tf1,
650  const Sphere<S>& s2,
651  const Transform3<S>& tf2,
652  S* dist,
653  Vector3<S>* p1,
654  Vector3<S>* p2)
655  {
656  return detail::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
657  }
658 };
659 
660 //==============================================================================
661 template<typename S>
662 struct ShapeDistanceLibccdImpl<S, Capsule<S>, Capsule<S>>
663 {
664  static bool run(
665  const GJKSolver_libccd<S>& /*gjkSolver*/,
666  const Capsule<S>& s1,
667  const Transform3<S>& tf1,
668  const Capsule<S>& s2,
669  const Transform3<S>& tf2,
670  S* dist,
671  Vector3<S>* p1,
672  Vector3<S>* p2)
673  {
674  return detail::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
675  }
676 };
677 
678 //==============================================================================
679 template<typename S, typename Shape>
680 struct ShapeTriangleDistanceLibccdImpl
681 {
682  static bool run(
683  const GJKSolver_libccd<S>& gjkSolver,
684  const Shape& s,
685  const Transform3<S>& tf,
686  const Vector3<S>& P1,
687  const Vector3<S>& P2,
688  const Vector3<S>& P3,
689  S* dist,
690  Vector3<S>* p1,
691  Vector3<S>* p2)
692  {
693  void* o1 = detail::GJKInitializer<S, Shape>::createGJKObject(s, tf);
694  void* o2 = detail::triCreateGJKObject(P1, P2, P3);
695 
696  bool res = detail::GJKDistance(
697  o1,
698  detail::GJKInitializer<S, Shape>::getSupportFunction(),
699  o2,
700  detail::triGetSupportFunction(),
701  gjkSolver.max_distance_iterations,
702  gjkSolver.distance_tolerance,
703  dist,
704  p1,
705  p2);
706  if(p1)
707  (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1;
708 
709  detail::GJKInitializer<S, Shape>::deleteGJKObject(o1);
710  detail::triDeleteGJKObject(o2);
711 
712  return res;
713  }
714 };
715 
716 //==============================================================================
717 template<typename S>
718 template<typename Shape>
719 bool GJKSolver_libccd<S>::shapeTriangleDistance(
720  const Shape& s,
721  const Transform3<S>& tf,
722  const Vector3<S>& P1,
723  const Vector3<S>& P2,
724  const Vector3<S>& P3,
725  S* dist,
726  Vector3<S>* p1,
727  Vector3<S>* p2) const
728 {
729  return ShapeTriangleDistanceLibccdImpl<S, Shape>::run(
730  *this, s, tf, P1, P2, P3, dist, p1, p2);
731 }
732 
733 //==============================================================================
734 template<typename S>
735 struct ShapeTriangleDistanceLibccdImpl<S, Sphere<S>>
736 {
737  static bool run(
738  const GJKSolver_libccd<S>& /*gjkSolver*/,
739  const Sphere<S>& s,
740  const Transform3<S>& tf,
741  const Vector3<S>& P1,
742  const Vector3<S>& P2,
743  const Vector3<S>& P3,
744  S* dist,
745  Vector3<S>* p1,
746  Vector3<S>* p2)
747  {
748  return detail::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
749  }
750 };
751 
752 //==============================================================================
753 template<typename S, typename Shape>
754 struct ShapeTransformedTriangleDistanceLibccdImpl
755 {
756  static bool run(
757  const GJKSolver_libccd<S>& gjkSolver,
758  const Shape& s,
759  const Transform3<S>& tf1,
760  const Vector3<S>& P1,
761  const Vector3<S>& P2,
762  const Vector3<S>& P3,
763  const Transform3<S>& tf2,
764  S* dist,
765  Vector3<S>* p1,
766  Vector3<S>* p2)
767  {
768  void* o1 = detail::GJKInitializer<S, Shape>::createGJKObject(s, tf1);
769  void* o2 = detail::triCreateGJKObject(P1, P2, P3, tf2);
770 
771  bool res = detail::GJKDistance(
772  o1,
773  detail::GJKInitializer<S, Shape>::getSupportFunction(),
774  o2,
775  detail::triGetSupportFunction(),
776  gjkSolver.max_distance_iterations,
777  gjkSolver.distance_tolerance,
778  dist,
779  p1,
780  p2);
781  if(p1)
782  (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1;
783  if(p2)
784  (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2;
785 
786  detail::GJKInitializer<S, Shape>::deleteGJKObject(o1);
787  detail::triDeleteGJKObject(o2);
788 
789  return res;
790  }
791 };
792 
793 //==============================================================================
794 template<typename S>
795 template<typename Shape>
796 bool GJKSolver_libccd<S>::shapeTriangleDistance(
797  const Shape& s,
798  const Transform3<S>& tf1,
799  const Vector3<S>& P1,
800  const Vector3<S>& P2,
801  const Vector3<S>& P3,
802  const Transform3<S>& tf2,
803  S* dist,
804  Vector3<S>* p1,
805  Vector3<S>* p2) const
806 {
807  return ShapeTransformedTriangleDistanceLibccdImpl<S, Shape>::run(
808  *this, s, tf1, P1, P2, P3, tf2, dist, p1, p2);
809 }
810 
811 //==============================================================================
812 template<typename S>
813 struct ShapeTransformedTriangleDistanceLibccdImpl<S, Sphere<S>>
814 {
815  static bool run(
816  const GJKSolver_libccd<S>& /*gjkSolver*/,
817  const Sphere<S>& s,
818  const Transform3<S>& tf1,
819  const Vector3<S>& P1,
820  const Vector3<S>& P2,
821  const Vector3<S>& P3,
822  const Transform3<S>& tf2,
823  S* dist,
824  Vector3<S>* p1,
825  Vector3<S>* p2)
826  {
827  return detail::sphereTriangleDistance(
828  s, tf1, P1, P2, P3, tf2, dist, p1, p2);
829  }
830 };
831 
832 //==============================================================================
833 template<typename S>
834 GJKSolver_libccd<S>::GJKSolver_libccd()
835 {
836  max_collision_iterations = 500;
837  max_distance_iterations = 1000;
838  collision_tolerance = 1e-6;
839  distance_tolerance = 1e-6;
840 }
841 
842 //==============================================================================
843 template<typename S>
844 void GJKSolver_libccd<S>::enableCachedGuess(bool if_enable) const
845 {
846  // TODO: need change libccd to exploit spatial coherence
847 }
848 
849 //==============================================================================
850 template<typename S>
851 void GJKSolver_libccd<S>::setCachedGuess(
852  const Vector3<S>& guess) const
853 {
854  // TODO: need change libccd to exploit spatial coherence
855 }
856 
857 //==============================================================================
858 template<typename S>
859 Vector3<S> GJKSolver_libccd<S>::getCachedGuess() const
860 {
861  return Vector3<S>(-1, 0, 0);
862 }
863 
864 } // namespace detail
865 } // namespace fcl
866 
867 #endif
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::detail::GJKSolver_libccd::shapeTriangleDistance
bool shapeTriangleDistance(const Shape &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *dist=nullptr, Vector3< S > *p1=nullptr, Vector3< S > *p2=nullptr) const
distance computation between one shape and a triangle
Definition: gjk_solver_libccd-inl.h:719
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::GJKSolver_libccd::max_collision_iterations
unsigned int max_collision_iterations
maximum number of iterations used in GJK algorithm for collision
Definition: gjk_solver_libccd.h:150
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
fcl::detail::ShapeDistanceLibccdImpl
Definition: gjk_solver_libccd-inl.h:528
fcl::detail::GJKSolver_libccd::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::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:48
fcl::detail::ShapeIntersectLibccdImpl
Definition: gjk_solver_libccd-inl.h:107
fcl::detail::GJKSolver_libccd::collision_tolerance
S collision_tolerance
the threshold used in GJK algorithm to stop collision iteration
Definition: gjk_solver_libccd.h:156
fcl::detail::ShapeTriangleIntersectLibccdImpl
Definition: gjk_solver_libccd-inl.h:331
fcl::detail::GJKSolver_libccd::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_libccd-inl.h:369
fcl::detail::ShapeTransformedTriangleIntersectLibccdImpl
Definition: gjk_solver_libccd-inl.h:405
fcl::detail::ShapeTransformedTriangleDistanceLibccdImpl
Definition: gjk_solver_libccd-inl.h:754
fcl::detail::GJKInitializer::createGJKObject
static void * createGJKObject(const T &, const Transform3< S > &)
Get GJK object from a shape Notice that only local transformation is applied. Gloal transformation ar...
Definition: gjk_libccd.h:82
fcl::detail::GJKInitializer
initialize GJK stuffs
Definition: gjk_libccd.h:70
fcl::detail::ShapeTriangleDistanceLibccdImpl
Definition: gjk_solver_libccd-inl.h:680
fcl::detail::GJKInitializer::getCenterFunction
static GJKCenterFunction getCenterFunction()
Get GJK center function.
Definition: gjk_libccd.h:77
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:53
fcl::detail::GJKSolver_libccd::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: gjk_solver_libccd.h:159
fcl::Capsule
Center at zero point capsule.
Definition: capsule.h:48
fcl::detail::GJKSolver_libccd::shapeDistance
bool shapeDistance(const Shape1 &s1, const Transform3< S > &tf1, const Shape2 &s2, const Transform3< S > &tf2, S *dist=nullptr, Vector3< S > *p1=nullptr, Vector3< S > *p2=nullptr) const
distance computation between two shapes
Definition: gjk_solver_libccd-inl.h:569
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:48
fcl::detail::GJKInitializer::deleteGJKObject
static void deleteGJKObject(void *o)
Delete GJK object.
Definition: gjk_libccd.h:85
fcl::Plane
Infinite plane.
Definition: plane.h:48
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:48
fcl::Cone
Center at zero cone.
Definition: cone.h:48
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:48
fcl::detail::GJKSolver_libccd::GJKSolver_libccd
GJKSolver_libccd()
default setting for GJK algorithm
Definition: gjk_solver_libccd-inl.h:834
fcl::detail::GJKSolver_libccd::max_distance_iterations
unsigned int max_distance_iterations
maximum number of iterations used in GJK algorithm for distance
Definition: gjk_solver_libccd.h:153
fcl::detail::GJKInitializer::getSupportFunction
static GJKSupportFunction getSupportFunction()
Get GJK support function.
Definition: gjk_libccd.h:74