Merge pull request #231 from jcarpent/devel

Additional fix for collision checking

src/collision_func_matrix.cpp

@@ -97,7 +97,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
     }
     num_contacts = result.numContacts();
   }
-  if (distance <= request.security_margin) {
+  else if (distance <= request.security_margin) {
     if (result.numContacts () < request.num_max_contacts) {
       const Vec3f& p1 = distanceResult.nearest_points [0];
       const Vec3f& p2 = distanceResult.nearest_points [1];

src/distance/sphere_sphere.cpp

@@ -121,7 +121,7 @@ namespace fcl {
         unit = c1c2/dist;
     // Unlike in distance computation, we consider the security margin.
     FCL_REAL penetrationDepth = r1 + r2 + margin - dist;
-    result.updateDistanceLowerBound (-penetrationDepth);
+    result.updateDistanceLowerBound (-penetrationDepth+margin);
     bool collision = (penetrationDepth >= 0);
     if (collision) {
       // Take contact point at the middle of intersection between each sphere