From c676c3ab829a43dd9d414b116ab5b64fd3ae5d08 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 18 Jun 2014 14:08:40 +0200
Subject: [PATCH] Add a test in test_fcl_box_box_distance.
---
test/test_fcl_box_box_distance.cpp | 92 ++++++------------------------
1 file changed, 17 insertions(+), 75 deletions(-)
diff --git a/test/test_fcl_box_box_distance.cpp b/test/test_fcl_box_box_distance.cpp
index 5c510d7e..84d5c3e2 100644
--- a/test/test_fcl_box_box_distance.cpp
+++ b/test/test_fcl_box_box_distance.cpp
@@ -89,99 +89,41 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1)
BOOST_CHECK_CLOSE (p2 [2], 4, 1e-6);
}
-#if 0
-BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY)
+BOOST_AUTO_TEST_CASE(distance_box_box_2)
{
- CollisionGeometryPtr_t s1 (new Capsule (5, 10));
- CollisionGeometryPtr_t s2 (new Capsule (5, 10));
-
- Transform3f tf1;
- Transform3f tf2 (Vec3f(20, 20,0));
-
- CollisionObject o1 (s1, tf1);
- CollisionObject o2 (s2, tf2);
-
- // Enable computation of nearest points
- DistanceRequest distanceRequest (true);
- DistanceResult distanceResult;
-
- distance (&o1, &o2, distanceRequest, distanceResult);
-
- 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));
-
+ CollisionGeometryPtr_t s1 (new Box (6, 10, 2));
+ CollisionGeometryPtr_t s2 (new Box (2, 2, 2));
+ static double pi = M_PI;
Transform3f tf1;
- Transform3f tf2 (Vec3f(0,0,20.1));
+ Transform3f tf2 (Quaternion3f (cos (pi/8), sin(pi/8)/sqrt(3),
+ sin(pi/8)/sqrt(3), sin(pi/8)/sqrt(3)),
+ Vec3f(0, 0, 10));
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
- DistanceRequest distanceRequest (true);
+ DistanceRequest distanceRequest (true, 0, 0, GST_INDEP);
DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
- std::cerr << "Applied translation on two capsules";
+ std::cerr << "Applied translation on two boxes";
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)
-{
- 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
- DistanceRequest distanceRequest (true);
- 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 Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
+ double distance = -1.62123444 + 10 - 1;
+ BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
- 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);
+ BOOST_CHECK_CLOSE (p1 [0], 0.60947571, 1e-4);
+ BOOST_CHECK_CLOSE (p1 [1], 0.01175873, 1e-4);
+ BOOST_CHECK_CLOSE (p1 [2], 1, 1e-6);
+ BOOST_CHECK_CLOSE (p2 [0], 0.60947571, 1e-4);
+ BOOST_CHECK_CLOSE (p2 [1], 0.01175873, 1e-4);
+ BOOST_CHECK_CLOSE (p2 [2], -1.62123444 + 10, 1e-4);
}
-#endif
--
GitLab