/************************************************************************* * * * 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