static BOOL _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
// reset best axis
iBestAxis = 0;
iExitAxis = -1;
fBestDepth = MAXVALUE;
// calculate edges
SUBTRACT(v1,v0,vE0);
SUBTRACT(v2,v0,vE1);
SUBTRACT(vE1,vE0,vE2);
// calculate poly normal
dCROSS(vN,=,vE0,vE1);
// extract box axes as vectors
dVector3 vA0,vA1,vA2;
GETCOL(mHullBoxRot,0,vA0);
GETCOL(mHullBoxRot,1,vA1);
GETCOL(mHullBoxRot,2,vA2);
// box halfsizes
dReal fa0 = vBoxHalfSize[0];
dReal fa1 = vBoxHalfSize[1];
dReal fa2 = vBoxHalfSize[2];
// calculate relative position between box and triangle
dVector3 vD;
SUBTRACT(v0,vHullBoxPos,vD);
// calculate length of face normal
dReal fNLen = LENGTHOF( vN );
dVector3 vL;
dReal fp0, fp1, fp2, fR, fD;
// Test separating axes for intersection
// ************************************************
// Axis 1 - Triangle Normal
SET(vL,vN);
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0;
fR=fa0*dFabs( dDOT(vN,vA0) ) + fa1 * dFabs( dDOT(vN,vA1) ) + fa2 * dFabs( dDOT(vN,vA2) );
if( !_cldTestNormal( fp0, fR, vL, 1) ) {
iExitAxis=1;
return FALSE;
}
// ************************************************
// Test Faces
// ************************************************
// Axis 2 - Box X-Axis
SET(vL,vA0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA0,vE0);
fp2 = fp0 + dDOT(vA0,vE1);
fR = fa0;
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 2) ) {
iExitAxis=2;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 3 - Box Y-Axis
SET(vL,vA1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA1,vE0);
fp2 = fp0 + dDOT(vA1,vE1);
fR = fa1;
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 3) ) {
iExitAxis=3;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 4 - Box Z-Axis
SET(vL,vA2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA2,vE0);
fp2 = fp0 + dDOT(vA2,vE1);
fR = fa2;
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 4) ) {
iExitAxis=4;
return FALSE;
}
// ************************************************
// Test Edges
// ************************************************
// Axis 5 - Box X-Axis cross Edge0
dCROSS(vL,=,vA0,vE0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA0,vN);
fR = fa1 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA1,vE0));
if( !_cldTestEdge( fp1, fp2, fR, fD, vL, 5) ) {
iExitAxis=5;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 6 - Box X-Axis cross Edge1
dCROSS(vL,=,vA0,vE1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,vN);
fp2 = fp0;
fR = fa1 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA1,vE1));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 6) ) {
iExitAxis=6;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 7 - Box X-Axis cross Edge2
dCROSS(vL,=,vA0,vE2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,vN);
fp2 = fp0 - dDOT(vA0,vN);
fR = fa1 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA1,vE2));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 7) ) {
iExitAxis=7;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 8 - Box Y-Axis cross Edge0
dCROSS(vL,=,vA1,vE0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA1,vN);
fR = fa0 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA0,vE0));
if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 8) ) {
iExitAxis=8;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 9 - Box Y-Axis cross Edge1
dCROSS(vL,=,vA1,vE1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA0,vE1));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 9) ) {
iExitAxis=9;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 10 - Box Y-Axis cross Edge2
dCROSS(vL,=,vA1,vE2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,vN);
fp2 = fp0 - dDOT(vA1,vN);
fR = fa0 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA0,vE2));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 10) ) {
iExitAxis=10;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 11 - Box Z-Axis cross Edge0
dCROSS(vL,=,vA2,vE0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA2,vN);
fR = fa0 * dFabs(dDOT(vA1,vE0)) + fa1 * dFabs(dDOT(vA0,vE0));
if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 11) ) {
iExitAxis=11;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 12 - Box Z-Axis cross Edge1
dCROSS(vL,=,vA2,vE1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA1,vE1)) + fa1 * dFabs(dDOT(vA0,vE1));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 12) ) {
iExitAxis=12;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 13 - Box Z-Axis cross Edge2
dCROSS(vL,=,vA2,vE2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,vN);
fp2 = fp0 - dDOT(vA2,vN);
fR = fa0 * dFabs(dDOT(vA1,vE2)) + fa1 * dFabs(dDOT(vA0,vE2));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 13) ) {
iExitAxis=13;
return FALSE;
}
// ************************************************
return TRUE;
}
bool sTrimeshBoxColliderData::_cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
// reset best axis
m_iBestAxis = 0;
m_iExitAxis = -1;
m_fBestDepth = MAXVALUE;
// calculate edges
SUBTRACT(v1,v0,m_vE0);
SUBTRACT(v2,v0,m_vE1);
SUBTRACT(m_vE1,m_vE0,m_vE2);
// calculate poly normal
dCROSS(m_vN,=,m_vE0,m_vE1);
// calculate length of face normal
dReal fNLen = LENGTHOF(m_vN);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!fNLen) {
return false;
}
// extract box axes as vectors
dVector3 vA0,vA1,vA2;
GETCOL(m_mHullBoxRot,0,vA0);
GETCOL(m_mHullBoxRot,1,vA1);
GETCOL(m_mHullBoxRot,2,vA2);
// box halfsizes
dReal fa0 = m_vBoxHalfSize[0];
dReal fa1 = m_vBoxHalfSize[1];
dReal fa2 = m_vBoxHalfSize[2];
// calculate relative position between box and triangle
dVector3 vD;
SUBTRACT(v0,m_vHullBoxPos,vD);
dVector3 vL;
dReal fp0, fp1, fp2, fR, fD;
// Test separating axes for intersection
// ************************************************
// Axis 1 - Triangle Normal
SET(vL,m_vN);
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0;
fR=fa0*dFabs( dDOT(m_vN,vA0) ) + fa1 * dFabs( dDOT(m_vN,vA1) ) + fa2 * dFabs( dDOT(m_vN,vA2) );
if (!_cldTestNormal(fp0, fR, vL, 1)) {
m_iExitAxis=1;
return false;
}
// ************************************************
// Test Faces
// ************************************************
// Axis 2 - Box X-Axis
SET(vL,vA0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA0,m_vE0);
fp2 = fp0 + dDOT(vA0,m_vE1);
fR = fa0;
if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 2)) {
m_iExitAxis=2;
return false;
}
// ************************************************
// ************************************************
// Axis 3 - Box Y-Axis
SET(vL,vA1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA1,m_vE0);
fp2 = fp0 + dDOT(vA1,m_vE1);
fR = fa1;
if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 3)) {
m_iExitAxis=3;
return false;
}
// ************************************************
// ************************************************
// Axis 4 - Box Z-Axis
SET(vL,vA2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA2,m_vE0);
fp2 = fp0 + dDOT(vA2,m_vE1);
fR = fa2;
if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 4)) {
m_iExitAxis=4;
return false;
}
// ************************************************
// Test Edges
// ************************************************
// Axis 5 - Box X-Axis cross Edge0
dCROSS(vL,=,vA0,m_vE0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA0,m_vN);
fR = fa1 * dFabs(dDOT(vA2,m_vE0)) + fa2 * dFabs(dDOT(vA1,m_vE0));
if (!_cldTestEdge(fp1, fp2, fR, fD, vL, 5)) {
m_iExitAxis=5;
return false;
}
// ************************************************
// ************************************************
// Axis 6 - Box X-Axis cross Edge1
dCROSS(vL,=,vA0,m_vE1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,m_vN);
fp2 = fp0;
fR = fa1 * dFabs(dDOT(vA2,m_vE1)) + fa2 * dFabs(dDOT(vA1,m_vE1));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 6)) {
m_iExitAxis=6;
return false;
}
// ************************************************
// ************************************************
// Axis 7 - Box X-Axis cross Edge2
dCROSS(vL,=,vA0,m_vE2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,m_vN);
fp2 = fp0 - dDOT(vA0,m_vN);
fR = fa1 * dFabs(dDOT(vA2,m_vE2)) + fa2 * dFabs(dDOT(vA1,m_vE2));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 7)) {
m_iExitAxis=7;
return false;
}
// ************************************************
// ************************************************
// Axis 8 - Box Y-Axis cross Edge0
dCROSS(vL,=,vA1,m_vE0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA1,m_vN);
fR = fa0 * dFabs(dDOT(vA2,m_vE0)) + fa2 * dFabs(dDOT(vA0,m_vE0));
if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 8)) {
m_iExitAxis=8;
return false;
}
// ************************************************
// ************************************************
// Axis 9 - Box Y-Axis cross Edge1
dCROSS(vL,=,vA1,m_vE1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,m_vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA2,m_vE1)) + fa2 * dFabs(dDOT(vA0,m_vE1));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 9)) {
m_iExitAxis=9;
return false;
}
// ************************************************
// ************************************************
// Axis 10 - Box Y-Axis cross Edge2
dCROSS(vL,=,vA1,m_vE2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,m_vN);
fp2 = fp0 - dDOT(vA1,m_vN);
fR = fa0 * dFabs(dDOT(vA2,m_vE2)) + fa2 * dFabs(dDOT(vA0,m_vE2));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 10)) {
m_iExitAxis=10;
return false;
}
// ************************************************
// ************************************************
// Axis 11 - Box Z-Axis cross Edge0
dCROSS(vL,=,vA2,m_vE0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA2,m_vN);
fR = fa0 * dFabs(dDOT(vA1,m_vE0)) + fa1 * dFabs(dDOT(vA0,m_vE0));
if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 11)) {
m_iExitAxis=11;
return false;
}
// ************************************************
// ************************************************
// Axis 12 - Box Z-Axis cross Edge1
dCROSS(vL,=,vA2,m_vE1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,m_vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA1,m_vE1)) + fa1 * dFabs(dDOT(vA0,m_vE1));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 12)) {
m_iExitAxis=12;
return false;
}
// ************************************************
// ************************************************
// Axis 13 - Box Z-Axis cross Edge2
dCROSS(vL,=,vA2,m_vE2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,m_vN);
fp2 = fp0 - dDOT(vA2,m_vN);
fR = fa0 * dFabs(dDOT(vA1,m_vE2)) + fa1 * dFabs(dDOT(vA0,m_vE2));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 13)) {
m_iExitAxis=13;
return false;
}
// ************************************************
return true;
}