From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- .../ode-0.9/ode/src/collision_trimesh_box.cpp | 1497 ++++++++++++++++++++ 1 file changed, 1497 insertions(+) create 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 new file mode 100644 index 0000000..0194d02 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp @@ -0,0 +1,1497 @@ +/************************************************************************* + * * + * 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