From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- .../ode/src/collision_trimesh_ccylinder.cpp | 1181 -------------------- 1 file changed, 1181 deletions(-) delete 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 deleted file mode 100644 index da235e0..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp +++ /dev/null @@ -1,1181 +0,0 @@ -/************************************************************************* -* * -* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * -* All rights reserved. Email: russ@q12.org Web: www.q12.org * -* * -* This library is free software; you can redistribute it and/or * -* modify it under the terms of EITHER: * -* (1) The GNU Lesser General Public License as published by the Free * -* Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. The text of the GNU Lesser * -* General Public License is included with this library in the * -* file LICENSE.TXT. * -* (2) The BSD-style license that is included with this library in * -* the file LICENSE-BSD.TXT. * -* * -* This library is distributed in the hope that it will be useful, * -* but WITHOUT ANY WARRANTY; without even the implied warranty of * -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * -* LICENSE.TXT and LICENSE-BSD.TXT for more details. * -* * -*************************************************************************/ - -/* - * Triangle-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