Implement kDOP::overlap with lower bound.

include/hpp/fcl/BV/kDOP.h

@@ -103,16 +103,15 @@ public:
   /// @brief Creating kDOP containing two points
   KDOP(const Vec3f& a, const Vec3f& b);
   
-  /// @brief Check whether two KDOPs are overlapped
+  /// @brief Check whether two KDOPs overlap.
   bool overlap(const KDOP<N>& other) const;
 
-  /// Not implemented
-  bool overlap(const KDOP<N>& other, const CollisionRequest&,
-               FCL_REAL& sqrDistLowerBound) const
-  {
-    sqrDistLowerBound = sqrt (-1);
-    return overlap (other);
-  }
+  /// @brief Check whether two KDOPs overlap.
+  /// @return true if collision happens. 
+  /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
+  ///         they do not overlap.
+  bool overlap(const KDOP<N>& other, const CollisionRequest& request,
+               FCL_REAL& sqrDistLowerBound) const;
 
     /// @brief The distance between two KDOP<N>. Not implemented.
   FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;

src/BV/kDOP.cpp

@@ -39,6 +39,8 @@
 #include <limits>
 #include <iostream>
 
+#include <hpp/fcl/collision_data.h>
+
 namespace hpp
 {
 namespace fcl
@@ -155,6 +157,28 @@ bool KDOP<N>::overlap(const KDOP<N>& other) const
   return true;
 }
 
+template<short N>
+bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
+    FCL_REAL& sqrDistLowerBound) const
+{
+  const FCL_REAL breakDistance (request.break_distance + request.security_margin);
+
+  FCL_REAL a = (dist_.template head<N/2>() - other.dist_.template tail<N/2>()).minCoeff();
+  if (a > breakDistance) {
+    sqrDistLowerBound = a*a;
+    return false;
+  }
+
+  FCL_REAL b = (other.dist_.template head<N/2>() - dist_.template tail<N/2>()).minCoeff();
+  if (b > breakDistance) {
+    sqrDistLowerBound = b*b;
+    return false;
+  }
+
+  sqrDistLowerBound = std::min(a, b);
+  return true;
+}
+
 template<short N>
 bool KDOP<N>::inside(const Vec3f& p) const
 {