From ab5321bc973443939cc32e984a7f2abfbf77b306 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Thu, 16 Apr 2015 14:49:09 -0400
Subject: [PATCH] Add box-box collision test to check correctness of contact
point
---
test/test_fcl_geometric_shapes.cpp | 58 ++++++++++++++++++++++++++++++
1 file changed, 58 insertions(+)
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 364f767c..1126820f 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -199,6 +199,56 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
BOOST_CHECK_FALSE(res);
}
+bool compareContactPoints(const Vec3f& c1,const Vec3f& c2)
+{
+ return c1[2] < c2[2];
+} // Ascending order
+
+void testBoxBoxContactPoints(const Matrix3f& R)
+{
+ Box s1(100, 100, 100);
+ Box s2(10, 20, 30);
+
+ // Vertices of s2
+ std::vector<Vec3f> vertices(8);
+ vertices[0].setValue( 1, 1, 1);
+ vertices[1].setValue( 1, 1, -1);
+ vertices[2].setValue( 1, -1, 1);
+ vertices[3].setValue( 1, -1, -1);
+ vertices[4].setValue(-1, 1, 1);
+ vertices[5].setValue(-1, 1, -1);
+ vertices[6].setValue(-1, -1, 1);
+ vertices[7].setValue(-1, -1, -1);
+
+ for (int i = 0; i < 8; ++i)
+ {
+ vertices[i][0] *= 0.5 * s2.side[0];
+ vertices[i][1] *= 0.5 * s2.side[1];
+ vertices[i][2] *= 0.5 * s2.side[2];
+ }
+
+ Transform3f tf1 = Transform3f(Vec3f(0, 0, -50));
+ Transform3f tf2 = Transform3f(R);
+
+ Vec3f normal;
+ Vec3f point;
+ double penetration;
+
+ // Make sure the two boxes are colliding
+ bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &point, &penetration, &normal);
+ BOOST_CHECK(res);
+
+ // Compute global vertices
+ for (int i = 0; i < 8; ++i)
+ vertices[i] = tf2.transform(vertices[i]);
+
+ // Sort the vertices so that the lowest vertex along z-axis comes first
+ std::sort(vertices.begin(), vertices.end(), compareContactPoints);
+
+ // The lowest vertex along z-axis should be the contact point
+ BOOST_CHECK(vertices[0].equal(point));
+}
+
BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
{
Box s1(20, 40, 50);
@@ -250,6 +300,14 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
result.clear();
res = (collide(&s1, transform, &s2, transform * Transform3f(q), request, result) > 0);
BOOST_CHECK(res);
+
+ FCL_UINT32 numTests = 1e+2;
+ for (FCL_UINT32 i = 0; i < numTests; ++i)
+ {
+ Transform3f tf;
+ generateRandomTransform(extents, tf);
+ testBoxBoxContactPoints(tf.getRotation());
+ }
}
BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
--
GitLab