diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 6a44b03055340fc65c66ca4e691890f822e8b5dd..54e3459a1542cd9d3c1ca78ac3f808521139b4f3 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -302,6 +302,12 @@ template<>
bool GJKSolver_libccd::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_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist) const;
+
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 0e5814ee838c50bd19c899bc06969e756eef0b2d..4102e0314dcf430d091f932a3de9636d4ed53ca7 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -100,16 +100,32 @@ bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
*contact_points = tf2.transform(segment_point + normal * distance);
}
- /*
- std::cout << "tf2 rot = " << tf2.getRotation() << std::endl;
- std::cout << "distance = " << distance << std::endl;
- std::cout << "segment_point = " << segment_point << std::endl;
- std::cout << "diff = " << diff << std::endl;
- std::cout << "normal(l)= " << (*normal_) << std::endl;
- std::cout << "normal(w)= " << normal << std::endl;
- std::cout << "contact = " << (*contact_points) << std::endl;
- std::cout << "origin = " << tf2.transform(Vec3f()) << std::endl;
- */
+ return true;
+}
+
+bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist)
+{
+ Transform3f tf2_inv (tf2);
+ tf2_inv.inverse();
+
+ Vec3f pos1 (0., 0., 0.5 * s2.lz);
+ Vec3f pos2 (0., 0., -0.5 * s2.lz);
+ Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f()));
+
+ Vec3f segment_point;
+
+ lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
+ Vec3f diff = s_c - segment_point;
+
+ FCL_REAL distance = diff.length() - s1.radius - s2.radius;
+
+ if (distance <= 0)
+ return false;
+
+ if (dist)
+ *dist = distance;
return true;
}
@@ -2540,6 +2556,14 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
+template<>
+bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist) const
+{
+ return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist);
+}
+
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
@@ -2747,6 +2771,14 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f&
}
+template<>
+bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist) const
+{
+ return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist);
+}
+
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp
index 62bff1a829f830c166a56ddd770f2ebb0ecf5f25..2111a9ca4a6d985c1394ecfc656077c82e90a2f5 100644
--- a/test/test_fcl_sphere_capsule.cpp
+++ b/test/test_fcl_sphere_capsule.cpp
@@ -43,7 +43,7 @@
using namespace fcl;
-BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z)
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z)
{
GJKSolver_libccd solver;
@@ -57,7 +57,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
-BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z_negative)
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z_negative)
{
GJKSolver_libccd solver;
@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z_negative)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
-BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_x)
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_x)
{
GJKSolver_libccd solver;
@@ -85,7 +85,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_x)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
-BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_capsule_rotated)
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated)
{
GJKSolver_libccd solver;
@@ -101,7 +101,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_capsule_rotated)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
-BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z)
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z)
{
GJKSolver_libccd solver;
@@ -124,7 +124,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z)
BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point));
}
-BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z_rotated)
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated)
{
GJKSolver_libccd solver;
@@ -148,3 +148,36 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z_rotated)
BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal));
BOOST_CHECK (Vec3f (0., 0., 50.).equal(contact_point, solver.collision_tolerance));
}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_collision)
+{
+ GJKSolver_libccd solver;
+
+ Sphere sphere1 (50);
+ Transform3f sphere1_transform;
+ sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+ Capsule capsule (50, 200.);
+ Transform3f capsule_transform (Vec3f (0., 0., 100));
+
+ BOOST_CHECK (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated)
+{
+ GJKSolver_libccd solver;
+
+ Sphere sphere1 (50);
+ Transform3f sphere1_transform;
+ sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+ Capsule capsule (50, 200.);
+ Transform3f capsule_transform (Vec3f (0., 0., 175));
+
+ FCL_REAL distance = 0.;
+ bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance);
+
+
+ BOOST_CHECK (is_separated);
+ BOOST_CHECK (distance == 25.);
+}