bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
{
Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
return !obbDisjoint(R, T, b1.extent, b2.extent);
}
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b)
{
register FCL_REAL t, s;
const FCL_REAL reps = 1e-6;
Matrix3f Bf = abs(B);
Bf += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
// A1 x A2 = A0
t = ((T[0] < 0.0) ? -T[0] : T[0]);
if(t > (a[0] + Bf.dotX(b)))
return true;
// B1 x B2 = B0
s = B.transposeDotX(T);
t = ((s < 0.0) ? -s : s);
if(t > (b[0] + Bf.transposeDotX(a)))
return true;
// A2 x A0 = A1
t = ((T[1] < 0.0) ? -T[1] : T[1]);
if(t > (a[1] + Bf.dotY(b)))
return true;
// A0 x A1 = A2
t =((T[2] < 0.0) ? -T[2] : T[2]);
if(t > (a[2] + Bf.dotZ(b)))
return true;
// B2 x B0 = B1
s = B.transposeDotY(T);
t = ((s < 0.0) ? -s : s);
if(t > (b[1] + Bf.transposeDotY(a)))
return true;
// B0 x B1 = B2
s = B.transposeDotZ(T);
t = ((s < 0.0) ? -s : s);
if(t > (b[2] + Bf.transposeDotZ(a)))
return true;
// A0 x B0
s = T[2] * B(1, 0) - T[1] * B(2, 0);
t = ((s < 0.0) ? -s : s);
if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
return true;
// A0 x B1
s = T[2] * B(1, 1) - T[1] * B(2, 1);
t = ((s < 0.0) ? -s : s);
if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
return true;
// A0 x B2
s = T[2] * B(1, 2) - T[1] * B(2, 2);
t = ((s < 0.0) ? -s : s);
if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
return true;
// A1 x B0
s = T[0] * B(2, 0) - T[2] * B(0, 0);
t = ((s < 0.0) ? -s : s);
if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
return true;
// A1 x B1
s = T[0] * B(2, 1) - T[2] * B(0, 1);
t = ((s < 0.0) ? -s : s);
if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
return true;
// A1 x B2
s = T[0] * B(2, 2) - T[2] * B(0, 2);
t = ((s < 0.0) ? -s : s);
if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
return true;
// A2 x B0
s = T[1] * B(0, 0) - T[0] * B(1, 0);
t = ((s < 0.0) ? -s : s);
if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
return true;
// A2 x B1
s = T[1] * B(0, 1) - T[0] * B(1, 1);
t = ((s < 0.0) ? -s : s);
if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
return true;
// A2 x B2
s = T[1] * B(0, 2) - T[0] * B(1, 2);
t = ((s < 0.0) ? -s : s);
if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
return true;
return false;
}