test: add test of for distanceLowerBounds with basic primitives

test/distance_lower_bound.cpp

@@ -151,7 +151,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
     col1 = testDistanceLowerBound (transforms[i], m1, m2, distanceLowerBound);
     col2 = testDistance (transforms[i], m1, m2, distance);
     col3 = testCollide (transforms[i], m1, m2);
-    std::cout << "col1 = " << col1 << ", col2 = " << col2 
+    std::cout << "col1 = " << col1 << ", col2 = " << col2
 	      << ", col3 = " << col3 << std::endl;
     std::cout << "distance lower bound = " << distanceLowerBound << std::endl;
     std::cout << "distance             = " << distance << std::endl;
@@ -164,6 +164,42 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
   }
 }
 
+BOOST_AUTO_TEST_CASE(box_sphere)
+{
+  shared_ptr < hpp::fcl::Sphere > sphere(new hpp::fcl::Sphere(0.5));
+  shared_ptr < hpp::fcl::Box > box(new hpp::fcl::Box(1.,1.,1.));
+  
+  Transform3f M1; M1.setIdentity();
+  Transform3f M2; M2.setIdentity();
+  
+  std::vector<Transform3f> transforms;
+  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
+  const std::size_t n = 1000;
+
+  generateRandomTransforms(extents, transforms, n);
+  
+  FCL_REAL distanceLowerBound, distance;
+  bool col1, col2;
+  col1 = testDistanceLowerBound(M1,sphere,box,distanceLowerBound);
+  col2 = testDistance(M1,sphere,box,distance);
+  BOOST_CHECK(col1 == col2);
+  BOOST_CHECK(distanceLowerBound <= distance);
+  
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    FCL_REAL distanceLowerBound, distance;
+    bool col1, col2;
+    col1 = testDistanceLowerBound (transforms[i], sphere, box, distanceLowerBound);
+    col2 = testDistance (transforms[i], sphere, box, distance);
+    BOOST_CHECK (col1 == col2);
+    if (!col1) {
+      BOOST_CHECK_MESSAGE (distanceLowerBound <= distance,
+         "distance = " << distance << ", lower bound = "
+         << distanceLowerBound);
+    }
+  }
+}
+
 BOOST_AUTO_TEST_CASE(box_mesh)
 {
   std::vector<Vec3f> p1;