Adding parameter to track distance lower bound between BVH (preliminary).

include/fcl/BV/AABB.h

@@ -38,7 +38,7 @@
 #ifndef FCL_AABB_H
 #define FCL_AABB_H
 
-
+#include <stdexcept>
 #include "fcl/math/vec_3f.h"
 
 namespace fcl
@@ -93,6 +93,12 @@ public:
     return true;
   }    
 
+  /// Not implemented
+  inline bool overlap(const AABB&, FCL_REAL&) const
+  {
+    throw std::runtime_error ("Not implemented");
+  }
+
   /// @brief Check whether the AABB contains another AABB
   inline bool contain(const AABB& other) const
   {

include/fcl/BV/BV_node.h

@@ -89,6 +89,11 @@ struct BVNode : public BVNodeBase
   {
     return bv.overlap(other.bv);
   }
+  /// @brief Check whether two BVNode collide
+  bool overlap(const BVNode& other, FCL_REAL& sqrDistLowerBound) const
+  {
+    return bv.overlap(other.bv, sqrDistLowerBound);
+  }
 
   /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points.
   FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const

include/fcl/BV/OBB.h

@@ -59,9 +59,16 @@ public:
   /// @brief Half dimensions of OBB
   Vec3f extent;
 
-  /// @brief Check collision between two OBB, return true if collision happens. 
+  /// Check collision between two OBB
+  /// \return true if collision happens. 
   bool overlap(const OBB& other) const;
 
+  /// Check collision between two OBB
+  /// \return true if collision happens. 
+  /// \retval sqrDistLowerBound squared lower bound on distance between boxes if
+  ///         they do not overlap.
+  bool overlap(const OBB& other, FCL_REAL& sqrDistLowerBound) const;
+
   
   /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. 
   bool overlap(const OBB& other, OBB& overlap_part) const
@@ -133,9 +140,16 @@ OBB translate(const OBB& bv, const Vec3f& t);
 /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
 bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
 
+/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
+	     const OBB& b2, FCL_REAL& sqrDistLowerBound);
+
 
-/// @brief Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a;
-/// the second box is in identity configuration and its half dimension is set by b.
+/// Check collision between two boxes
+/// \param B, T orientation and position of first box,
+/// \param a half dimensions of first box,
+/// \param b half dimensions of second box.
+/// The second box is in identity configuration.
 bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
 }
 

include/fcl/BV/OBBRSS.h

@@ -63,6 +63,14 @@ public:
     return obb.overlap(other.obb);
   }
 
+  /// Check collision between two OBBRSS
+  /// \retval sqrDistLowerBound squared lower bound on distance between
+  ///         objects if they do not overlap.
+  bool overlap(const OBBRSS& other, FCL_REAL& sqrDistLowerBound) const
+  {
+    return obb.overlap(other.obb, sqrDistLowerBound);
+  }
+
   /// @brief Check collision between two OBBRSS and return the overlap part.
   bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
   {
@@ -148,6 +156,13 @@ OBBRSS translate(const OBBRSS& bv, const Vec3f& t);
 /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
 bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
 
+/// Check collision between two OBBRSS
+/// \param  b1 first OBBRSS in configuration (R0, T0)
+/// \param  b2 second OBBRSS in identity position
+/// \retval squared lower bound on the distance if OBBRSS do not overlap.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2,
+	     FCL_REAL& sqrDistLowerBound);
+
 /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
 

include/fcl/BV/RSS.h

@@ -38,7 +38,7 @@
 #ifndef FCL_RSS_H
 #define FCL_RSS_H
 
-
+#include <stdexcept>
 #include "fcl/math/vec_3f.h"
 #include "fcl/math/matrix_3f.h"
 #include <boost/math/constants/constants.hpp>
@@ -66,6 +66,13 @@ public:
   /// @brief Check collision between two RSS
   bool overlap(const RSS& other) const;
 
+  /// Not implemented
+  bool overlap(const RSS& other, FCL_REAL& sqrDistLowerBound) const
+  {
+    throw std::runtime_error ("Not implemented.");
+    return false;
+  }
+
   /// @brief Check collision between two RSS and return the overlap part.
   /// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
   bool overlap(const RSS& other, RSS& overlap_part) const

include/fcl/BV/kDOP.h

@@ -38,7 +38,7 @@
 #ifndef FCL_KDOP_H
 #define FCL_KDOP_H
 
-
+#include <stdexcept>
 #include "fcl/math/vec_3f.h"
 
 namespace fcl
@@ -96,6 +96,12 @@ public:
   /// @brief Check whether two KDOPs are overlapped
   bool overlap(const KDOP<N>& other) const;
 
+  /// Not implemented
+  bool overlap(const KDOP<N>& other, FCL_REAL&) const
+  {
+    throw std::runtime_error ("Not implemented");
+  }
+
   //// @brief Check whether one point is inside the KDOP
   bool inside(const Vec3f& p) const;
 

include/fcl/BV/kIOS.h

@@ -93,6 +93,9 @@ public:
   /// @brief Check collision between two kIOS
   bool overlap(const kIOS& other) const;
 
+  /// @brief Check collision between two kIOS
+  bool overlap(const kIOS& other, FCL_REAL& sqrDistLowerBound) const;
+
   /// @brief Check collision between two kIOS and return the overlap part.
   /// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
   /// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling.

include/fcl/collision_data.h

@@ -205,16 +205,18 @@ struct CollisionRequest
 
   CollisionRequest(size_t num_max_contacts_ = 1,
                    bool enable_contact_ = false,
-		   bool enable_distance_lower_bound = false,
+		   bool enable_distance_lower_bound_ = false,
                    size_t num_max_cost_sources_ = 1,
                    bool enable_cost_ = false,
                    bool use_approximate_cost_ = true,
-                   GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_),
-                                                                  enable_contact(enable_contact_),
-                                                                  num_max_cost_sources(num_max_cost_sources_),
-                                                                  enable_cost(enable_cost_),
-                                                                  use_approximate_cost(use_approximate_cost_),
-                                                                  gjk_solver_type(gjk_solver_type_)
+                   GJKSolverType gjk_solver_type_ = GST_LIBCCD) :
+  num_max_contacts(num_max_contacts_),
+    enable_contact(enable_contact_),
+    enable_distance_lower_bound (enable_distance_lower_bound_),
+    num_max_cost_sources(num_max_cost_sources_),
+    enable_cost(enable_cost_),
+    use_approximate_cost(use_approximate_cost_),
+    gjk_solver_type(gjk_solver_type_)
   {
     enable_cached_gjk_guess = false;
     cached_gjk_guess = Vec3f(1, 0, 0);

include/fcl/collision_node.h

@@ -50,8 +50,15 @@ namespace fcl
 {
 
 
-/// @brief collision on collision traversal node; can use front list to accelerate
-void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
+/// collision on collision traversal node
+/// 
+/// \param node node containing both objects to test,
+/// \retval squared lower bound to the distance between the objects if they
+///         do not collide.
+/// \param front_list list of nodes visited by the query, can be used to
+///        accelerate computation
+  void collide(CollisionTraversalNodeBase* node, FCL_REAL& sqrDistLowerBound,
+	       BVHFrontList* front_list = NULL);
 
 /// @brief self collision on collision traversal node; can use front list to accelerate
 void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);

include/fcl/traversal/traversal_node_base.h

@@ -90,13 +90,21 @@ public:
 class CollisionTraversalNodeBase : public TraversalNodeBase
 {
 public:
-  CollisionTraversalNodeBase() : result(NULL), enable_statistics(false) {}
+  CollisionTraversalNodeBase(bool enable_distance_lower_bound_ = false) :
+  result(NULL), enable_statistics(false),
+    enable_distance_lower_bound (enable_distance_lower_bound_){}
 
   virtual ~CollisionTraversalNodeBase();
 
   /// @brief BV test between b1 and b2
   virtual bool BVTesting(int b1, int b2) const;
 
+  /// BV test between b1 and b2
+  /// \param b1, b2 Bounding volumes to test,
+  /// \retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
   /// @brief Leaf test between node b1 and b2, if they are both leafs
   virtual void leafTesting(int b1, int b2) const;
 
@@ -114,6 +122,9 @@ public:
 
   /// @brief Whether stores statistics 
   bool enable_statistics;
+
+  /// Whether to compute a lower bound on distance between bounding volumes
+  bool enable_distance_lower_bound;
 };
 
 /// @brief Node structure encoding the information required for distance traversal.

include/fcl/traversal/traversal_node_bvh_shape.h

@@ -54,7 +54,8 @@ template<typename BV, typename S>
 class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
 {
 public:
-  BVHShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
+  BVHShapeCollisionTraversalNode(bool enable_distance_lower_bound) :
+  CollisionTraversalNodeBase (enable_distance_lower_bound)
   {
     model1 = NULL;
     model2 = NULL;
@@ -89,6 +90,17 @@ public:
     return !model1->getBV(b1).bv.overlap(model2_bv);
   }
 
+  /// BV test between b1 and b2
+  /// \param b1, b2 Bounding volumes to test,
+  /// \retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  /// @brief BV culling test in one BVTT node
+  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+  {
+    if(this->enable_statistics) num_bv_tests++;
+    return !model1->getBV(b1).bv.overlap(model2_bv, sqrDistLowerBound);
+  }
+
   const BVHModel<BV>* model1;
   const S* model2;
   BV model2_bv;
@@ -137,13 +149,24 @@ public:
     return model2->getBV(b).rightChild();
   }
 
-  /// @brief BV culling test in one BVTT node
+  /// BV test between b1 and b2
+  /// \param b1, b2 Bounding volumes to test,
   bool BVTesting(int b1, int b2) const
   {
     if(this->enable_statistics) num_bv_tests++;
     return !model2->getBV(b2).bv.overlap(model1_bv);
   }
 
+  /// BV test between b1 and b2
+  /// \param b1, b2 Bounding volumes to test,
+  /// \retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+  {
+    if(this->enable_statistics) num_bv_tests++;
+    return !model2->getBV(b2).bv.overlap(model1_bv, sqrDistLowerBound);
+  }
+
   const S* model1;
   const BVHModel<BV>* model2;
   BV model1_bv;
@@ -159,7 +182,8 @@ template<typename BV, typename S, typename NarrowPhaseSolver>
 class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
 {
 public:
-  MeshShapeCollisionTraversalNode() : BVHShapeCollisionTraversalNode<BV, S>()
+  MeshShapeCollisionTraversalNode(bool enable_distance_lower_bound = false) :
+  BVHShapeCollisionTraversalNode<BV, S> (enable_distance_lower_bound)
   {
     vertices = NULL;
     tri_indices = NULL;
@@ -330,7 +354,9 @@ template<typename S, typename NarrowPhaseSolver>
 class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
 {
 public:
-  MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>()
+  MeshShapeCollisionTraversalNodeOBB(bool enable_distance_lower_bound = false) :
+  MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
+    (enable_distance_lower_bound)
   {
   }
 
@@ -352,7 +378,9 @@ template<typename S, typename NarrowPhaseSolver>
 class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
 {
 public:
-  MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>()
+  MeshShapeCollisionTraversalNodeRSS (bool enable_distance_lower_bound = false):
+  MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
+    (enable_distance_lower_bound)
   {
   }
 
@@ -374,7 +402,9 @@ template<typename S, typename NarrowPhaseSolver>
 class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
 {
 public:
-  MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>()
+  MeshShapeCollisionTraversalNodekIOS(bool enable_distance_lower_bound = false):
+  MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
+    (enable_distance_lower_bound)
   {
   }
 
@@ -396,7 +426,10 @@ template<typename S, typename NarrowPhaseSolver>
 class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>
 {
 public:
-  MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
+  MeshShapeCollisionTraversalNodeOBBRSS
+    (bool enable_distance_lower_bound = false) :
+  MeshShapeCollisionTraversalNode
+    <OBBRSS, S, NarrowPhaseSolver>(enable_distance_lower_bound)
   {
   }
 
@@ -406,6 +439,14 @@ public:
     return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
   }
 
+  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+  {
+    if(this->enable_statistics) this->num_bv_tests++;
+    return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+		    this->model2_bv, this->model1->getBV(b1).bv,
+		    sqrDistLowerBound);
+  }
+
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
@@ -594,6 +635,14 @@ public:
     return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
   }
 
+  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+  {
+    if(this->enable_statistics) this->num_bv_tests++;
+    return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
+		    this->model1_bv, this->model2->getBV(b2).bv,
+		    sqrDistLowerBound);
+  }
+
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 

include/fcl/traversal/traversal_node_bvhs.h

@@ -129,6 +129,16 @@ public:
     return !model1->getBV(b1).overlap(model2->getBV(b2));
   }
   
+  /// BV test between b1 and b2
+  /// \param b1, b2 Bounding volumes to test,
+  /// \retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+  {
+    if(enable_statistics) num_bv_tests++;
+    return !model1->getBV(b1).overlap(model2->getBV(b2), sqrDistLowerBound);
+  }
+  
   /// @brief The first BVH model
   const BVHModel<BV>* model1;
   /// @brief The second BVH model
@@ -302,6 +312,8 @@ public:
  
   bool BVTesting(int b1, int b2) const;
 
+  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
   void leafTesting(int b1, int b2) const;
 
   Matrix3f R;
@@ -659,6 +671,8 @@ public:
 
   FCL_REAL BVTesting(int b1, int b2) const;
 
+  FCL_REAL BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
   void leafTesting(int b1, int b2) const;
 
   Matrix3f R;
@@ -980,6 +994,8 @@ public:
 
   FCL_REAL BVTesting(int b1, int b2) const;
 
+  FCL_REAL BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
   void leafTesting(int b1, int b2) const;
 
   bool canStop(FCL_REAL c) const;

include/fcl/traversal/traversal_node_shapes.h

@@ -70,6 +70,12 @@ public:
     return false;
   }
 
+  /// @brief BV culling test in one BVTT node
+  bool BVTesting(int, int, FCL_REAL&) const
+  {
+    throw std::runtime_error ("Not implemented");
+  }
+
   /// @brief Intersection testing between leaves (two shapes)
   void leafTesting(int, int) const
   {

include/fcl/traversal/traversal_recurse.h

@@ -47,8 +47,12 @@
 namespace fcl
 {
 
-/// @brief Recurse function for collision
-void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
+/// Recurse function for collision
+/// \param node collision node,
+/// \param b1, b2 ids of bounding volume nodes for object 1 and object 2
+/// \retval sqrDistLowerBound squared lower bound on distance between objects.
+void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
+		      BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound);
 
 /// @brief Recurse function for collision, specialized for OBB type
 void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
@@ -66,7 +70,9 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontLi
 void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize);
 
 /// @brief Recurse function for front list propagation
-void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list);
+void propagateBVHFrontListCollisionRecurse
+  (CollisionTraversalNodeBase* node, BVHFrontList* front_list,
+   FCL_REAL& sqrDistLowerBound);
 
 
 }

src/BV/OBB.cpp

@@ -513,6 +513,24 @@ bool OBB::overlap(const OBB& other) const
   return !obbDisjoint(R, T, extent, other.extent);
 }
 
+  bool OBB::overlap(const OBB& other, FCL_REAL& sqrDistLowerBound) const
+  {
+    /// compute what transform [R,T] that takes us from cs1 to cs2.
+    /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
+    /// First compute the rotation part, then translation part
+    Vec3f t = other.To - To; // T2 - T1
+    Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
+    Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
+	       axis[0].dot(other.axis[2]),
+	       axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
+	       axis[1].dot(other.axis[2]),
+	       axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
+	       axis[2].dot(other.axis[2]));
+
+  return !obbDisjointAndLowerBoundDistance
+    (R, T, extent, other.extent, sqrDistLowerBound);
+}
+
 
 bool OBB::contain(const Vec3f& p) const
 {
@@ -584,6 +602,24 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
   return !obbDisjoint(R, T, b1.extent, b2.extent);
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
+	     FCL_REAL& sqrDistLowerBound)
+{
+  Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
+                R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
+                R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
+
+  Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
+             R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
+             R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+
+  Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
+  Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
+
+  return !obbDisjointAndLowerBoundDistance (R, T, b1.extent, b2.extent,
+					    sqrDistLowerBound);
+}
+
 OBB translate(const OBB& bv, const Vec3f& t)
 {
   OBB res(bv);

src/BV/OBBRSS.cpp

@@ -45,6 +45,12 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS
   return overlap(R0, T0, b1.obb, b2.obb);
 }
 
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2,
+	     FCL_REAL& sqrDistLowerBound)
+{
+  return overlap(R0, T0, b1.obb, b2.obb, sqrDistLowerBound);
+}
+
 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P, Vec3f* Q)
 {
   return distance(R0, T0, b1.rss, b2.rss, P, Q);

src/BV/kIOS.cpp

@@ -63,6 +63,11 @@ bool kIOS::overlap(const kIOS& other) const
   return true;
 }
 
+  bool kIOS::overlap(const kIOS& other, FCL_REAL& sqrDistLowerBound) const
+  {
+    throw std::runtime_error ("Not implemented yet.");
+  }
+
 
 bool kIOS::contain(const Vec3f& p) const
 {

src/collision_func_matrix.cpp

@@ -83,7 +83,7 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& t
   return result.numContacts();
 }
 
-template<typename NarrowPhaseSolver>
+etemplate<typename NarrowPhaseSolver>
 std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
                           const NarrowPhaseSolver* nsolver,
                           const CollisionRequest& request, CollisionResult& result)
@@ -234,7 +234,9 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
   }
 
   initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
-  collide(&node);
+  FCL_REAL sqrDistance = 0;
+  collide(&node, sqrDistance);
+  result.distance_lower_bound = sqrt (sqrDistance);
 
   if(request.enable_cached_gjk_guess)
     result.cached_gjk_guess = nsolver->getCachedGuess();
@@ -263,7 +265,9 @@ struct BVHShapeCollider
       const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
       initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result);
-      fcl::collide(&node);
+      FCL_REAL sqrDistance;
+      fcl::collide(&node, sqrDistance);
+      result.distance_lower_bound = sqrt (sqrDistance);
 
       delete obj1_tmp;
 
@@ -287,7 +291,9 @@ struct BVHShapeCollider
       const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
       initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
-      fcl::collide(&node);
+      FCL_REAL sqrDistance;
+      fcl::collide(&node, sqrDistance);
+      result.distance_lower_bound = sqrt (sqrDistance);
 
       delete obj1_tmp;
     }
@@ -316,7 +322,9 @@ std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform
     const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result);
-    fcl::collide(&node);
+    FCL_REAL sqrDistance;
+    fcl::collide(&node, sqrDistance);
+    result.distance_lower_bound = sqrt (sqrDistance);
    
     Box box;
     Transform3f box_tf;
@@ -331,12 +339,15 @@ std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform
   }
   else
   {
-    OrientMeshShapeCollisionTraveralNode node;
+    OrientMeshShapeCollisionTraveralNode node
+      (request.enable_distance_lower_bound);
     const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
     const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
-    fcl::collide(&node);
+    FCL_REAL sqrDistance = 0;
+    fcl::collide(&node, sqrDistance);
+    result.distance_lower_bound = sqrt (sqrDistance);
   }
 
   return result.numContacts();
@@ -407,7 +418,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
   Transform3f tf2_tmp = tf2;