Add CollisionRequest::updateDistanceLowerBound

include/hpp/fcl/collision_data.h

@@ -223,9 +223,16 @@ public:
 
 public:
   CollisionResult()
+    : distance_lower_bound (std::numeric_limits<FCL_REAL>::max())
   {
   }
 
+  /// @brief Update the lower bound only if the distance in inferior.
+  inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
+  {
+    if (distance_lower_bound_ < distance_lower_bound)
+      distance_lower_bound = distance_lower_bound_;
+  }
 
   /// @brief add one contact into result structure
   inline void addContact(const Contact& c) 

src/collision.cpp

@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
     solver.cached_guess = request.cached_gjk_guess;
 
   const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
-  result.distance_lower_bound = -1;
   std::size_t res;
   if(request.num_max_contacts == 0)
   {

src/collision_func_matrix.cpp

@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
     }
     return 1;
   }
-  result.distance_lower_bound = distance;
+  result.updateDistanceLowerBound (distance);
   return 0;
 }
 

src/collision_node.cpp

@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node,
       collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
     else
       collisionNonRecurse(node, front_list, sqrDistLowerBound);
-    result.distance_lower_bound = sqrt (sqrDistLowerBound);
+    result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
   }
 }
 

src/distance_sphere_sphere.cpp

@@ -132,7 +132,7 @@ namespace fcl {
       result.addContact (contact);
       return 1;
     }
-    result.distance_lower_bound = -penetrationDepth;
+    result.updateDistanceLowerBound (-penetrationDepth);
     return 0;
   }
 } // namespace fcl

src/traversal/traversal_recurse.cpp

@@ -399,7 +399,7 @@ void propagateBVHFrontListCollisionRecurse
         }
       }
     }
-    result.distance_lower_bound = sqrt (sqrDistLowerBound);
+    result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
   }