Merge pull request #231 from jcarpent/devel

Additional fix for collision checking
coal-library · Jun 10, 2021 · 6ce7e41 · 6ce7e41
2 parents c21cebf + 3a574ad
commit 6ce7e41
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 deletions.
diff --git a/src/collision_func_matrix.cpp b/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];

diff --git a/src/distance/sphere_sphere.cpp b/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