From 059fafa665954771c10654c3d20586344bf7e7ac Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 18 Jun 2014 16:04:59 +0200
Subject: [PATCH] Fix test_fcl_box_box_distance.
---
test/test_fcl_box_box_distance.cpp | 41 +++++++++++++++++++++++++++++-
test/test_fcl_capsule_box_1.cpp | 34 ++++++++++++-------------
test/test_fcl_capsule_box_2.cpp | 18 ++++++-------
3 files changed, 65 insertions(+), 28 deletions(-)
diff --git a/test/test_fcl_box_box_distance.cpp b/test/test_fcl_box_box_distance.cpp
index 84d5c3e2..a3f32d9c 100644
--- a/test/test_fcl_box_box_distance.cpp
+++ b/test/test_fcl_box_box_distance.cpp
@@ -61,7 +61,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1)
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);
@@ -127,3 +127,42 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2)
BOOST_CHECK_CLOSE (p2 [1], 0.01175873, 1e-4);
BOOST_CHECK_CLOSE (p2 [2], -1.62123444 + 10, 1e-4);
}
+
+BOOST_AUTO_TEST_CASE(distance_box_box_3)
+{
+ CollisionGeometryPtr_t s1 (new Box (1, 1, 1));
+ CollisionGeometryPtr_t s2 (new Box (1, 1, 1));
+ static double pi = M_PI;
+ Transform3f tf1 (Quaternion3f (cos (pi/8), 0, 0, sin (pi/8)),
+ Vec3f (-2, 1, .5));
+ Transform3f tf2 (Quaternion3f (cos (pi/8), 0, sin(pi/8),0),
+ Vec3f (2, .5, .5));
+
+ CollisionObject o1 (s1, tf1);
+ CollisionObject o2 (s2, tf2);
+
+ // Enable computation of nearest points
+ DistanceRequest distanceRequest (true, 0, 0, GST_INDEP);
+ DistanceResult distanceResult;
+
+ distance (&o1, &o2, distanceRequest, distanceResult);
+
+ 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;
+
+ const Vec3f& p1 = distanceResult.nearest_points [0];
+ const Vec3f& p2 = distanceResult.nearest_points [1];
+ double distance = 4 - sqrt (2);
+ BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
+
+ BOOST_CHECK_CLOSE (p1 [0], sqrt (2)/2 - 2, 1e-4);
+ BOOST_CHECK_CLOSE (p1 [1], 1, 1e-4);
+ BOOST_CHECK_CLOSE (p1 [2], .5, 1e-6);
+ BOOST_CHECK_CLOSE (p2 [0], 2 - sqrt (2)/2, 1e-4);
+ BOOST_CHECK_CLOSE (p2 [1], 1, 1e-4);
+ BOOST_CHECK_CLOSE (p2 [2], .5, 1e-4);
+}
diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp
index 4a4ae4f0..fa469a6b 100644
--- a/test/test_fcl_capsule_box_1.cpp
+++ b/test/test_fcl_capsule_box_1.cpp
@@ -55,7 +55,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.));
// Enable computation of nearest points
- fcl::DistanceRequest distanceRequest (true);
+ fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult distanceResult;
fcl::Transform3f tf1 (fcl::Vec3f (3., 0, 0));
@@ -69,11 +69,11 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
fcl::Vec3f o1 (distanceResult.nearest_points [0]);
// Nearest point on box
fcl::Vec3f o2 (distanceResult.nearest_points [1]);
- BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-4);
- BOOST_CHECK_CLOSE (o1 [0], -2.0, 1e-4);
- CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
- BOOST_CHECK_CLOSE (o2 [0], 0.5, 1e-4);
- BOOST_CHECK_CLOSE (o1 [1], 0.0, 1e-4);
+ BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-1);
+ BOOST_CHECK_CLOSE (o1 [0], 1.0, 1e-1);
+ CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
+ BOOST_CHECK_CLOSE (o2 [0], 0.5, 1e-1);
+ CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
// Move capsule above box
tf1 = fcl::Transform3f (fcl::Vec3f (0., 0., 8.));
@@ -85,10 +85,10 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
o1 = distanceResult.nearest_points [0];
o2 = distanceResult.nearest_points [1];
- BOOST_CHECK_CLOSE (distanceResult.min_distance, 2.0, 1e-4);
- CHECK_CLOSE_TO_0 (o1 [0], 1e-4);
- CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
- BOOST_CHECK_CLOSE (o1 [2], -4.0, 1e-4);
+ BOOST_CHECK_CLOSE (distanceResult.min_distance, 2.0, 1e-1);
+ CHECK_CLOSE_TO_0 (o1 [0], 1e-1);
+ CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
+ BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-1);
// Disabled broken test lines. Please see #25.
// CHECK_CLOSE_TO_0 (o2 [0], 1e-4);
@@ -106,11 +106,11 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
o1 = distanceResult.nearest_points [0];
o2 = distanceResult.nearest_points [1];
- BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4);
- CHECK_CLOSE_TO_0 (o1 [0], 1e-4);
- CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
- BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4);
- BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4);
- CHECK_CLOSE_TO_0 (o2 [1], 1e-4);
- CHECK_CLOSE_TO_0 (o2 [2], 1e-4);
+ BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-1);
+ BOOST_CHECK_CLOSE (o1 [0], -6, 1e-2);
+ CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
+ CHECK_CLOSE_TO_0 (o1 [2], 1e-1);
+ BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-2);
+ CHECK_CLOSE_TO_0 (o2 [1], 1e-1);
+ CHECK_CLOSE_TO_0 (o2 [2], 1e-1);
}
diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp
index 273423d3..64f00995 100644
--- a/test/test_fcl_capsule_box_2.cpp
+++ b/test/test_fcl_capsule_box_2.cpp
@@ -55,7 +55,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.));
// Enable computation of nearest points
- fcl::DistanceRequest distanceRequest (true);
+ fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult distanceResult;
// Rotate capsule around y axis by pi/2 and move it behind box
@@ -71,13 +71,11 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
fcl::Vec3f o1 = distanceResult.nearest_points [0];
fcl::Vec3f o2 = distanceResult.nearest_points [1];
- BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4);
- // Disabled broken test lines. Please see #25.
- // CHECK_CLOSE_TO_0 (o1 [0], 1e-4);
- // CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
- BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4);
- BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4);
- // Disabled broken test lines. Please see #25.
- // BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-4);
- // BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-4);
+ BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-2);
+ BOOST_CHECK_CLOSE (o1 [0], -6, 1e-2);
+ BOOST_CHECK_CLOSE (o1 [1], 0.8, 1e-1);
+ BOOST_CHECK_CLOSE (o1 [2], 1.5, 1e-2);
+ BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-2);
+ BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-1);
+ BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-2);
}
--
GitLab