From bcee670e20ca226595a32be2d30d9117b3b92713 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin.felis@iwr.uni-heidelberg.de>
Date: Mon, 11 Mar 2013 17:57:06 +0100
Subject: [PATCH] added a hopefully fast sphereCapsuleIntersect()
---
include/fcl/narrowphase/narrowphase.h | 6 ++
src/narrowphase/narrowphase.cpp | 84 ++++++++++++++++++++++++++-
test/CMakeLists.txt | 2 +
test/test_fcl_sphere_capsule.cpp | 27 ++++++++-
4 files changed, 116 insertions(+), 3 deletions(-)
diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 36c36101..6a44b030 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/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,
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index cbdec592..d05d9159 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/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,
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5c21b4cc..6638b19e 100644
--- a/test/CMakeLists.txt
+++ b/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()
diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp
index df8820c1..62bff1a8 100644
--- a/test/test_fcl_sphere_capsule.cpp
+++ b/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));
+}
--
GitLab