diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index 0b93c48a0ab6829d21abf18557526aa2f1d442b5..a67aeece127a9963ce5d97761fd1ccaad7c09573 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/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
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index 782e0bc16b778e3a31ad3b92acdd95a1671679a6..e9829aae6756b48c1f554579b8b68dcc37312f44 100644
--- a/src/BV/AABB.cpp
+++ b/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;