diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 36c361013d87f7ca308a4b8b4d5aa9ed6c14ff5a..54e3459a1542cd9d3c1ca78ac3f808521139b4f3 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,
@@ -296,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 cbdec59204c6c714f6593c37e91107c6d016f624..4102e0314dcf430d091f932a3de9636d4ed53ca7 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -45,6 +45,91 @@ namespace fcl
namespace details
{
+// Compute the point on a line segment that is the closest point on the
+// segment to to another point. The code is inspired by the explanation
+// given by Dan Sunday's page:
+// http://geomalgorithms.com/a02-_lines.html
+static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp) {
+ Vec3f v = s2 - s1;
+ Vec3f w = p - s1;
+
+ FCL_REAL c1 = w.dot(v);
+ FCL_REAL c2 = v.dot(v);
+
+ if (c1 <= 0) {
+ sp = s1;
+ } else if (c2 <= c1) {
+ sp = s2;
+ } else {
+ FCL_REAL b = c1/c2;
+ Vec3f Pb = s1 + v * b;
+ sp = Pb;
+ }
+}
+
+bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
+{
+ 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;
+
+ Vec3f 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(segment_point + normal * distance);
+ }
+
+ 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;
+}
+
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 +2375,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,
@@ -2464,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,
@@ -2492,7 +2592,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,
@@ -2665,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/CMakeLists.txt b/test/CMakeLists.txt
index 5c21b4cc501e616f778670dba7c12403445149b3..6638b19ef5847404a4aeaecba8315c50a32e90cd 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
new file mode 100644
index 0000000000000000000000000000000000000000..eb4a421d3c5268f3d290624f6e5895e1143a4a2f
--- /dev/null
+++ b/test/test_fcl_sphere_capsule.cpp
@@ -0,0 +1,185 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Martin Felis <martin.felis@iwr.uni-heidelberg.de> */
+
+#define BOOST_TEST_MODULE "FCL_SPHERE_CAPSULE"
+#include <boost/test/unit_test.hpp>
+
+#include "fcl/collision.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "fcl/narrowphase/narrowphase.h"
+
+using namespace fcl;
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z)
+{
+ 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., 200));
+
+ BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z_negative)
+{
+ 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., -200));
+
+ BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_x)
+{
+ GJKSolver_libccd solver;
+
+ Sphere sphere1 (50);
+ Transform3f sphere1_transform;
+ sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+ Capsule capsule (50, 200.);
+ Transform3f capsule_transform (Vec3f (150., 0., 0.));
+
+ BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated)
+{
+ GJKSolver_libccd solver;
+
+ Sphere sphere1 (50);
+ Transform3f sphere1_transform;
+ sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+ Capsule capsule (50, 200.);
+ Matrix3f rotation;
+ rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
+ Transform3f capsule_transform (rotation, Vec3f (150., 0., 0.));
+
+ BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z)
+{
+ 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., 125));
+
+ 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 (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_Intersect_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));
+}
+
+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));
+
+ FCL_REAL distance;
+
+ BOOST_CHECK (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance));
+}
+
+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.);
+}