gjk_solver_libccd-inl.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-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 
42 
43 #include <algorithm>
44 
45 #include "fcl/common/unused.h"
46 
58 
59 namespace fcl
60 {
61 
62 namespace detail
63 {
64 
65 //==============================================================================
66 extern template
67 struct GJKSolver_libccd<double>;
68 
69 //==============================================================================
70 template<typename S>
71 template<typename Shape1, typename Shape2>
73  const Shape1& s1, const Transform3<S>& tf1,
74  const Shape2& s2, const Transform3<S>& tf2,
75  Vector3<S>* contact_points,
76  S* penetration_depth,
77  Vector3<S>* normal) const
78 {
79  bool res;
80 
81  if (contact_points || penetration_depth || normal)
82  {
83  std::vector<ContactPoint<S>> contacts;
84 
85  res = shapeIntersect(s1, tf1, s2, tf2, &contacts);
86 
87  if (!contacts.empty())
88  {
89  // Get the deepest contact point
90  const ContactPoint<S>& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth<S>);
91 
92  if (contact_points)
93  *contact_points = maxDepthContact.pos;
94 
95  if (penetration_depth)
96  *penetration_depth = maxDepthContact.penetration_depth;
97 
98  if (normal)
99  *normal = maxDepthContact.normal;
100  }
101  }
102  else
103  {
104  res = shapeIntersect(s1, tf1, s2, tf2, nullptr);
105  }
106 
107  return res;
108 }
109 
110 //==============================================================================
111 template<typename S, typename Shape1, typename Shape2>
113 {
114  static bool run(
115  const GJKSolver_libccd<S>& gjkSolver,
116  const Shape1& s1, const Transform3<S>& tf1,
117  const Shape2& s2, const Transform3<S>& tf2,
118  std::vector<ContactPoint<S>>* contacts)
119  {
122 
123  bool res;
124 
125  if(contacts)
126  {
127  Vector3<S> normal;
129  S depth;
130  res = detail::GJKCollide<S>(
131  o1,
136  gjkSolver.max_collision_iterations,
137  gjkSolver.collision_tolerance,
138  &point,
139  &depth,
140  &normal);
141  contacts->emplace_back(normal, point, depth);
142  }
143  else
144  {
145  res = detail::GJKCollide<S>(
146  o1,
149  o2,
152  gjkSolver.max_collision_iterations,
153  gjkSolver.collision_tolerance,
154  nullptr,
155  nullptr,
156  nullptr);
157  }
158 
161 
162  return res;
163  }
164 };
165 
166 //==============================================================================
167 template<typename S>
168 template<typename Shape1, typename Shape2>
170  const Shape1& s1, const Transform3<S>& tf1,
171  const Shape2& s2, const Transform3<S>& tf2,
172  std::vector<ContactPoint<S>>* contacts) const
173 {
175  *this, s1, tf1, s2, tf2, contacts);
176 }
177 
178 // Shape intersect algorithms not using libccd
179 //
180 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
181 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | convex |
182 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
183 // | box | O | O | | | | | O | O | | |
184 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
185 // | sphere |/////| O | | O | | O | O | O | O | |
186 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
187 // | ellipsoid |/////|////////| | | | | O | O | TODO | |
188 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
189 // | capsule |/////|////////|///////////| | | | O | O | | |
190 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
191 // | cone |/////|////////|///////////|/////////| | | O | O | | |
192 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
193 // | cylinder |/////|////////|///////////|/////////|//////| | O | O | | |
194 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
195 // | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | |
196 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
197 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | O |
198 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
199 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | |
200 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
201 // | convex |/////|////////|///////////|/////////|//////|//////////|///////|////////////|//////////| |
202 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
203 
204 #define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
205  template <typename S>\
206  struct ShapeIntersectLibccdImpl<S, SHAPE1<S>, SHAPE2<S>>\
207  {\
208  static bool run(\
209  const GJKSolver_libccd<S>& /*gjkSolver*/,\
210  const SHAPE1<S>& s1,\
211  const Transform3<S>& tf1,\
212  const SHAPE2<S>& s2,\
213  const Transform3<S>& tf2,\
214  std::vector<ContactPoint<S>>* contacts)\
215  {\
216  return ALG(s1, tf1, s2, tf2, contacts);\
217  }\
218  };
219 
220 #define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)\
221  template <typename S>\
222  struct ShapeIntersectLibccdImpl<S, SHAPE2<S>, SHAPE1<S>>\
223  {\
224  static bool run(\
225  const GJKSolver_libccd<S>& /*gjkSolver*/,\
226  const SHAPE2<S>& s1,\
227  const Transform3<S>& tf1,\
228  const SHAPE1<S>& s2,\
229  const Transform3<S>& tf2,\
230  std::vector<ContactPoint<S>>* contacts)\
231  {\
232  const bool res = ALG(s2, tf2, s1, tf1, contacts);\
233  if (contacts) flipNormal(*contacts);\
234  return res;\
235  }\
236  };
237 
238 #define FCL_GJK_LIBCCD_SHAPE_INTERSECT(SHAPE, ALG)\
239  FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE, SHAPE, ALG)
240 
241 #define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(SHAPE1, SHAPE2, ALG)\
242  FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
243  FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)
244 
247 
249 
251 
253 
261 
268 
269 template <typename S>
271 {
272  static bool run(
273  const GJKSolver_libccd<S>& /*gjkSolver*/,
274  const Halfspace<S>& s1,
275  const Transform3<S>& tf1,
276  const Halfspace<S>& s2,
277  const Transform3<S>& tf2,
278  std::vector<ContactPoint<S>>* contacts)
279  {
280  FCL_UNUSED(contacts);
281 
282  Halfspace<S> s;
283  Vector3<S> p, d;
284  S depth;
285  int ret;
286  return detail::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
287  }
288 };
289 
290 template <typename S>
292 {
293  static bool run(
294  const GJKSolver_libccd<S>& /*gjkSolver*/,
295  const Plane<S>& s1,
296  const Transform3<S>& tf1,
297  const Plane<S>& s2,
298  const Transform3<S>& tf2,
299  std::vector<ContactPoint<S>>* contacts)
300  {
301  return detail::planeIntersect(s1, tf1, s2, tf2, contacts);
302  }
303 };
304 
305 template <typename S>
307 {
308  static bool run(
309  const GJKSolver_libccd<S>& /*gjkSolver*/,
310  const Plane<S>& s1,
311  const Transform3<S>& tf1,
312  const Halfspace<S>& s2,
313  const Transform3<S>& tf2,
314  std::vector<ContactPoint<S>>* contacts)
315  {
316  FCL_UNUSED(contacts);
317 
318  Plane<S> pl;
319  Vector3<S> p, d;
320  S depth;
321  int ret;
322  return detail::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
323  }
324 };
325 
326 template <typename S>
328 {
329  static bool run(
330  const GJKSolver_libccd<S>& /*gjkSolver*/,
331  const Halfspace<S>& s1,
332  const Transform3<S>& tf1,
333  const Plane<S>& s2,
334  const Transform3<S>& tf2,
335  std::vector<ContactPoint<S>>* contacts)
336  {
337  FCL_UNUSED(contacts);
338 
339  Plane<S> pl;
340  Vector3<S> p, d;
341  S depth;
342  int ret;
343  return detail::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
344  }
345 };
346 
347 //==============================================================================
348 template<typename S, typename Shape>
350 {
351  static bool run(
352  const GJKSolver_libccd<S>& gjkSolver,
353  const Shape& s,
354  const Transform3<S>& tf,
355  const Vector3<S>& P1,
356  const Vector3<S>& P2,
357  const Vector3<S>& P3,
358  Vector3<S>* contact_points,
359  S* penetration_depth,
360  Vector3<S>* normal)
361  {
363  void* o2 = detail::triCreateGJKObject(P1, P2, P3);
364 
365  bool res = detail::GJKCollide<S>(
366  o1,
369  o2,
372  gjkSolver.max_collision_iterations,
373  gjkSolver.collision_tolerance,
374  contact_points,
375  penetration_depth,
376  normal);
377 
380 
381  return res;
382  }
383 };
384 
385 template<typename S>
386 template<typename Shape>
388  const Shape& s,
389  const Transform3<S>& tf,
390  const Vector3<S>& P1,
391  const Vector3<S>& P2,
392  const Vector3<S>& P3,
393  Vector3<S>* contact_points,
394  S* penetration_depth,
395  Vector3<S>* normal) const
396 {
398  *this, s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
399 }
400 
401 //==============================================================================
402 template<typename S>
404 {
405  static bool run(
406  const GJKSolver_libccd<S>& /*gjkSolver*/,
407  const Sphere<S>& s,
408  const Transform3<S>& tf,
409  const Vector3<S>& P1,
410  const Vector3<S>& P2,
411  const Vector3<S>& P3,
412  Vector3<S>* contact_points,
413  S* penetration_depth,
414  Vector3<S>* normal)
415  {
417  s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
418  }
419 };
420 
421 //==============================================================================
422 template<typename S, typename Shape>
424 {
425  static bool run(
426  const GJKSolver_libccd<S>& gjkSolver,
427  const Shape& s,
428  const Transform3<S>& tf1,
429  const Vector3<S>& P1,
430  const Vector3<S>& P2,
431  const Vector3<S>& P3,
432  const Transform3<S>& tf2,
433  Vector3<S>* contact_points,
434  S* penetration_depth,
435  Vector3<S>* normal)
436  {
438  void* o2 = detail::triCreateGJKObject(P1, P2, P3, tf2);
439 
440  bool res = detail::GJKCollide<S>(
441  o1,
444  o2,
447  gjkSolver.max_collision_iterations,
448  gjkSolver.collision_tolerance,
449  contact_points,
450  penetration_depth,
451  normal);
452 
455 
456  return res;
457  }
458 };
459 
460 template<typename S>
461 template<typename Shape>
463  const Shape& s,
464  const Transform3<S>& tf1,
465  const Vector3<S>& P1,
466  const Vector3<S>& P2,
467  const Vector3<S>& P3,
468  const Transform3<S>& tf2,
469  Vector3<S>* contact_points,
470  S* penetration_depth,
471  Vector3<S>* normal) const
472 {
474  *this, s, tf1, P1, P2, P3, tf2,
475  contact_points, penetration_depth, normal);
476 }
477 
478 //==============================================================================
479 template<typename S>
481 {
482  static bool run(
483  const GJKSolver_libccd<S>& /*gjkSolver*/,
484  const Sphere<S>& s,
485  const Transform3<S>& tf1,
486  const Vector3<S>& P1,
487  const Vector3<S>& P2,
488  const Vector3<S>& P3,
489  const Transform3<S>& tf2,
490  Vector3<S>* contact_points,
491  S* penetration_depth,
492  Vector3<S>* normal)
493  {
495  s, tf1, tf2 * P1, tf2 * P2, tf2 * P3,
496  contact_points, penetration_depth, normal);
497  }
498 };
499 
500 //==============================================================================
501 template<typename S>
503 {
504  static bool run(
505  const GJKSolver_libccd<S>& /*gjkSolver*/,
506  const Halfspace<S>& s,
507  const Transform3<S>& tf1,
508  const Vector3<S>& P1,
509  const Vector3<S>& P2,
510  const Vector3<S>& P3,
511  const Transform3<S>& tf2,
512  Vector3<S>* contact_points,
513  S* penetration_depth,
514  Vector3<S>* normal)
515  {
517  s, tf1, P1, P2, P3, tf2,
518  contact_points, penetration_depth, normal);
519  }
520 };
521 
522 //==============================================================================
523 template<typename S>
525 {
526  static bool run(
527  const GJKSolver_libccd<S>& /*gjkSolver*/,
528  const Plane<S>& s,
529  const Transform3<S>& tf1,
530  const Vector3<S>& P1,
531  const Vector3<S>& P2,
532  const Vector3<S>& P3,
533  const Transform3<S>& tf2,
534  Vector3<S>* contact_points,
535  S* penetration_depth,
536  Vector3<S>* normal)
537  {
539  s, tf1, P1, P2, P3, tf2,
540  contact_points, penetration_depth, normal);
541  }
542 };
543 
544 
545 //==============================================================================
546 //==============================================================================
547 template<typename S, typename Shape1, typename Shape2>
549 {
550  static bool run(
551  const GJKSolver_libccd<S>& gjkSolver,
552  const Shape1& s1,
553  const Transform3<S>& tf1,
554  const Shape2& s2,
555  const Transform3<S>& tf2,
556  S* dist,
557  Vector3<S>* p1,
558  Vector3<S>* p2)
559  {
562 
563  bool res = detail::GJKSignedDistance(
564  o1,
566  o2,
568  gjkSolver.max_distance_iterations,
569  gjkSolver.distance_tolerance,
570  dist,
571  p1,
572  p2);
573 
576 
577  return res;
578  }
579 };
580 
581 template<typename S>
582 template<typename Shape1, typename Shape2>
584  const Shape1& s1,
585  const Transform3<S>& tf1,
586  const Shape2& s2,
587  const Transform3<S>& tf2,
588  S* dist,
589  Vector3<S>* p1,
590  Vector3<S>* p2) const
591 {
592  bool result = false;
593  try {
595  *this, s1, tf1, s2, tf2, dist, p1, p2);
596  } catch (const FailedAtThisConfiguration& e) {
597  ThrowDetailedConfiguration(s1, tf1, s2, tf2, *this, e);
598  }
599  return result;
600 }
601 
602 
603 //==============================================================================
604 template<typename S, typename Shape1, typename Shape2>
606 {
607  static bool run(
608  const GJKSolver_libccd<S>& gjkSolver,
609  const Shape1& s1,
610  const Transform3<S>& tf1,
611  const Shape2& s2,
612  const Transform3<S>& tf2,
613  S* dist,
614  Vector3<S>* p1,
615  Vector3<S>* p2)
616  {
619 
620  bool res = detail::GJKDistance(
621  o1,
623  o2,
625  gjkSolver.max_distance_iterations,
626  gjkSolver.distance_tolerance,
627  dist,
628  p1,
629  p2);
630 
633 
634  return res;
635  }
636 };
637 
638 template<typename S>
639 template<typename Shape1, typename Shape2>
641  const Shape1& s1,
642  const Transform3<S>& tf1,
643  const Shape2& s2,
644  const Transform3<S>& tf2,
645  S* dist,
646  Vector3<S>* p1,
647  Vector3<S>* p2) const
648 {
650  *this, s1, tf1, s2, tf2, dist, p1, p2);
651 }
652 
653 // Shape distance algorithms not using libccd
654 //
655 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
656 // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
657 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
658 // | box | | O | | | | | | | |
659 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
660 // | sphere |/////| O | | O | | O | | | O |
661 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
662 // | ellipsoid |/////|////////| | | | | | | |
663 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
664 // | capsule |/////|////////|///////////| O | | | | | |
665 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
666 // | cone |/////|////////|///////////|/////////| | | | | |
667 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
668 // | cylinder |/////|////////|///////////|/////////|//////| | | | |
669 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
670 // | plane |/////|////////|///////////|/////////|//////|//////////| | | |
671 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
672 // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | |
673 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
674 // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
675 // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
676 
677 //==============================================================================
678 template<typename S>
680 {
681  static bool run(
682  const GJKSolver_libccd<S>& /*gjkSolver*/,
683  const Sphere<S>& s1,
684  const Transform3<S>& tf1,
685  const Box<S>& s2,
686  const Transform3<S>& tf2,
687  S* dist,
688  Vector3<S>* p1,
689  Vector3<S>* p2)
690  {
691  return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2);
692  }
693 };
694 
695 //==============================================================================
696 template<typename S>
698 {
699  static bool run(
700  const GJKSolver_libccd<S>& /*gjkSolver*/,
701  const Box<S>& s1,
702  const Transform3<S>& tf1,
703  const Sphere<S>& s2,
704  const Transform3<S>& tf2,
705  S* dist,
706  Vector3<S>* p1,
707  Vector3<S>* p2)
708  {
709  return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1);
710  }
711 };
712 
713 //==============================================================================
714 template<typename S>
716 {
717  static bool run(
718  const GJKSolver_libccd<S>& /*gjkSolver*/,
719  const Sphere<S>& s1,
720  const Transform3<S>& tf1,
721  const Capsule<S>& s2,
722  const Transform3<S>& tf2,
723  S* dist,
724  Vector3<S>* p1,
725  Vector3<S>* p2)
726  {
727  return detail::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
728  }
729 };
730 
731 //==============================================================================
732 template<typename S>
734 {
735  static bool run(
736  const GJKSolver_libccd<S>& /*gjkSolver*/,
737  const Capsule<S>& s1,
738  const Transform3<S>& tf1,
739  const Sphere<S>& s2,
740  const Transform3<S>& tf2,
741  S* dist,
742  Vector3<S>* p1,
743  Vector3<S>* p2)
744  {
745  return detail::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
746  }
747 };
748 
749 //==============================================================================
750 template<typename S>
752 {
753  static bool run(
754  const GJKSolver_libccd<S>& /*gjkSolver*/,
755  const Sphere<S>& s1,
756  const Transform3<S>& tf1,
757  const Cylinder<S>& s2,
758  const Transform3<S>& tf2,
759  S* dist,
760  Vector3<S>* p1,
761  Vector3<S>* p2)
762  {
763  return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2);
764  }
765 };
766 
767 //==============================================================================
768 template<typename S>
770 {
771  static bool run(
772  const GJKSolver_libccd<S>& /*gjkSolver*/,
773  const Cylinder<S>& s1,
774  const Transform3<S>& tf1,
775  const Sphere<S>& s2,
776  const Transform3<S>& tf2,
777  S* dist,
778  Vector3<S>* p1,
779  Vector3<S>* p2)
780  {
781  return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1);
782  }
783 };
784 
785 //==============================================================================
786 template<typename S>
788 {
789  static bool run(
790  const GJKSolver_libccd<S>& /*gjkSolver*/,
791  const Sphere<S>& s1,
792  const Transform3<S>& tf1,
793  const Sphere<S>& s2,
794  const Transform3<S>& tf2,
795  S* dist,
796  Vector3<S>* p1,
797  Vector3<S>* p2)
798  {
799  return detail::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
800  }
801 };
802 
803 //==============================================================================
804 template<typename S>
806 {
807  static bool run(
808  const GJKSolver_libccd<S>& /*gjkSolver*/,
809  const Capsule<S>& s1,
810  const Transform3<S>& tf1,
811  const Capsule<S>& s2,
812  const Transform3<S>& tf2,
813  S* dist,
814  Vector3<S>* p1,
815  Vector3<S>* p2)
816  {
817  return detail::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
818  }
819 };
820 
821 //==============================================================================
822 template<typename S, typename Shape>
824 {
825  static bool run(
826  const GJKSolver_libccd<S>& gjkSolver,
827  const Shape& s,
828  const Transform3<S>& tf,
829  const Vector3<S>& P1,
830  const Vector3<S>& P2,
831  const Vector3<S>& P3,
832  S* dist,
833  Vector3<S>* p1,
834  Vector3<S>* p2)
835  {
837  void* o2 = detail::triCreateGJKObject(P1, P2, P3);
838 
839  bool res = detail::GJKDistance(
840  o1,
842  o2,
844  gjkSolver.max_distance_iterations,
845  gjkSolver.distance_tolerance,
846  dist,
847  p1,
848  p2);
849 
852 
853  return res;
854  }
855 };
856 
857 //==============================================================================
858 template<typename S>
859 template<typename Shape>
861  const Shape& s,
862  const Transform3<S>& tf,
863  const Vector3<S>& P1,
864  const Vector3<S>& P2,
865  const Vector3<S>& P3,
866  S* dist,
867  Vector3<S>* p1,
868  Vector3<S>* p2) const
869 {
871  *this, s, tf, P1, P2, P3, dist, p1, p2);
872 }
873 
874 //==============================================================================
875 template<typename S>
877 {
878  static bool run(
879  const GJKSolver_libccd<S>& /*gjkSolver*/,
880  const Sphere<S>& s,
881  const Transform3<S>& tf,
882  const Vector3<S>& P1,
883  const Vector3<S>& P2,
884  const Vector3<S>& P3,
885  S* dist,
886  Vector3<S>* p1,
887  Vector3<S>* p2)
888  {
889  return detail::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
890  }
891 };
892 
893 //==============================================================================
894 template<typename S, typename Shape>
896 {
897  static bool run(
898  const GJKSolver_libccd<S>& gjkSolver,
899  const Shape& s,
900  const Transform3<S>& tf1,
901  const Vector3<S>& P1,
902  const Vector3<S>& P2,
903  const Vector3<S>& P3,
904  const Transform3<S>& tf2,
905  S* dist,
906  Vector3<S>* p1,
907  Vector3<S>* p2)
908  {
910  void* o2 = detail::triCreateGJKObject(P1, P2, P3, tf2);
911 
912  bool res = detail::GJKDistance(
913  o1,
915  o2,
917  gjkSolver.max_distance_iterations,
918  gjkSolver.distance_tolerance,
919  dist,
920  p1,
921  p2);
922 
925 
926  return res;
927  }
928 };
929 
930 //==============================================================================
931 template<typename S>
932 template<typename Shape>
934  const Shape& s,
935  const Transform3<S>& tf1,
936  const Vector3<S>& P1,
937  const Vector3<S>& P2,
938  const Vector3<S>& P3,
939  const Transform3<S>& tf2,
940  S* dist,
941  Vector3<S>* p1,
942  Vector3<S>* p2) const
943 {
945  *this, s, tf1, P1, P2, P3, tf2, dist, p1, p2);
946 }
947 
948 //==============================================================================
949 template<typename S>
951 {
952  static bool run(
953  const GJKSolver_libccd<S>& /*gjkSolver*/,
954  const Sphere<S>& s,
955  const Transform3<S>& tf1,
956  const Vector3<S>& P1,
957  const Vector3<S>& P2,
958  const Vector3<S>& P3,
959  const Transform3<S>& tf2,
960  S* dist,
961  Vector3<S>* p1,
962  Vector3<S>* p2)
963  {
965  s, tf1, P1, P2, P3, tf2, dist, p1, p2);
966  }
967 };
968 
969 //==============================================================================
970 template<typename S>
972 {
973  max_collision_iterations = 500;
974  max_distance_iterations = 1000;
975  collision_tolerance = constants<S>::gjk_default_tolerance();
976  distance_tolerance = 1e-6;
977 }
978 
979 //==============================================================================
980 template<typename S>
981 void GJKSolver_libccd<S>::enableCachedGuess(bool if_enable) const
982 {
983  FCL_UNUSED(if_enable);
984 
985  // TODO: need change libccd to exploit spatial coherence
986 }
987 
988 //==============================================================================
989 template<typename S>
991  const Vector3<S>& guess) const
992 {
993  FCL_UNUSED(guess);
994 
995  // TODO: need change libccd to exploit spatial coherence
996 }
997 
998 //==============================================================================
999 template<typename S>
1001 {
1002  return Vector3<S>(-1, 0, 0);
1003 }
1004 
1005 } // namespace detail
1006 } // namespace fcl
1007 
1008 #endif
fcl::detail::GJKSolver_libccd::S
S_ S
Definition: gjk_solver_libccd.h:56
fcl::detail::sphereCapsuleIntersect
template bool sphereCapsuleIntersect(const Sphere< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::halfspaceTriangleIntersect
template bool halfspaceTriangleIntersect(const Halfspace< double > &s1, const Transform3< double > &tf1, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal)
fcl::detail::sphereBoxDistance
template FCL_EXPORT bool sphereBoxDistance(const Sphere< double > &sphere, const Transform3< double > &X_FS, const Box< double > &box, const Transform3< double > &X_FB, double *distance, Vector3< double > *p_FSb, Vector3< double > *p_FBs)
fcl::detail::ShapeIntersectLibccdImpl< S, Halfspace< S >, Halfspace< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Halfspace< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts)
Definition: gjk_solver_libccd-inl.h:272
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::detail::ShapeIntersectLibccdImpl< S, Plane< S >, Plane< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Plane< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts)
Definition: gjk_solver_libccd-inl.h:293
fcl::detail::ShapeSignedDistanceLibccdImpl
Definition: gjk_solver_libccd-inl.h:548
fcl::detail::capsulePlaneIntersect
template bool capsulePlaneIntersect(const Capsule< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2)
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
fcl::detail::ShapeDistanceLibccdImpl< S, Box< S >, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Box< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:699
fcl::detail::ShapeTriangleIntersectLibccdImpl::run
static bool run(const GJKSolver_libccd< S > &gjkSolver, const Shape &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal)
Definition: gjk_solver_libccd-inl.h:351
fcl::detail::ShapeTransformedTriangleIntersectLibccdImpl< S, Halfspace< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Halfspace< S > &s, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal)
Definition: gjk_solver_libccd-inl.h:504
fcl::detail::cylinderHalfspaceIntersect
template bool cylinderHalfspaceIntersect(const Cylinder< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::ShapeTransformedTriangleIntersectLibccdImpl< S, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal)
Definition: gjk_solver_libccd-inl.h:482
fcl::ContactPoint::normal
Vector3< S > normal
Contact normal, pointing from o1 to o2.
Definition: contact_point.h:51
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:54
fcl::detail::ShapeTriangleDistanceLibccdImpl::run
static bool run(const GJKSolver_libccd< S > &gjkSolver, const Shape &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:825
fcl::detail::sphereHalfspaceIntersect
template bool sphereHalfspaceIntersect(const Sphere< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::GJKSolver_libccd::collision_tolerance
S collision_tolerance
the threshold used in GJK algorithm to stop collision iteration
Definition: gjk_solver_libccd.h:168
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:387
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:860
fcl::detail::planeTriangleIntersect
template bool planeTriangleIntersect(const Plane< double > &s1, const Transform3< double > &tf1, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal)
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::detail::triDeleteGJKObject
void triDeleteGJKObject(void *o_)
Definition: gjk_libccd-inl.h:2958
fcl::detail::triGetSupportFunction
GJKSupportFunction triGetSupportFunction()
initialize GJK Triangle
Definition: gjk_libccd-inl.h:2908
fcl::detail::ShapeTransformedTriangleDistanceLibccdImpl::run
static bool run(const GJKSolver_libccd< S > &gjkSolver, const Shape &s, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:897
fcl::detail::ShapeTriangleDistanceLibccdImpl< S, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:878
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:162
fcl::detail::planeHalfspaceIntersect
template bool planeHalfspaceIntersect(const Plane< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Plane< double > &pl, Vector3< double > &p, Vector3< double > &d, double &penetration_depth, int &ret)
fcl::detail::capsuleHalfspaceIntersect
template bool capsuleHalfspaceIntersect(const Capsule< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::boxBoxIntersect
template bool boxBoxIntersect(const Box< double > &s1, const Transform3< double > &tf1, const Box< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts_)
fcl::detail::ShapeDistanceLibccdImpl< S, Capsule< S >, Capsule< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Capsule< S > &s1, const Transform3< S > &tf1, const Capsule< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:807
fcl::detail::ShapeIntersectLibccdImpl< S, Halfspace< S >, Plane< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Halfspace< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts)
Definition: gjk_solver_libccd-inl.h:329
unused.h
fcl::detail::ShapeDistanceLibccdImpl
Definition: gjk_solver_libccd-inl.h:605
fcl::ContactPoint::penetration_depth
S penetration_depth
Penetration depth.
Definition: contact_point.h:57
fcl::detail::sphereSphereIntersect
template FCL_EXPORT bool sphereSphereIntersect(const Sphere< double > &s1, const Transform3< double > &tf1, const Sphere< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::GJKInitializer
initialize GJK stuffs
Definition: gjk_libccd.h:76
fcl::detail::ShapeTransformedTriangleIntersectLibccdImpl< S, Plane< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Plane< S > &s, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal)
Definition: gjk_solver_libccd-inl.h:526
failed_at_this_configuration.h
fcl::detail::ShapeDistanceLibccdImpl< S, Sphere< S >, Box< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s1, const Transform3< S > &tf1, const Box< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:681
fcl::detail::planeIntersect
template bool planeIntersect(const Plane< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::sphereTriangleIntersect
template bool sphereTriangleIntersect(const Sphere< double > &s, const Transform3< double > &tf, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal_)
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::ShapeDistanceLibccdImpl< S, Sphere< S >, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:789
fcl::detail::convexHalfspaceIntersect
template bool convexHalfspaceIntersect(const Convex< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal)
fcl::detail::ShapeTransformedTriangleDistanceLibccdImpl
Definition: gjk_solver_libccd-inl.h:895
gjk_solver_libccd.h
halfspace.h
fcl::detail::ShapeDistanceLibccdImpl< S, Capsule< S >, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Capsule< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:735
fcl::detail::coneHalfspaceIntersect
template bool coneHalfspaceIntersect(const Cone< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::spherePlaneIntersect
template bool spherePlaneIntersect(const Sphere< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::ShapeDistanceLibccdImpl::run
static bool run(const GJKSolver_libccd< S > &gjkSolver, const Shape1 &s1, const Transform3< S > &tf1, const Shape2 &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:607
fcl::detail::ShapeDistanceLibccdImpl< S, Cylinder< S >, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Cylinder< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:771
fcl::detail::sphereTriangleDistance
template bool sphereTriangleDistance(const Sphere< double > &sp, const Transform3< double > &tf, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, double *dist)
fcl::detail::ShapeIntersectLibccdImpl::run
static bool run(const GJKSolver_libccd< S > &gjkSolver, const Shape1 &s1, const Transform3< S > &tf1, const Shape2 &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts)
Definition: gjk_solver_libccd-inl.h:114
fcl::detail::ellipsoidPlaneIntersect
template bool ellipsoidPlaneIntersect(const Ellipsoid< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::ShapeDistanceLibccdImpl< S, Sphere< S >, Cylinder< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s1, const Transform3< S > &tf1, const Cylinder< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:753
fcl::detail::boxPlaneIntersect
template bool boxPlaneIntersect(const Box< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::ContactPoint::pos
Vector3< S > pos
Contact position, in world space.
Definition: contact_point.h:54
fcl::detail::GJKSolver_libccd::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: gjk_solver_libccd.h:171
fcl::detail::triCreateGJKObject
template void * triCreateGJKObject(const Vector3d &P1, const Vector3d &P2, const Vector3d &P3)
fcl::detail::conePlaneIntersect
template bool conePlaneIntersect(const Cone< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::halfspaceIntersect
template bool halfspaceIntersect(const Halfspace< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Vector3< double > &p, Vector3< double > &d, Halfspace< double > &s, double &penetration_depth, int &ret)
fcl::detail::sphereSphereDistance
template FCL_EXPORT bool sphereSphereDistance(const Sphere< double > &s1, const Transform3< double > &tf1, const Sphere< double > &s2, const Transform3< double > &tf2, double *dist, Vector3< double > *p1, Vector3< double > *p2)
fcl::detail::sphereBoxIntersect
template FCL_EXPORT bool sphereBoxIntersect(const Sphere< double > &sphere, const Transform3< double > &X_FS, const Box< double > &box, const Transform3< double > &X_FB, std::vector< ContactPoint< double >> *contacts)
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d....
Definition: geometry/shape/halfspace.h:59
fcl::detail::halfspacePlaneIntersect
template bool halfspacePlaneIntersect(const Halfspace< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, Plane< double > &pl, Vector3< double > &p, Vector3< double > &d, double &penetration_depth, int &ret)
fcl::detail::GJKInitializer::deleteGJKObject
static void deleteGJKObject(void *o)
Delete GJK object.
Definition: gjk_libccd.h:91
sphere_capsule.h
gjk_libccd.h
fcl::time::point
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: time.h:52
fcl::detail::ShapeTransformedTriangleDistanceLibccdImpl< S, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:952
fcl::detail::GJKSolver_libccd::getCachedGuess
Vector3< S > getCachedGuess() const
Definition: gjk_solver_libccd-inl.h:1000
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:50
fcl::constants::gjk_default_tolerance
static Real gjk_default_tolerance()
Definition: constants.h:145
fcl::detail::boxHalfspaceIntersect
template bool boxHalfspaceIntersect(const Box< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2)
fcl::detail::ShapeDistanceLibccdImpl< S, Sphere< S >, Capsule< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s1, const Transform3< S > &tf1, const Capsule< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:717
fcl::detail::capsuleCapsuleDistance
template bool capsuleCapsuleDistance(const Capsule< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, double *dist, Vector3d *p1_res, Vector3d *p2_res)
fcl::detail::ShapeTransformedTriangleIntersectLibccdImpl
Definition: gjk_solver_libccd-inl.h:423
fcl::detail::ShapeSignedDistanceLibccdImpl::run
static bool run(const GJKSolver_libccd< S > &gjkSolver, const Shape1 &s1, const Transform3< S > &tf1, const Shape2 &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_solver_libccd-inl.h:550
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:50
fcl::detail::GJKSolver_libccd::enableCachedGuess
void enableCachedGuess(bool if_enable) const
Definition: gjk_solver_libccd-inl.h:981
fcl::detail::sphereCylinderDistance
template FCL_EXPORT bool sphereCylinderDistance(const Sphere< double > &sphere, const Transform3< double > &X_FS, const Cylinder< double > &cylinder, const Transform3< double > &X_FC, double *distance, Vector3< double > *p_FSc, Vector3< double > *p_FCs)
box_box.h
fcl::detail::ShapeTriangleIntersectLibccdImpl< S, Sphere< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Sphere< S > &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal)
Definition: gjk_solver_libccd-inl.h:405
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:640
fcl::detail::ShapeTriangleIntersectLibccdImpl
Definition: gjk_solver_libccd-inl.h:349
fcl::detail::GJKDistance
template bool GJKDistance(void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2)
sphere_box.h
FCL_GJK_LIBCCD_SHAPE_INTERSECT
#define FCL_GJK_LIBCCD_SHAPE_INTERSECT(SHAPE, ALG)
Definition: gjk_solver_libccd-inl.h:238
fcl::detail::sphereCylinderIntersect
template FCL_EXPORT bool sphereCylinderIntersect(const Sphere< double > &sphere, const Transform3< double > &X_FS, const Cylinder< double > &cylinder, const Transform3< double > &X_FC, std::vector< ContactPoint< double >> *contacts)
fcl::Plane
Infinite plane.
Definition: geometry/shape/plane.h:50
fcl::detail::triGetCenterFunction
GJKCenterFunction triGetCenterFunction()
Definition: gjk_libccd-inl.h:2913
fcl::detail::sphereCapsuleDistance
template bool sphereCapsuleDistance(const Sphere< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, double *dist, Vector3< double > *p1, Vector3< double > *p2)
fcl::detail::ShapeTriangleDistanceLibccdImpl
Definition: gjk_solver_libccd-inl.h:823
fcl::detail::GJKSolver_libccd::setCachedGuess
void setCachedGuess(const Vector3< S > &guess) const
Definition: gjk_solver_libccd-inl.h:990
fcl::detail::GJKSolver_libccd::shapeSignedDistance
bool shapeSignedDistance(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
Definition: gjk_solver_libccd-inl.h:583
fcl::detail::GJKSignedDistance
template bool GJKSignedDistance(void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2)
fcl::detail::ShapeIntersectLibccdImpl< S, Plane< S >, Halfspace< S > >::run
static bool run(const GJKSolver_libccd< S > &, const Plane< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts)
Definition: gjk_solver_libccd-inl.h:308
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT
#define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(SHAPE1, SHAPE2, ALG)
Definition: gjk_solver_libccd-inl.h:241
fcl::detail::cylinderPlaneIntersect
template bool cylinderPlaneIntersect(const Cylinder< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2)
fcl::detail::ShapeIntersectLibccdImpl
Definition: gjk_solver_libccd-inl.h:112
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:165
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::GJKSolver_libccd::GJKSolver_libccd
GJKSolver_libccd()
default setting for GJK algorithm
Definition: gjk_solver_libccd-inl.h:971
fcl::detail::ellipsoidHalfspaceIntersect
template bool ellipsoidHalfspaceIntersect(const Ellipsoid< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::ThrowDetailedConfiguration
void ThrowDetailedConfiguration(const Shape1 &s1, const Pose &X_FS1, const Shape2 &s2, const Pose &X_FS2, const Solver &solver, const std::exception &e)
Definition: failed_at_this_configuration.h:103
fcl::detail::FailedAtThisConfiguration
Definition: failed_at_this_configuration.h:64
fcl::Capsule
Center at zero point capsule.
Definition: capsule.h:50
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:88
fcl::detail::ShapeTransformedTriangleIntersectLibccdImpl::run
static bool run(const GJKSolver_libccd< S > &gjkSolver, const Shape &s, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal)
Definition: gjk_solver_libccd-inl.h:425
sphere_triangle.h
capsule_capsule.h
plane.h
sphere_cylinder.h
sphere_sphere.h


fcl
Author(s):
autogenerated on Fri Jun 11 2021 02:38:03