diff --git a/test/test_fcl_capsule_capsule.cpp b/test/test_fcl_capsule_capsule.cpp
index 40910184ca5207cf8fe7a6855b19d84ee121f1d3..72c2661a03a87f37c368010cbc92374f6d010a4a 100644
--- a/test/test_fcl_capsule_capsule.cpp
+++ b/test/test_fcl_capsule_capsule.cpp
@@ -36,119 +36,138 @@
/** \author Karsten Knese <Karsten.Knese@googlemail.com> */
#define BOOST_TEST_MODULE "FCL_CAPSULE_CAPSULE"
-#include <boost/test/unit_test.hpp>
+#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
-#include "fcl/collision.h"
-#include "fcl/shape/geometric_shapes.h"
-#include "fcl/narrowphase/narrowphase.h"
+#include <boost/test/unit_test.hpp>
-#include "math.h"
+#include <cmath>
+#include <fcl/distance.h>
+#include <fcl/math/transform.h>
+#include <fcl/collision.h>
+#include <fcl/collision_object.h>
+#include <fcl/shape/geometric_shapes.h>
using namespace fcl;
+typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin)
{
+ CollisionGeometryPtr_t s1 (new Capsule (5, 10));
+ CollisionGeometryPtr_t s2 (new Capsule (5, 10));
- GJKSolver_indep solver;
- Capsule s1(5, 10);
- Capsule s2(5, 10);
+ Transform3f tf1;
+ Transform3f tf2 (Vec3f(20.1, 0,0));
- Vec3f closest_p1, closest_p2;
+ CollisionObject o1 (s1, tf1);
+ CollisionObject o2 (s2, tf2);
- Transform3f transform;
- Transform3f transform2;
- transform2 = Transform3f(Vec3f(20.1, 0,0));
+ // Enable computation of nearest points
+ fcl::DistanceRequest distanceRequest (true);
+ fcl::DistanceResult distanceResult;
- bool res;
- FCL_REAL dist;
+ distance (&o1, &o2, distanceRequest, distanceResult);
- 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);
+ std::cerr << "Applied translation on two capsules";
+ std::cerr << " T1 = " << tf1.getTranslation()
+ << ", T2 = " << tf2.getTranslation() << std::endl;
+ std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
+ << ", p2 = " << distanceResult.nearest_points [1]
+ << ", distance = " << distanceResult.min_distance << std::endl;
+ BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
}
-
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY)
{
+ CollisionGeometryPtr_t s1 (new Capsule (5, 10));
+ CollisionGeometryPtr_t s2 (new Capsule (5, 10));
- GJKSolver_indep solver;
- Capsule s1(5, 10);
- Capsule s2(5, 10);
+ Transform3f tf1;
+ Transform3f tf2 (Vec3f(20, 20,0));
- Vec3f closest_p1, closest_p2;
+ CollisionObject o1 (s1, tf1);
+ CollisionObject o2 (s2, tf2);
- Transform3f transform;
- Transform3f transform2;
- transform2 = Transform3f(Vec3f(20, 20,0));
+ // Enable computation of nearest points
+ fcl::DistanceRequest distanceRequest (true);
+ fcl::DistanceResult distanceResult;
- bool res;
- FCL_REAL dist;
+ distance (&o1, &o2, distanceRequest, distanceResult);
- 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);
+ std::cerr << "Applied translation on two capsules";
+ std::cerr << " T1 = " << tf1.getTranslation()
+ << ", T2 = " << tf2.getTranslation() << std::endl;
+ std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
+ << ", p2 = " << distanceResult.nearest_points [1]
+ << ", distance = " << distanceResult.min_distance << std::endl;
+ FCL_REAL expected = sqrt(800) - 10;
+ BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6);
}
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ)
{
+ CollisionGeometryPtr_t s1 (new Capsule (5, 10));
+ CollisionGeometryPtr_t s2 (new Capsule (5, 10));
- GJKSolver_indep solver;
- Capsule s1(5, 10);
- Capsule s2(5, 10);
-
- Vec3f closest_p1, closest_p2;
+ Transform3f tf1;
+ Transform3f tf2 (Vec3f(0,0,20.1));
- Transform3f transform;
- Transform3f transform2;
- transform2 = Transform3f(Vec3f(0,0,20.1));
+ CollisionObject o1 (s1, tf1);
+ CollisionObject o2 (s2, tf2);
- bool res;
- FCL_REAL dist;
+ // Enable computation of nearest points
+ fcl::DistanceRequest distanceRequest (true);
+ fcl::DistanceResult distanceResult;
- 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;
+ distance (&o1, &o2, distanceRequest, distanceResult);
- BOOST_CHECK(fabs(dist - 0.1) < 0.001);
- BOOST_CHECK(res);
+ std::cerr << "Applied translation on two capsules";
+ std::cerr << " T1 = " << tf1.getTranslation()
+ << ", T2 = " << tf2.getTranslation() << std::endl;
+ std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
+ << ", p2 = " << distanceResult.nearest_points [1]
+ << ", distance = " << distanceResult.min_distance << std::endl;
+ BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.1, 1e-6);
}
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);
-
+ CollisionGeometryPtr_t s1 (new Capsule (5, 10));
+ CollisionGeometryPtr_t s2 (new Capsule (5, 10));
+
+ Transform3f tf1;
+ Transform3f tf2 (Quaternion3f (sqrt (2)/2, 0, sqrt (2)/2, 0),
+ Vec3f(0,0,25.1));
+
+ CollisionObject o1 (s1, tf1);
+ CollisionObject o2 (s2, tf2);
+
+ // Enable computation of nearest points
+ fcl::DistanceRequest distanceRequest (true);
+ fcl::DistanceResult distanceResult;
+
+ distance (&o1, &o2, distanceRequest, distanceResult);
+
+ std::cerr << "Applied rotation and translation on two capsules" << std::endl;
+ std::cerr << "R1 = " << tf1.getRotation ()
+ << ", T1 = " << tf1.getTranslation() << std::endl
+ << "R2 = " << tf2.getRotation ()
+ << ", T2 = " << tf2.getTranslation() << std::endl;
+ std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
+ << ", 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];
+
+ BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
+ CHECK_CLOSE_TO_0 (p1 [0], 1e-4);
+ CHECK_CLOSE_TO_0 (p1 [1], 1e-4);
+ BOOST_CHECK_CLOSE (p1 [2], 10, 1e-4);
+ CHECK_CLOSE_TO_0 (p2 [0], 1e-4);
+ CHECK_CLOSE_TO_0 (p2 [1], 1e-4);
+ BOOST_CHECK_CLOSE (p2 [2], 20.1, 1e-4);
}