Commit 3e4d1787 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

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 [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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment