Merge pull request #228 from jcarpent/devel

Fix update of distanceLowerBound

cmake @ 09483408

-Subproject commit 3a52692a40839b10f38352c1b06ccfebc0b53f36
+Subproject commit 094834088ad9de32f1abdc2d56d29bca2190772c

include/hpp/fcl/collision_data.h

@@ -285,7 +285,7 @@ public:
   {
   }
 
-  /// @brief Update the lower bound only if the distance in inferior.
+  /// @brief Update the lower bound only if the distance is inferior.
   inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
   {
     if (distance_lower_bound_ < distance_lower_bound)

src/collision_func_matrix.cpp

@@ -82,6 +82,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
   FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2>
     (o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
 
+  size_t num_contacts = 0;
   if (distance <= 0) {
     if (result.numContacts () < request.num_max_contacts) {
       const Vec3f& p1 = distanceResult.nearest_points [0];
@@ -94,7 +95,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
 
       result.addContact (contact);
     }
-    return 1;
+    num_contacts = result.numContacts();
   }
   if (distance <= request.security_margin) {
     if (result.numContacts () < request.num_max_contacts) {
@@ -107,10 +108,10 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
           -distance+request.security_margin);
       result.addContact (contact);
     }
-    return 1;
+    num_contacts = result.numContacts();
   }
   result.updateDistanceLowerBound (distance);
-  return 0;
+  return num_contacts;
 }
 
 namespace details

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;