bool CHAABB::AABB_Overlap(ChMatrix33<>& B, Vector T, CHAABB *b1, CHAABB *b2)
{
ChMatrix33<> Bf;
const double reps = (double)1e-6;
// Bf = fabs(B)
Bf(0,0) = myfabs(B.Get33Element(0,0)); Bf(0,0) += reps;
Bf(0,1) = myfabs(B.Get33Element(0,1)); Bf(0,1) += reps;
Bf(0,2) = myfabs(B.Get33Element(0,2)); Bf(0,2) += reps;
Bf(1,0) = myfabs(B.Get33Element(1,0)); Bf(1,0) += reps;
Bf(1,1) = myfabs(B.Get33Element(1,1)); Bf(1,1) += reps;
Bf(1,2) = myfabs(B.Get33Element(1,2)); Bf(1,2) += reps;
Bf(2,0) = myfabs(B.Get33Element(2,0)); Bf(2,0) += reps;
Bf(2,1) = myfabs(B.Get33Element(2,1)); Bf(2,1) += reps;
Bf(2,2) = myfabs(B.Get33Element(2,2)); Bf(2,2) += reps;
return AABB_Overlap(B, Bf, T, b1, b2);
}
bool CHOBB::OBB_Overlap(ChMatrix33<>& B, Vector T, Vector a, Vector b)
{
register double t, s;
register int r;
static ChMatrix33<> Bf;
const double reps = (double)1e-6;
//Vector& a = b1->d;
//Vector& b = b2->d;
// Bf = fabs(B)
Bf(0,0) = myfabs(B.Get33Element(0,0)); Bf(0,0) += reps;
Bf(0,1) = myfabs(B.Get33Element(0,1)); Bf(0,1) += reps;
Bf(0,2) = myfabs(B.Get33Element(0,2)); Bf(0,2) += reps;
Bf(1,0) = myfabs(B.Get33Element(1,0)); Bf(1,0) += reps;
Bf(1,1) = myfabs(B.Get33Element(1,1)); Bf(1,1) += reps;
Bf(1,2) = myfabs(B.Get33Element(1,2)); Bf(1,2) += reps;
Bf(2,0) = myfabs(B.Get33Element(2,0)); Bf(2,0) += reps;
Bf(2,1) = myfabs(B.Get33Element(2,1)); Bf(2,1) += reps;
Bf(2,2) = myfabs(B.Get33Element(2,2)); Bf(2,2) += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
r = 1;
// A1 x A2 = A0
t = myfabs(T.x);
r &= (t <=
(a.x + b.x * Bf.Get33Element(0,0) + b.y * Bf.Get33Element(0,1) + b.z * Bf.Get33Element(0,2)));
if (!r) return false;
// B1 x B2 = B0
s = T.x*B.Get33Element(0,0) + T.y*B.Get33Element(1,0) + T.z*B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(b.x + a.x * Bf.Get33Element(0,0) + a.y * Bf.Get33Element(1,0) + a.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x A0 = A1
t = myfabs(T.y);
r &= ( t <=
(a.y + b.x * Bf.Get33Element(1,0) + b.y * Bf.Get33Element(1,1) + b.z * Bf.Get33Element(1,2)));
if (!r) return false;
// A0 x A1 = A2
t = myfabs(T.z);
r &= ( t <=
(a.z + b.x * Bf.Get33Element(2,0) + b.y * Bf.Get33Element(2,1) + b.z * Bf.Get33Element(2,2)));
if (!r) return false;
// B2 x B0 = B1
s = T.x*B.Get33Element(0,1) + T.y*B.Get33Element(1,1) + T.z*B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(b.y + a.x * Bf.Get33Element(0,1) + a.y * Bf.Get33Element(1,1) + a.z * Bf.Get33Element(2,1)));
if (!r) return false;
// B0 x B1 = B2
s = T.x*B.Get33Element(0,2) + T.y*B.Get33Element(1,2) + T.z*B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(b.z + a.x * Bf.Get33Element(0,2) + a.y * Bf.Get33Element(1,2) + a.z * Bf.Get33Element(2,2)));
if (!r) return false;
// A0 x B0
s = T.z * B.Get33Element(1,0) - T.y * B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(1,0) +
b.y * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,1)));
if (!r) return false;
// A0 x B1
s = T.z * B.Get33Element(1,1) - T.y * B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(1,1) +
b.x * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,0)));
if (!r) return false;
// A0 x B2
s = T.z * B.Get33Element(1,2) - T.y * B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(1,2) +
b.x * Bf.Get33Element(0,1) + b.y * Bf.Get33Element(0,0)));
if (!r) return false;
// A1 x B0
s = T.x * B.Get33Element(2,0) - T.z * B.Get33Element(0,0);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,1)));
if (!r) return false;
// A1 x B1
s = T.x * B.Get33Element(2,1) - T.z * B.Get33Element(0,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,0)));
if (!r) return false;
// A1 x B2
s = T.x * B.Get33Element(2,2) - T.z * B.Get33Element(0,2);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(1,1) + b.y * Bf.Get33Element(1,0)));
if (!r) return false;
// A2 x B0
s = T.y * B.Get33Element(0,0) - T.x * B.Get33Element(1,0);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(1,0) + a.y * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,1)));
if (!r) return false;
// A2 x B1
s = T.y * B.Get33Element(0,1) - T.x * B.Get33Element(1,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,1) + a.y * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x B2
s = T.y * B.Get33Element(0,2) - T.x * B.Get33Element(1,2);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,2) + a.y * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(2,1) + b.y * Bf.Get33Element(2,0)));
if (!r) return false;
return true; // no separation: BV collide!!
}
bool CHAABB::AABB_Overlap(ChMatrix33<>& B, ChMatrix33<>& Bf, Vector T, CHAABB *b1, CHAABB *b2)
{
register double t, s;
register int r;
Vector& a = b1->d;
Vector& b = b2->d;
// if any of these tests are one-sided, then the polyhedra are disjoint
r = 1;
// A1 x A2 = A0
t = myfabs(T.x);
r &= (t <=
(a.x + b.x * Bf.Get33Element(0,0) + b.y * Bf.Get33Element(0,1) + b.z * Bf.Get33Element(0,2)));
if (!r) return false;
// B1 x B2 = B0
s = T.x*B.Get33Element(0,0) + T.y*B.Get33Element(1,0) + T.z*B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(b.x + a.x * Bf.Get33Element(0,0) + a.y * Bf.Get33Element(1,0) + a.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x A0 = A1
t = myfabs(T.y);
r &= ( t <=
(a.y + b.x * Bf.Get33Element(1,0) + b.y * Bf.Get33Element(1,1) + b.z * Bf.Get33Element(1,2)));
if (!r) return false;
// A0 x A1 = A2
t = myfabs(T.z);
r &= ( t <=
(a.z + b.x * Bf.Get33Element(2,0) + b.y * Bf.Get33Element(2,1) + b.z * Bf.Get33Element(2,2)));
if (!r) return false;
// B2 x B0 = B1
s = T.x*B.Get33Element(0,1) + T.y*B.Get33Element(1,1) + T.z*B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(b.y + a.x * Bf.Get33Element(0,1) + a.y * Bf.Get33Element(1,1) + a.z * Bf.Get33Element(2,1)));
if (!r) return false;
// B0 x B1 = B2
s = T.x*B.Get33Element(0,2) + T.y*B.Get33Element(1,2) + T.z*B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(b.z + a.x * Bf.Get33Element(0,2) + a.y * Bf.Get33Element(1,2) + a.z * Bf.Get33Element(2,2)));
if (!r) return false;
// A0 x B0
s = T.z * B.Get33Element(1,0) - T.y * B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(1,0) +
b.y * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,1)));
if (!r) return false;
// A0 x B1
s = T.z * B.Get33Element(1,1) - T.y * B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(1,1) +
b.x * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,0)));
if (!r) return false;
// A0 x B2
s = T.z * B.Get33Element(1,2) - T.y * B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(1,2) +
b.x * Bf.Get33Element(0,1) + b.y * Bf.Get33Element(0,0)));
if (!r) return false;
// A1 x B0
s = T.x * B.Get33Element(2,0) - T.z * B.Get33Element(0,0);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,1)));
if (!r) return false;
// A1 x B1
s = T.x * B.Get33Element(2,1) - T.z * B.Get33Element(0,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,0)));
if (!r) return false;
// A1 x B2
s = T.x * B.Get33Element(2,2) - T.z * B.Get33Element(0,2);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(1,1) + b.y * Bf.Get33Element(1,0)));
if (!r) return false;
// A2 x B0
s = T.y * B.Get33Element(0,0) - T.x * B.Get33Element(1,0);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(1,0) + a.y * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,1)));
if (!r) return false;
// A2 x B1
s = T.y * B.Get33Element(0,1) - T.x * B.Get33Element(1,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,1) + a.y * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x B2
s = T.y * B.Get33Element(0,2) - T.x * B.Get33Element(1,2);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,2) + a.y * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(2,1) + b.y * Bf.Get33Element(2,0)));
if (!r) return false;
return true; // no separation: BV collide
}