Implement AABB::overlap with distance lower bound.

include/hpp/fcl/BV/AABB.h

@@ -97,12 +97,8 @@ public:
   }    
 
   /// Not implemented
-  inline bool overlap(const AABB& other, const CollisionRequest&,
-                      FCL_REAL& sqrDistLowerBound) const
-  {
-    sqrDistLowerBound = sqrt (-1);
-    return overlap (other);
-  }
+  bool overlap(const AABB& other, const CollisionRequest& request,
+               FCL_REAL& sqrDistLowerBound) const;
 
   /// @brief Check whether the AABB contains another AABB
   inline bool contain(const AABB& other) const

src/BV/AABB.cpp

@@ -38,7 +38,7 @@
 #include <hpp/fcl/BV/AABB.h>
 
 #include <limits>
-#include <iostream>
+#include <hpp/fcl/collision_data.h>
 
 namespace hpp
 {
@@ -50,6 +50,21 @@ AABB::AABB() : min_(Vec3f::Constant(std::numeric_limits<FCL_REAL>::max())),
 {
 }
 
+bool AABB::overlap(const AABB& other, const CollisionRequest& request,
+                    FCL_REAL& sqrDistLowerBound) const
+{
+  const FCL_REAL breakDistance (request.break_distance + request.security_margin);
+  const FCL_REAL breakDistance2 = breakDistance * breakDistance;
+
+  sqrDistLowerBound = (min_ - other.max_).array().max(0).matrix().squaredNorm();
+  if(sqrDistLowerBound > breakDistance2) return false;
+
+  sqrDistLowerBound = (other.min_ - max_).array().max(0).matrix().squaredNorm();
+  if(sqrDistLowerBound > breakDistance2) return false;
+
+  return true;
+}
+
 FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
 {
   FCL_REAL result = 0;