added a hopefully fast sphereCapsuleIntersect()

include/fcl/narrowphase/narrowphase.h

@@ -195,6 +195,12 @@ struct GJKSolver_libccd
 };
 
 
+/// @brief Fast implementation for sphere-capsule collision
+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;
+
 /// @brief Fast implementation for sphere-sphere collision
 template<>
 bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,

src/narrowphase/narrowphase.cpp

@@ -45,6 +45,75 @@ namespace fcl
 namespace details
 {
 
+bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1, 
+                            const Capsule& s2, const Transform3f& tf2,
+                           Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
+{
+	// Code is inspired by the explanation given by Dan Sunday's page:
+	//   http://geomalgorithms.com/a02-_lines.html
+	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 v = pos2 - pos1;
+	Vec3f w = s_c - pos1;
+
+	FCL_REAL c1 = w.dot(v);
+	FCL_REAL c2 = v.dot(v);
+
+	Vec3f base;
+	Vec3f diff;
+	Vec3f normal;
+
+	if (c1 <= 0) {
+		diff = s_c - pos1;
+		base = pos1;
+	} else if (c2 <= c1) {
+		diff = s_c - pos2;
+		base = pos2;
+	} else {
+		FCL_REAL b = c1/c2;
+		Vec3f Pb = pos1 + v * b;
+
+		diff = s_c - Pb;
+		base = Pb;
+	}
+
+	FCL_REAL distance = diff.length() - s1.radius - s2.radius;
+
+	if (distance > 0)
+		return false;
+
+	normal = diff.normalize() * - FCL_REAL(1);
+
+	if (distance < 0 && penetration_depth)
+		*penetration_depth = -distance;
+
+	if (normal_)
+		*normal_ = tf2.getQuatRotation().transform(normal);
+
+	if (contact_points) {
+		*contact_points = tf2.transform(base + normal * distance);
+	}
+
+	/*
+	std::cout << "tf2 rot  = " << tf2.getRotation() << std::endl;
+	std::cout << "distance = " << distance << std::endl;
+	std::cout << "base     = " << base << 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 sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, 
                            const Sphere& s2, const Transform3f& tf2,
                            Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
@@ -2290,6 +2359,13 @@ bool planeIntersect(const Plane& s1, const Transform3f& tf1,
 
 } // details
 
+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);
+}
 
 template<>
 bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
@@ -2492,7 +2568,13 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Tran
 
 
 
-
+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);
+}
 
 template<>
 bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,

test/CMakeLists.txt

@@ -27,6 +27,8 @@ add_fcl_test(test_fcl_shape_mesh_consistency test_fcl_shape_mesh_consistency.cpp
 add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp)
 add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
 
+add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
+
 if (FCL_HAVE_OCTOMAP)
   add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
 endif()

test/test_fcl_sphere_capsule.cpp

@@ -118,10 +118,33 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z)
 
 	bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contact_point, &penetration, &normal);
 
-//	std::cout << "contact _point = " << contact_point << std::endl;
-
 	BOOST_CHECK (is_intersecting);
 	BOOST_CHECK (penetration == 25.);
 	BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal));
 	BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point));
 }
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z_rotated)
+{
+	GJKSolver_libccd solver;
+
+	Sphere sphere1 (50);
+	Transform3f sphere1_transform;
+	sphere1_transform.setTranslation (Vec3f (0., 0., 0));
+
+	Capsule capsule (50, 200.);
+	Matrix3f rotation;
+	rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
+	Transform3f capsule_transform (rotation, Vec3f (0., 50., 75));
+
+	FCL_REAL penetration = 0.;
+	Vec3f contact_point;
+	Vec3f normal;
+
+	bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contact_point, &penetration, &normal);
+
+	BOOST_CHECK (is_intersecting);
+	BOOST_CHECK_CLOSE (25, penetration, solver.collision_tolerance);
+	BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal));
+	BOOST_CHECK (Vec3f (0., 0., 50.).equal(contact_point, solver.collision_tolerance));
+}