From f205de7847da7ae1c10212d82e7042d0100b4ce0 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:24:38 +0000 Subject: from the start... checking in ode-0.9 --- .../ode/src/collision_trimesh_ccylinder.cpp | 1181 ++++++++++++++++++++ 1 file changed, 1181 insertions(+) create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp new file mode 100644 index 0000000..da235e0 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp @@ -0,0 +1,1181 @@ +/************************************************************************* +* * +* 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-Capsule(Capsule) collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +// NOTES from Nguyen Binh +// 14 Apr : Seem to be robust +// There is a problem when you use original Step and set contact friction +// surface.mu = dInfinity; +// More description : +// When I dropped Capsule over the bunny ears, it seems to stuck +// there for a while. I think the cause is when you set surface.mu = dInfinity; +// the friction force is too high so it just hang the capsule there. +// So the good cure for this is to set mu = around 1.5 (in my case) +// For StepFast1, this become as solid as rock : StepFast1 just approximate +// friction force. + +// NOTES from Croteam's Alen +//As a side note... there are some extra contacts that can be generated +//on the edge between two triangles, and if the capsule penetrates deeply into +//the triangle (usually happens with large mass or low FPS), some such +//contacts can in some cases push the capsule away from the edge instead of +//away from the two triangles. This shows up as capsule slowing down a bit +//when hitting an edge while sliding along a flat tesselated grid of +//triangles. This is only if capsule is standing upwards. + +//Same thing can appear whenever a smooth object (e.g sphere) hits such an +//edge, and it needs to be solved as a special case probably. This is a +//problem we are looking forward to address soon. + +#include +#include +#include +#include +#include "collision_util.h" + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_ENABLED + +// OPCODE version +#if dTRIMESH_OPCODE +// largest number, double or float +#if defined(dSINGLE) +#define MAX_REAL FLT_MAX +#define MIN_REAL (-FLT_MAX) +#else +#define MAX_REAL DBL_MAX +#define MIN_REAL (-DBL_MAX) +#endif + +// To optimize before send contacts to dynamic part +#define OPTIMIZE_CONTACTS + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) \ + (r)[0]=(a)[0] - (b)[0]; \ + (r)[1]=(a)[1] - (b)[1]; \ + (r)[2]=(a)[2] - (b)[2]; + + +// dVector3 +// a=b +#define SET(a,b) \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; + + +// dMatrix3 +// a=b +#define SETM(a,b) \ + (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]; + + +// dVector3 +// r=a+b +#define ADD(a,b,r) \ + (r)[0]=(a)[0] + (b)[0]; \ + (r)[1]=(a)[1] + (b)[1]; \ + (r)[2]=(a)[2] + (b)[2]; + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) \ + (v)[0]=(m)[(a)+0]; \ + (v)[1]=(m)[(a)+4]; \ + (v)[2]=(m)[(a)+8]; + + +// 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) \ + plane[0]=normal[0];\ + plane[1]=normal[1];\ + plane[2]=normal[2];\ + plane[3]=d; + + +// dVector3 +// length of vector a +#define LENGTHOF(a) \ + dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);\ + +inline dReal _length2OfVector3(dVector3 v) +{ + return (v[0] * v[0] + v[1] * v[1] + v[2] * v[2] ); +} + + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +static sLocalContactData *gLocalContacts; +static unsigned int ctContacts = 0; + +// capsule data +// real time data +static dMatrix3 mCapsuleRotation; +static dVector3 vCapsulePosition; +static dVector3 vCapsuleAxis; +// static data +static dReal vCapsuleRadius; +static dReal fCapsuleSize; + +// mesh data +static dMatrix4 mHullDstPl; +static dMatrix3 mTriMeshRot; +static dVector3 mTriMeshPos; +static dVector3 vE0, vE1, vE2; + +// Two geom +dxGeom* gCylinder; +dxGeom* gTriMesh; + +// global collider data +static dVector3 vNormal; +static dReal fBestDepth; +static dReal fBestCenter; +static dReal fBestrt; +static int iBestAxis; +static dVector3 vN = {0,0,0,0}; + +static dVector3 vV0; +static dVector3 vV1; +static dVector3 vV2; + +// ODE contact's specific +static unsigned int iFlags; +static dContactGeom *ContactGeoms; +static int iStride; + +// Capsule lie on axis number 3 = (Z axis) +static const int nCAPSULE_AXIS = 2; + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + SUBTRACT(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + SUBTRACT(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +inline void _OptimizeLocalContacts() +{ + int nContacts = ctContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(gLocalContacts[i],gLocalContacts[j])) + { + // If they are seem to be the samed then filtered + // out the least penetrate one + if (_IsBetter(gLocalContacts[j],gLocalContacts[i])) + { + gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} + +inline int _ProcessLocalContacts() +{ + if (ctContacts == 0) + { + return 0; + } + +#ifdef OPTIMIZE_CONTACTS + if (ctContacts > 1 && !(iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + unsigned int iContact = 0; + dContactGeom* Contact = 0; + + unsigned int nFinalContact = 0; + + for (iContact = 0; iContact < ctContacts; iContact ++) + { + // Ensure that we haven't created too many contacts + if( nFinalContact >= (iFlags & NUMC_MASK)) + { + break; + } + + if (1 == gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(iFlags, ContactGeoms, nFinalContact, iStride); + Contact->depth = gLocalContacts[iContact].fDepth; + SET(Contact->normal,gLocalContacts[iContact].vNormal); + SET(Contact->pos,gLocalContacts[iContact].vPos); + Contact->g1 = gTriMesh; + Contact->g2 = gCylinder; + Contact->side2 = gLocalContacts[iContact].triIndex; + + nFinalContact++; + } + } + // debug + //if (nFinalContact != ctContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",ctContacts,ctContacts-nFinalContact); + //} + + return nFinalContact; +} + +BOOL _cldClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) +{ + // calculate distance of edge points to plane + dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 ); + dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 ); + + // if both points are behind the plane + if ( fDistance0 < 0 && fDistance1 < 0 ) + { + // do nothing + return FALSE; + // if both points in front of the plane + } else if ( fDistance0 > 0 && fDistance1 > 0 ) + { + // accept them + return TRUE; + // if we have edge/plane intersection + } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); + + // clamp correct edge to intersection point + if ( fDistance0 < 0 ) + { + SET(vEpnt0,vIntersectionPoint); + } else + { + SET(vEpnt1,vIntersectionPoint); + } + return TRUE; + } + return TRUE; +} + +static BOOL _cldTestAxis(const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + dVector3 vAxis, + int iAxis, + BOOL bNoFlip = FALSE) +{ + + // calculate length of separating axis vector + dReal fL = LENGTHOF(vAxis); + // if not long enough + // TODO : dReal epsilon please + if ( fL < REAL(1e-5) ) + { + // do nothing + //iLastOutAxis = 0; + return TRUE; + } + + // otherwise normalize it + dNormalize3(vAxis); + + // project capsule on vAxis + dReal frc = dFabs(dDOT(vCapsuleAxis,vAxis))*(fCapsuleSize*REAL(0.5)-vCapsuleRadius) + vCapsuleRadius; + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dDOT( vV0 , vAxis ); + afv[1] = dDOT( vV1 , vAxis ); + afv[2] = dDOT( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = MIN_REAL; + + // for each vertex + for(int i=0; i<3; i++) + { + // find minimum + if (afv[i]fMax) + { + fMax = afv[i]; + } + } + + // find triangle's center of interval on axis + dReal fCenter = (fMin+fMax)*REAL(0.5); + // calculate triangles half interval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if( dFabs(fCenter) > ( frc + fTriangleRadius ) ) + { + // exit, we have no intersection + return FALSE; + } + + // calculate depth + dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius); + + // if greater then best found so far + if ( fDepth > fBestDepth ) + { + // remember depth + fBestDepth = fDepth; + fBestCenter = fCenter; + fBestrt = fTriangleRadius; + + vNormal[0] = vAxis[0]; + vNormal[1] = vAxis[1]; + vNormal[2] = vAxis[2]; + + iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if (fCenter<0 && !bNoFlip) + { + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + + fBestCenter = -fCenter; + } + } + + return TRUE; +} + +// helper for less key strokes +inline void _CalculateAxis(const dVector3& v1, + const dVector3& v2, + const dVector3& v3, + const dVector3& v4, + dVector3& r) +{ + dVector3 t1; + dVector3 t2; + + SUBTRACT(v1,v2,t1); + dCROSS(t2,=,t1,v3); + dCROSS(r,=,t2,v4); +} + +static BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + uint8 flags) +{ + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = vCapsulePosition[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp0[1] = vCapsulePosition[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp0[2] = vCapsulePosition[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + dVector3 vCp1; + vCp1[0] = vCapsulePosition[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp1[1] = vCapsulePosition[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp1[2] = vCapsulePosition[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + // reset best axis + iBestAxis = 0; + // reset best depth + fBestDepth = -MAX_REAL; + // reset separating axis vector + dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + + // Epsilon value for checking axis vector length + const dReal fEpsilon = 1e-6f; + + // Translate triangle to Cc cord. + SUBTRACT(v0 , vCapsulePosition, vV0); + SUBTRACT(v1 , vCapsulePosition, vV1); + SUBTRACT(v2 , vCapsulePosition, vV2); + + // We begin to test for 19 separating axis now + // I wonder does it help if we employ the method like ISA-GJK??? + // Or at least we should do experiment and find what axis will + // be most likely to be separating axis to check it first. + + // Original + // axis vN + //vAxis = -vN; + vAxis[0] = - vN[0]; + vAxis[1] = - vN[1]; + vAxis[2] = - vN[2]; + if (!_cldTestAxis( v0, v1, v2, vAxis, 1, TRUE)) + { + return FALSE; + } + + if (flags & dxTriMeshData::kEdge0) + { + // axis CxE0 - Edge 0 + dCROSS(vAxis,=,vCapsuleAxis,vE0); + //vAxis = dCROSS( vCapsuleAxis cross vE0 ); + if( _length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 2)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge1) + { + // axis CxE1 - Edge 1 + dCROSS(vAxis,=,vCapsuleAxis,vE1); + //vAxis = ( vCapsuleAxis cross vE1 ); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 3)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge2) + { + // axis CxE2 - Edge 2 + //vAxis = ( vCapsuleAxis cross vE2 ); + dCROSS(vAxis,=,vCapsuleAxis,vE2); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 4)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge0) + { + // first capsule point + // axis ((Cp0-V0) x E0) x E0 + _CalculateAxis(vCp0,v0,vE0,vE0,vAxis); + // vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 5)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge1) + { + // axis ((Cp0-V1) x E1) x E1 + _CalculateAxis(vCp0,v1,vE1,vE1,vAxis); + //vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 6)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge2) + { + // axis ((Cp0-V2) x E2) x E2 + _CalculateAxis(vCp0,v2,vE2,vE2,vAxis); + //vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 7)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge0) + { + // second capsule point + // axis ((Cp1-V0) x E0) x E0 + _CalculateAxis(vCp1,v0,vE0,vE0,vAxis); + //vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 8)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge1) + { + // axis ((Cp1-V1) x E1) x E1 + _CalculateAxis(vCp1,v1,vE1,vE1,vAxis); + //vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 9)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge2) + { + // axis ((Cp1-V2) x E2) x E2 + _CalculateAxis(vCp1,v2,vE2,vE2,vAxis); + //vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 10)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert0) + { + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + _CalculateAxis(v0,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v0-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 11)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert1) + { + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + _CalculateAxis(v1,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 12)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert2) + { + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + _CalculateAxis(v2,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 13)) { + return FALSE; + } + } + } + + // Test as separating axes direction vectors between each triangle + // edge and each capsule's cap center + + if (flags & dxTriMeshData::kVert0) + { + // first triangle vertex and first capsule point + //vAxis = v0 - vCp0; + SUBTRACT(v0,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 14)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert1) + { + // second triangle vertex and first capsule point + //vAxis = v1 - vCp0; + SUBTRACT(v1,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 15)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert2) + { + // third triangle vertex and first capsule point + //vAxis = v2 - vCp0; + SUBTRACT(v2,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 16)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert0) + { + // first triangle vertex and second capsule point + //vAxis = v0 - vCp1; + SUBTRACT(v0,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 17)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert1) + { + // second triangle vertex and second capsule point + //vAxis = v1 - vCp1; + SUBTRACT(v1,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 18)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert2) + { + // third triangle vertex and second capsule point + //vAxis = v2 - vCp1; + SUBTRACT(v2,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 19)) { + return FALSE; + } + } + } + + return TRUE; +} + +// test one mesh triangle on intersection with capsule +static void _cldTestOneTriangleVSCapsule( const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + uint8 flags) +{ + + // calculate edges + SUBTRACT(v1,v0,vE0); + SUBTRACT(v2,v1,vE1); + SUBTRACT(v0,v2,vE2); + + dVector3 _minus_vE0; + SUBTRACT(v0,v1,_minus_vE0); + + // calculate poly normal + dCROSS(vN,=,vE1,_minus_vE0); + dNormalize3(vN); + + // create plane from triangle + dReal plDistance = -dDOT(v0,vN); + dVector4 plTrianglePlane; + CONSTRUCTPLANE(plTrianglePlane,vN,plDistance); + + // calculate capsule distance to plane + dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,vCapsulePosition); + + // Capsule must be over positive side of triangle + if(fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + SET (vPnt0,v0); + dVector3 vPnt1; + SET (vPnt1,v1); + dVector3 vPnt2; + SET (vPnt2,v2); + + if (fDistanceCapsuleCenterToPlane < 0 ) + { + SET (vPnt0,v0); + SET (vPnt1,v2); + SET (vPnt2,v1); + } + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags) ) + { + // 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) + dIASSERT(FALSE); + // do nothing + return; + } + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = vCapsulePosition[0] + vNormal[0]*vCapsuleRadius; + vCposTrans[1] = vCapsulePosition[1] + vNormal[1]*vCapsuleRadius; + vCposTrans[2] = vCapsulePosition[2] + vNormal[2]*vCapsuleRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint0[1] = vCposTrans[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint0[2] = vCposTrans[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint1[1] = vCposTrans[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint1[2] = vCposTrans[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + // transform capsule edge points into triangle space + vCEdgePoint0[0] -= vPnt0[0]; + vCEdgePoint0[1] -= vPnt0[1]; + vCEdgePoint0[2] -= vPnt0[2]; + + vCEdgePoint1[0] -= vPnt0[0]; + vCEdgePoint1[1] -= vPnt0[1]; + vCEdgePoint1[2] -= vPnt0[2]; + + dVector4 plPlane; + dVector3 _minus_vN; + _minus_vN[0] = -vN[0]; + _minus_vN[1] = -vN[1]; + _minus_vN[2] = -vN[2]; + // triangle plane + CONSTRUCTPLANE(plPlane,_minus_vN,0); + //plPlane = Plane4f( -vN, 0); + + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + // plane with edge 0 + dVector3 vTemp; + dCROSS(vTemp,=,vN,vE0); + CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCROSS(vTemp,=,vN,vE1); + CONSTRUCTPLANE(plPlane, vTemp, -(dDOT(vE0,vTemp)-REAL(1e-5))); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCROSS(vTemp,=,vN,vE2); + CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { + return; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += vPnt0[0]; + vCEdgePoint0[1] += vPnt0[1]; + vCEdgePoint0[2] += vPnt0[2]; + + vCEdgePoint1[0] += vPnt0[0]; + vCEdgePoint1[1] += vPnt0[1]; + vCEdgePoint1[2] += vPnt0[2]; + + // calculate depths for both contact points + SUBTRACT(vCEdgePoint0,vCapsulePosition,vTemp); + dReal fDepth0 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt); + SUBTRACT(vCEdgePoint1,vCapsulePosition,vTemp); + dReal fDepth1 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt); + + // clamp depths to zero + if(fDepth0 < 0) + { + fDepth0 = 0.0f; + } + + if(fDepth1 < 0 ) + { + fDepth1 = 0.0f; + } + + // Cached contacts's data + // contact 0 + dIASSERT(ctContacts < (iFlags & NUMC_MASK)); // Do not call function if there is no room to store result + gLocalContacts[ctContacts].fDepth = fDepth0; + SET(gLocalContacts[ctContacts].vNormal,vNormal); + SET(gLocalContacts[ctContacts].vPos,vCEdgePoint0); + gLocalContacts[ctContacts].nFlags = 1; + ctContacts++; + + if (ctContacts < (iFlags & NUMC_MASK)) { + // contact 1 + gLocalContacts[ctContacts].fDepth = fDepth1; + SET(gLocalContacts[ctContacts].vNormal,vNormal); + SET(gLocalContacts[ctContacts].vPos,vCEdgePoint1); + gLocalContacts[ctContacts].nFlags = 1; + ctContacts++; + } + +} + +// capsule - trimesh by CroTeam +// Ported by Nguyem Binh +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)o1; + gCylinder = o2; + gTriMesh = o1; + + const dMatrix3* pRot = (const dMatrix3*) dGeomGetRotation(gCylinder); + memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3)); + + const dVector3* pDst = (const dVector3*)dGeomGetPosition(gCylinder); + memcpy(vCapsulePosition,pDst,sizeof(dVector3)); + + vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + + // Get size of Capsule + dGeomCapsuleGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize); + fCapsuleSize += 2*vCapsuleRadius; + + const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh); + memcpy(mTriMeshRot,pTriRot,sizeof(dMatrix3)); + + const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh); + memcpy(mTriMeshPos,pTriPos,sizeof(dVector3)); + + // global info for contact creation + iStride =skip; + iFlags =flags; + ContactGeoms =contact; + + // reset contact counter + ctContacts = 0; + + // reset best depth + fBestDepth = - MAX_REAL; + fBestCenter = 0; + fBestrt = 0; + + + + + // reset collision normal + vNormal[0] = REAL(0.0); + vNormal[1] = REAL(0.0); + vNormal[2] = REAL(0.0); + + // Will it better to use LSS here? -> confirm Pierre. + OBBCollider& Collider = TriMesh->_OBBCollider; + + // 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. + + Point cCenter(/*(float)*/ vCapsulePosition[0], /*(float)*/ vCapsulePosition[1], /*(float)*/ vCapsulePosition[2]); + Point cExtents(/*(float)*/ vCapsuleRadius, /*(float)*/ vCapsuleRadius,/*(float)*/ fCapsuleSize/2); + + Matrix3x3 obbRot; + + obbRot[0][0] = /*(float)*/ mCapsuleRotation[0]; + obbRot[1][0] = /*(float)*/ mCapsuleRotation[1]; + obbRot[2][0] = /*(float)*/ mCapsuleRotation[2]; + + obbRot[0][1] = /*(float)*/ mCapsuleRotation[4]; + obbRot[1][1] = /*(float)*/ mCapsuleRotation[5]; + obbRot[2][1] = /*(float)*/ mCapsuleRotation[6]; + + obbRot[0][2] = /*(float)*/ mCapsuleRotation[8]; + obbRot[1][2] = /*(float)*/ mCapsuleRotation[9]; + obbRot[2][2] = /*(float)*/ mCapsuleRotation[10]; + + OBB obbCapsule(cCenter,cExtents,obbRot); + + Matrix4x4 CapsuleMatrix; + MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix); + + Matrix4x4 MeshMatrix; + MakeMatrix(mTriMeshPos, mTriMeshRot, MeshMatrix); + + // TC results + if (TriMesh->doBoxTC) { + dxTriMesh::BoxTC* BoxTC = 0; + for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){ + if (TriMesh->BoxTCCache[i].Geom == gCylinder){ + BoxTC = &TriMesh->BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1]; + BoxTC->Geom = gCylinder; + BoxTC->FatCoeff = 1.0f; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix); + } + + 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, gCylinder, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(iFlags & NUMC_MASK)); + + unsigned int ctContacts0 = ctContacts; + + uint8* UseFlags = TriMesh->Data->UseFlags; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!Callback(TriMesh, gCylinder, Triint)) continue; + + + dVector3 dv[3]; + FetchTriangle(TriMesh, Triint, mTriMeshPos, mTriMeshRot, dv); + + uint8 flags = UseFlags ? UseFlags[Triint] : dxTriMeshData::kUseAll; + + // test this triangle + _cldTestOneTriangleVSCapsule(dv[0],dv[1],dv[2], flags); + + // fill-in tri index for generated contacts + for (; ctContacts0=(iFlags & NUMC_MASK)) + { + break; + } + + } + } + + return _ProcessLocalContacts(); +} +#endif + +// GIMPACT version +#if dTRIMESH_GIMPACT +#define nCAPSULE_AXIS 2 +// capsule - trimesh By francisco leon +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)o1; + dxGeom* gCylinder = o2; + + //Get capsule params + dMatrix3 mCapsuleRotation; + dVector3 vCapsulePosition; + dVector3 vCapsuleAxis; + dReal vCapsuleRadius; + dReal fCapsuleSize; + dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder); + memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3)); + dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder); + memcpy(vCapsulePosition,pDst,sizeof(dVector3)); + //Axis + vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + // Get size of CCylinder + dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize); + fCapsuleSize*=0.5f; + //Set Capsule params + GIM_CAPSULE_DATA capsule; + + capsule.m_radius = vCapsuleRadius; + VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis); + VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1); + VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis); + VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2); + + +//Create contact list + GDYNAMIC_ARRAY trimeshcontacts; + GIM_CREATE_CONTACT_LIST(trimeshcontacts); + + //Collide trimeshe vs capsule + gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts); + + + if(trimeshcontacts.m_size == 0) + { + GIM_DYNARRAY_DESTROY(trimeshcontacts); + return 0; + } + + GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts); + + unsigned contactcount = trimeshcontacts.m_size; + unsigned contactmax = (unsigned)(flags & NUMC_MASK); + if (contactcount > contactmax) + { + contactcount = contactmax; + } + + dContactGeom* pcontact; + unsigned i; + + for (i=0;ipos[0] = ptrimeshcontacts->m_point[0]; + pcontact->pos[1] = ptrimeshcontacts->m_point[1]; + pcontact->pos[2] = ptrimeshcontacts->m_point[2]; + pcontact->pos[3] = 1.0f; + + pcontact->normal[0] = ptrimeshcontacts->m_normal[0]; + pcontact->normal[1] = ptrimeshcontacts->m_normal[1]; + pcontact->normal[2] = ptrimeshcontacts->m_normal[2]; + pcontact->normal[3] = 0; + + pcontact->depth = ptrimeshcontacts->m_depth; + pcontact->g1 = TriMesh; + pcontact->g2 = gCylinder; + + ptrimeshcontacts++; + } + + GIM_DYNARRAY_DESTROY(trimeshcontacts); + + return (int)contactcount; +} +#endif + +#endif // dTRIMESH_ENABLED -- cgit v1.1