Add lower bound on distance in collision detection (preliminary)

  - add a Boolean value in CollisionRequest to trigger this option,
  - if selected, fcl::collision function will compute a lower bound
    on the distance between the two objects if they do not collide.

include/fcl/collision_data.h

@@ -182,6 +182,9 @@ struct CollisionRequest
   /// @brief whether the contact information (normal, penetration depth and contact position) will return
   bool enable_contact;
 
+  /// Whether a lower bound on distance is returned when objects are disjoint
+  bool enable_distance_lower_bound;
+
   /// @brief The maximum number of cost sources will return
   size_t num_max_cost_sources;
 
@@ -202,6 +205,7 @@ struct CollisionRequest
 
   CollisionRequest(size_t num_max_contacts_ = 1,
                    bool enable_contact_ = false,
+		   bool enable_distance_lower_bound = false,
                    size_t num_max_cost_sources_ = 1,
                    bool enable_cost_ = false,
                    bool use_approximate_cost_ = true,
@@ -232,6 +236,10 @@ private:
 public:
   Vec3f cached_gjk_guess;
 
+  /// Lower bound on distance between objects if they are disjoint
+  /// \note computed only on request.
+  FCL_REAL distance_lower_bound;
+
 public:
   CollisionResult()
   {

include/fcl/math/matrix_3f.h

@@ -274,41 +274,49 @@ public:
     return res.timesTranspose(*this);
   }
 
+  // (1 0 0)^T (*this)^T v
   inline U transposeDotX(const Vec3fX<S>& v) const
   {
     return data.transposeDot(0, v.data);
   }
 
+  // (0 1 0)^T (*this)^T v
   inline U transposeDotY(const Vec3fX<S>& v) const
   {
     return data.transposeDot(1, v.data);
   }
 
+  // (0 0 1)^T (*this)^T v
   inline U transposeDotZ(const Vec3fX<S>& v) const
   {
     return data.transposeDot(2, v.data);
   }
 
+  // (\delta_{i3})^T (*this)^T v
   inline U transposeDot(size_t i, const Vec3fX<S>& v) const
   {
     return data.transposeDot(i, v.data);
   }
 
+  // (1 0 0)^T (*this) v
   inline U dotX(const Vec3fX<S>& v) const
   {
     return data.dot(0, v.data);
   }
 
+  // (0 1 0)^T (*this) v
   inline U dotY(const Vec3fX<S>& v) const
   {
     return data.dot(1, v.data);
   }
 
+  // (0 0 1)^T (*this) v
   inline U dotZ(const Vec3fX<S>& v) const
   {
     return data.dot(2, v.data);
   }
 
+  // (\delta_{i3})^T (*this) v
   inline U dot(size_t i, const Vec3fX<S>& v) const
   {
     return data.dot(i, v.data);

src/BV/OBB.cpp

@@ -298,6 +298,205 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
 
 }
 
+// B, T orientation and position of 2nd OBB in frame of 1st OBB,
+// a extent of 1st OBB,
+// b extent of 2nd OBB.
+bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
+				       const Vec3f& a, const Vec3f& b,
+				       FCL_REAL& squaredLowerBoundDistance)
+{
+  FCL_REAL t, s;
+  const FCL_REAL reps = 1e-6;
+  FCL_REAL diff;
+
+  Matrix3f Bf = abs(B);
+  Bf += reps;
+  squaredLowerBoundDistance = 0;
+
+  // if any of these tests are one-sided, then the polyhedra are disjoint
+
+  // A1 x A2 = A0
+  t = ((T[0] < 0.0) ? -T[0] : T[0]);
+
+  diff = t - (a[0] + Bf.dotX(b));
+  if (diff > 0) {
+    squaredLowerBoundDistance += diff*diff;
+  }
+
+  // A2 x A0 = A1
+  t = ((T[1] < 0.0) ? -T[1] : T[1]);
+
+  diff = t - (a[1] + Bf.dotY(b));
+  if (diff > 0) {
+    squaredLowerBoundDistance += diff*diff;
+  }
+
+  // A0 x A1 = A2
+  t =((T[2] < 0.0) ? -T[2] : T[2]);
+
+  diff = t - (a[2] + Bf.dotZ(b));
+  if (diff > 0) {
+    squaredLowerBoundDistance += diff*diff;
+  }
+
+  if (squaredLowerBoundDistance > 0)
+    return true;
+
+  // B1 x B2 = B0
+  s =  B.transposeDotX(T);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (b[0] + Bf.transposeDotX(a));
+  if (diff > 0) {
+    squaredLowerBoundDistance += diff*diff;
+  }
+
+  // B2 x B0 = B1
+  s = B.transposeDotY(T);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (b[1] + Bf.transposeDotY(a));
+  if (diff > 0) {
+    squaredLowerBoundDistance += diff*diff;
+  }
+
+  // B0 x B1 = B2
+  s = B.transposeDotZ(T);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (b[2] + Bf.transposeDotZ(a));
+  if (diff > 0) {
+    squaredLowerBoundDistance += diff*diff;
+  }
+  
+  if (squaredLowerBoundDistance > 0)
+    return true;
+
+  // A0 x B0
+  s = T[2] * B(1, 0) - T[1] * B(2, 0);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
+	      b[1] * Bf(0, 2) + b[2] * Bf(0, 1));
+  // We need to divide by the norm || A0 x B0 ||
+  // As ||A0|| = ||B0|| = 1,
+  //              2            2
+  // || A0 x B0 ||  + (A0 | B0)  = 1
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (0,0) * Bf (0,0);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A0 x B1
+  s = T[2] * B(1, 1) - T[1] * B(2, 1);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
+	      b[0] * Bf(0, 2) + b[2] * Bf(0, 0));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (0,1) * Bf (0,1);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A0 x B2
+  s = T[2] * B(1, 2) - T[1] * B(2, 2);
+  t = ((s < 0.0) ? -s : s);
+  
+  diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
+	      b[0] * Bf(0, 1) + b[1] * Bf(0, 0));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (0,2) * Bf (0,2);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A1 x B0
+  s = T[0] * B(2, 0) - T[2] * B(0, 0);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
+	      b[1] * Bf(1, 2) + b[2] * Bf(1, 1));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (1,0) * Bf (1,0);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A1 x B1
+  s = T[0] * B(2, 1) - T[2] * B(0, 1);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
+	      b[0] * Bf(1, 2) + b[2] * Bf(1, 0));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (1,1) * Bf (1,1);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A1 x B2
+  s = T[0] * B(2, 2) - T[2] * B(0, 2);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
+	      b[0] * Bf(1, 1) + b[1] * Bf(1, 0));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (1,2) * Bf (1,2);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A2 x B0
+  s = T[1] * B(0, 0) - T[0] * B(1, 0);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
+	      b[1] * Bf(2, 2) + b[2] * Bf(2, 1));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (2,0) * Bf (2,0);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A2 x B1
+  s = T[1] * B(0, 1) - T[0] * B(1, 1);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
+	      b[0] * Bf(2, 2) + b[2] * Bf(2, 0));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (2,1) * Bf (2,1);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  // A2 x B2
+  s = T[1] * B(0, 2) - T[0] * B(1, 2);
+  t = ((s < 0.0) ? -s : s);
+
+  diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
+	      b[0] * Bf(2, 1) + b[1] * Bf(2, 0));
+  if (diff > 0) {
+    FCL_REAL sinus2 = 1 - Bf (2,2) * Bf (2,2);
+    assert (sinus2 > 0);
+    squaredLowerBoundDistance = diff * diff / sinus2;      
+    return true;
+  }
+
+  return false;
+
+}
+
 
 
 bool OBB::overlap(const OBB& other) const

src/BV/OBB.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, CNRS
+ *  Author: Florent Lamiraux
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of CNRS nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef FCL_SRC_OBB_H
+# define FCL_SRC_OBB_H
+
+namespace fcl
+{
+
+  bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
+					 const Vec3f& a, const Vec3f& b,
+					 FCL_REAL& squaredLowerBoundDistance);
+
+  bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
+		   const Vec3f& b);
+} // namespace fcl
+
+#endif // FCL_SRC_OBB_H

src/BV/RSS.cpp

@@ -852,10 +852,14 @@ bool RSS::overlap(const RSS& other) const
 
 bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
 {
+  // ROb2 = R0 . b2
+  // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
   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]));
 
+  // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
+  // R = b2^T RO^T b1
   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]));

src/collision.cpp

@@ -74,7 +74,7 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
     nsolver = new NarrowPhaseSolver();  
 
   const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
-
+  result.distance_lower_bound = -1;
   std::size_t res; 
   if(request.num_max_contacts == 0)
   {

src/collision_func_matrix.cpp

@@ -41,7 +41,7 @@
 #include "fcl/traversal/traversal_node_setup.h"
 #include "fcl/collision_node.h"
 #include "fcl/narrowphase/narrowphase.h"
-
+#include "distance_func_matrix.h"
 
 namespace fcl
 {
@@ -200,6 +200,25 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
 {
   if(request.isSatisfied(result)) return result.numContacts();
 
+  if (request.enable_distance_lower_bound) {
+    DistanceResult distanceResult;
+    DistanceRequest distanceRequest (request.enable_contact);
+    FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, NarrowPhaseSolver>
+      (o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
+
+    if (distance <= 0) {
+      Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
+      const Vec3f& p1 = distanceResult.nearest_points [0];
+      const Vec3f& p2 = distanceResult.nearest_points [1];
+      contact.pos = .5*(p1+p2);
+      contact.normal = (p2-p1)/(p2-p1).length ();
+      result.addContact (contact);
+      return 1;
+    }
+    result.distance_lower_bound = distance;
+    return 0;
+  }
+
   ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
   const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
   const T_SH2* obj2 = static_cast<const T_SH2*>(o2);

src/distance_capsule_capsule.cpp

@@ -346,16 +346,15 @@ namespace fcl {
       (o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
 
     if (distance <= 0) {
-      if (request.enable_contact) {
-	Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
-	const Vec3f& p1 = distanceResult.nearest_points [0];
-	const Vec3f& p2 = distanceResult.nearest_points [1];
-	contact.pos = .5*(p1+p2);
-	contact.normal = (p2-p1)/(p2-p1).length ();
-	result.addContact (contact);
-      }
+      Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
+      const Vec3f& p1 = distanceResult.nearest_points [0];
+      const Vec3f& p2 = distanceResult.nearest_points [1];
+      contact.pos = .5*(p1+p2);
+      contact.normal = (p2-p1)/(p2-p1).length ();
+      result.addContact (contact);
       return 1;
     }
+    result.distance_lower_bound = distance;
     return 0;
   }
 

test/CMakeLists.txt

@@ -27,6 +27,8 @@ add_fcl_test(test_fcl_box_box_distance test_fcl_box_box_distance.cpp)
 add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
 add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp)
 add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp)
+add_fcl_test(test_fcl_obb test_fcl_obb.cpp)
+
 #add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
 add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
 

test/test_fcl_obb.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, CNRS
+ *  Author: Florent Lamiraux
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of CNRS nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define BOOST_TEST_MODULE "FCL_DISTANCE_OBB"
+#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
+
+#include <boost/test/included/unit_test.hpp>
+#include <fcl/narrowphase/narrowphase.h>
+
+#include "../src/BV/OBB.h"
+#include "../src/distance_func_matrix.h"
+
+using fcl::FCL_REAL;
+
+struct Sample
+{
+  fcl::Matrix3f R;
+  fcl::Vec3f T;
+  fcl::Vec3f extent1;
+  fcl::Vec3f extent2;
+};
+
+struct Result
+{
+  bool overlap;
+  FCL_REAL distance;
+};
+
+BOOST_AUTO_TEST_CASE(obb_overlap)
+{
+  static const size_t nbSamples = 10000;
+  Sample sample [nbSamples];
+  FCL_REAL range = 2;
+  FCL_REAL sumDistance = 0, sumDistanceLowerBound = 0;
+  for (std::size_t i=0; i<nbSamples; ++i) {
+    // sample unit quaternion
+    FCL_REAL u1 = (FCL_REAL) rand() / RAND_MAX;
+    FCL_REAL u2 = (FCL_REAL) rand() / RAND_MAX;
+    FCL_REAL u3 = (FCL_REAL) rand() / RAND_MAX;
+
+    FCL_REAL q1 = sqrt (1-u1)*sin(2*M_PI*u2);
+    FCL_REAL q2 = sqrt (1-u1)*cos(2*M_PI*u2);
+    FCL_REAL q3 = sqrt (u1) * sin(2*M_PI*u3);
+    FCL_REAL q4 = sqrt (u1) * cos(2*M_PI*u3);
+    fcl::Quaternion3f (q1, q2, q3, q4).toRotation (sample [i].R);
+
+    // sample translation
+    sample [i].T = fcl::Vec3f ((FCL_REAL) range * rand() / RAND_MAX,
+			       (FCL_REAL) range * rand() / RAND_MAX,
+			       (FCL_REAL) range * rand() / RAND_MAX);
+
+    // sample extents
+    sample [i].extent1 = fcl::Vec3f ((FCL_REAL) rand() / RAND_MAX,
+				     (FCL_REAL) rand() / RAND_MAX,
+				     (FCL_REAL) rand() / RAND_MAX);
+
+    sample [i].extent2 = fcl::Vec3f ((FCL_REAL) rand() / RAND_MAX,
+				     (FCL_REAL) rand() / RAND_MAX,
+				     (FCL_REAL) rand() / RAND_MAX);
+  }
+
+  // Compute result for each function
+  Result resultDistance [nbSamples];
+  Result resultDisjoint [nbSamples];
+  Result resultDisjointAndLowerBound [nbSamples];
+
+  fcl::Transform3f tf1;
+  fcl::Transform3f tf2;
+  fcl::Box box1 (0, 0, 0);
+  fcl::Box box2 (0, 0, 0);
+  fcl::DistanceRequest request (false, 0, 0, fcl::GST_INDEP);
+  fcl::DistanceResult result;
+  FCL_REAL distance;
+  FCL_REAL squaredDistance;
+  timeval t0, t1;
+  fcl::GJKSolver_indep gjkSolver;
+
+  // ShapeShapeDistance
+  gettimeofday (&t0, NULL);
+  for (std::size_t i=0; i<nbSamples; ++i) {
+    box1.side = 2*sample [i].extent1;
+    box2.side = 2*sample [i].extent2;
+    tf2.setTransform (sample [i].R, sample [i].T);
+    resultDistance [i].distance =
+      fcl::ShapeShapeDistance<fcl::Box, fcl::Box, fcl::GJKSolver_indep>
+      (&box1, tf1, &box2, tf2, &gjkSolver, request, result);
+    resultDistance [i].overlap = (resultDistance [i].distance < 0);
+    if (resultDistance [i].distance < 0) {
+      resultDistance [i].distance = 0;
+    }
+    result.clear ();
+  }    
+  gettimeofday (&t1, NULL);
+  double t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
+  std::cout << "Time for " << nbSamples
+	    << " calls to ShapeShapeDistance (in seconds): "
+	    << t << std::endl;
+
+  // obbDisjointAndLowerBoundDistance
+  gettimeofday (&t0, NULL);
+  for (std::size_t i=0; i<nbSamples; ++i) {
+    resultDisjointAndLowerBound [i].overlap =
+      !obbDisjointAndLowerBoundDistance (sample [i].R, sample [i].T,
+					 sample [i].extent1,
+					 sample [i].extent2,
+					 squaredDistance);
+    resultDisjointAndLowerBound [i].distance = sqrt (squaredDistance);
+  }    
+  gettimeofday (&t1, NULL);
+  t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
+  std::cout << "Time for " << nbSamples
+	    << " calls to obbDisjointAndLowerBoundDistance"
+	    << " (in seconds): "
+	    << t << std::endl;
+
+  gettimeofday (&t0, NULL);
+  for (std::size_t i=0; i<nbSamples; ++i) {
+    resultDisjoint [i].overlap =
+      !obbDisjoint (sample [i].R, sample [i].T,
+		    sample [i].extent1,
+		    sample [i].extent2);
+    resultDisjoint [i].distance = 0;
+  }    
+  gettimeofday (&t1, NULL);
+  t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
+  std::cout << "Time for " << nbSamples << " calls to obbDisjoint"
+	    << " (in seconds): "
+	    << t << std::endl;
+
+  // Test correctness of results
+  bool res1, res2, res3;
+  for (std::size_t i=0; i<nbSamples; ++i) {
+    res1 = resultDisjoint [i].overlap;
+    res2 = resultDisjointAndLowerBound [i].overlap;
+    res3 = resultDistance [i].overlap;
+    sumDistanceLowerBound += resultDisjointAndLowerBound [i].distance;
+    sumDistance += resultDistance [i].distance;
+
+#if 0
+    std::cout << "ShapeShapeDistance:    overlap: "
+	      << resultDistance [i].overlap
+	      << ", distance: " << resultDistance [i].distance << std::endl;
+    std::cout << "disjoint:              overlap: "
+	      << resultDisjoint [i].overlap
+	      << ", distance: " << resultDisjoint [i].distance << std::endl;
+    std::cout << "disjointAndLowerBound: overlap: "
+	      << resultDisjointAndLowerBound [i].overlap
+	      << ", distance: " << resultDisjointAndLowerBound [i].distance
+	      << std::endl << std::endl;
+#endif
+    BOOST_CHECK (res1 == res2);
+    BOOST_CHECK (res1 == res3);
+    BOOST_CHECK ((!res1 && resultDisjointAndLowerBound [i].distance > 0) ||
+		 (res1 && resultDisjointAndLowerBound [i].distance == 0));
+    BOOST_CHECK (resultDisjointAndLowerBound [i].distance <=
+		 resultDistance [i].distance);
+  }
+  std::cout << "Sum distances ShapeShapeDistance:               "
+	    << sumDistance << std::endl;
+  std::cout << "Sum distances obbDisjointAndLowerBoundDistance: "
+	    << sumDistanceLowerBound << std::endl;
+}