Sphere EncloseSphere ( OrientedBox const & shape )
{
float x = shape.getExtentX();
float y = shape.getExtentY();
float z = shape.getExtentZ();
float radius = sqrt( x*x + y*y + z*z );
return Sphere( shape.getCenter(), radius );
}
Range ProjectAxis ( Line3d const & L, OrientedBox const & B )
{
Vector const & N = L.getNormal();
real x = std::abs( Collision3d::ComponentAlong( B.getAxisX(), N ) ) * B.getExtentX();
real y = std::abs( Collision3d::ComponentAlong( B.getAxisY(), N ) ) * B.getExtentY();
real z = std::abs( Collision3d::ComponentAlong( B.getAxisZ(), N ) ) * B.getExtentZ();
real d = x + y + z;
real c = ProjectAxis( L, B.getCenter() );
return Range( c - d, c + d );
}
bool TestOBoxOBox ( OrientedBox const & boxA, OrientedBox const & boxB )
{
// convenience variables
const Vector* akA = boxA.getAxes();
const Vector* akB = boxB.getAxes();
const float* afEA = boxA.getExtents();
const float* afEB = boxB.getExtents();
// compute difference of box centers, D = C1-C0
Vector kD = boxB.getCenter() - boxA.getCenter();
float aafC[3][3]; // matrix C = A^T B, c_{ij} = Dot(A_i,B_j)
float aafAbsC[3][3]; // |c_{ij}|
float afAD[3]; // Dot(A_i,D)
float fR0, fR1, fR; // interval radii and distance between centers
float fR01; // = R0 + R1
// axis C0+t*A0
aafC[0][0] = akA[0].dot(akB[0]);
aafC[0][1] = akA[0].dot(akB[1]);
aafC[0][2] = akA[0].dot(akB[2]);
afAD[0] = akA[0].dot(kD);
aafAbsC[0][0] = static_cast<float>(fabs(aafC[0][0]));
aafAbsC[0][1] = static_cast<float>(fabs(aafC[0][1]));
aafAbsC[0][2] = static_cast<float>(fabs(aafC[0][2]));
fR = static_cast<float>(fabs(afAD[0]));
fR1 = afEB[0]*aafAbsC[0][0]+afEB[1]*aafAbsC[0][1]+afEB[2]*aafAbsC[0][2];
fR01 = afEA[0] + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A1
aafC[1][0] = akA[1].dot(akB[0]);
aafC[1][1] = akA[1].dot(akB[1]);
aafC[1][2] = akA[1].dot(akB[2]);
afAD[1] = akA[1].dot(kD);
aafAbsC[1][0] = static_cast<float>(fabs(aafC[1][0]));
aafAbsC[1][1] = static_cast<float>(fabs(aafC[1][1]));
aafAbsC[1][2] = static_cast<float>(fabs(aafC[1][2]));
fR = static_cast<float>(fabs(afAD[1]));
fR1 = afEB[0]*aafAbsC[1][0]+afEB[1]*aafAbsC[1][1]+afEB[2]*aafAbsC[1][2];
fR01 = afEA[1] + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A2
aafC[2][0] = akA[2].dot(akB[0]);
aafC[2][1] = akA[2].dot(akB[1]);
aafC[2][2] = akA[2].dot(akB[2]);
afAD[2] = akA[2].dot(kD);
aafAbsC[2][0] = static_cast<float>(fabs(aafC[2][0]));
aafAbsC[2][1] = static_cast<float>(fabs(aafC[2][1]));
aafAbsC[2][2] = static_cast<float>(fabs(aafC[2][2]));
fR = static_cast<float>(fabs(afAD[2]));
fR1 = afEB[0]*aafAbsC[2][0]+afEB[1]*aafAbsC[2][1]+afEB[2]*aafAbsC[2][2];
fR01 = afEA[2] + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*B0
fR = static_cast<float>(fabs(akB[0].dot(kD)));
fR0 = afEA[0]*aafAbsC[0][0]+afEA[1]*aafAbsC[1][0]+afEA[2]*aafAbsC[2][0];
fR01 = fR0 + afEB[0];
if ( fR > fR01 )
return false;
// axis C0+t*B1
fR = static_cast<float>(fabs(akB[1].dot(kD)));
fR0 = afEA[0]*aafAbsC[0][1]+afEA[1]*aafAbsC[1][1]+afEA[2]*aafAbsC[2][1];
fR01 = fR0 + afEB[1];
if ( fR > fR01 )
return false;
// axis C0+t*B2
fR = static_cast<float>(fabs(akB[2].dot(kD)));
fR0 = afEA[0]*aafAbsC[0][2]+afEA[1]*aafAbsC[1][2]+afEA[2]*aafAbsC[2][2];
fR01 = fR0 + afEB[2];
if ( fR > fR01 )
return false;
// axis C0+t*A0xB0
fR = static_cast<float>(fabs(afAD[2]*aafC[1][0]-afAD[1]*aafC[2][0]));
fR0 = afEA[1]*aafAbsC[2][0] + afEA[2]*aafAbsC[1][0];
fR1 = afEB[1]*aafAbsC[0][2] + afEB[2]*aafAbsC[0][1];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A0xB1
fR = static_cast<float>(fabs(afAD[2]*aafC[1][1]-afAD[1]*aafC[2][1]));
fR0 = afEA[1]*aafAbsC[2][1] + afEA[2]*aafAbsC[1][1];
fR1 = afEB[0]*aafAbsC[0][2] + afEB[2]*aafAbsC[0][0];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A0xB2
fR = static_cast<float>(fabs(afAD[2]*aafC[1][2]-afAD[1]*aafC[2][2]));
fR0 = afEA[1]*aafAbsC[2][2] + afEA[2]*aafAbsC[1][2];
fR1 = afEB[0]*aafAbsC[0][1] + afEB[1]*aafAbsC[0][0];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A1xB0
fR = static_cast<float>(fabs(afAD[0]*aafC[2][0]-afAD[2]*aafC[0][0]));
fR0 = afEA[0]*aafAbsC[2][0] + afEA[2]*aafAbsC[0][0];
fR1 = afEB[1]*aafAbsC[1][2] + afEB[2]*aafAbsC[1][1];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A1xB1
fR = static_cast<float>(fabs(afAD[0]*aafC[2][1]-afAD[2]*aafC[0][1]));
fR0 = afEA[0]*aafAbsC[2][1] + afEA[2]*aafAbsC[0][1];
fR1 = afEB[0]*aafAbsC[1][2] + afEB[2]*aafAbsC[1][0];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A1xB2
fR = static_cast<float>(fabs(afAD[0]*aafC[2][2]-afAD[2]*aafC[0][2]));
fR0 = afEA[0]*aafAbsC[2][2] + afEA[2]*aafAbsC[0][2];
fR1 = afEB[0]*aafAbsC[1][1] + afEB[1]*aafAbsC[1][0];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A2xB0
fR = static_cast<float>(fabs(afAD[1]*aafC[0][0]-afAD[0]*aafC[1][0]));
fR0 = afEA[0]*aafAbsC[1][0] + afEA[1]*aafAbsC[0][0];
fR1 = afEB[1]*aafAbsC[2][2] + afEB[2]*aafAbsC[2][1];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A2xB1
fR = static_cast<float>(fabs(afAD[1]*aafC[0][1]-afAD[0]*aafC[1][1]));
fR0 = afEA[0]*aafAbsC[1][1] + afEA[1]*aafAbsC[0][1];
fR1 = afEB[0]*aafAbsC[2][2] + afEB[2]*aafAbsC[2][0];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
// axis C0+t*A2xB2
fR = static_cast<float>(fabs(afAD[1]*aafC[0][2]-afAD[0]*aafC[1][2]));
fR0 = afEA[0]*aafAbsC[1][2] + afEA[1]*aafAbsC[0][2];
fR1 = afEB[0]*aafAbsC[2][1] + afEB[1]*aafAbsC[2][0];
fR01 = fR0 + fR1;
if ( fR > fR01 )
return false;
return true;
}