diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index b462aaff78f2731f814740a76697dc7c736faf3c..8e0fdcc35b0d2ca9b2e456c9fab08518d08f0a13 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -314,18 +314,13 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T, // Corner of b axis aligned bounding box the closest to the origin Vec3f AABB_corner (T.cwiseAbs () - Bf * b); - Vec3f diff3 (AABB_corner - a); - diff3 = diff3.cwiseMax (0); - //for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); - squaredLowerBoundDistance = diff3.squaredNorm (); + squaredLowerBoundDistance = (AABB_corner - a).cwiseMax (0).squaredNorm (); if (squaredLowerBoundDistance > breakDistance2) return true; AABB_corner = (B.transpose () * T).cwiseAbs () - Bf.transpose () * a; - // diff3 = | B^T T| - b - Bf^T a - diff3 = AABB_corner - b; - diff3 = diff3.cwiseMax (0); - squaredLowerBoundDistance = diff3.squaredNorm (); + // | B^T T| - b - Bf^T a + squaredLowerBoundDistance = (AABB_corner - b).cwiseMax (0).squaredNorm (); if (squaredLowerBoundDistance > breakDistance2) return true;