From d42d61d5eee82fa8fb85b93f6cbb70b46eddefda Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 22 Aug 2019 18:54:57 +0200
Subject: [PATCH] [OBB] Reimplement obbDisjointAndLowerBoundDistance according
to bench
test_fcl_obb.
---
src/BV/OBB.cpp | 108 +++++++++++++++++++++++++++++++++++--------------
1 file changed, 77 insertions(+), 31 deletions(-)
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index aed6c595..1f13f021 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -294,6 +294,71 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
}
+namespace internal
+{
+ inline FCL_REAL obbDisjoint_check_A_axis (
+ const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf)
+ {
+ // |T| - |B| * b - a
+ Vec3f AABB_corner (T.cwiseAbs () - a);
+ AABB_corner.noalias() -= Bf.col(0) * b[0];
+ AABB_corner.noalias() -= Bf.col(1) * b[1];
+ AABB_corner.noalias() -= Bf.col(2) * b[2];
+ return AABB_corner.array().max(0).matrix().squaredNorm ();
+ }
+
+ inline FCL_REAL obbDisjoint_check_B_axis (
+ const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf)
+ {
+ // Bf = |B|
+ // | B^T T| - Bf^T * a - b
+ register FCL_REAL s, t = 0;
+ s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
+ if (s > 0) t += s*s;
+ s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
+ if (s > 0) t += s*s;
+ s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2];
+ if (s > 0) t += s*s;
+ return t;
+ }
+
+ template <int ib, int jb = (ib+1)%3, int kb = (ib+2)%3 >
+ struct obbDisjoint_check_Ai_cross_Bi
+ {
+ static inline bool run (int ia, int ja, int ka,
+ const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,
+ const Matrix3f& Bf,
+ const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance)
+ {
+ const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
+
+ const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
+ b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
+ // We need to divide by the norm || Aia x Bib ||
+ // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
+ if (diff > 0) {
+ FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
+ if (sinus2 > 1e-6) {
+ squaredLowerBoundDistance = diff * diff / sinus2;
+ if (squaredLowerBoundDistance > breakDistance2) {
+ return true;
+ }
+ }
+ /* // or
+ FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
+ squaredLowerBoundDistance = diff * diff;
+ if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
+ squaredLowerBoundDistance /= sinus2;
+ return true;
+ }
+ // */
+ }
+ return false;
+ }
+ };
+}
+
+
// B, T orientation and position of 2nd OBB in frame of 1st OBB,
// a extent of 1st OBB,
// b extent of 2nd OBB.
@@ -301,58 +366,39 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
// This function tests whether bounding boxes should be broken down.
//
bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
+ const Vec3f& a, const Vec3f& b,
const CollisionRequest& request,
- FCL_REAL& squaredLowerBoundDistance)
+ FCL_REAL& squaredLowerBoundDistance)
{
const FCL_REAL breakDistance (request.break_distance + request.security_margin);
const FCL_REAL breakDistance2 = breakDistance * breakDistance;
- Vec3f AABB_corner;
Matrix3f Bf (B.cwiseAbs());
// Corner of b axis aligned bounding box the closest to the origin
- AABB_corner.noalias() = T.cwiseAbs () - Bf * b - a;
- squaredLowerBoundDistance = AABB_corner.array().max(0).matrix().squaredNorm ();
+ squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis (T, a, b, Bf);
if (squaredLowerBoundDistance > breakDistance2)
return true;
- // | B^T T| - b - Bf^T a
- AABB_corner.noalias() = (B.transpose () * T).cwiseAbs () - Bf.transpose () * a - b;
- squaredLowerBoundDistance = AABB_corner.array().max(0).matrix().squaredNorm();
+ squaredLowerBoundDistance = internal::obbDisjoint_check_B_axis (B, T, a, b, Bf);
if (squaredLowerBoundDistance > breakDistance2)
return true;
- int ja = 1, ka = 2, jb = 1, kb = 2;
+ // Ai x Bj
+ int ja = 1, ka = 2;
for (int ia = 0; ia < 3; ++ia) {
- for (int ib = 0; ib < 3; ++ib) {
- const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
-
- const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
- b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
- // We need to divide by the norm || Aia x Bib ||
- // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
- if (diff > 0) {
- FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
- if (sinus2 > 1e-6) {
- squaredLowerBoundDistance = diff * diff / sinus2;
- if (squaredLowerBoundDistance > breakDistance2) {
- return true;
- }
- }
- }
-
- jb = kb; kb = ib;
- }
+ if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run (ia, ja, ka,
+ B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true;
+ if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run (ia, ja, ka,
+ B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true;
+ if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run (ia, ja, ka,
+ B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true;
ja = ka; ka = ia;
}
return false;
-
}
-
-
bool OBB::overlap(const OBB& other) const
{
/// compute what transform [R,T] that takes us from cs1 to cs2.
--
GitLab