diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 0ef7245cfb34a91ab45709366c96b37d3458a608..5abf8acf6f3b8d8420f05eec8bf0f8c8ab3bef8c 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -373,6 +373,11 @@ template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
@@ -838,6 +843,13 @@ bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tra
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+ template<>
+ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
+
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 2a1fc51393dc5ba41f2af2a926c8c2a3dea65f38..c1088f3b550e36d994cf451032306a4addd5eec6 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -46,6 +46,132 @@ namespace fcl
namespace details
{
+ // Clamp n to lie within the range [min, max]
+ float clamp(float n, float min, float max) {
+ if (n < min) return min;
+ if (n > max) return max;
+ return n;
+ }
+
+ // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
+ // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
+ // distance between between S1(s) and S2(t)
+ float closestPtSegmentSegment(Vec3f p1, Vec3f q1, Vec3f p2, Vec3f q2,
+ float &s, float &t, Vec3f &c1, Vec3f &c2)
+ {
+ const float EPSILON = 0.001;
+ Vec3f d1 = q1 - p1; // Direction vector of segment S1
+ Vec3f d2 = q2 - p2; // Direction vector of segment S2
+ Vec3f r = p1 - p2;
+ float a = d1.dot(d1); // Squared length of segment S1, always nonnegative
+
+ float e = d2.dot(d2); // Squared length of segment S2, always nonnegative
+ float f = d2.dot(r);
+ // Check if either or both segments degenerate into points
+ if (a <= EPSILON && e <= EPSILON) {
+ // Both segments degenerate into points
+ s = t = 0.0f;
+ c1 = p1;
+ c2 = p2;
+ Vec3f diff = c1-c2;
+ float res = diff.dot(diff);
+ return res;
+ }
+ if (a <= EPSILON) {
+ // First segment degenerates into a point
+ s = 0.0f;
+ t = f / e; // s = 0 => t = (b*s + f) / e = f / e
+ t = clamp(t, 0.0f, 1.0f);
+ } else {
+ float c = d1.dot(r);
+ if (e <= EPSILON) {
+ // Second segment degenerates into a point
+ t = 0.0f;
+ s = clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
+ } else {
+ // The general nondegenerate case starts here
+ float b = d1.dot(d2);
+ float denom = a*e-b*b; // Always nonnegative
+ // If segments not parallel, compute closest point on L1 to L2 and
+ // clamp to segment S1. Else pick arbitrary s (here 0)
+ if (denom != 0.0f) {
+ std::cerr << "demoninator equals zero, using 0 as reference" << std::endl;
+ s = clamp((b*f - c*e) / denom, 0.0f, 1.0f);
+ } else s = 0.0f;
+ // Compute point on L2 closest to S1(s) using
+ // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
+ t = (b*s + f) / e;
+
+ //
+ //If t in [0,1] done. Else clamp t, recompute s for the new value
+ //of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
+ //and clamp s to [0, 1]
+ if(t < 0.0f) {
+ t = 0.0f;
+ s = clamp(-c / a, 0.0f, 1.0f);
+ } else if (t > 1.0f) {
+ t = 1.0f;
+ s = clamp((b - c) / a, 0.0f, 1.0f);
+ }
+ }
+ }
+ c1 = p1 + d1 * s;
+ c2 = p2 + d2 * t;
+ Vec3f diff = c1-c2;
+ float res = diff.dot(diff);
+ return res;
+ }
+
+
+ // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
+ // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
+ // distance between between S1(s) and S2(t)
+
+ bool capsuleCapsuleDistance(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1_res, Vec3f* p2_res)
+ {
+
+ Vec3f p1(tf1.getTranslation());
+ Vec3f p2(tf2.getTranslation());
+
+ // line segment composes two points. First point is given by the origin, second point is computed by the origin transformed along z.
+ // extension along z-axis means transformation with identity matrix and translation vector z pos
+ Transform3f transformQ1(Vec3f(0,0,s1.lz));
+ transformQ1 = tf1 * transformQ1;
+ Vec3f q1 = transformQ1.getTranslation();
+
+
+ Transform3f transformQ2(Vec3f(0,0,s2.lz));
+ transformQ2 = tf2 * transformQ2;
+ Vec3f q2 = transformQ2.getTranslation();
+
+ // s and t correspont to the length of the line segment
+ float s, t;
+ Vec3f c1, c2;
+
+ float result = closestPtSegmentSegment(p1, q1, p2, q2, s, t, c1, c2);
+ *dist = sqrt(result)-s1.radius-s2.radius;
+
+ // getting directional unit vector
+ Vec3f distVec = c2 -c1;
+ distVec.normalize();
+
+ // extend the point to be border of the capsule.
+ // Done by following the directional unit vector for the length of the capsule radius
+ *p1_res = c1 + distVec*s1.radius;
+
+ distVec = c1-c2;
+ distVec.normalize();
+
+ *p2_res = c2 + distVec*s2.radius;
+
+ return true;
+ }
+
+
+
+
// 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:
@@ -2858,8 +2984,20 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Trans
}
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+}
-
-
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+}
} // fcl
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2ab2a8cc5abc446959543edb2c225dbd7725dbbe..42a7ed237e0790885f75163b6273f84dde63424a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,6 +28,7 @@ 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)
+add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp)
add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
#add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
diff --git a/test/test_fcl_capsule_capsule.cpp b/test/test_fcl_capsule_capsule.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cadced0b963e81033bf6fe5bebfbd3384c09f0b4
--- /dev/null
+++ b/test/test_fcl_capsule_capsule.cpp
@@ -0,0 +1,153 @@
+/*
+ * 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 Karsten Knese <Karsten.Knese@googlemail.com> */
+
+#define BOOST_TEST_MODULE "FCL_CAPSULE_CAPSULE"
+#include <boost/test/unit_test.hpp>
+
+#include "fcl/collision.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "fcl/narrowphase/narrowphase.h"
+
+#include "math.h"
+
+using namespace fcl;
+
+BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin)
+{
+
+ GJKSolver_indep solver;
+ Capsule s1(5, 10);
+ Capsule s2(5, 10);
+
+ Vec3f closest_p1, closest_p2;
+
+ Transform3f transform;
+ Transform3f transform2;
+ transform2 = Transform3f(Vec3f(20.1, 0,0));
+
+ bool res;
+ FCL_REAL dist;
+
+ res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
+ std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
+ std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
+
+ BOOST_CHECK(fabs(dist - 10.1) < 0.001);
+ BOOST_CHECK(res);
+
+}
+
+
+BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY)
+{
+
+ GJKSolver_indep solver;
+ Capsule s1(5, 10);
+ Capsule s2(5, 10);
+
+ Vec3f closest_p1, closest_p2;
+
+ Transform3f transform;
+ Transform3f transform2;
+ transform2 = Transform3f(Vec3f(20, 20,0));
+
+ bool res;
+ FCL_REAL dist;
+
+ res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
+ std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
+ std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
+
+ float expected = sqrt(800) - 10;
+ BOOST_CHECK(fabs(expected-dist) < 0.01);
+ BOOST_CHECK(res);
+
+}
+
+BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ)
+{
+
+ GJKSolver_indep solver;
+ Capsule s1(5, 10);
+ Capsule s2(5, 10);
+
+ Vec3f closest_p1, closest_p2;
+
+ Transform3f transform;
+ Transform3f transform2;
+ transform2 = Transform3f(Vec3f(0,0,20.1));
+
+ bool res;
+ FCL_REAL dist;
+
+ res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
+ std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
+ std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
+
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+}
+
+
+BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
+{
+
+ GJKSolver_indep solver;
+ Capsule s1(5, 10);
+ Capsule s2(5, 10);
+
+ Vec3f closest_p1, closest_p2;
+
+ Transform3f transform;
+ Transform3f transform2;
+ transform2 = Transform3f(Vec3f(0,0,25.1));
+ Matrix3f rot2;
+ rot2.setEulerZYX(0,M_PI_2,0);
+ transform2.setRotation(rot2);
+
+ bool res;
+ FCL_REAL dist;
+
+ res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
+ std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
+ std::cerr << "applied transformation of two caps: " << transform.getRotation() << " & " << transform2.getRotation() << std::endl;
+ std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
+
+ BOOST_CHECK(fabs(dist - 5.1) < 0.001);
+ BOOST_CHECK(res);
+
+}
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 0d45432b23350247878ed478d8c2bbb0a3e7d019..6182629a7c926220e15e1e4f9b75987e96502c2f 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -1862,12 +1862,12 @@ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
Sphere s2(10);
Transform3f transform;
- generateRandomTransform(extents, transform);
+ //generateRandomTransform(extents, transform);
bool res;
FCL_REAL dist = -1;
Vec3f closest_p1, closest_p2;
- res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+ res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 40, 0)), &dist, &closest_p1, &closest_p2);
BOOST_CHECK(fabs(dist - 10) < 0.001);
BOOST_CHECK(res);
@@ -1922,9 +1922,10 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
{
Box s1(20, 40, 50);
Box s2(10, 10, 10);
+ Vec3f closest_p1, closest_p2;
Transform3f transform;
- generateRandomTransform(extents, transform);
+ //generateRandomTransform(extents, transform);
bool res;
FCL_REAL dist;
@@ -1937,10 +1938,44 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
BOOST_CHECK(dist < 0);
BOOST_CHECK_FALSE(res);
- res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist);
+ std::cerr << " SOVLER NUMBER 1" << std::endl;
+ res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2);
+ std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+ res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2);
+ std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+ res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)), &dist, &closest_p1, &closest_p2);
+ std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+ res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2);
+ std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+ res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2);
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+ res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
+ res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)), &dist, &closest_p1, &closest_p2);
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+ res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2);
+ BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+ BOOST_CHECK(res);
+
+
res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
@@ -2719,40 +2754,6 @@ BOOST_AUTO_TEST_CASE(conecone)
BOOST_CHECK(res);
}
-BOOST_AUTO_TEST_CASE(conecylinder)
-{
- Cylinder s1(5, 10);
- Cone s2(5, 10);
- Transform3f transform;
- generateRandomTransform(extents, transform);
-
- bool res;
- FCL_REAL dist;
-
- res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
- BOOST_CHECK(dist < 0);
- BOOST_CHECK_FALSE(res);
-
- res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
- BOOST_CHECK(dist < 0);
- BOOST_CHECK_FALSE(res);
-
- res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
- BOOST_CHECK(fabs(dist - 0.1) < 0.001);
- BOOST_CHECK(res);
-
- res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
- BOOST_CHECK(fabs(dist - 0.1) < 0.001);
- BOOST_CHECK(res);
-
- res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
- BOOST_CHECK(fabs(dist - 30) < 0.001);
- BOOST_CHECK(res);
-
- res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
- BOOST_CHECK(fabs(dist - 30) < 0.001);
- BOOST_CHECK(res);
-}
diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp
index 5da0def053516bafb865d1fb716e57205150533e..35e1d9c2f098a90803a8cf64455053255060960c 100644
--- a/test/test_fcl_sphere_capsule.cpp
+++ b/test/test_fcl_sphere_capsule.cpp
@@ -167,6 +167,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_collision)
FCL_REAL distance;
BOOST_CHECK (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance));
+
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated)
@@ -181,8 +182,33 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated)
Transform3f capsule_transform (Vec3f (0., 0., 175));
FCL_REAL distance = 0.;
+ Vec3f p1;
+ Vec3f p2;
bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance);
+ BOOST_CHECK (is_separated);
+ BOOST_CHECK (distance == 25.);
+}
+
+BOOST_AUTO_TEST_CASE(Capsule_Capsule_Distance_test_separated)
+{
+ GJKSolver_libccd solver;
+
+ Capsule sphere1 (50, 0);
+ Transform3f sphere1_transform(Vec3f (0., 0., 0));
+
+ Capsule capsule (50, 00.);
+ Transform3f capsule_transform (Vec3f (150., 0., 0));
+
+ FCL_REAL distance = 0.;
+ Vec3f p1;
+ Vec3f p2;
+ bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance, &p1, &p2);
+
+ std::cerr << "computed distance: " << distance << std::endl;
+ std::cerr << "computed p1: " << p1 << std::endl;
+ std::cerr << "computed p2: " << p2 << std::endl;
+
BOOST_CHECK (is_separated);
BOOST_CHECK (distance == 25.);