Add missing overlap method.

include/hpp/fcl/BV/AABB.h

@@ -262,6 +262,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
   return res;
 }
 
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2);
+
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+	     const AABB& b2, const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound);
+
 }
 
 } // namespace hpp

include/hpp/fcl/BV/RSS.h

@@ -155,6 +155,10 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
 /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
 bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
 
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+             const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound);
 
 }
 

include/hpp/fcl/BV/kDOP.h

@@ -39,7 +39,7 @@
 #define HPP_FCL_KDOP_H
 
 #include <stdexcept>
-#include <hpp/fcl/math/vec_3f.h>
+#include <hpp/fcl/math/matrix_3f.h>
 
 namespace hpp
 {
@@ -176,6 +176,20 @@ public:
 
 };
 
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+             const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/)
+{
+  throw std::logic_error ("not implemented");
+}
+
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+             const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
+             const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
+{
+  throw std::logic_error ("not implemented");
+}
 
 /// @brief translate the KDOP BV
 template<size_t N>

include/hpp/fcl/BV/kIOS.h

@@ -157,6 +157,12 @@ kIOS translate(const kIOS& bv, const Vec3f& t);
 /// @todo Not efficient
 bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
 
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
+/// @todo Not efficient
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+             const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound);
+
 /// @brief Approximate distance between two kIOS bounding volumes
 /// @todo P and Q is not returned, need implementation
 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);

src/BV/AABB.cpp

@@ -128,6 +128,20 @@ FCL_REAL AABB::distance(const AABB& other) const
   return std::sqrt(result);
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2)
+{
+  AABB bb1 (translate (rotate (b1, R0), T0));
+  return bb1.overlap (b2);
+}
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+	     const AABB& b2, const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound)
+{
+  AABB bb1 (translate (rotate (b1, R0), T0));
+  return bb1.overlap (b2, request, sqrDistLowerBound);
+}
+
 }
 
 } // namespace hpp

src/BV/RSS.cpp

@@ -862,6 +862,25 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
   return (dist <= (b1.r + b2.r));
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+             const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound)
+{
+  // ROb2 = R0 . b2
+  // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
+
+  // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
+  // R = b2^T RO^T b1
+  Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr);
+  Vec3f T(b1.axes.transpose() * Ttemp);
+  Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
+
+  FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
+  if (dist <= 0) return true;
+  sqrDistLowerBound = dist * dist;
+  return false;
+}
+
 bool RSS::contain(const Vec3f& p) const
 {
   Vec3f local_p = p - Tr;

src/BV/kIOS.cpp

@@ -61,17 +61,27 @@ bool kIOS::overlap(const kIOS& other) const
   }
 
   return obb.overlap(other.obb);
-
-  return true;
 }
 
-  bool kIOS::overlap(const kIOS& other, const CollisionRequest&,
-                     FCL_REAL& sqrDistLowerBound) const
+bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
+                   FCL_REAL& sqrDistLowerBound) const
+{
+  for(unsigned int i = 0; i < num_spheres; ++i)
   {
-    sqrDistLowerBound = sqrt (-1);
-    return overlap (other);
+    for(unsigned int j = 0; j < other.num_spheres; ++j)
+    {
+      FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
+      FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
+      if(o_dist > sum_r * sum_r) {
+        o_dist = sqrt(o_dist) - sum_r;
+        sqrDistLowerBound = o_dist*o_dist;
+        return false;
+      }
+    }
   }
 
+  return obb.overlap(other.obb, request, sqrDistLowerBound);
+}
 
 bool kIOS::contain(const Vec3f& p) const
 {
@@ -192,6 +202,23 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
   return b1.overlap(b2_temp);
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+             const CollisionRequest& request,
+             FCL_REAL& sqrDistLowerBound)
+{
+  kIOS b2_temp = b2;
+  for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
+  {
+    b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
+  }
+
+  
+  b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
+  b2_temp.obb.axes.applyOnTheLeft(R0);
+
+  return b1.overlap(b2_temp, request, sqrDistLowerBound);
+}
+
 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
 {
   kIOS b2_temp = b2;