diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp | 1171 |
1 files changed, 1171 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp b/libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp new file mode 100644 index 0000000..fd22e1a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp @@ -0,0 +1,1171 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + * Cylinder-trimesh collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + + +#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 + +#define MAX_REAL dInfinity +static const int nCYLINDER_AXIS = 2; +static const int nCYLINDER_CIRCLE_SEGMENTS = 8; +static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12; + +#define OPTIMIZE_CONTACTS 1 + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +struct sCylinderTrimeshColliderData +{ + sCylinderTrimeshColliderData(int flags, int skip): m_iFlags(flags), m_iSkip(skip), m_nContacts(0), m_gLocalContacts(NULL) {} + +#ifdef OPTIMIZE_CONTACTS + void _OptimizeLocalContacts(); +#endif + void _InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh); + int _ProcessLocalContacts(dContactGeom *contact, dxGeom *Cylinder, dxTriMesh *Trimesh); + + bool _cldTestAxis(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + dVector3& vAxis, int iAxis, bool bNoFlip = false); + bool _cldTestCircleToEdgeAxis( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis); + bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + bool _cldClipCylinderEdgeToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + void _cldClipCylinderToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + void TestOneTriangleVsCylinder(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const bool bDoubleSided); + int TestCollisionForSingleTriangle(int ctContacts0, int Triint, dVector3 dv[3], + bool &bOutFinishSearching); + + // cylinder data + dMatrix3 m_mCylinderRot; + dQuaternion m_qCylinderRot; + dQuaternion m_qInvCylinderRot; + dVector3 m_vCylinderPos; + dVector3 m_vCylinderAxis; + dReal m_fCylinderRadius; + dReal m_fCylinderSize; + dVector3 m_avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS]; + + // mesh data + dQuaternion m_qTrimeshRot; + dQuaternion m_qInvTrimeshRot; + dMatrix3 m_mTrimeshRot; + dVector3 m_vTrimeshPos; + + // global collider data + dVector3 m_vBestPoint; + dReal m_fBestDepth; + dReal m_fBestCenter; + dReal m_fBestrt; + int m_iBestAxis; + dVector3 m_vContactNormal; + dVector3 m_vNormal; + dVector3 m_vE0; + dVector3 m_vE1; + dVector3 m_vE2; + + // ODE stuff + int m_iFlags; + int m_iSkip; + int m_nContacts;// = 0; + sLocalContactData* m_gLocalContacts; +}; + + +#ifdef 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 + dVector3Subtract(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 + dVector3Subtract(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 sCylinderTrimeshColliderData::_OptimizeLocalContacts() +{ + int nContacts = m_nContacts; + + 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 same 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 sCylinderTrimeshColliderData::_ProcessLocalContacts(dContactGeom *contact, + dxGeom *Cylinder, dxTriMesh *Trimesh) +{ +#ifdef OPTIMIZE_CONTACTS + if (m_nContacts > 1 && !(m_iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + int iContact = 0; + dContactGeom* Contact = 0; + + int nFinalContact = 0; + + for (iContact = 0; iContact < m_nContacts; iContact ++) + { + if (1 == m_gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(m_iFlags, contact, nFinalContact, m_iSkip); + Contact->depth = m_gLocalContacts[iContact].fDepth; + dVector3Copy(m_gLocalContacts[iContact].vNormal,Contact->normal); + dVector3Copy(m_gLocalContacts[iContact].vPos,Contact->pos); + Contact->g1 = Cylinder; + Contact->g2 = Trimesh; + Contact->side1 = -1; + Contact->side2 = m_gLocalContacts[iContact].triIndex; + dVector3Inv(Contact->normal); + + nFinalContact++; + } + } + // debug + //if (nFinalContact != m_nContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",m_nContacts,m_nContacts-nFinalContact); + //} + + return nFinalContact; +} + + +bool sCylinderTrimeshColliderData::_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 = dVector3Length(vAxis); + // if not long enough + if ( fL < REAL(1e-5) ) + { + // do nothing + return true; + } + + // otherwise normalize it + vAxis[0] /= fL; + vAxis[1] /= fL; + vAxis[2] /= fL; + + dReal fdot1 = dVector3Dot(m_vCylinderAxis,vAxis); + // project capsule on vAxis + dReal frc; + + if (dFabs(fdot1) > REAL(1.0) ) + { + // fdot1 = REAL(1.0); + frc = dFabs(m_fCylinderSize* REAL(0.5)); + } + else + { + frc = dFabs((m_fCylinderSize* REAL(0.5)) * fdot1) + + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); + } + + dVector3 vV0; + dVector3Subtract(v0,m_vCylinderPos,vV0); + dVector3 vV1; + dVector3Subtract(v1,m_vCylinderPos,vV1); + dVector3 vV2; + dVector3Subtract(v2,m_vCylinderPos,vV2); + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dVector3Dot( vV0 , vAxis ); + afv[1] = dVector3Dot( vV1 , vAxis ); + afv[2] = dVector3Dot( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = -MAX_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 capsule's center of interval on axis + dReal fCenter = (fMin+fMax)* REAL(0.5); + // calculate triangles halfinterval + 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 = frc; + dVector3Copy(vAxis,m_vContactNormal); + m_iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if ( fCenter< REAL(0.0) && !bNoFlip) + { + dVector3Inv(m_vContactNormal); + m_fBestCenter = -fCenter; + } + } + + return true; +} + +// intersection test between edge and circle +bool sCylinderTrimeshColliderData::_cldTestCircleToEdgeAxis( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) +{ + // calculate direction of edge + dVector3 vkl; + dVector3Subtract( vVx1 , vVx0 , vkl); + dNormalize3(vkl); + // starting point of edge + dVector3 vol; + dVector3Copy(vVx0,vol); + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2)<REAL(1e-5)) + { + // this can't be separating axis, because edge is parallel to circle plane + return true; + } + + // find point of intersection between edge line and circle plane + dVector3 vTemp; + dVector3Subtract(vCenterPoint,vol,vTemp); + dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1); + dVector3 vpnt;// = vol + vkl * (fdot1/fdot2); + vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2; + vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2; + vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2; + + // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt) + dVector3 vTangent; + dVector3Subtract(vCenterPoint,vpnt,vTemp); + dVector3Cross(vTemp,vCylinderAxis1,vTangent); + + // find vector orthogonal both to tangent and edge direction + dVector3 vAxis; + dVector3Cross(vTangent,vkl,vAxis); + + // use that vector as separating axis + return _cldTestAxis( v0, v1, v2, vAxis, iAxis ); +} + +// helper for less key strokes +// r = ( (v1 - v2) cross v3 ) cross v3 +inline void _CalculateAxis(const dVector3& v1, + const dVector3& v2, + const dVector3& v3, + dVector3& r) +{ + dVector3 t1; + dVector3 t2; + + dVector3Subtract(v1,v2,t1); + dVector3Cross(t1,v3,t2); + dVector3Cross(t2,v3,r); +} + +bool sCylinderTrimeshColliderData::_cldTestSeparatingAxes( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2) +{ + + // calculate edge vectors + dVector3Subtract(v1 ,v0 , m_vE0); + // m_vE1 has been calculated before -> so save some cycles here + dVector3Subtract(v0 ,v2 , m_vE2); + + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); + vCp0[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); + vCp0[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); + +#if 0 + dVector3 vCp1; + vCp1[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); + vCp1[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); + vCp1[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); +#endif + + // reset best axis + m_iBestAxis = 0; + dVector3 vAxis; + + // axis m_vNormal + //vAxis = -m_vNormal; + vAxis[0] = -m_vNormal[0]; + vAxis[1] = -m_vNormal[1]; + vAxis[2] = -m_vNormal[2]; + if (!_cldTestAxis(v0, v1, v2, vAxis, 1, true)) + { + return false; + } + + // axis CxE0 + // vAxis = ( m_vCylinderAxis cross m_vE0 ); + dVector3Cross(m_vCylinderAxis, m_vE0,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) + { + return false; + } + + // axis CxE1 + // vAxis = ( m_vCylinderAxis cross m_vE1 ); + dVector3Cross(m_vCylinderAxis, m_vE1,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) + { + return false; + } + + // axis CxE2 + // vAxis = ( m_vCylinderAxis cross m_vE2 ); + dVector3Cross(m_vCylinderAxis, m_vE2,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) + { + return false; + } + + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + //vAxis = ( ( v0-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v0 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) + { + return false; + } + + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + // vAxis = ( ( v1-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v1 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) + { + return false; + } + + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + //vAxis = ( ( v2-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v2 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 13)) + { + return false; + } + + // test cylinder axis + // vAxis = m_vCylinderAxis; + dVector3Copy(m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) + { + return false; + } + + // Test top and bottom circle ring of cylinder for separation + dVector3 vccATop; + vccATop[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); + vccATop[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); + vccATop[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); + + dVector3 vccABottom; + vccABottom[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); + vccABottom[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); + vccABottom[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); + + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v1, 15)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v1, v2, 16)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v2, 17)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v1, 18)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v1, v2, 19)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v2, 20)) + { + return false; + } + + return true; +} + +bool sCylinderTrimeshColliderData::_cldClipCylinderEdgeToTriangle( + const dVector3 &v0, const dVector3 &/*v1*/, const dVector3 &/*v2*/) +{ + // translate cylinder + dReal fTemp = dVector3Dot(m_vCylinderAxis , m_vContactNormal); + dVector3 vN2; + vN2[0] = m_vContactNormal[0] - m_vCylinderAxis[0]*fTemp; + vN2[1] = m_vContactNormal[1] - m_vCylinderAxis[1]*fTemp; + vN2[2] = m_vContactNormal[2] - m_vCylinderAxis[2]*fTemp; + + fTemp = dVector3Length(vN2); + if (fTemp < REAL(1e-5)) + { + return false; + } + + // Normalize it + vN2[0] /= fTemp; + vN2[1] /= fTemp; + vN2[2] /= fTemp; + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = m_vCylinderPos[0] + vN2[0]*m_fCylinderRadius; + vCposTrans[1] = m_vCylinderPos[1] + vN2[1]*m_fCylinderRadius; + vCposTrans[2] = m_vCylinderPos[2] + vN2[2]*m_fCylinderRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint0[1] = vCposTrans[1] + m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint0[2] = vCposTrans[2] + m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint1[1] = vCposTrans[1] - m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint1[2] = vCposTrans[2] - m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); + + // transform cylinder edge points into triangle space + vCEdgePoint0[0] -= v0[0]; + vCEdgePoint0[1] -= v0[1]; + vCEdgePoint0[2] -= v0[2]; + + vCEdgePoint1[0] -= v0[0]; + vCEdgePoint1[1] -= v0[1]; + vCEdgePoint1[2] -= v0[2]; + + dVector4 plPlane; + dVector3 vPlaneNormal; + + // triangle plane + //plPlane = Plane4f( -m_vNormal, 0); + vPlaneNormal[0] = -m_vNormal[0]; + vPlaneNormal[1] = -m_vNormal[1]; + vPlaneNormal[2] = -m_vNormal[2]; + dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 0 + //plPlane = Plane4f( ( m_vNormal cross m_vE0 ), REAL(1e-5)); + dVector3Cross(m_vNormal,m_vE0,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 1 + //dVector3 vTemp = ( m_vNormal cross m_vE1 ); + dVector3Cross(m_vNormal,m_vE1,vPlaneNormal); + fTemp = dVector3Dot(m_vE0 , vPlaneNormal) - REAL(1e-5); + //plPlane = Plane4f( vTemp, -(( m_vE0 dot vTemp )-REAL(1e-5))); + dConstructPlane(vPlaneNormal,-fTemp,plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 2 + // plPlane = Plane4f( ( m_vNormal cross m_vE2 ), REAL(1e-5)); + dVector3Cross(m_vNormal,m_vE2,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += v0[0]; + vCEdgePoint0[1] += v0[1]; + vCEdgePoint0[2] += v0[2]; + + vCEdgePoint1[0] += v0[0]; + vCEdgePoint1[1] += v0[1]; + vCEdgePoint1[2] += v0[2]; + + // calculate depths for both contact points + dVector3 vTemp; + dVector3Subtract(vCEdgePoint0,m_vCylinderPos, vTemp); + dReal fRestDepth0 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; + dVector3Subtract(vCEdgePoint1,m_vCylinderPos, vTemp); + dReal fRestDepth1 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; + + dReal fDepth0 = m_fBestDepth - (fRestDepth0); + dReal fDepth1 = m_fBestDepth - (fRestDepth1); + + // clamp depths to zero + if(fDepth0 < REAL(0.0) ) + { + fDepth0 = REAL(0.0); + } + + if(fDepth1<REAL(0.0)) + { + fDepth1 = REAL(0.0); + } + + // Generate contact 0 + { + m_gLocalContacts[m_nContacts].fDepth = fDepth0; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vCEdgePoint0,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return true; + } + + // Generate contact 1 + { + // generate contacts + m_gLocalContacts[m_nContacts].fDepth = fDepth1; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vCEdgePoint1,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + } + + return true; +} + +void sCylinderTrimeshColliderData::_cldClipCylinderToTriangle( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) +{ + int i = 0; + dVector3 avPoints[3]; + dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + + dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + + // setup array of triangle vertices + dVector3Copy(v0,avPoints[0]); + dVector3Copy(v1,avPoints[1]); + dVector3Copy(v2,avPoints[2]); + + dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; + dSetZero(vCylinderCircleNormal_Rel,4); + // check which circle from cylinder we take for clipping + if ( dVector3Dot(m_vCylinderAxis , m_vContactNormal) > REAL(0.0)) + { + // get top circle + vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + dVector3 vTemp; + dQuatInv(m_qCylinderRot , m_qInvCylinderRot); + // transform triangle points to space of cylinder circle + for(i=0; i<3; i++) + { + dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); + dQuatTransform(m_qInvCylinderRot,vTemp,avPoints[i]); + } + + int iTmpCounter1 = 0; + int iTmpCounter2 = 0; + dVector4 plPlane; + + // plane of cylinder that contains circle for intersection + //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f ); + dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); + dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane); + + // Body of base circle of Cylinder + int nCircleSegment = 0; + for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++) + { + dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane); + + if (0 == (nCircleSegment % 2)) + { + dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); + } + else + { + dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); + } + + dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + if (nCircleSegment %2) + { + for( i=0; i<iTmpCounter2; i++) + { + dQuatTransform(m_qCylinderRot,avTempArray2[i], vPoint); + vPoint[0] += vCylinderCirclePos[0]; + vPoint[1] += vCylinderCirclePos[1]; + vPoint[2] += vCylinderCirclePos[2]; + + dVector3Subtract(vPoint,m_vCylinderPos,vTemp); + ftmpdot = dFabs(dVector3Dot(vTemp, m_vContactNormal)); + fTempDepth = m_fBestrt - ftmpdot; + // Depth must be positive + if (fTempDepth > REAL(0.0)) + { + m_gLocalContacts[m_nContacts].fDepth = fTempDepth; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return;; + } + } + } + else + { + for( i=0; i<iTmpCounter1; i++) + { + dQuatTransform(m_qCylinderRot,avTempArray1[i], vPoint); + vPoint[0] += vCylinderCirclePos[0]; + vPoint[1] += vCylinderCirclePos[1]; + vPoint[2] += vCylinderCirclePos[2]; + + dVector3Subtract(vPoint,m_vCylinderPos,vTemp); + ftmpdot = dFabs(dVector3Dot(vTemp, m_vContactNormal)); + fTempDepth = m_fBestrt - ftmpdot; + // Depth must be positive + if (fTempDepth > REAL(0.0)) + { + m_gLocalContacts[m_nContacts].fDepth = fTempDepth; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return;; + } + } + } +} + +void sCylinderTrimeshColliderData::TestOneTriangleVsCylinder( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + const bool bDoubleSided) +{ + // calculate triangle normal + dVector3Subtract( v2 , v1 , m_vE1); + dVector3 vTemp; + dVector3Subtract( v0 , v1 ,vTemp); + dVector3Cross(m_vE1 , vTemp , m_vNormal ); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (!dSafeNormalize3( m_vNormal)) + { + return; + } + + // create plane from triangle + //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); + dReal plDistance = -dVector3Dot(v0, m_vNormal); + dVector4 plTrianglePlane; + dConstructPlane( m_vNormal,plDistance,plTrianglePlane); + + // calculate sphere distance to plane + dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(m_vCylinderPos , plTrianglePlane); + + // Sphere must be over positive side of triangle + if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + dVector3 vPnt1; + dVector3 vPnt2; + + if (fDistanceCylinderCenterToPlane < REAL(0.0) ) + { + // flip it + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt2); + dVector3Copy(v2 , vPnt1); + } + else + { + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt1); + dVector3Copy(v2 , vPnt2); + } + + m_fBestDepth = MAX_REAL; + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes(vPnt0, vPnt1, vPnt2) ) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( m_iBestAxis == 0 ) + { + // this should not happen (the function should have already returned in this case) + dIASSERT(false); + // do nothing + return; + } + + dReal fdot = dVector3Dot( m_vContactNormal , m_vCylinderAxis ); + + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + if (!_cldClipCylinderEdgeToTriangle(vPnt0, vPnt1, vPnt2)) + { + return; + } + } + else + { + _cldClipCylinderToTriangle(vPnt0, vPnt1, vPnt2); + } +} + +void sCylinderTrimeshColliderData::_InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh) +{ + // get cylinder information + // Rotation + const dReal* pRotCyc = dGeomGetRotation(Cylinder); + dMatrix3Copy(pRotCyc,m_mCylinderRot); + dGeomGetQuaternion(Cylinder,m_qCylinderRot); + + // Position + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(Cylinder); + dVector3Copy(*pPosCyc,m_vCylinderPos); + // Cylinder axis + dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis); + // get cylinder radius and size + dGeomCylinderGetParams(Cylinder,&m_fCylinderRadius,&m_fCylinderSize); + + // get trimesh position and orientation + const dReal* pRotTris = dGeomGetRotation(Trimesh); + dMatrix3Copy(pRotTris,m_mTrimeshRot); + dGeomGetQuaternion(Trimesh,m_qTrimeshRot); + + // Position + const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(Trimesh); + dVector3Copy(*pPosTris,m_vTrimeshPos); + + + // calculate basic angle for 8-gon + dReal fAngle = (dReal) (M_PI / nCYLINDER_CIRCLE_SEGMENTS); + // calculate angle increment + dReal fAngleIncrement = fAngle*REAL(2.0); + + // calculate plane normals + // axis dependant code + for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++) + { + m_avCylinderNormals[i][0] = -dCos(fAngle); + m_avCylinderNormals[i][1] = -dSin(fAngle); + m_avCylinderNormals[i][2] = REAL(0.0); + + fAngle += fAngleIncrement; + } + + dSetZero(m_vBestPoint,4); + // reset best depth + m_fBestCenter = REAL(0.0); +} + +int sCylinderTrimeshColliderData::TestCollisionForSingleTriangle(int ctContacts0, + int Triint, dVector3 dv[3], bool &bOutFinishSearching) +{ + // test this triangle + TestOneTriangleVsCylinder(dv[0],dv[1],dv[2], false); + + // fill-in tri index for generated contacts + for (; ctContacts0<m_nContacts; ctContacts0++) + m_gLocalContacts[ctContacts0].triIndex = Triint; + + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + bOutFinishSearching = (m_nContacts >= (m_iFlags & NUMC_MASK)); + + return ctContacts0; +} + +// OPCODE version of cylinder to mesh collider +#if dTRIMESH_OPCODE +static void dQueryCTLPotentialCollisionTriangles(OBBCollider &Collider, + sCylinderTrimeshColliderData &cData, dxGeom *Cylinder, dxTriMesh *Trimesh, + OBBCache &BoxCache) +{ + Matrix4x4 MeshMatrix; + const dVector3 vZeroVector3 = { REAL(0.0), }; + MakeMatrix(vZeroVector3, cData.m_mTrimeshRot, MeshMatrix); + + const dVector3 &vCylinderPos = cData.m_vCylinderPos; + const dMatrix3 &mCylinderRot = cData.m_mCylinderRot; + + dVector3 vCylinderOffsetPos; + dSubtractVectors3(vCylinderOffsetPos, vCylinderPos, cData.m_vTrimeshPos); + + const dReal fCylinderRadius = cData.m_fCylinderRadius, fCylinderHalfAxis = cData.m_fCylinderSize * REAL(0.5); + + OBB obbCylinder; + obbCylinder.mCenter.Set(vCylinderOffsetPos[0], vCylinderOffsetPos[1], vCylinderOffsetPos[2]); + obbCylinder.mExtents.Set( + 0 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, + 1 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, + 2 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius); + obbCylinder.mRot.Set( + mCylinderRot[0], mCylinderRot[4], mCylinderRot[8], + mCylinderRot[1], mCylinderRot[5], mCylinderRot[9], + mCylinderRot[2], mCylinderRot[6], mCylinderRot[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 == Cylinder) + { + 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 = Cylinder; + BoxTC->FatCoeff = REAL(1.0); + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + else + { + Collider.SetTemporalCoherence(false); + Collider.Collide(BoxCache, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } +} + +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int nContactCount = 0; + + dxGeom *Cylinder = o1; + dxTriMesh *Trimesh = (dxTriMesh *)o2; + + // Main data holder + sCylinderTrimeshColliderData cData(flags, skip); + cData._InitCylinderTrimeshData(Cylinder, Trimesh); + + const unsigned uiTLSKind = Trimesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == Cylinder->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + OBBCollider& Collider = pccColliderCache->m_OBBCollider; + + dQueryCTLPotentialCollisionTriangles(Collider, cData, Cylinder, Trimesh, pccColliderCache->m_DefaultBoxCache); + + // 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, Cylinder, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + int ctContacts0 = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!Trimesh->invokeCallback(Cylinder, Triint)) continue; + + + dVector3 dv[3]; + Trimesh->fetchMeshTriangle(dv, Triint, cData.m_vTrimeshPos, cData.m_mTrimeshRot); + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + if (cData.m_nContacts != 0) + { + nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh); + } + } + + return nContactCount; +} +#endif + +// GIMPACT version of cylinder to mesh collider +#if dTRIMESH_GIMPACT +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int nContactCount = 0; + + dxGeom *Cylinder = o1; + dxTriMesh *Trimesh = (dxTriMesh *)o2; + + // Main data holder + sCylinderTrimeshColliderData cData(flags, skip); + cData._InitCylinderTrimeshData(Cylinder, Trimesh); + + //*****at first , collide box aabb******// + + aabb3f test_aabb(o1->aabb[0], o1->aabb[1], o1->aabb[2], o1->aabb[3], o1->aabb[4], o1->aabb[5]); + + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_BOXQUERY_LIST(collision_result); + + gim_aabbset_box_collision(&test_aabb, &Trimesh->m_collision_trimesh.m_aabbset , &collision_result); + + if (collision_result.m_size != 0) + { + //*****Set globals for box collision******// + + int ctContacts0 = 0; + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result); + GIM_TRIMESH * ptrimesh = &Trimesh->m_collision_trimesh; + + gim_trimesh_locks_work_data(ptrimesh); + + for(unsigned int i=0;i<collision_result.m_size;i++) + { + const int Triint = boxesresult[i]; + + dVector3 dv[3]; + gim_trimesh_get_triangle_vertices(ptrimesh, Triint, dv[0], dv[1], dv[2]); + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + gim_trimesh_unlocks_work_data(ptrimesh); + + if (cData.m_nContacts != 0) + { + nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh); + } + } + + GIM_DYNARRAY_DESTROY(collision_result); + + return nContactCount; +} +#endif + +#endif // dTRIMESH_ENABLED + + |