From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- .../ode-0.9/ode/src/collision_trimesh_box.cpp | 1497 -------------------- 1 file changed, 1497 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_box.cpp (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_box.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp deleted file mode 100644 index 0194d02..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp +++ /dev/null @@ -1,1497 +0,0 @@ -/************************************************************************* - * * - * 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) { - dIASSERT( !(iFlags & CONTACTS_UNIMPORTANT) || ctContacts < (iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results - - // 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 -#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else - 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); - -#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else - 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); - - if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { - break; - } - } - - //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); - -#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else - 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); - - if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { - break; - } - } - - //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){ - dIASSERT (Stride >= (int)sizeof(dContactGeom)); - dIASSERT (g1->type == dTriMeshClass); - dIASSERT (BoxGeom->type == dBoxClass); - dIASSERT ((Flags & NUMC_MASK) >= 1); - - - 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]; - - // It is a potential issue to explicitly cast to float - // if custom width floating point type is introduced in OPCODE. - // It is necessary to make a typedef and cast to it - // (e.g. typedef float opc_float;) - // However I'm not sure in what header it should be added. - - 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 = 0; - - // 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; - - /* - NOTE by Oleh_Derevenko: - The function continues checking triangles after maximal number - of contacts is reached because it selects maximal penetration depths. - See also comments in GenerateContact() - */ - // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" - if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) - break; - } - } - - - 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) -{ - dIASSERT (Stride >= (int)sizeof(dContactGeom)); - dIASSERT (g1->type == dTriMeshClass); - dIASSERT (BoxGeom->type == dBoxClass); - dIASSERT ((Flags & NUMC_MASK) >= 1); - - - 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); - - int ctContacts0 = 0; - - for(unsigned int i=0;iside1 = Triint; - - /* - NOTE by Oleh_Derevenko: - The function continues checking triangles after maximal number - of contacts is reached because it selects maximal penetration depths. - See also comments in GenerateContact() - */ - // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" - if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) - break; - } - - gim_trimesh_unlocks_work_data(ptrimesh); - GIM_DYNARRAY_DESTROY(collision_result); - - return ctContacts; -} -#endif - - -// GenerateContact - Written by Jeff Smith (jeff@burri.to) -// Generate a "unique" contact. A unique contact has a unique -// position or normal. If the potential contact has the same -// position and normal as an existing contact, but a larger -// penetration depth, this new depth is used instead -// -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) -{ - /* - NOTE by Oleh_Derevenko: - This function is called after maximal number of contacts has already been - collected because it has a side effect of replacing penetration depth of - existing contact with larger penetration depth of another matching normal contact. - If this logic is not necessary any more, you can bail out on reach of contact - number maximum immediately in dCollideBTL(). You will also need to correct - conditional statements after invocations of GenerateContact() in _cldClipping(). - */ - do - { - dContactGeom* Contact; - dVector3 diff; - - if (!(in_Flags & CONTACTS_UNIMPORTANT)) - { - bool duplicate = false; - for (int i=0; ipos[j]; - if (dDOT(diff, diff) < dEpsilon) - { - // same normal? - if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon)) - { - if (in_Depth > Contact->depth) - Contact->depth = in_Depth; - duplicate = true; - /* - NOTE by Oleh_Derevenko: - There may be a case when two normals are close to each other but not duplicate - while third normal is detected to be duplicate for both of them. - This is the only reason I can think of, there is no "break" statement. - Perhaps author considered it to be logical that the third normal would - replace the depth in both of initial contacts. - However, I consider it a questionable practice which should not - be applied without deep understanding of underlaying physics. - Even more, is this situation with close normal triplet acceptable at all? - Should not be two initial contacts reduced to one (replaced with the latter)? - If you know the answers for these questions, you may want to change this code. - See the same statement in GenerateContact() of collision_trimesh_trimesh.cpp - */ - } - } - } - if (duplicate || OutTriCount == (in_Flags & NUMC_MASK)) - { - break; - } - } - else - { - dIASSERT(OutTriCount < (in_Flags & NUMC_MASK)); - } - - // 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++; - } - while (false); -} - -#endif // dTRIMESH_ENABLED -- cgit v1.1