Skip to content
GitLab
Commit e50d403a authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Prepare simplification of obbDisjointAndLowerBoundDistance

parent 64dc11fa
Hide whitespace changes
Inline Side-by-side
include/hpp/fcl/BV/OBBRSS.h
View file @ e50d403a
......@@ -157,18 +157,27 @@ public:
OBBRSS translate(const OBBRSS& bv, const Vec3f& t);
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
{
return overlap(R0, T0, b1.obb, b2.obb);
}
/// Check collision between two OBBRSS
/// \param b1 first OBBRSS in configuration (R0, T0)
/// \param b2 second OBBRSS in identity position
/// \retval squared lower bound on the distance if OBBRSS do not overlap.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
const OBBRSS& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
const OBBRSS& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound)
{
return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
}
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL)
{
return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
}
......
src/BV/OBB.cpp
View file @ e50d403a
......@@ -294,6 +294,39 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
}
bool obbDisjointAndLowerBoundDistanceCase (
const Matrix3f& B, const Vec3f& T,
const Vec3f& a, const Vec3f& b,
const FCL_REAL& breakDistance2,
const Matrix3f& Bf,
const int& ia,
const int& ib,
FCL_REAL& squaredLowerBoundDistance,
FCL_REAL& diff)
{
const int ja = (ia+1)%3, jb = (ib+1)%3,
ka = (ja+1)%3, kb = (jb+1)%3;
const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
FCL_REAL sinus2;
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) {
sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
if (sinus2 > 1e-6) {
squaredLowerBoundDistance = diff * diff / sinus2;
if (squaredLowerBoundDistance > breakDistance2) {
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.
......@@ -306,30 +339,24 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
FCL_REAL& squaredLowerBoundDistance)
{
FCL_REAL t, s;
FCL_REAL diff;
FCL_REAL breakDistance (request.break_distance + request.security_margin);
FCL_REAL breakDistance2 = breakDistance * breakDistance;
FCL_REAL diff, diff2;
const FCL_REAL breakDistance (request.break_distance + request.security_margin);
const FCL_REAL breakDistance2 = breakDistance * breakDistance;
Vec3f AABB_corner;
// Matrix3f Bf = abs(B);
// Bf += reps;
Matrix3f Bf (B.cwiseAbs());
// 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 (Vec3f::Zero());
//for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
squaredLowerBoundDistance = diff3.squaredNorm ();
AABB_corner.noalias() = T.cwiseAbs () - Bf * b - a;
squaredLowerBoundDistance = AABB_corner.array().max(0).matrix().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 (Vec3f::Zero());
squaredLowerBoundDistance = diff3.squaredNorm ();
// | 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();
if (squaredLowerBoundDistance > breakDistance2)
return true;
......@@ -344,6 +371,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
// As ||A0|| = ||B0|| = 1,
// 2 2
// || A0 x B0 || + (A0 | B0) = 1
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 0, 0, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (0,0) * Bf (0,0);
if (sinus2 > 1e-6) {
......@@ -359,7 +389,10 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
t = ((s < 0.0) ? -s : s); assert (t == fabs (s));
diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
b[0] * Bf(0, 2) + b[2] * Bf(0, 0));
b[2] * Bf(0, 0) + b[0] * Bf(0, 2));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 0, 1, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (0,1) * Bf (0,1);
if (sinus2 > 1e-6) {
......@@ -376,6 +409,9 @@ 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));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 0, 2, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (0,2) * Bf (0,2);
if (sinus2 > 1e-6) {
......@@ -392,6 +428,9 @@ 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));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 1, 0, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (1,0) * Bf (1,0);
if (sinus2 > 1e-6) {
......@@ -407,7 +446,10 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
t = ((s < 0.0) ? -s : s); assert (t == fabs (s));
diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
b[0] * Bf(1, 2) + b[2] * Bf(1, 0));
b[2] * Bf(1, 0) + b[0] * Bf(1, 2));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 1, 1, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (1,1) * Bf (1,1);
if (sinus2 > 1e-6) {
......@@ -424,6 +466,9 @@ 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));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 1, 2, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (1,2) * Bf (1,2);
if (sinus2 > 1e-6) {
......@@ -440,6 +485,9 @@ 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));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 2, 0, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (2,0) * Bf (2,0);
if (sinus2 > 1e-6) {
......@@ -455,7 +503,10 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
t = ((s < 0.0) ? -s : s); assert (t == fabs (s));
diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
b[0] * Bf(2, 2) + b[2] * Bf(2, 0));
b[2] * Bf(2, 0) + b[0] * Bf(2, 2));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 2, 1, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (2,1) * Bf (2,1);
if (sinus2 > 1e-6) {
......@@ -472,6 +523,9 @@ 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));
obbDisjointAndLowerBoundDistanceCase (B, T, a, b, breakDistance2,
Bf, 2, 2, squaredLowerBoundDistance, diff2);
assert (diff == diff2);
if (diff > 0) {
sinus2 = 1 - Bf (2,2) * Bf (2,2);
if (sinus2 > 1e-6) {
......
src/BV/OBBRSS.cpp
View file @ e50d403a
......@@ -42,23 +42,6 @@ namespace hpp
namespace fcl
{
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
{
return overlap(R0, T0, b1.obb, b2.obb);
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
const OBBRSS& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound)
{
return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
}
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P, Vec3f* Q)
{
return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
OBBRSS translate(const OBBRSS& bv, const Vec3f& t)
{
OBBRSS res(bv);
......
Supports Markdown
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