If distance lower bound is requested, break OBBRSS if they are too close.

  - returning a small value for the distance lower bound makes time of
    computation of continuous collision checking very high. It is better to
    break the bounding volumes to get a better bound, eventhough it increases
    the time of computation.

src/BV/OBB.cpp

@@ -308,6 +308,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
   FCL_REAL t, s;
   const FCL_REAL reps = 1e-6;
   FCL_REAL diff;
+  FCL_REAL breakDistance = 2e-3 * (a [0] + a [1] + a [2] +
+				   b [0] + b [1] + b [2]);
+  FCL_REAL breakDistance2 = breakDistance * breakDistance;
 
   Matrix3f Bf = abs(B);
   Bf += reps;
@@ -339,7 +342,7 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
     squaredLowerBoundDistance += diff*diff;
   }
 
-  if (squaredLowerBoundDistance > 0)
+  if (squaredLowerBoundDistance > breakDistance2)
     return true;
 
   // B1 x B2 = B0
@@ -369,7 +372,7 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
     squaredLowerBoundDistance += diff*diff;
   }
   
-  if (squaredLowerBoundDistance > 0)
+  if (squaredLowerBoundDistance > breakDistance2)
     return true;
 
   // A0 x B0
@@ -382,11 +385,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
   // 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);
+  FCL_REAL sinus2 = 1 - Bf (0,0) * Bf (0,0);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A0 x B1
@@ -395,11 +399,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
 
   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);
+  sinus2 = 1 - Bf (0,1) * Bf (0,1);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A0 x B2
@@ -408,11 +413,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
   
   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);
+  sinus2 = 1 - Bf (0,2) * Bf (0,2);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A1 x B0
@@ -421,11 +427,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
 
   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);
+  sinus2 = 1 - Bf (1,0) * Bf (1,0);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A1 x B1
@@ -434,11 +441,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
 
   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);
+  sinus2 = 1 - Bf (1,1) * Bf (1,1);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A1 x B2
@@ -447,11 +455,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
 
   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);
+  sinus2 = 1 - Bf (1,2) * Bf (1,2);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A2 x B0
@@ -460,11 +469,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
 
   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);
+  sinus2 = 1 - Bf (2,0) * Bf (2,0);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A2 x B1
@@ -473,11 +483,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
 
   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);
+  sinus2 = 1 - Bf (2,1) * Bf (2,1);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   // A2 x B2
@@ -486,11 +497,12 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
 
   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);
+  sinus2 = 1 - Bf (2,2) * Bf (2,2);
+  if (sinus2 > 1e-6) {
     squaredLowerBoundDistance = diff * diff / sinus2;      
-    return true;
+    if (squaredLowerBoundDistance > breakDistance2) {
+      return true;
+    }
   }
 
   return false;