/************************************************************************* * * * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * * All rights reserved. Email: russ@q12.org Web: www.q12.org * * * * This library is free software; you can redistribute it and/or * * modify it under the terms of EITHER: * * (1) The GNU Lesser General Public License as published by the Free * * Software Foundation; either version 2.1 of the License, or (at * * your option) any later version. The text of the GNU Lesser * * General Public License is included with this library in the * * file LICENSE.TXT. * * (2) The BSD-style license that is included with this library in * * the file LICENSE-BSD.TXT. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * * LICENSE.TXT and LICENSE-BSD.TXT for more details. * * * *************************************************************************/ /************************************************************************* * * * Triangle-box collider by Alen Ladavac and Vedran Klanac. * * Ported to ODE by Oskari Nyman. * * * *************************************************************************/ #include #include #include #include #include "collision_util.h" #define TRIMESH_INTERNAL #include "collision_trimesh_internal.h" #if dTRIMESH_ENABLED static void GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, dxGeom* in_g1, dxGeom* in_g2, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, int& OutTriCount); // largest number, double or float #if defined(dSINGLE) #define MAXVALUE FLT_MAX #else #define MAXVALUE DBL_MAX #endif // dVector3 // r=a-b #define SUBTRACT(a,b,r) do{ \ (r)[0]=(a)[0] - (b)[0]; \ (r)[1]=(a)[1] - (b)[1]; \ (r)[2]=(a)[2] - (b)[2]; }while(0) // dVector3 // a=b #define SET(a,b) do{ \ (a)[0]=(b)[0]; \ (a)[1]=(b)[1]; \ (a)[2]=(b)[2]; }while(0) // dMatrix3 // a=b #define SETM(a,b) do{ \ (a)[0]=(b)[0]; \ (a)[1]=(b)[1]; \ (a)[2]=(b)[2]; \ (a)[3]=(b)[3]; \ (a)[4]=(b)[4]; \ (a)[5]=(b)[5]; \ (a)[6]=(b)[6]; \ (a)[7]=(b)[7]; \ (a)[8]=(b)[8]; \ (a)[9]=(b)[9]; \ (a)[10]=(b)[10]; \ (a)[11]=(b)[11]; }while(0) // dVector3 // r=a+b #define ADD(a,b,r) do{ \ (r)[0]=(a)[0] + (b)[0]; \ (r)[1]=(a)[1] + (b)[1]; \ (r)[2]=(a)[2] + (b)[2]; }while(0) // dMatrix3, int, dVector3 // v=column a from m #define GETCOL(m,a,v) do{ \ (v)[0]=(m)[(a)+0]; \ (v)[1]=(m)[(a)+4]; \ (v)[2]=(m)[(a)+8]; }while(0) // dVector4, dVector3 // distance between plane p and point v #define POINTDISTANCE(p,v) \ ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ) // dVector4, dVector3, dReal // construct plane from normal and d #define CONSTRUCTPLANE(plane,normal,d) do{ \ plane[0]=normal[0];\ plane[1]=normal[1];\ plane[2]=normal[2];\ plane[3]=d; }while(0) // dVector3 // length of vector a #define LENGTHOF(a) \ dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]) // box data static dMatrix3 mHullBoxRot; static dVector3 vHullBoxPos; static dVector3 vBoxHalfSize; // mesh data static dVector3 vHullDstPos; // global collider data static dVector3 vBestNormal; static dReal fBestDepth; static int iBestAxis = 0; static int iExitAxis = 0; static dVector3 vE0, vE1, vE2, vN; // global info for contact creation static int iFlags; static dContactGeom *ContactGeoms; static int iStride; static dxGeom *Geom1; static dxGeom *Geom2; static int ctContacts = 0; // Test normal of mesh face as separating axis for intersection static bool _cldTestNormal( dReal fp0, dReal fR, dVector3 vNormal, int iAxis ) { // calculate overlapping interval of box and triangle dReal fDepth = fR+fp0; // if we do not overlap if ( fDepth<0 ) { // do nothing return false; } // calculate normal's length dReal fLength = LENGTHOF(vNormal); // if long enough if ( fLength > 0.0f ) { dReal fOneOverLength = 1.0f/fLength; // normalize depth fDepth = fDepth*fOneOverLength; // get minimum depth if (fDepth=0); fBestDepth = fDepth; } } return true; } // Test box axis as separating axis static bool _cldTestFace( dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD, dVector3 vNormal, int iAxis ) { dReal fMin, fMax; // find min of triangle interval if ( fp0 < fp1 ) { if ( fp0 < fp2 ) { fMin = fp0; } else { fMin = fp2; } } else { if( fp1 < fp2 ) { fMin = fp1; } else { fMin = fp2; } } // find max of triangle interval if ( fp0 > fp1 ) { if ( fp0 > fp2 ) { fMax = fp0; } else { fMax = fp2; } } else { if( fp1 > fp2 ) { fMax = fp1; } else { fMax = fp2; } } // calculate minimum and maximum depth dReal fDepthMin = fR - fMin; dReal fDepthMax = fMax + fR; // if we dont't have overlapping interval if ( fDepthMin < 0 || fDepthMax < 0 ) { // do nothing return false; } dReal fDepth = 0; // if greater depth is on negative side if ( fDepthMin > fDepthMax ) { // use smaller depth (one from positive side) fDepth = fDepthMax; // flip normal direction vNormal[0] = -vNormal[0]; vNormal[1] = -vNormal[1]; vNormal[2] = -vNormal[2]; fD = -fD; // if greater depth is on positive side } else { // use smaller depth (one from negative side) fDepth = fDepthMin; } // if lower depth than best found so far if (fDepth=0); fBestDepth = fDepth; } return true; } // Test cross products of box axis and triangle edges as separating axis static bool _cldTestEdge( dReal fp0, dReal fp1, dReal fR, dReal fD, dVector3 vNormal, int iAxis ) { dReal fMin, fMax; // ===== Begin Patch by Francisco Leon, 2006/10/28 ===== // Fixed Null Normal. This prevents boxes passing // through trimeshes at certain contact angles fMin = vNormal[0] * vNormal[0] + vNormal[1] * vNormal[1] + vNormal[2] * vNormal[2]; if ( fMin <= dEpsilon ) /// THIS NORMAL WOULD BE DANGEROUS return true; // ===== Ending Patch by Francisco Leon ===== // calculate min and max interval values if ( fp0 < fp1 ) { fMin = fp0; fMax = fp1; } else { fMin = fp1; fMax = fp0; } // check if we overlapp dReal fDepthMin = fR - fMin; dReal fDepthMax = fMax + fR; // if we don't overlapp if ( fDepthMin < 0 || fDepthMax < 0 ) { // do nothing return false; } dReal fDepth; // if greater depth is on negative side if ( fDepthMin > fDepthMax ) { // use smaller depth (one from positive side) fDepth = fDepthMax; // flip normal direction vNormal[0] = -vNormal[0]; vNormal[1] = -vNormal[1]; vNormal[2] = -vNormal[2]; fD = -fD; // if greater depth is on positive side } else { // use smaller depth (one from negative side) fDepth = fDepthMin; } // calculate normal's length dReal fLength = LENGTHOF(vNormal); // if long enough if ( fLength > 0.0f ) { // normalize depth dReal fOneOverLength = 1.0f/fLength; fDepth = fDepth*fOneOverLength; fD*=fOneOverLength; // if lower depth than best found so far (favor face over edges) if (fDepth*1.5f=0); fBestDepth = fDepth; } } return true; } // clip polygon with plane and generate new polygon points static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ) { // start with no output points ctOut = 0; int i0 = ctIn-1; // for each edge in input polygon for (int i1=0; i1= 0 ) { // emit point avArrayOut[ctOut][0] = avArrayIn[i0][0]; avArrayOut[ctOut][1] = avArrayIn[i0][1]; avArrayOut[ctOut][2] = avArrayIn[i0][2]; ctOut++; } // if points are on different sides if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) { // find intersection point of edge and plane dVector3 vIntersectionPoint; vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); // emit intersection point avArrayOut[ctOut][0] = vIntersectionPoint[0]; avArrayOut[ctOut][1] = vIntersectionPoint[1]; avArrayOut[ctOut][2] = vIntersectionPoint[2]; ctOut++; } } } 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; } // find two closest points on two lines static bool _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1, dVector3 vPoint2, dVector3 vLenVec2, dReal &fvalue1, dReal &fvalue2) { // calulate denominator dVector3 vp; SUBTRACT(vPoint2,vPoint1,vp); dReal fuaub = dDOT(vLenVec1,vLenVec2); dReal fq1 = dDOT(vLenVec1,vp); dReal fq2 = -dDOT(vLenVec2,vp); dReal fd = 1.0f - fuaub * fuaub; // if denominator is positive if (fd > 0.0f) { // calculate points of closest approach fd = 1.0f/fd; fvalue1 = (fq1 + fuaub*fq2)*fd; fvalue2 = (fuaub*fq1 + fq2)*fd; return true; // otherwise } else { // lines are parallel fvalue1 = 0.0f; fvalue2 = 0.0f; return false; } } // 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 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 // get normal of box face dVector3 vNormal2; SET(vNormal2,vBestNormal); // get indices of box axes in correct order int iA0,iA1,iA2; iA0 = iBestAxis-2; if ( iA0 == 0 ) { iA1 = 1; iA2 = 2; } else if ( iA0 == 1 ) { iA1 = 0; iA2 = 2; } else { iA1 = 0; iA2 = 1; } dVector3 avPoints[3]; // calculate triangle vertices in box frame SUBTRACT(v0,vHullBoxPos,avPoints[0]); SUBTRACT(v1,vHullBoxPos,avPoints[1]); SUBTRACT(v2,vHullBoxPos,avPoints[2]); // CLIP Polygons // define temp data for clipping dVector3 avTempArray1[9]; dVector3 avTempArray2[9]; int iTempCnt1, iTempCnt2; // 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; } // clip triangle with 5 box planes (1 face plane, 4 edge planes) dVector4 plPlane; // Normal plane dVector3 vTemp; vTemp[0]=-vNormal2[0]; vTemp[1]=-vNormal2[1]; vTemp[2]=-vNormal2[2]; CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA0]); _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane ); // Plane p0 GETCOL(mHullBoxRot,iA1,vTemp); CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // Plane p1 GETCOL(mHullBoxRot,iA1,vTemp); vTemp[0]=-vTemp[0]; vTemp[1]=-vTemp[1]; vTemp[2]=-vTemp[2]; CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]); _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); // Plane p2 GETCOL(mHullBoxRot,iA2,vTemp); CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // Plane p3 GETCOL(mHullBoxRot,iA2,vTemp); vTemp[0]=-vTemp[0]; vTemp[1]=-vTemp[1]; vTemp[2]=-vTemp[2]; CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]); _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); // for each generated contact point for ( int i=0; i 0) { fTempDepth = 0; } // generate contact data dVector3 vPntTmp; ADD(avTempArray1[i],vHullBoxPos,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); } } // test one mesh triangle on intersection with given box static void _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)//, void *pvUser) { // do intersection test and find best separating axis if(!_cldTestSeparatingAxes(v0, v1, v2) ) { // if not found do nothing return; } // if best separation axis is not found if ( iBestAxis == 0 ) { // this should not happen (we should already exit in that case) //dMessage (0, "best separation axis not found"); // do nothing return; } _cldClipping(v0, v1, v2); } // OPCODE version of box to mesh collider #if dTRIMESH_OPCODE int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){ dxTriMesh* TriMesh = (dxTriMesh*)g1; // get source hull position, orientation and half size const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom); const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom); // to global SETM(mHullBoxRot,mRotBox); SET(vHullBoxPos,vPosBox); dGeomBoxGetLengths(BoxGeom, vBoxHalfSize); vBoxHalfSize[0] *= 0.5f; vBoxHalfSize[1] *= 0.5f; vBoxHalfSize[2] *= 0.5f; // get destination hull position and orientation const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh); const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); // to global SET(vHullDstPos,vPosMesh); // global info for contact creation ctContacts = 0; iStride=Stride; iFlags=Flags; ContactGeoms=Contacts; Geom1=TriMesh; Geom2=BoxGeom; // reset stuff fBestDepth = MAXVALUE; vBestNormal[0]=0; vBestNormal[1]=0; vBestNormal[2]=0; OBBCollider& Collider = TriMesh->_OBBCollider; // Make OBB OBB Box; Box.mCenter.x = vPosBox[0]; Box.mCenter.y = vPosBox[1]; Box.mCenter.z = vPosBox[2]; Box.mExtents.x = (float)vBoxHalfSize[0]; Box.mExtents.y = (float)vBoxHalfSize[1]; Box.mExtents.z = (float)vBoxHalfSize[2]; Box.mRot.m[0][0] = (float)mRotBox[0]; Box.mRot.m[1][0] = (float)mRotBox[1]; Box.mRot.m[2][0] = (float)mRotBox[2]; Box.mRot.m[0][1] = (float)mRotBox[4]; Box.mRot.m[1][1] = (float)mRotBox[5]; Box.mRot.m[2][1] = (float)mRotBox[6]; Box.mRot.m[0][2] = (float)mRotBox[8]; Box.mRot.m[1][2] = (float)mRotBox[9]; Box.mRot.m[2][2] = (float)mRotBox[10]; Matrix4x4 amatrix; Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix); Matrix4x4 InvBoxMatrix; InvertPRMatrix(InvBoxMatrix, BoxMatrix); // TC results if (TriMesh->doBoxTC) { dxTriMesh::BoxTC* BoxTC = 0; for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){ if (TriMesh->BoxTCCache[i].Geom == BoxGeom){ BoxTC = &TriMesh->BoxTCCache[i]; break; } } if (!BoxTC){ TriMesh->BoxTCCache.push(dxTriMesh::BoxTC()); BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1]; BoxTC->Geom = BoxGeom; BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0 } // Intersect Collider.SetTemporalCoherence(true); Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix)); } else { Collider.SetTemporalCoherence(false); Collider.Collide(dxTriMesh::defaultBoxCache, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix)); } if (! Collider.GetContactStatus()) { // no collision occurred return 0; } // Retrieve data int TriCount = Collider.GetNbTouchedPrimitives(); const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); if (TriCount != 0){ if (TriMesh->ArrayCallback != null){ TriMesh->ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount); } int ctContacts0 = ctContacts; // loop through all intersecting triangles for (int i = 0; i < TriCount; i++){ const int& Triint = Triangles[i]; if (!Callback(TriMesh, BoxGeom, Triint)) continue; dVector3 dv[3]; FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv); // test this triangle _cldTestOneTriangle(dv[0],dv[1],dv[2]); // fill-in tri index for generated contacts for (; ctContacts0side1 = Triint; } } return ctContacts; } #endif // GIMPACT version of box to mesh collider #if dTRIMESH_GIMPACT int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride) { dxTriMesh* TriMesh = (dxTriMesh*)g1; // get source hull position, orientation and half size const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom); const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom); // to global SETM(mHullBoxRot,mRotBox); SET(vHullBoxPos,vPosBox); dGeomBoxGetLengths(BoxGeom, vBoxHalfSize); vBoxHalfSize[0] *= 0.5f; vBoxHalfSize[1] *= 0.5f; vBoxHalfSize[2] *= 0.5f; // get destination hull position and orientation /*const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh); const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); // to global SET(vHullDstPos,vPosMesh);*/ // global info for contact creation ctContacts = 0; iStride=Stride; iFlags=Flags; ContactGeoms=Contacts; Geom1=TriMesh; Geom2=BoxGeom; // reset stuff fBestDepth = MAXVALUE; vBestNormal[0]=0; vBestNormal[1]=0; vBestNormal[2]=0; //*****at first , collide box aabb******// GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh; aabb3f test_aabb; test_aabb.minX = BoxGeom->aabb[0]; test_aabb.maxX = BoxGeom->aabb[1]; test_aabb.minY = BoxGeom->aabb[2]; test_aabb.maxY = BoxGeom->aabb[3]; test_aabb.minZ = BoxGeom->aabb[4]; test_aabb.maxZ = BoxGeom->aabb[5]; GDYNAMIC_ARRAY collision_result; GIM_CREATE_BOXQUERY_LIST(collision_result); gim_aabbset_box_collision(&test_aabb, &ptrimesh->m_aabbset , &collision_result); if(collision_result.m_size==0) { GIM_DYNARRAY_DESTROY(collision_result); return 0; } //*****Set globals for box collision******// //collide triangles GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result); gim_trimesh_locks_work_data(ptrimesh); for(unsigned int i=0;ipos[j]; if (dDOT(diff, diff) < dEpsilon) { // same normal? if (fabs(dDOT(in_Normal, Contact->normal)) > (dReal(1.0)-dEpsilon)) { if (in_Depth > Contact->depth) Contact->depth = in_Depth; duplicate = true; } } } if (!duplicate) { // Add a new contact Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride); Contact->pos[0] = in_ContactPos[0]; Contact->pos[1] = in_ContactPos[1]; Contact->pos[2] = in_ContactPos[2]; Contact->pos[3] = 0.0; Contact->normal[0] = in_Normal[0]; Contact->normal[1] = in_Normal[1]; Contact->normal[2] = in_Normal[2]; Contact->normal[3] = 0.0; Contact->depth = in_Depth; Contact->g1 = in_g1; Contact->g2 = in_g2; OutTriCount++; } } #endif // dTRIMESH_ENABLED