### 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.```
parent 4c77f921
 ... ... @@ -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  + a  + a  + b  + b  + b ); 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 * Bf(2, 1) + a * Bf(1, 1) + b * Bf(0, 2) + b * 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 * Bf(2, 2) + a * Bf(1, 2) + b * Bf(0, 1) + b * 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 * Bf(2, 0) + a * Bf(0, 0) + b * Bf(1, 2) + b * 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 * Bf(2, 1) + a * Bf(0, 1) + b * Bf(1, 2) + b * 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 * Bf(2, 2) + a * Bf(0, 2) + b * Bf(1, 1) + b * 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 * Bf(1, 0) + a * Bf(0, 0) + b * Bf(2, 2) + b * 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 * Bf(1, 1) + a * Bf(0, 1) + b * Bf(2, 2) + b * 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 * Bf(1, 2) + a * Bf(0, 2) + b * Bf(2, 1) + b * 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; ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!