diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 5abf8acf6f3b8d8420f05eec8bf0f8c8ab3bef8c..b4dc1c1360e5386d57a53c60ca12a431e2c73a0a 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -249,6 +249,11 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const T
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
@@ -266,26 +271,51 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -296,35 +326,60 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_libccd::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
@@ -356,12 +411,23 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tr
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
@@ -373,12 +439,6 @@ template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
-template<>
-bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-
/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver_indep
@@ -732,18 +792,24 @@ struct GJKSolver_indep
mutable Vec3f cached_guess;
};
+/// @brief Fast implementation for sphere-capsule collision
template<>
-bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
- const Capsule &s2, const Transform3f& tf2,
+bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/// @brief Fast implementation for sphere-sphere collision
+template<>
+bool GJKSolver_indep::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
+/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/// @brief Fast implementation for box-box collision
+/// @brief Fast implementation for box-box collision
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
@@ -754,26 +820,51 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -784,35 +875,60 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
@@ -820,15 +936,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
-template<>
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
-template<>
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
@@ -837,18 +954,16 @@ template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-
+/// @brief Fast implementation for sphere-capsule distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
- template<>
- bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
@@ -856,19 +971,24 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
-template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+template<>
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-
}
#endif
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index b701a7ea997e4be63ef0d9448cafe6ab46781340..0931513aa39b7fcc553dfd2af4d80ebb5f3689ec 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -2555,12 +2555,46 @@ bool planeIntersect(const Plane& s1, const Transform3f& tf1,
} // details
+// Shape intersect algorithms not using libccd
+//
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | | box | ellipsoid | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | box | O | | | | | | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | ellipsoid |/////| | | | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////|///////////| O | O | | | O | O | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | capsule |/////|///////////|////////| | | | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cone |/////|///////////|////////|/////////| | | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cylinder |/////|///////////|////////|/////////|//////| | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | plane |/////|///////////|////////|/////////|//////|//////////| O | O | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|///////////|////////|/////////|//////|//////////|///////| O | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | triangle |/////|///////////|////////|/////////|//////|//////////|///////|////////////| |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
const Capsule &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+}
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2587,6 +2621,16 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2595,6 +2639,16 @@ bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Trans
return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2603,6 +2657,16 @@ bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, con
return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2611,6 +2675,16 @@ bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, c
return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2619,6 +2693,16 @@ bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Tra
return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2643,6 +2727,18 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ Plane pl;
+ Vec3f p, d;
+ FCL_REAL depth;
+ int ret;
+ return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2651,6 +2747,16 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Tra
return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2659,6 +2765,16 @@ bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform
return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2667,6 +2783,16 @@ bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const T
return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2675,6 +2801,16 @@ bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const
return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2684,15 +2820,13 @@ bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transfo
}
template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_libccd::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- Plane pl;
- Vec3f p, d;
- FCL_REAL depth;
- int ret;
- return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+ const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2704,6 +2838,8 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Trans
}
+
+
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
@@ -2718,7 +2854,6 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f
return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
}
-
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
@@ -2733,8 +2868,29 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal);
}
-
-
+// Shape distance algorithms not using libccd
+//
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | | box | ellipsoid | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | box | | | | | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | ellipsoid |/////| | | | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////|///////////| O | O | | | | | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | capsule |/////|///////////|////////| O | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cone |/////|///////////|////////|/////////| | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cylinder |/////|///////////|////////|/////////|//////| | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | plane |/////|///////////|////////|/////////|//////|//////////| | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|///////////|////////|/////////|//////|//////////|///////| | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | triangle |/////|///////////|////////|/////////|//////|//////////|///////|////////////| |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
@@ -2744,6 +2900,14 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tr
return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
+}
+
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
@@ -2752,32 +2916,73 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tra
return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+}
+
+
+
+
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
}
-template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+template<>
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
}
-
-
-
+// Shape intersect algorithms not using built-in GJK algorithm
+//
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | | box | ellipsoid | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | box | O | | | | | | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | ellipsoid |/////| | | | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////|///////////| O | O | | | O | O | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | capsule |/////|///////////|////////| | | | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cone |/////|///////////|////////|/////////| | | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cylinder |/////|///////////|////////|/////////|//////| | O | O | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | plane |/////|///////////|////////|/////////|//////|//////////| O | O | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|///////////|////////|/////////|//////|//////////|///////| O | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | triangle |/////|///////////|////////|/////////|//////|//////////|///////|////////////| |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
const Capsule &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+}
+
+template<>
+bool GJKSolver_indep::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2804,6 +3009,16 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2812,6 +3027,16 @@ bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transf
return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2820,6 +3045,16 @@ bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, cons
return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2828,6 +3063,16 @@ bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, co
return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2836,6 +3081,16 @@ bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Tran
return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2845,7 +3100,6 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1,
Vec3f p, d;
FCL_REAL depth;
int ret;
-
return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
}
@@ -2861,6 +3115,18 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ Plane pl;
+ Vec3f p, d;
+ FCL_REAL depth;
+ int ret;
+ return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2869,6 +3135,16 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Tran
return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2877,6 +3153,16 @@ bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3
return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2885,6 +3171,16 @@ bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Tr
return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2893,6 +3189,16 @@ bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const
return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2902,15 +3208,13 @@ bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transfor
}
template<>
-bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- Plane pl;
- Vec3f p, d;
- FCL_REAL depth;
- int ret;
- return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+ const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2922,14 +3226,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
}
-template<>
+
+
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
}
-template<>
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
@@ -2950,6 +3256,29 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f&
return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal);
}
+// Shape distance algorithms not using built-in GJK algorithm
+//
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | | box | ellipsoid | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | box | | | | | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | ellipsoid |/////| | | | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////|///////////| O | O | | | | | O |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | capsule |/////|///////////|////////| O | | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cone |/////|///////////|////////|/////////| | | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | cylinder |/////|///////////|////////|/////////|//////| | | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | plane |/////|///////////|////////|/////////|//////|//////////| | | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|///////////|////////|/////////|//////|//////////|///////| | |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
+// | triangle |/////|///////////|////////|/////////|//////|//////////|///////|////////////| |
+// +------------+-----+-----------+--------+---------+------+----------+-------+------------+----------+
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
@@ -2959,6 +3288,14 @@ bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tra
return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
+}
+
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
@@ -2968,36 +3305,30 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
}
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
+ return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
-template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
-{
- return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
-}
+
template<>
-bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+ return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
}
template<>
-bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+ return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
}
} // fcl
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 624fff11c15550f9641bae4c7f197985c17167b7..78924dc4dade6419618b04abd4331071112b0eb3 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -2752,3 +2752,221 @@ BOOST_AUTO_TEST_CASE(conecone)
+template<typename S1, typename S2>
+void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance)
+{
+ Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
+ Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
+
+ Vec3f contactA;
+ Vec3f contactB;
+ FCL_REAL depthA;
+ FCL_REAL depthB;
+ Vec3f normalA;
+ Vec3f normalB;
+
+ bool resA;
+ bool resB;
+
+ const double tol = 1e-6;
+
+ resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
+ resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK(contactA.equal(contactB, tol)); // contact point should be same
+ BOOST_CHECK(normalA.equal(-normalB, tol)); // normal should be opposite
+ BOOST_CHECK_CLOSE(depthA, depthB, tol); // penetration depth should be same
+
+ resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
+ resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK(contactA.equal(contactB, tol));
+ BOOST_CHECK(normalA.equal(-normalB, tol));
+ BOOST_CHECK_CLOSE(depthA, depthB, tol);
+}
+
+BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes)
+{
+ // This test check whether a shape intersection algorithm is called for the
+ // reverse case as well. For example, if FCL has sphere-capsule intersection
+ // algorithm, then this algorithm should be called for capsule-sphere case.
+
+ // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+ Box box(10, 10, 10);
+ Sphere sphere(5);
+ Capsule capsule(5, 10);
+ Cone cone(5, 10);
+ Cylinder cylinder(5, 10);
+ Plane plane(Vec3f(), 0.0);
+ Halfspace halfspace(Vec3f(), 0.0);
+
+ // Use sufficiently short distance so that all the primitive shapes can intersect
+ FCL_REAL distance = 5.0;
+
+ // If new shape intersection algorithm is added for two distinct primitive
+ // shapes, uncommnet associated lines. For example, box-sphere intersection
+ // algorithm is added, then uncommnet box-sphere and sphere-box.
+
+// testReversibleShapeIntersection(box, sphere, distance);
+// testReversibleShapeIntersection(box, capsule, distance);
+// testReversibleShapeIntersection(box, cone, distance);
+// testReversibleShapeIntersection(box, cylinder, distance);
+ testReversibleShapeIntersection(box, plane, distance);
+ testReversibleShapeIntersection(box, halfspace, distance);
+
+// testReversibleShapeIntersection(sphere, box, distance);
+ testReversibleShapeIntersection(sphere, capsule, distance);
+// testReversibleShapeIntersection(sphere, cone, distance);
+// testReversibleShapeIntersection(sphere, cylinder, distance);
+ testReversibleShapeIntersection(sphere, plane, distance);
+ testReversibleShapeIntersection(sphere, halfspace, distance);
+
+// testReversibleShapeIntersection(capsule, box, distance);
+ testReversibleShapeIntersection(capsule, sphere, distance);
+// testReversibleShapeIntersection(capsule, cone, distance);
+// testReversibleShapeIntersection(capsule, cylinder, distance);
+ testReversibleShapeIntersection(capsule, plane, distance);
+ testReversibleShapeIntersection(capsule, halfspace, distance);
+
+// testReversibleShapeIntersection(cone, box, distance);
+// testReversibleShapeIntersection(cone, sphere, distance);
+// testReversibleShapeIntersection(cone, capsule, distance);
+// testReversibleShapeIntersection(cone, cylinder, distance);
+ testReversibleShapeIntersection(cone, plane, distance);
+ testReversibleShapeIntersection(cone, halfspace, distance);
+
+// testReversibleShapeIntersection(cylinder, box, distance);
+// testReversibleShapeIntersection(cylinder, sphere, distance);
+// testReversibleShapeIntersection(cylinder, capsule, distance);
+// testReversibleShapeIntersection(cylinder, cone, distance);
+ testReversibleShapeIntersection(cylinder, plane, distance);
+ testReversibleShapeIntersection(cylinder, halfspace, distance);
+
+ testReversibleShapeIntersection(plane, box, distance);
+ testReversibleShapeIntersection(plane, sphere, distance);
+ testReversibleShapeIntersection(plane, capsule, distance);
+ testReversibleShapeIntersection(plane, cone, distance);
+ testReversibleShapeIntersection(plane, cylinder, distance);
+// testReversibleShapeIntersection(plane, halfspace, distance);
+
+ testReversibleShapeIntersection(halfspace, box, distance);
+ testReversibleShapeIntersection(halfspace, sphere, distance);
+ testReversibleShapeIntersection(halfspace, capsule, distance);
+ testReversibleShapeIntersection(halfspace, cone, distance);
+ testReversibleShapeIntersection(halfspace, cylinder, distance);
+// testReversibleShapeIntersection(halfspace, plane, distance);
+}
+
+template<typename S1, typename S2>
+void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
+{
+ Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
+ Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
+
+ FCL_REAL distA;
+ FCL_REAL distB;
+ Vec3f p1A;
+ Vec3f p1B;
+ Vec3f p2A;
+ Vec3f p2B;
+
+ bool resA;
+ bool resB;
+
+ const double tol = 1e-6;
+
+ resA = solver1.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
+ resB = solver1.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
+ BOOST_CHECK(p1A.equal(p2B, tol)); // closest points should in reverse order
+ BOOST_CHECK(p2A.equal(p1B, tol));
+
+ resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
+ resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK_CLOSE(distA, distB, tol);
+ BOOST_CHECK(p1A.equal(p2B, tol));
+ BOOST_CHECK(p2A.equal(p1B, tol));
+}
+
+BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
+{
+ // This test check whether a shape distance algorithm is called for the
+ // reverse case as well. For example, if FCL has sphere-capsule distance
+ // algorithm, then this algorithm should be called for capsule-sphere case.
+
+ // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+ Box box(10, 10, 10);
+ Sphere sphere(5);
+ Capsule capsule(5, 10);
+ Cone cone(5, 10);
+ Cylinder cylinder(5, 10);
+ Plane plane(Vec3f(), 0.0);
+ Halfspace halfspace(Vec3f(), 0.0);
+
+ // Use sufficiently long distance so that all the primitive shapes CANNOT intersect
+ FCL_REAL distance = 15.0;
+
+ // If new shape distance algorithm is added for two distinct primitive
+ // shapes, uncommnet associated lines. For example, box-sphere intersection
+ // algorithm is added, then uncommnet box-sphere and sphere-box.
+
+// testReversibleShapeDistance(box, sphere, distance);
+// testReversibleShapeDistance(box, capsule, distance);
+// testReversibleShapeDistance(box, cone, distance);
+// testReversibleShapeDistance(box, cylinder, distance);
+// testReversibleShapeDistance(box, plane, distance);
+// testReversibleShapeDistance(box, halfspace, distance);
+
+// testReversibleShapeDistance(sphere, box, distance);
+ testReversibleShapeDistance(sphere, capsule, distance);
+// testReversibleShapeDistance(sphere, cone, distance);
+// testReversibleShapeDistance(sphere, cylinder, distance);
+// testReversibleShapeDistance(sphere, plane, distance);
+// testReversibleShapeDistance(sphere, halfspace, distance);
+
+// testReversibleShapeDistance(capsule, box, distance);
+ testReversibleShapeDistance(capsule, sphere, distance);
+// testReversibleShapeDistance(capsule, cone, distance);
+// testReversibleShapeDistance(capsule, cylinder, distance);
+// testReversibleShapeDistance(capsule, plane, distance);
+// testReversibleShapeDistance(capsule, halfspace, distance);
+
+// testReversibleShapeDistance(cone, box, distance);
+// testReversibleShapeDistance(cone, sphere, distance);
+// testReversibleShapeDistance(cone, capsule, distance);
+// testReversibleShapeDistance(cone, cylinder, distance);
+// testReversibleShapeDistance(cone, plane, distance);
+// testReversibleShapeDistance(cone, halfspace, distance);
+
+// testReversibleShapeDistance(cylinder, box, distance);
+// testReversibleShapeDistance(cylinder, sphere, distance);
+// testReversibleShapeDistance(cylinder, capsule, distance);
+// testReversibleShapeDistance(cylinder, cone, distance);
+// testReversibleShapeDistance(cylinder, plane, distance);
+// testReversibleShapeDistance(cylinder, halfspace, distance);
+
+// testReversibleShapeDistance(plane, box, distance);
+// testReversibleShapeDistance(plane, sphere, distance);
+// testReversibleShapeDistance(plane, capsule, distance);
+// testReversibleShapeDistance(plane, cone, distance);
+// testReversibleShapeDistance(plane, cylinder, distance);
+// testReversibleShapeDistance(plane, halfspace, distance);
+
+// testReversibleShapeDistance(halfspace, box, distance);
+// testReversibleShapeDistance(halfspace, sphere, distance);
+// testReversibleShapeDistance(halfspace, capsule, distance);
+// testReversibleShapeDistance(halfspace, cone, distance);
+// testReversibleShapeDistance(halfspace, cylinder, distance);
+// testReversibleShapeDistance(halfspace, plane, distance);
+}
+