diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..112a1ff6cef93ebae20a8507c15d61a55b5adea1
--- /dev/null
+++ b/src/distance_capsule_capsule.cpp
@@ -0,0 +1,373 @@
+// Geometric Tools, LLC
+// Copyright (c) 1998-2011
+// Distributed under the Boost Software License, Version 1.0.
+// http://www.boost.org/LICENSE_1_0.txt
+// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
+//
+// Modified by Florent Lamiraux 2014
+
+#include <cmath>
+#include <limits>
+#include <fcl/math/transform.h>
+#include <fcl/shape/geometric_shapes.h>
+#include "distance_func_matrix.h"
+
+// Note that partial specialization of template functions is not allowed.
+// Therefore, two implementations with the default narrow phase solvers are
+// provided. If another narrow phase solver were to be used, the default
+// template implementation would be called, unless the function is also
+// specialized for this new type.
+//
+// One solution would be to make narrow phase solvers derive from an abstract
+// class and specialize the template for this abstract class.
+namespace fcl {
+ class GJKSolver_libccd;
+ class GJKSolver_indep;
+
+ template <>
+ FCL_REAL ShapeShapeDistance <Capsule, Capsule, GJKSolver_libccd>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_libccd*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ const Capsule* c1 = static_cast <const Capsule*> (o1);
+ const Capsule* c2 = static_cast <const Capsule*> (o2);
+
+ // We assume that capsules are centered at the origin.
+ fcl::Vec3f center1 = tf1.getTranslation ();
+ fcl::Vec3f center2 = tf2.getTranslation ();
+ // We assume that capsules are oriented along z-axis.
+ fcl::Vec3f direction1 = tf1.getRotation ().getColumn (2);
+ fcl::Vec3f direction2 = tf2.getRotation ().getColumn (2);
+ FCL_REAL halfLength1 = 0.5*c1->lz;
+ FCL_REAL halfLength2 = 0.5*c2->lz;
+
+ Vec3f diff = center1 - center2;
+ FCL_REAL a01 = -direction1.dot (direction2);
+ FCL_REAL b0 = diff.dot (direction1);
+ FCL_REAL b1 = -diff.dot (direction2);
+ FCL_REAL c = diff.sqrLength ();
+ FCL_REAL det = fabs (1.0 - a01*a01);
+ FCL_REAL s1, s2, sqrDist, extDet0, extDet1, tmpS0, tmpS1;
+ FCL_REAL epsilon = std::numeric_limits<FCL_REAL>::epsilon () * 100;
+
+ if (det >= epsilon) {
+ // Segments are not parallel.
+ s1 = a01*b1 - b0;
+ s2 = a01*b0 - b1;
+ extDet0 = halfLength1*det;
+ extDet1 = halfLength2*det;
+
+ if (s1 >= -extDet0) {
+ if (s1 <= extDet0) {
+ if (s2 >= -extDet1) {
+ if (s2 <= extDet1) { // region 0 (interior)
+ // Minimum at interior points of segments.
+ FCL_REAL invDet = (1.0)/det;
+ s1 *= invDet;
+ s2 *= invDet;
+ sqrDist = s1*(s1 + a01*s2 + 2.0*b0) +
+ s2*(a01*s1 + s2 + 2.0*b1) + c;
+ }
+ else { // region 3 (side)
+ s2 = halfLength2;
+ tmpS0 = -(a01*s2 + b0);
+ if (tmpS0 < -halfLength1) {
+ s1 = -halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ }
+ else if (tmpS0 <= halfLength1) {
+ s1 = tmpS0;
+ sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
+ }
+ else {
+ s1 = halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ }
+ }
+ }
+ else { // region 7 (side)
+ s2 = -halfLength2;
+ tmpS0 = -(a01*s2 + b0);
+ if (tmpS0 < -halfLength1) {
+ s1 = -halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ } else if (tmpS0 <= halfLength1) {
+ s1 = tmpS0;
+ sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
+ } else {
+ s1 = halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ }
+ }
+ }
+ else {
+ if (s2 >= -extDet1) {
+ if (s2 <= extDet1) { // region 1 (side)
+ s1 = halfLength1;
+ tmpS1 = -(a01*s1 + b1);
+ if (tmpS1 < -halfLength2) {
+ s2 = -halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ else if (tmpS1 <= halfLength2) {
+ s2 = tmpS1;
+ sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
+ }
+ else {
+ s2 = halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ }
+ else { // region 2 (corner)
+ s2 = halfLength2;
+ tmpS0 = -(a01*s2 + b0);
+ if (tmpS0 < -halfLength1) {
+ s1 = -halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ }
+ else if (tmpS0 <= halfLength1) {
+ s1 = tmpS0;
+ sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
+ }
+ else {
+ s1 = halfLength1;
+ tmpS1 = -(a01*s1 + b1);
+ if (tmpS1 < -halfLength2) {
+ s2 = -halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ else if (tmpS1 <= halfLength2) {
+ s2 = tmpS1;
+ sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
+ }
+ else {
+ s2 = halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ }
+ }
+ }
+ else { // region 8 (corner)
+ s2 = -halfLength2;
+ tmpS0 = -(a01*s2 + b0);
+ if (tmpS0 < -halfLength1) {
+ s1 = -halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ }
+ else if (tmpS0 <= halfLength1) {
+ s1 = tmpS0;
+ sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
+ }
+ else {
+ s1 = halfLength1;
+ tmpS1 = -(a01*s1 + b1);
+ if (tmpS1 > halfLength2) {
+ s2 = halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ else if (tmpS1 >= -halfLength2) {
+ s2 = tmpS1;
+ sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
+ }
+ else {
+ s2 = -halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ }
+ }
+ }
+ }
+ else {
+ if (s2 >= -extDet1) {
+ if (s2 <= extDet1) { // region 5 (side)
+ s1 = -halfLength1;
+ tmpS1 = -(a01*s1 + b1);
+ if (tmpS1 < -halfLength2) {
+ s2 = -halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ else if (tmpS1 <= halfLength2) {
+ s2 = tmpS1;
+ sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
+ }
+ else {
+ s2 = halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ }
+ else { // region 4 (corner)
+ s2 = halfLength2;
+ tmpS0 = -(a01*s2 + b0);
+ if (tmpS0 > halfLength1) {
+ s1 = halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ }
+ else if (tmpS0 >= -halfLength1) {
+ s1 = tmpS0;
+ sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
+ }
+ else {
+ s1 = -halfLength1;
+ tmpS1 = -(a01*s1 + b1);
+ if (tmpS1 < -halfLength2) {
+ s2 = -halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ else if (tmpS1 <= halfLength2) {
+ s2 = tmpS1;
+ sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
+ }
+ else {
+ s2 = halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ }
+ }
+ }
+ else { // region 6 (corner)
+ s2 = -halfLength2;
+ tmpS0 = -(a01*s2 + b0);
+ if (tmpS0 > halfLength1) {
+ s1 = halfLength1;
+ sqrDist = s1*(s1 - 2.0*tmpS0) +
+ s2*(s2 + 2.0*b1) + c;
+ }
+ else if (tmpS0 >= -halfLength1) {
+ s1 = tmpS0;
+ sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
+ }
+ else {
+ s1 = -halfLength1;
+ tmpS1 = -(a01*s1 + b1);
+ if (tmpS1 < -halfLength2) {
+ s2 = -halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ else if (tmpS1 <= halfLength2) {
+ s2 = tmpS1;
+ sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
+ }
+ else {
+ s2 = halfLength2;
+ sqrDist = s2*(s2 - 2.0*tmpS1) +
+ s1*(s1 + 2.0*b0) + c;
+ }
+ }
+ }
+ }
+ }
+ else {
+ // The segments are parallel. The average b0 term is designed to
+ // ensure symmetry of the function. That is, dist(seg0,seg1) and
+ // dist(seg1,seg0) should produce the same number.
+ FCL_REAL e0pe1 = halfLength1 + halfLength2;
+ FCL_REAL sign = (a01 > 0.0 ? -1.0 : 1.0);
+ FCL_REAL b0Avr = (0.5)*(b0 - sign*b1);
+ FCL_REAL lambda = -b0Avr;
+ if (lambda < -e0pe1) {
+ lambda = -e0pe1;
+ }
+ else if (lambda > e0pe1) {
+ lambda = e0pe1;
+ }
+
+ s2 = -sign*lambda*halfLength2/e0pe1;
+ s1 = lambda + sign*s2;
+ sqrDist = lambda*(lambda + 2.0*b0Avr) + c;
+ }
+
+ Vec3f closestOnSegment1 = center1 + s1*direction1;
+ Vec3f closestOnSegment2 = center2 + s2*direction2;
+
+ Vec3f unitVector = closestOnSegment2 - closestOnSegment1;
+ FCL_REAL distance = sqrt (sqrDist);
+ if (distance >= epsilon) {
+ unitVector /= distance;
+ } else {
+ unitVector.setZero ();
+ }
+ // Update distance result.
+ result.min_distance = distance - c1->radius - c2->radius;
+ if (request.enable_nearest_points) {
+ result.nearest_points [0] = closestOnSegment1 + c1->radius * unitVector;
+ result.nearest_points [1] = closestOnSegment2 - c2->radius * unitVector;
+ }
+ result.o1 = o1;
+ result.o2 = o2;
+ result.b1 = -1;
+ result.b2 = -1;
+ return result.min_distance;
+ }
+
+ template <>
+ FCL_REAL ShapeShapeDistance <Capsule, Capsule, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ GJKSolver_libccd* unused = 0x0;
+ return ShapeShapeDistance <Capsule, Capsule, GJKSolver_libccd>
+ (o1, tf1, o2, tf2, unused, request, result);
+ }
+
+ template <>
+ std::size_t ShapeShapeCollide <Capsule, Capsule, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const CollisionRequest& request,
+ CollisionResult& result)
+ {
+ GJKSolver_libccd* unused = 0x0;
+ DistanceResult distanceResult;
+ DistanceRequest distanceRequest (request.enable_contact);
+
+ FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule, GJKSolver_libccd>
+ (o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
+
+ if (distance <= 0) {
+ if (request.enable_contact) {
+ Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
+ const Vec3f& p1 = distanceResult.nearest_points [0];
+ const Vec3f& p2 = distanceResult.nearest_points [1];
+ contact.pos = .5*(p1+p2);
+ contact.normal = (p2-p1)/(p2-p1).length ();
+ result.addContact (contact);
+ }
+ return 1;
+ }
+ return 0;
+ }
+
+ template<>
+ std::size_t ShapeShapeCollide <Capsule, Capsule, GJKSolver_libccd>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_libccd* nsolver, const CollisionRequest& request,
+ CollisionResult& result)
+ {
+ GJKSolver_indep* unused = 0x0;
+ return ShapeShapeCollide <Capsule, Capsule, GJKSolver_indep>
+ (o1, tf1, o2, tf2, unused, request, result);
+ }
+} // namespace fcl
diff --git a/src/distance_func_matrix.h b/src/distance_func_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..a4933c69d3443065b79d1e9cbd4889acfb2e47e9
--- /dev/null
+++ b/src/distance_func_matrix.h
@@ -0,0 +1,53 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2014, CNRS-LAAS
+ * 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 Florent Lamiraux */
+
+#include <fcl/collision_data.h>
+
+namespace fcl {
+ template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+ FCL_REAL ShapeShapeDistance
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver, const DistanceRequest& request,
+ DistanceResult& result);
+
+ template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+ std::size_t ShapeShapeCollide
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver, const CollisionRequest& request,
+ CollisionResult& result);
+}
diff --git a/test/test_fcl_capsule_capsule.cpp b/test/test_fcl_capsule_capsule.cpp
index 72c2661a03a87f37c368010cbc92374f6d010a4a..4e77a4387be132ef6261168c14b876323506c749 100644
--- a/test/test_fcl_capsule_capsule.cpp
+++ b/test/test_fcl_capsule_capsule.cpp
@@ -48,7 +48,7 @@
#include <fcl/shape/geometric_shapes.h>
using namespace fcl;
-typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
+typedef boost::shared_ptr <CollisionGeometry> CollisionGeometryPtr_t;
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin)
{
@@ -62,8 +62,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin)
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
- fcl::DistanceRequest distanceRequest (true);
- fcl::DistanceResult distanceResult;
+ DistanceRequest distanceRequest (true);
+ DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
@@ -89,8 +89,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY)
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
- fcl::DistanceRequest distanceRequest (true);
- fcl::DistanceResult distanceResult;
+ DistanceRequest distanceRequest (true);
+ DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
@@ -117,8 +117,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ)
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
- fcl::DistanceRequest distanceRequest (true);
- fcl::DistanceResult distanceResult;
+ DistanceRequest distanceRequest (true);
+ DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
@@ -146,8 +146,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
- fcl::DistanceRequest distanceRequest (true);
- fcl::DistanceResult distanceResult;
+ DistanceRequest distanceRequest (true);
+ DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
@@ -160,8 +160,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
<< ", p2 = " << distanceResult.nearest_points [1]
<< ", distance = " << distanceResult.min_distance << std::endl;
- const fcl::Vec3f& p1 = distanceResult.nearest_points [0];
- const fcl::Vec3f& p2 = distanceResult.nearest_points [1];
+ const Vec3f& p1 = distanceResult.nearest_points [0];
+ const Vec3f& p2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
CHECK_CLOSE_TO_0 (p1 [0], 1e-4);