diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index cb16958ae..91fab427a 100644
--- 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
index 979c6f1f6..ed585ae4a 100644
--- 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