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; }