Add a unit-test that compares results from Box and Convex.

test/test_fcl_geometric_shapes.cpp

@@ -43,6 +43,7 @@
 
 #include <hpp/fcl/narrowphase/narrowphase.h>
 #include <hpp/fcl/collision.h>
+#include <hpp/fcl/distance.h>
 #include "test_fcl_utility.h"
 #include <iostream>
 
@@ -54,6 +55,22 @@ FCL_REAL tol_gjk = 0.01;
 GJKSolver_indep solver1;
 GJKSolver_indep solver2;
 
+Eigen::IOFormat fmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
+Eigen::IOFormat pyfmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
+
+namespace hpp {
+namespace fcl {
+std::ostream& operator<< (std::ostream& os, const Transform3f& tf)
+{
+  return os << "[ " <<
+    tf.getTranslation().format(fmt)
+    << ", "
+    << tf.getQuatRotation().coeffs().format(fmt)
+    << " ]" ;
+}
+}
+}
+
 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
 
 template <typename S1, typename S2>
@@ -164,7 +181,6 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1,
   CollisionResult result;
 
   Vec3f contact;
-  FCL_REAL depth;
   Vec3f normal;  // normal direction should be from object 1 to object 2
   bool res;
 
@@ -188,6 +204,70 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1,
   }
 }
 
+template <typename Sa, typename Sb> void compareShapeIntersection (
+    const Sa& sa, const Sb& sb, 
+    const Transform3f& tf1, const Transform3f& tf2,
+    FCL_REAL tol = 1e-9)
+{
+  CollisionRequest request (CONTACT | DISTANCE_LOWER_BOUND, 1);
+  CollisionResult resA, resB;
+
+  collide(&sa, tf1, &sa, tf2, request, resA);
+  collide(&sb, tf1, &sb, tf2, request, resB);
+
+  BOOST_CHECK_EQUAL(resA.isCollision(), resB.isCollision());
+  BOOST_CHECK_EQUAL(resA.numContacts(), resB.numContacts());
+
+  if (resA.isCollision() && resB.isCollision()) {
+    Contact cA = resA.getContact(0),
+            cB = resB.getContact(0);
+
+    BOOST_TEST_MESSAGE(
+        tf1 << '\n'
+        << cA.pos.format(pyfmt) << '\n'
+        << '\n'
+        << tf2 << '\n'
+        << cB.pos.format(pyfmt) << '\n'
+        );
+    // Only warnings because there are still some bugs.
+    BOOST_WARN_SMALL((cA.pos    - cB.pos   ).squaredNorm(), tol);
+    BOOST_WARN_SMALL((cA.normal - cB.normal).squaredNorm(), tol);
+  } else {
+    BOOST_CHECK_CLOSE(resA.distance_lower_bound, resB.distance_lower_bound, tol); // distances should be same
+  }
+}
+
+template <typename Sa, typename Sb> void compareShapeDistance (
+    const Sa& sa, const Sb& sb, 
+    const Transform3f& tf1, const Transform3f& tf2,
+    FCL_REAL tol = 1e-9)
+{
+  DistanceRequest request (true);
+  DistanceResult resA, resB;
+
+  distance(&sa, tf1, &sa, tf2, request, resA);
+  distance(&sb, tf1, &sb, tf2, request, resB);
+
+  BOOST_MESSAGE(
+      tf1 << '\n'
+      << resA.normal.format(pyfmt) << '\n'
+      << resA.nearest_points[0].format(pyfmt) << '\n'
+      << resA.nearest_points[1].format(pyfmt) << '\n'
+      << '\n'
+      << tf2 << '\n'
+      << resB.normal.format(pyfmt) << '\n'
+      << resB.nearest_points[0].format(pyfmt) << '\n'
+      << resB.nearest_points[1].format(pyfmt) << '\n'
+      );
+  //BOOST_WARN_CLOSE(resA.min_distance, resB.min_distance, tol);
+  BOOST_CHECK_CLOSE(resA.min_distance, resB.min_distance, tol);
+
+  // Only warnings because there are still some bugs.
+  BOOST_WARN_SMALL((resA.normal - resA.normal).squaredNorm(), tol);
+  BOOST_WARN_SMALL((resA.nearest_points[0] - resB.nearest_points[0]).squaredNorm(), tol);
+  BOOST_WARN_SMALL((resA.nearest_points[1] - resB.nearest_points[1]).squaredNorm(), tol);
+}
+
 BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
 {
   Cylinder s1 (0.029, 0.1);
@@ -443,6 +523,82 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
   }
 }
 
+BOOST_AUTO_TEST_CASE(compare_convex_box)
+{
+  FCL_REAL l = 1, w = 1, d = 1;
+  Box box(l*2, w*2, d*2);
+
+  Vec3f pts[8];
+  pts[0] = Vec3f( l, w, d);
+  pts[1] = Vec3f( l, w,-d);
+  pts[2] = Vec3f( l,-w, d);
+  pts[3] = Vec3f( l,-w,-d);
+  pts[4] = Vec3f(-l, w, d);
+  pts[5] = Vec3f(-l, w,-d);
+  pts[6] = Vec3f(-l,-w, d);
+  pts[7] = Vec3f(-l,-w,-d);
+  std::vector<int> polygons;
+  polygons.push_back(4);
+  polygons.push_back(0);
+  polygons.push_back(2);
+  polygons.push_back(3);
+  polygons.push_back(1);
+
+  polygons.push_back(4);
+  polygons.push_back(2);
+  polygons.push_back(6);
+  polygons.push_back(7);
+  polygons.push_back(3);
+
+  polygons.push_back(4);
+  polygons.push_back(4);
+  polygons.push_back(5);
+  polygons.push_back(7);
+  polygons.push_back(6);
+
+  polygons.push_back(4);
+  polygons.push_back(0);
+  polygons.push_back(1);
+  polygons.push_back(5);
+  polygons.push_back(4);
+
+  polygons.push_back(4);
+  polygons.push_back(1);
+  polygons.push_back(3);
+  polygons.push_back(7);
+  polygons.push_back(5);
+
+  polygons.push_back(4);
+  polygons.push_back(0);
+  polygons.push_back(2);
+  polygons.push_back(6);
+  polygons.push_back(4);
+
+  Convex convex_box (
+      pts, // points
+      8, // num points
+      polygons.data(),
+      6 // number of polygons
+      );
+
+  Transform3f tf1;
+  Transform3f tf2;
+
+  tf2.setTranslation (Vec3f (3, 0, 0));
+  compareShapeIntersection(box, convex_box, tf1, tf2);
+  compareShapeDistance    (box, convex_box, tf1, tf2);
+
+  tf2.setTranslation (Vec3f (0, 0, 0));
+  compareShapeIntersection(box, convex_box, tf1, tf2);
+  compareShapeDistance    (box, convex_box, tf1, tf2);
+
+  for (int i = 0; i < 1000; ++i) {
+    generateRandomTransform(extents, tf2);
+    compareShapeIntersection(box, convex_box, tf1, tf2);
+    compareShapeDistance    (box, convex_box, tf1, tf2);
+  }
+}
+
 BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
 {
   Sphere s1(20);