diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp | 1183 |
1 files changed, 1183 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp new file mode 100644 index 0000000..6681cc6 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp @@ -0,0 +1,1183 @@ +/************************************************************************* + * * + * 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 <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_util.h" +#include "collision_trimesh_internal.h" +#include "util.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 1 + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) dSubtractVectors3(r, a, b) + + +// dVector3 +// a=b +#define SET(a,b) dCopyVector3(a, b) + + +// dMatrix3 +// a=b +#define SETM(a,b) dCopyMatrix4x4(a, b) + + +// dVector3 +// r=a+b +#define ADD(a,b,r) dAddVectors3(r, a, b) + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) dGetMatrixColumn3(v, m, a) + + +// dVector4, dVector3 +// distance between plane p and point v +#define POINTDISTANCE(p,v) dPointPlaneDistance(v, p) + + +// dVector4, dVector3, dReal +// construct plane from normal and d +#define CONSTRUCTPLANE(plane,normal,d) dConstructPlane(normal, d, plane) + + +// dVector3 +// length of vector a +#define LENGTHOF(a) dCalcVectorLength3(a) + + +static inline dReal _length2OfVector3(dVector3 v) +{ + return dCalcVectorLengthSquare3(v); +} + + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +struct sTrimeshCapsuleColliderData +{ + sTrimeshCapsuleColliderData(): m_gLocalContacts(NULL), m_ctContacts(0) { memset(m_vN, 0, sizeof(dVector3)); } + + void SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Capsule, int flags, int skip); + int TestCollisionForSingleTriangle(int ctContacts0, int Triint, dVector3 dv[3], + uint8 flags, bool &bOutFinishSearching); + +#if OPTIMIZE_CONTACTS + void _OptimizeLocalContacts(); +#endif + int _ProcessLocalContacts(dContactGeom *contact, dxTriMesh *TriMesh, dxGeom *Capsule); + + static BOOL _cldClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane); + BOOL _cldTestAxis(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + dVector3 vAxis, int iAxis, BOOL bNoFlip = FALSE); + BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0, const dVector3 &v1, + const dVector3 &v2, uint8 flags); + void _cldTestOneTriangleVSCapsule(const dVector3 &v0, const dVector3 &v1, + const dVector3 &v2, uint8 flags); + + sLocalContactData *m_gLocalContacts; + unsigned int m_ctContacts; + + // capsule data + // real time data + dMatrix3 m_mCapsuleRotation; + dVector3 m_vCapsulePosition; + dVector3 m_vCapsuleAxis; + // static data + dReal m_vCapsuleRadius; + dReal m_fCapsuleSize; + + // mesh data + // dMatrix4 mHullDstPl; + dMatrix3 m_mTriMeshRot; + dVector3 m_vTriMeshPos; + dVector3 m_vE0, m_vE1, m_vE2; + + // global collider data + dVector3 m_vNormal; + dReal m_fBestDepth; + dReal m_fBestCenter; + dReal m_fBestrt; + int m_iBestAxis; + dVector3 m_vN; + + dVector3 m_vV0; + dVector3 m_vV1; + dVector3 m_vV2; + + // ODE contact's specific + unsigned int m_iFlags; + int m_iStride; +}; + +// Capsule lie on axis number 3 = (Z axis) +static const int nCAPSULE_AXIS = 2; + + +#if OPTIMIZE_CONTACTS + +// 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" +void sTrimeshCapsuleColliderData::_OptimizeLocalContacts() +{ + int nContacts = m_ctContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(m_gLocalContacts[i],m_gLocalContacts[j])) + { + // If they are seem to be the samed then filtered + // out the least penetrate one + if (_IsBetter(m_gLocalContacts[j],m_gLocalContacts[i])) + { + m_gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + m_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??? + } + } + } +} +#endif // OPTIMIZE_CONTACTS + +int sTrimeshCapsuleColliderData::_ProcessLocalContacts(dContactGeom *contact, + dxTriMesh *TriMesh, dxGeom *Capsule) +{ +#if OPTIMIZE_CONTACTS + if (m_ctContacts > 1 && !(m_iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + unsigned int iContact = 0; + dContactGeom* Contact = 0; + + unsigned int nFinalContact = 0; + + for (iContact = 0; iContact < m_ctContacts; iContact ++) + { + // Ensure that we haven't created too many contacts + if( nFinalContact >= (m_iFlags & NUMC_MASK)) + { + break; + } + + if (1 == m_gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(m_iFlags, contact, nFinalContact, m_iStride); + Contact->depth = m_gLocalContacts[iContact].fDepth; + SET(Contact->normal,m_gLocalContacts[iContact].vNormal); + SET(Contact->pos,m_gLocalContacts[iContact].vPos); + Contact->g1 = TriMesh; + Contact->g2 = Capsule; + Contact->side1 = m_gLocalContacts[iContact].triIndex; + Contact->side2 = -1; + + nFinalContact++; + } + } + // debug + //if (nFinalContact != m_ctContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",m_ctContacts,m_ctContacts-nFinalContact); + //} + + return nFinalContact; +} + +BOOL sTrimeshCapsuleColliderData::_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; +} + +BOOL sTrimeshCapsuleColliderData::_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(dCalcVectorDot3(m_vCapsuleAxis,vAxis))*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius) + m_vCapsuleRadius; + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dCalcVectorDot3(m_vV0, vAxis); + afv[1] = dCalcVectorDot3(m_vV1, vAxis); + afv[2] = dCalcVectorDot3(m_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]<fMin) + { + fMin = afv[i]; + } + // find maximum + 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 > m_fBestDepth ) + { + // remember depth + m_fBestDepth = fDepth; + m_fBestCenter = fCenter; + m_fBestrt = fTriangleRadius; + + m_vNormal[0] = vAxis[0]; + m_vNormal[1] = vAxis[1]; + m_vNormal[2] = vAxis[2]; + + m_iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if (fCenter<0 && !bNoFlip) + { + m_vNormal[0] = -m_vNormal[0]; + m_vNormal[1] = -m_vNormal[1]; + m_vNormal[2] = -m_vNormal[2]; + + m_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); + dCalcVectorCross3(t2,t1,v3); + dCalcVectorCross3(r,t2,v4); +} + +BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + uint8 flags) +{ + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = m_vCapsulePosition[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp0[1] = m_vCapsulePosition[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp0[2] = m_vCapsulePosition[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + + dVector3 vCp1; + vCp1[0] = m_vCapsulePosition[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp1[1] = m_vCapsulePosition[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp1[2] = m_vCapsulePosition[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + + // reset best axis + m_iBestAxis = 0; + // reset best depth + m_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, m_vCapsulePosition, m_vV0); + SUBTRACT(v1, m_vCapsulePosition, m_vV1); + SUBTRACT(v2, m_vCapsulePosition, m_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 m_vN + //vAxis = -m_vN; + vAxis[0] = - m_vN[0]; + vAxis[1] = - m_vN[1]; + vAxis[2] = - m_vN[2]; + if (!_cldTestAxis(v0, v1, v2, vAxis, 1, TRUE)) + { + return FALSE; + } + + if (flags & dxTriMeshData::CUF_USE_FIRST_EDGE) + { + // axis CxE0 - Edge 0 + dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE0); + //vAxis = dCalcVectorCross3( m_vCapsuleAxis cross vE0 ); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_EDGE) + { + // axis CxE1 - Edge 1 + dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE1); + //vAxis = ( m_vCapsuleAxis cross m_vE1 ); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_THIRD_EDGE) + { + // axis CxE2 - Edge 2 + //vAxis = ( m_vCapsuleAxis cross m_vE2 ); + dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE2); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_FIRST_EDGE) + { + // first capsule point + // axis ((Cp0-V0) x E0) x E0 + _CalculateAxis(vCp0,v0,m_vE0,m_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::CUF_USE_SECOND_EDGE) + { + // axis ((Cp0-V1) x E1) x E1 + _CalculateAxis(vCp0,v1,m_vE1,m_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::CUF_USE_THIRD_EDGE) + { + // axis ((Cp0-V2) x E2) x E2 + _CalculateAxis(vCp0,v2,m_vE2,m_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::CUF_USE_FIRST_EDGE) + { + // second capsule point + // axis ((Cp1-V0) x E0) x E0 + _CalculateAxis(vCp1,v0,m_vE0,m_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::CUF_USE_SECOND_EDGE) + { + // axis ((Cp1-V1) x E1) x E1 + _CalculateAxis(vCp1,v1,m_vE1,m_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::CUF_USE_THIRD_EDGE) + { + // axis ((Cp1-V2) x E2) x E2 + _CalculateAxis(vCp1,v2,m_vE2,m_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::CUF_USE_FIRST_VERTEX) + { + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + _CalculateAxis(v0,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis); + //vAxis = ( ( v0-vCp0 ) cross m_vCapsuleAxis ) cross m_vCapsuleAxis; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_VERTEX) + { + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + _CalculateAxis(v1,vCp0,m_vCapsuleAxis,m_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::CUF_USE_THIRD_VERTEX) + { + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + _CalculateAxis(v2,vCp0,m_vCapsuleAxis,m_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::CUF_USE_FIRST_VERTEX) + { + // 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::CUF_USE_SECOND_VERTEX) + { + // 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::CUF_USE_THIRD_VERTEX) + { + // 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::CUF_USE_FIRST_VERTEX) + { + // 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::CUF_USE_SECOND_VERTEX) + { + // 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::CUF_USE_THIRD_VERTEX) + { + // 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 +void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + uint8 flags) +{ + // calculate edges + SUBTRACT(v1,v0,m_vE0); + SUBTRACT(v2,v1,m_vE1); + SUBTRACT(v0,v2,m_vE2); + + dVector3 _minus_vE0; + SUBTRACT(v0,v1,_minus_vE0); + + // calculate poly normal + dCalcVectorCross3(m_vN,m_vE1,_minus_vE0); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (!dSafeNormalize3(m_vN)) + { + return; + } + + // create plane from triangle + dReal plDistance = -dCalcVectorDot3(v0,m_vN); + dVector4 plTrianglePlane; + CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance); + + // calculate capsule distance to plane + dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition); + + // Capsule must be over positive side of triangle + if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0, vPnt1, vPnt2; + SET (vPnt0,v0); + + if (fDistanceCapsuleCenterToPlane < 0) + { + SET (vPnt1,v2); + SET (vPnt2,v1); + } + else + { + SET (vPnt1,v1); + SET (vPnt2,v2); + } + + // 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 (m_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] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius; + vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius; + vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_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] = -m_vN[0]; + _minus_vN[1] = -m_vN[1]; + _minus_vN[2] = -m_vN[2]; + // triangle plane + CONSTRUCTPLANE(plPlane,_minus_vN,0); + //plPlane = Plane4f( -m_vN, 0); + + if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + // plane with edge 0 + dVector3 vTemp; + dCalcVectorCross3(vTemp,m_vN,m_vE0); + CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); + if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCalcVectorCross3(vTemp,m_vN,m_vE1); + CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5))); + if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCalcVectorCross3(vTemp,m_vN,m_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,m_vCapsulePosition,vTemp); + dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt); + SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp); + dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt); + + // clamp depths to zero + if (fDepth0 < 0) + { + fDepth0 = 0.0f; + } + + if (fDepth1 < 0 ) + { + fDepth1 = 0.0f; + } + + // Cached contacts's data + // contact 0 + dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result + m_gLocalContacts[m_ctContacts].fDepth = fDepth0; + SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal); + SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0); + m_gLocalContacts[m_ctContacts].nFlags = 1; + m_ctContacts++; + + if (m_ctContacts < (m_iFlags & NUMC_MASK)) { + // contact 1 + m_gLocalContacts[m_ctContacts].fDepth = fDepth1; + SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal); + SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1); + m_gLocalContacts[m_ctContacts].nFlags = 1; + m_ctContacts++; + } +} + +void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Capsule, + int flags, int skip) +{ + const dMatrix3* pRot = (const dMatrix3*)dGeomGetRotation(Capsule); + memcpy(m_mCapsuleRotation, pRot, sizeof(dMatrix3)); + + const dVector3* pDst = (const dVector3*)dGeomGetPosition(Capsule); + memcpy(m_vCapsulePosition, pDst, sizeof(dVector3)); + + m_vCapsuleAxis[0] = m_mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + m_vCapsuleAxis[1] = m_mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + m_vCapsuleAxis[2] = m_mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + + // Get size of Capsule + dGeomCapsuleGetParams(Capsule, &m_vCapsuleRadius, &m_fCapsuleSize); + m_fCapsuleSize += 2*m_vCapsuleRadius; + + const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh); + memcpy(m_mTriMeshRot, pTriRot, sizeof(dMatrix3)); + + const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh); + memcpy(m_vTriMeshPos, pTriPos, sizeof(dVector3)); + + // global info for contact creation + m_iStride =skip; + m_iFlags =flags; + + // reset contact counter + m_ctContacts = 0; + + // reset best depth + m_fBestDepth = - MAX_REAL; + m_fBestCenter = 0; + m_fBestrt = 0; + + // reset collision normal + m_vNormal[0] = REAL(0.0); + m_vNormal[1] = REAL(0.0); + m_vNormal[2] = REAL(0.0); +} + +int sTrimeshCapsuleColliderData::TestCollisionForSingleTriangle(int ctContacts0, + int Triint, dVector3 dv[3], uint8 flags, bool &bOutFinishSearching) +{ + // test this triangle + _cldTestOneTriangleVSCapsule(dv[0],dv[1],dv[2], flags); + + // fill-in tri index for generated contacts + for (; ctContacts0 < (int)m_ctContacts; ctContacts0++) + m_gLocalContacts[ctContacts0].triIndex = Triint; + + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + bOutFinishSearching = (m_ctContacts >= (m_iFlags & NUMC_MASK)); + + return ctContacts0; +} + + +static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider, + const sTrimeshCapsuleColliderData &cData, dxTriMesh *TriMesh, dxGeom *Capsule, + OBBCache &BoxCache) +{ + Matrix4x4 MeshMatrix; + const dVector3 vZeroVector3 = { REAL(0.0), }; + MakeMatrix(vZeroVector3, cData.m_mTriMeshRot, MeshMatrix); + + const dVector3 &vCapsulePos = cData.m_vCapsulePosition; + const dMatrix3 &mCapsuleRot = cData.m_mCapsuleRotation; + + dVector3 vCapsuleOffsetPos; + dSubtractVectors3(vCapsuleOffsetPos, vCapsulePos, cData.m_vTriMeshPos); + + const dReal fCapsuleRadius = cData.m_vCapsuleRadius, fCapsuleHalfAxis = cData.m_fCapsuleSize * REAL(0.5); + + OBB obbCapsule; + obbCapsule.mCenter.Set(vCapsuleOffsetPos[0], vCapsuleOffsetPos[1], vCapsuleOffsetPos[2]); + obbCapsule.mExtents.Set( + 0 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius, + 1 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius, + 2 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius); + obbCapsule.mRot.Set( + mCapsuleRot[0], mCapsuleRot[4], mCapsuleRot[8], + mCapsuleRot[1], mCapsuleRot[5], mCapsuleRot[9], + mCapsuleRot[2], mCapsuleRot[6], mCapsuleRot[10]); + + // TC results + if (TriMesh->getDoTC(dxTriMesh::TTC_BOX)) { + dxTriMesh::BoxTC* BoxTC = 0; + const int iBoxCacheSize = TriMesh->m_BoxTCCache.size(); + for (int i = 0; i != iBoxCacheSize; i++){ + if (TriMesh->m_BoxTCCache[i].Geom == Capsule){ + BoxTC = &TriMesh->m_BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->m_BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->m_BoxTCCache[TriMesh->m_BoxTCCache.size() - 1]; + BoxTC->Geom = Capsule; + BoxTC->FatCoeff = 1.0f; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCapsule, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(BoxCache, obbCapsule, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } +} + +// 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); + + int nContactCount = 0; + + dxTriMesh *TriMesh = (dxTriMesh*)o1; + dxGeom *Capsule = o2; + + sTrimeshCapsuleColliderData cData; + cData.SetupInitialContext(TriMesh, Capsule, flags, skip); + + const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == Capsule->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + OBBCollider& Collider = pccColliderCache->m_OBBCollider; + + // Will it better to use LSS here? -> confirm Pierre. + dQueryCCTLPotentialCollisionTriangles(Collider, cData, + TriMesh, Capsule, pccColliderCache->m_DefaultBoxCache); + + if (Collider.GetContactStatus()) + { + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + + if (TriCount != 0) + { + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriMesh->m_ArrayCallback != null) + { + TriMesh->m_ArrayCallback(TriMesh, Capsule, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + unsigned int ctContacts0 = cData.m_ctContacts; + + const uint8 *useFlags = TriMesh->retrieveMeshSmartUseFlags(); + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!TriMesh->invokeCallback(Capsule, Triint)) continue; + + dVector3 dv[3]; + TriMesh->fetchMeshTriangle(dv, Triint, cData.m_vTriMeshPos, cData.m_mTriMeshRot); + + uint8 flags = useFlags != NULL ? useFlags[Triint] : (uint8)dxTriMeshData::CUF__USE_ALL_COMPONENTS; + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, flags, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + if (cData.m_ctContacts != 0) + { + nContactCount = cData._ProcessLocalContacts(contact, TriMesh, Capsule); + } + } + } + + return nContactCount; +} + + +#endif + + +// GIMPACT version +#if dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "gimpact_gim_contact_accessor.h" + +#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; + + dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, TriMesh, gCylinder, -1); + contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, flags, contact, skip); + + GIM_DYNARRAY_DESTROY(trimeshcontacts); + + return (int)contactcount; +} + + +#endif // dTRIMESH_GIMPACT + + +#endif // dTRIMESH_ENABLED |