From 3e4d1787800e656073f955bfe05066b59f593aa5 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 2 Sep 2014 17:00:02 +0200
Subject: [PATCH] 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 | 88 ++++++++++++++++++++++++++++----------------------
1 file changed, 50 insertions(+), 38 deletions(-)
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 6bb9b02d..46973369 100644
--- a/src/BV/OBB.cpp
+++ b/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;
--
GitLab