diff --git a/include/fcl/math/transform.h b/include/fcl/math/transform.h
index 9e45f8b15e09970d7fd5dce25c5172f679423e87..84fd8c4efd26ed85f4124fe467bd0c8d80dfe3bf 100644
--- a/include/fcl/math/transform.h
+++ b/include/fcl/math/transform.h
@@ -171,6 +171,12 @@ Quaternion3f conj(const Quaternion3f& q);
/// @brief inverse of quaternion
Quaternion3f inverse(const Quaternion3f& q);
+static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q)
+{
+ o << "(" << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << ")";
+ return o;
+}
+
/// @brief Simple transform class used locally by InterpMotion
class Transform3f
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 624fff11c15550f9641bae4c7f197985c17167b7..02f749292c638320b2ac486faba39ae992d8eae0 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -113,90 +113,257 @@ BOOST_AUTO_TEST_CASE(gjkcache)
}
}
-BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
+template <typename S1, typename S2>
+void printComparisonError(const std::string& comparison_type,
+ const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ const Vec3f& contact_or_normal,
+ const Vec3f& expected_contact_or_normal,
+ bool check_opposite_normal,
+ FCL_REAL tol)
{
- Sphere s1(20);
- Sphere s2(10);
+ std::cout << "Disagreement between " << comparison_type
+ << " and expected_" << comparison_type << " for "
+ << getNodeTypeName(s1.getNodeType()) << " and "
+ << getNodeTypeName(s2.getNodeType()) << " with '"
+ << getGJKSolverName(solver_type) << "' solver." << std::endl
+ << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
+ << "tf1.translation: " << tf1.getTranslation() << std::endl
+ << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
+ << "tf2.translation: " << tf2.getTranslation() << std::endl
+ << comparison_type << ": " << contact_or_normal << std::endl
+ << "expected_" << comparison_type << ": " << expected_contact_or_normal;
+
+ if (check_opposite_normal)
+ std::cout << " or " << -expected_contact_or_normal;
+
+ std::cout << std::endl
+ << "difference: " << (contact_or_normal - expected_contact_or_normal).norm() << std::endl
+ << "tolerance: " << tol << std::endl;
+}
- Transform3f transform;
- generateRandomTransform(extents, transform);
- Transform3f identity;
+template <typename S1, typename S2>
+void printComparisonError(const std::string& comparison_type,
+ const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ FCL_REAL depth,
+ FCL_REAL expected_depth,
+ FCL_REAL tol)
+{
+ std::cout << "Disagreement between " << comparison_type
+ << " and expected_" << comparison_type << " for "
+ << getNodeTypeName(s1.getNodeType()) << " and "
+ << getNodeTypeName(s2.getNodeType()) << " with '"
+ << getGJKSolverName(solver_type) << "' solver." << std::endl
+ << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
+ << "tf1.translation: " << tf1.getTranslation() << std::endl
+ << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
+ << "tf2.translation: " << tf2.getTranslation() << std::endl
+ << "depth: " << depth << std::endl
+ << "expected_depth: " << expected_depth << std::endl
+ << "difference: " << std::fabs(depth - expected_depth) << std::endl
+ << "tolerance: " << tol << std::endl;
+}
+
+template <typename S1, typename S2>
+void compareContact(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ const Vec3f& contact, Vec3f* expected_point,
+ FCL_REAL depth, FCL_REAL* expected_depth,
+ const Vec3f& normal, Vec3f* expected_normal, bool check_opposite_normal,
+ FCL_REAL tol)
+{
+ if (expected_point)
+ {
+ bool contact_equal = contact.equal(*expected_point, tol);
+ BOOST_CHECK(contact_equal);
+ if (!contact_equal)
+ printComparisonError("contact", s1, tf1, s2, tf2, solver_type, contact, *expected_point, false, tol);
+ }
+ if (expected_depth)
+ {
+ bool depth_equal = std::fabs(depth - *expected_depth) < tol;
+ BOOST_CHECK(depth_equal);
+ if (!depth_equal)
+ printComparisonError("depth", s1, tf1, s2, tf2, solver_type, depth, *expected_depth, tol);
+ }
+
+ if (expected_normal)
+ {
+ bool normal_equal = normal.equal(*expected_normal, tol);
+
+ if (!normal_equal && check_opposite_normal)
+ normal_equal = normal.equal(-(*expected_normal), tol);
+
+ BOOST_CHECK(normal_equal);
+ if (!normal_equal)
+ printComparisonError("normal", s1, tf1, s2, tf2, solver_type, normal, *expected_normal, check_opposite_normal, tol);
+ }
+}
+
+template <typename S1, typename S2>
+void testShapeInersection(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ bool expected_res,
+ Vec3f* expected_point = NULL,
+ FCL_REAL* expected_depth = NULL,
+ Vec3f* expected_normal = NULL,
+ bool check_opposite_normal = false,
+ FCL_REAL tol = 1e-9)
+{
CollisionRequest request;
+ request.gjk_solver_type = solver_type;
CollisionResult result;
- bool res;
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ Vec3f contact;
+ FCL_REAL depth;
+ Vec3f normal; // normal direction should be from object 1 to object 2
+ bool res;
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ if (solver_type == GST_LIBCCD)
+ {
+ res = solver1.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL);
+ }
+ else if (solver_type == GST_INDEP)
+ {
+ res = solver2.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL);
+ }
+ else
+ {
+ std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
+ return;
+ }
+ BOOST_CHECK_EQUAL(res, expected_res);
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ if (solver_type == GST_LIBCCD)
+ {
+ res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal);
+ }
+ else if (solver_type == GST_INDEP)
+ {
+ res = solver2.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal);
+ }
+ else
+ {
+ std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
+ return;
+ }
+ BOOST_CHECK_EQUAL(res, expected_res);
+ if (expected_res)
+ compareContact(s1, tf1, s2, tf2, solver_type, contact, expected_point, depth, expected_depth, normal, expected_normal, check_opposite_normal, tol);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ if (s1.getNodeType() == GEOM_HALFSPACE || s2.getNodeType() == GEOM_HALFSPACE)
+ {
+ std::cout << "Abort test since Halfspace is not registered to the collision matrix. "
+ << "Please see issue #57." << std::endl;
+ return;
+ }
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
+ request.enable_contact = false;
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
+ BOOST_CHECK_EQUAL(res, expected_res);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
+ request.enable_contact = true;
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
+ res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
+ BOOST_CHECK_EQUAL(res, expected_res);
+ if (expected_res)
+ {
+ BOOST_CHECK_EQUAL(result.numContacts(), 1);
+ if (result.numContacts() == 1)
+ {
+ Contact contact = result.getContact(0);
+ compareContact(s1, tf1, s2, tf2, solver_type, contact.pos, expected_point, contact.penetration_depth, expected_depth, contact.normal, expected_normal, check_opposite_normal, tol);
+ }
+ }
+}
- res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
+BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
+{
+ Sphere s1(20);
+ Sphere s2(10);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ Transform3f tf1;
+ Transform3f tf2;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(40, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(30, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(30.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(29.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-29.9, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-30.0, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-30.01, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index e3bda2d8870ebb4be1550eaec6b228864ebcc291..76f8e952f29abf4c6538fbbf500c50b27afb458c 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -400,4 +400,58 @@ bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, Continuous
return true;
}
+std::string getNodeTypeName(NODE_TYPE node_type)
+{
+ if (node_type == BV_UNKNOWN)
+ return std::string("BV_UNKNOWN");
+ else if (node_type == BV_AABB)
+ return std::string("BV_AABB");
+ else if (node_type == BV_OBB)
+ return std::string("BV_OBB");
+ else if (node_type == BV_RSS)
+ return std::string("BV_RSS");
+ else if (node_type == BV_kIOS)
+ return std::string("BV_kIOS");
+ else if (node_type == BV_OBBRSS)
+ return std::string("BV_OBBRSS");
+ else if (node_type == BV_KDOP16)
+ return std::string("BV_KDOP16");
+ else if (node_type == BV_KDOP18)
+ return std::string("BV_KDOP18");
+ else if (node_type == BV_KDOP24)
+ return std::string("BV_KDOP24");
+ else if (node_type == GEOM_BOX)
+ return std::string("GEOM_BOX");
+ else if (node_type == GEOM_SPHERE)
+ return std::string("GEOM_SPHERE");
+ else if (node_type == GEOM_CAPSULE)
+ return std::string("GEOM_CAPSULE");
+ else if (node_type == GEOM_CONE)
+ return std::string("GEOM_CONE");
+ else if (node_type == GEOM_CYLINDER)
+ return std::string("GEOM_CYLINDER");
+ else if (node_type == GEOM_CONVEX)
+ return std::string("GEOM_CONVEX");
+ else if (node_type == GEOM_PLANE)
+ return std::string("GEOM_PLANE");
+ else if (node_type == GEOM_HALFSPACE)
+ return std::string("GEOM_HALFSPACE");
+ else if (node_type == GEOM_TRIANGLE)
+ return std::string("GEOM_TRIANGLE");
+ else if (node_type == GEOM_OCTREE)
+ return std::string("GEOM_OCTREE");
+ else
+ return std::string("invalid");
+}
+
+std::string getGJKSolverName(GJKSolverType solver_type)
+{
+ if (solver_type == GST_LIBCCD)
+ return std::string("libccd");
+ else if (solver_type == GST_INDEP)
+ return std::string("built-in");
+ else
+ return std::string("invalid");
+}
+
}
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 45d58936011be0c515ad9d610ec92cb1767a50e3..401e16eb438a4d6b7c2c83f0bd176bb183f67f78 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -182,6 +182,10 @@ bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, Continuou
bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist);
+std::string getNodeTypeName(NODE_TYPE node_type);
+
+std::string getGJKSolverName(GJKSolverType solver_type);
+
}
#endif