void vsip_minterp_spline_f(
const vsip_vview_f *ax,
const vsip_mview_f *ay,
vsip_spline_f *spl,
vsip_major major,
const vsip_vview_f *bx,
const vsip_mview_f *by){
vsip_index i;
vsip_vview_f *y,yy;
vsip_vview_f *yf,yyf;
y = &yy;
yf = &yyf;
if(major == VSIP_ROW) {
vsip_length M = ay->col_length;
for(i=0; i<M; i++){
GETROW(ay,i,y);
GETROW(by,i,yf);
VINTERP_SPLINE(ax,y,spl,bx,yf);
}
} else { /* must be by column */
vsip_length M = ay->row_length;
for(i=0; i<M; i++){
GETCOL(ay,i,y);
GETCOL(by,i,yf);
VINTERP_SPLINE(ax,y,spl,bx,yf);
}
}
return;
}
int gengetcol(int typ, int j, int m, int n, int *a, int *b)
{
int r = 0, i;
switch (typ)
{
case 1:
GETCOL(integer1);
break;
case 2:
GETCOL(integer2);
break;
case 4:
GETCOL(int) ;
break;
case 11:
GETCOL(unsigned char);
break;
case 12:
GETCOL(unsigned short);
break;
case 14:
GETCOL(unsigned int);
break;
}
return r;
}
// clip and generate contacts
static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
// if we have edge/edge intersection
if ( iBestAxis > 4 ) {
dVector3 vub,vPb,vPa;
SET(vPa,vHullBoxPos);
// calculate point on box edge
for( int i=0; i<3; i++) {
dVector3 vRotCol;
GETCOL(mHullBoxRot,i,vRotCol);
dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
}
int iEdge = (iBestAxis-5)%3;
// decide which edge is on triangle
if ( iEdge == 0 ) {
SET(vPb,v0);
SET(vub,vE0);
} else if ( iEdge == 1) {
SET(vPb,v2);
SET(vub,vE1);
} else {
SET(vPb,v1);
SET(vub,vE2);
}
// setup direction parameter for face edge
dNormalize3(vub);
dReal fParam1, fParam2;
// setup direction parameter for box edge
dVector3 vua;
int col=(iBestAxis-5)/3;
GETCOL(mHullBoxRot,col,vua);
// find two closest points on both edges
_cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
vPa[0] += vua[0]*fParam1;
vPa[1] += vua[1]*fParam1;
vPa[2] += vua[2]*fParam1;
vPb[0] += vub[0]*fParam2;
vPb[1] += vub[1]*fParam2;
vPb[2] += vub[2]*fParam2;
// calculate collision point
dVector3 vPntTmp;
ADD(vPa,vPb,vPntTmp);
vPntTmp[0]*=0.5f;
vPntTmp[1]*=0.5f;
vPntTmp[2]*=0.5f;
// generate contact point between two closest points
#ifdef ORIG
if (ctContacts < (iFlags & 0x0ffff)) {
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
Contact->depth = fBestDepth;
SET(Contact->normal,vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
ctContacts++;
}
#endif
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
vPntTmp, vBestNormal, fBestDepth, ctContacts);
// if triangle is the referent face then clip box to triangle face
} else if ( iBestAxis == 1 ) {
dVector3 vNormal2;
vNormal2[0]=-vBestNormal[0];
vNormal2[1]=-vBestNormal[1];
vNormal2[2]=-vBestNormal[2];
// vNr is normal in box frame, pointing from triangle to box
dMatrix3 mTransposed;
mTransposed[0*4+0]=mHullBoxRot[0*4+0];
mTransposed[0*4+1]=mHullBoxRot[1*4+0];
mTransposed[0*4+2]=mHullBoxRot[2*4+0];
mTransposed[1*4+0]=mHullBoxRot[0*4+1];
mTransposed[1*4+1]=mHullBoxRot[1*4+1];
mTransposed[1*4+2]=mHullBoxRot[2*4+1];
mTransposed[2*4+0]=mHullBoxRot[0*4+2];
mTransposed[2*4+1]=mHullBoxRot[1*4+2];
mTransposed[2*4+2]=mHullBoxRot[2*4+2];
dVector3 vNr;
vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2];
vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2];
vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2];
dVector3 vAbsNormal;
vAbsNormal[0] = dFabs( vNr[0] );
vAbsNormal[1] = dFabs( vNr[1] );
vAbsNormal[2] = dFabs( vNr[2] );
// get closest face from box
int iB0, iB1, iB2;
if (vAbsNormal[1] > vAbsNormal[0]) {
if (vAbsNormal[1] > vAbsNormal[2]) {
iB1 = 0; iB0 = 1; iB2 = 2;
} else {
iB1 = 0; iB2 = 1; iB0 = 2;
}
} else {
if (vAbsNormal[0] > vAbsNormal[2]) {
iB0 = 0; iB1 = 1; iB2 = 2;
} else {
iB1 = 0; iB2 = 1; iB0 = 2;
}
}
// Here find center of box face we are going to project
dVector3 vCenter;
dVector3 vRotCol;
GETCOL(mHullBoxRot,iB0,vRotCol);
if (vNr[iB0] > 0) {
vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
} else {
vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
}
// Here find 4 corner points of box
dVector3 avPoints[4];
dVector3 vRotCol2;
GETCOL(mHullBoxRot,iB1,vRotCol);
GETCOL(mHullBoxRot,iB2,vRotCol2);
for(int x=0;x<3;x++) {
avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
}
// clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
dVector3 avTempArray1[9];
dVector3 avTempArray2[9];
dVector4 plPlane;
int iTempCnt1=0;
int iTempCnt2=0;
// zeroify vectors - necessary?
for(int i=0; i<9; i++) {
avTempArray1[i][0]=0;
avTempArray1[i][1]=0;
avTempArray1[i][2]=0;
avTempArray2[i][0]=0;
avTempArray2[i][1]=0;
avTempArray2[i][2]=0;
}
// Normal plane
dVector3 vTemp;
vTemp[0]=-vN[0];
vTemp[1]=-vN[1];
vTemp[2]=-vN[2];
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane );
// Plane p0
dVector3 vTemp2;
SUBTRACT(v1,v0,vTemp2);
dCROSS(vTemp,=,vN,vTemp2);
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
// Plane p1
SUBTRACT(v2,v1,vTemp2);
dCROSS(vTemp,=,vN,vTemp2);
dNormalize3(vTemp);
SUBTRACT(v0,v2,vTemp2);
CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));
_cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
// Plane p2
SUBTRACT(v0,v2,vTemp2);
dCROSS(vTemp,=,vN,vTemp2);
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
// END of clipping polygons
// for each generated contact point
for ( int i=0; i<iTempCnt2; i++ ) {
// calculate depth
dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]);
// clamp depth to zero
if (fTempDepth > 0) {
fTempDepth = 0;
}
dVector3 vPntTmp;
ADD(avTempArray2[i],v0,vPntTmp);
#ifdef ORIG
if (ctContacts < (iFlags & 0x0ffff)) {
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
Contact->depth = -fTempDepth;
SET(Contact->normal,vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
ctContacts++;
}
#endif
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
vPntTmp, vBestNormal, -fTempDepth, ctContacts);
}
//dAASSERT(ctContacts>0);
// if box face is the referent face, then clip triangle on box face
} else { // 2 <= if iBestAxis <= 4
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;
}
