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-0.9/ode/src/collision_cylinder_trimesh.cpp | 1145 -------------------- 1 file changed, 1145 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp (limited to 'libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp deleted file mode 100644 index 342be04..0000000 --- a/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp +++ /dev/null @@ -1,1145 +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. * -* * -*************************************************************************/ - -/* - * Cylinder-trimesh collider by Alen Ladavac - * Ported to ODE by Nguyen Binh - */ - - -#include -#include -#include -#include -#include "collision_util.h" - -#define TRIMESH_INTERNAL -#include "collision_trimesh_internal.h" - -#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; - -typedef struct _sCylinderTrimeshColliderData -{ - // cylinder data - dMatrix3 mCylinderRot; - dQuaternion qCylinderRot; - dQuaternion qInvCylinderRot; - dVector3 vCylinderPos; - dVector3 vCylinderAxis; - dReal fCylinderRadius; - dReal fCylinderSize; - dVector3 avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS]; - - // mesh data - dQuaternion qTrimeshRot; - dQuaternion qInvTrimeshRot; - dMatrix3 mTrimeshRot; - dVector3 vTrimeshPos; - - // global collider data - dVector3 vBestPoint; - dReal fBestDepth; - dReal fBestCenter; - dReal fBestrt; - int iBestAxis; - dVector3 vContactNormal; - dVector3 vNormal; - dVector3 vE0; - dVector3 vE1; - dVector3 vE2; - - // ODE stuff - dGeomID gCylinder; - dxTriMesh* gTrimesh; - dContactGeom* gContact; - int iFlags; - int iSkip; - int nContacts;// = 0; - sLocalContactData* gLocalContacts; -} sCylinderTrimeshColliderData; - -// Short type name -typedef sCylinderTrimeshColliderData sData; - -// 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" -inline void _OptimizeLocalContacts(sData& cData) -{ - int nContacts = cData.nContacts; - - for (int i = 0; i < nContacts-1; i++) - { - for (int j = i+1; j < nContacts; j++) - { - if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j])) - { - // If they are seem to be the same then filtered - // out the least penetrate one - if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i])) - { - cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact - } - else - { - cData.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(sData& cData) -{ - if (cData.nContacts == 0) - { - return 0; - } - -#ifdef OPTIMIZE_CONTACTS - if (cData.nContacts > 1 && !(cData.iFlags & CONTACTS_UNIMPORTANT)) - { - // Can be optimized... - _OptimizeLocalContacts(cData); - } -#endif - - int iContact = 0; - dContactGeom* Contact = 0; - - int nFinalContact = 0; - - for (iContact = 0; iContact < cData.nContacts; iContact ++) - { - if (1 == cData.gLocalContacts[iContact].nFlags) - { - Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip); - Contact->depth = cData.gLocalContacts[iContact].fDepth; - dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal); - dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos); - Contact->g1 = cData.gCylinder; - Contact->g2 = cData.gTrimesh; - Contact->side2 = cData.gLocalContacts[iContact].triIndex; - dVector3Inv(Contact->normal); - - nFinalContact++; - } - } - // debug - //if (nFinalContact != cData.nContacts) - //{ - // printf("[Info] %d contacts generated,%d filtered.\n",cData.nContacts,cData.nContacts-nFinalContact); - //} - - return nFinalContact; -} - - -bool _cldTestAxis(sData& cData, - 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(cData.vCylinderAxis,vAxis); - // project capsule on vAxis - dReal frc; - - if (dFabs(fdot1) > REAL(1.0) ) - { -// fdot1 = REAL(1.0); - frc = dFabs(cData.fCylinderSize* REAL(0.5)); - } - else - { - frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1) - + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); - } - - dVector3 vV0; - dVector3Subtract(v0,cData.vCylinderPos,vV0); - dVector3 vV1; - dVector3Subtract(v1,cData.vCylinderPos,vV1); - dVector3 vV2; - dVector3Subtract(v2,cData.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]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 < cData.fBestDepth ) - { - // remember depth - cData.fBestDepth = fDepth; - cData.fBestCenter = fCenter; - cData.fBestrt = frc; - dVector3Copy(vAxis,cData.vContactNormal); - cData.iBestAxis = iAxis; - - // flip normal if interval is wrong faced - if ( fCenter< REAL(0.0) && !bNoFlip) - { - dVector3Inv(cData.vContactNormal); - cData.fBestCenter = -fCenter; - } - } - - return true; -} - -// intersection test between edge and circle -bool _cldTestCircleToEdgeAxis(sData& cData, - 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) so save some cycles here - dVector3Subtract(v0 ,v2 , cData.vE2); - - // calculate caps centers in absolute space - dVector3 vCp0; - vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); - vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); - vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); - - dVector3 vCp1; - vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); - vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); - vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); - - // reset best axis - cData.iBestAxis = 0; - dVector3 vAxis; - - // axis cData.vNormal - //vAxis = -cData.vNormal; - vAxis[0] = -cData.vNormal[0]; - vAxis[1] = -cData.vNormal[1]; - vAxis[2] = -cData.vNormal[2]; - if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true)) - { - return false; - } - - // axis CxE0 - // vAxis = ( cData.vCylinderAxis cross cData.vE0 ); - dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis); - if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2)) - { - return false; - } - - // axis CxE1 - // vAxis = ( cData.vCylinderAxis cross cData.vE1 ); - dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis); - if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3)) - { - return false; - } - - // axis CxE2 - // vAxis = ( cData.vCylinderAxis cross cData.vE2 ); - dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis); - if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4)) - { - return false; - } - - // first vertex on triangle - // axis ((V0-Cp0) x C) x C - //vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; - _CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis); - if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11)) - { - return false; - } - - // second vertex on triangle - // axis ((V1-Cp0) x C) x C - // vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; - _CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis); - if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12)) - { - return false; - } - - // third vertex on triangle - // axis ((V2-Cp0) x C) x C - //vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; - _CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis); - if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13)) - { - return false; - } - - // test cylinder axis - // vAxis = cData.vCylinderAxis; - dVector3Copy(cData.vCylinderAxis , vAxis); - if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14)) - { - return false; - } - - // Test top and bottom circle ring of cylinder for separation - dVector3 vccATop; - vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); - vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); - vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); - - dVector3 vccABottom; - vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); - vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); - vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); - - - if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20)) - { - return false; - } - - return true; -} - -bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) -{ - // translate cylinder - dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal); - dVector3 vN2; - vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp; - vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp; - vN2[2] = cData.vContactNormal[2] - cData.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] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius; - vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius; - vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius; - - dVector3 vCEdgePoint0; - vCEdgePoint0[0] = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); - vCEdgePoint0[1] = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); - vCEdgePoint0[2] = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5)); - - dVector3 vCEdgePoint1; - vCEdgePoint1[0] = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); - vCEdgePoint1[1] = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); - vCEdgePoint1[2] = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.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( -cData.vNormal, 0); - vPlaneNormal[0] = -cData.vNormal[0]; - vPlaneNormal[1] = -cData.vNormal[1]; - vPlaneNormal[2] = -cData.vNormal[2]; - dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); - if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) - { - return false; - } - - // plane with edge 0 - //plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), REAL(1e-5)); - dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal); - dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); - if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) - { - return false; - } - - // plane with edge 1 - //dVector3 vTemp = ( cData.vNormal cross cData.vE1 ); - dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal); - fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - REAL(1e-5); - //plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-REAL(1e-5))); - dConstructPlane(vPlaneNormal,-fTemp,plPlane); - if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) - { - return false; - } - - // plane with edge 2 - // plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), REAL(1e-5)); - dVector3Cross(cData.vNormal,cData.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,cData.vCylinderPos, vTemp); - dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; - dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp); - dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; - - dReal fDepth0 = cData.fBestDepth - (fRestDepth0); - dReal fDepth1 = cData.fBestDepth - (fRestDepth1); - - // clamp depths to zero - if(fDepth0 < REAL(0.0) ) - { - fDepth0 = REAL(0.0); - } - - if(fDepth1= (cData.iFlags & NUMC_MASK)) - return true; - } - - // Generate contact 1 - { - // generate contacts - cData.gLocalContacts[cData.nContacts].fDepth = fDepth1; - dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); - dVector3Copy(vCEdgePoint1,cData.gLocalContacts[cData.nContacts].vPos); - cData.gLocalContacts[cData.nContacts].nFlags = 1; - cData.nContacts++; - } - - return true; -} - -void _cldClipCylinderToTriangle(sData& cData,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(cData.vCylinderAxis , cData.vContactNormal) > REAL(0.0)) - { - // get top circle - vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); - vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); - vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); - - vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); - } - else - { - // get bottom circle - vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); - vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); - vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); - - vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); - } - - dVector3 vTemp; - dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot); - // transform triangle points to space of cylinder circle - for(i=0; i<3; i++) - { - dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); - dQuatTransform(cData.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(cData.avCylinderNormals[nCircleSegment],cData.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 REAL(0.0)) - { - cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; - dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); - dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); - cData.gLocalContacts[cData.nContacts].nFlags = 1; - cData.nContacts++; - if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) - return;; - } - } - } - else - { - for( i=0; i REAL(0.0)) - { - cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; - dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); - dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); - cData.gLocalContacts[cData.nContacts].nFlags = 1; - cData.nContacts++; - if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) - return;; - } - } - } -} - -void TestOneTriangleVsCylinder( sData& cData, - const dVector3 &v0, - const dVector3 &v1, - const dVector3 &v2, - const bool bDoubleSided) -{ - - // calculate triangle normal - dVector3Subtract( v2 , v1 ,cData.vE1); - dVector3 vTemp; - dVector3Subtract( v0 , v1 ,vTemp); - dVector3Cross(cData.vE1 , vTemp , cData.vNormal ); - - dNormalize3( cData.vNormal); - - // create plane from triangle - //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); - dReal plDistance = -dVector3Dot(v0, cData.vNormal); - dVector4 plTrianglePlane; - dConstructPlane( cData.vNormal,plDistance,plTrianglePlane); - - // calculate sphere distance to plane - dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.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); - } - - cData.fBestDepth = MAX_REAL; - - // do intersection test and find best separating axis - if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) ) - { - // if not found do nothing - return; - } - - // if best separation axis is not found - if ( cData.iBestAxis == 0 ) - { - // this should not happen (we should already exit in that case) - dIASSERT(false); - // do nothing - return; - } - - dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis ); - - // choose which clipping method are we going to apply - if (dFabs(fdot) < REAL(0.9) ) - { - if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2)) - { - return; - } - } - else - { - _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2); - } - -} - -void _InitCylinderTrimeshData(sData& cData) -{ - // get cylinder information - // Rotation - const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); - dMatrix3Copy(pRotCyc,cData.mCylinderRot); - dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot); - - // Position - const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); - dVector3Copy(*pPosCyc,cData.vCylinderPos); - // Cylinder axis - dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); - // get cylinder radius and size - dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); - - // get trimesh position and orientation - const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh); - dMatrix3Copy(pRotTris,cData.mTrimeshRot); - dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot); - - // Position - const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh); - dVector3Copy(*pPosTris,cData.vTrimeshPos); - - - // calculate basic angle for 8-gon - dReal fAngle = 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= (int)sizeof( dContactGeom ) ); - dIASSERT( o1->type == dCylinderClass ); - dIASSERT( o2->type == dTriMeshClass ); - dIASSERT ((flags & NUMC_MASK) >= 1); - - // Main data holder - sData cData; - - // Assign ODE stuff - cData.gCylinder = o1; - cData.gTrimesh = (dxTriMesh*)o2; - cData.iFlags = flags; - cData.iSkip = skip; - cData.gContact = contact; - cData.nContacts = 0; - - _InitCylinderTrimeshData(cData); - - OBBCollider& Collider = cData.gTrimesh->_OBBCollider; - - Point cCenter(cData.vCylinderPos[0],cData.vCylinderPos[1],cData.vCylinderPos[2]); - - Point cExtents(cData.fCylinderRadius,cData.fCylinderRadius,cData.fCylinderRadius); - cExtents[nCYLINDER_AXIS] = cData.fCylinderSize * REAL(0.5); - - Matrix3x3 obbRot; - - // 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. - - obbRot[0][0] = /*(float)*/cData.mCylinderRot[0]; - obbRot[1][0] = /*(float)*/cData.mCylinderRot[1]; - obbRot[2][0] = /*(float)*/cData.mCylinderRot[2]; - - obbRot[0][1] = /*(float)*/cData.mCylinderRot[4]; - obbRot[1][1] = /*(float)*/cData.mCylinderRot[5]; - obbRot[2][1] = /*(float)*/cData.mCylinderRot[6]; - - obbRot[0][2] = /*(float)*/cData.mCylinderRot[8]; - obbRot[1][2] = /*(float)*/cData.mCylinderRot[9]; - obbRot[2][2] = /*(float)*/cData.mCylinderRot[10]; - - OBB obbCapsule(cCenter,cExtents,obbRot); - - Matrix4x4 CapsuleMatrix; - MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CapsuleMatrix); - - Matrix4x4 MeshMatrix; - MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix); - - // TC results - if (cData.gTrimesh->doBoxTC) - { - dxTriMesh::BoxTC* BoxTC = 0; - for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++) - { - if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder) - { - BoxTC = &cData.gTrimesh->BoxTCCache[i]; - break; - } - } - if (!BoxTC) - { - cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC()); - - BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1]; - BoxTC->Geom = cData.gCylinder; - BoxTC->FatCoeff = REAL(1.0); - } - - // Intersect - Collider.SetTemporalCoherence(true); - Collider.Collide(*BoxTC, obbCapsule, cData.gTrimesh->Data->BVTree, null, &MeshMatrix); - } - else - { - Collider.SetTemporalCoherence(false); - Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, cData.gTrimesh->Data->BVTree, null,&MeshMatrix); - } - - // Retrieve data - int TriCount = Collider.GetNbTouchedPrimitives(); - const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); - - - if (TriCount != 0) - { - if (cData.gTrimesh->ArrayCallback != null) - { - cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount); - } - - // allocate buffer for local contacts on stack - cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK)); - - int ctContacts0 = 0; - - // loop through all intersecting triangles - for (int i = 0; i < TriCount; i++) - { - const int Triint = Triangles[i]; - if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue; - - - dVector3 dv[3]; - FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv); - - // test this triangle - TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false); - - // fill-in tri index for generated contacts - for (; ctContacts0= (cData.iFlags & NUMC_MASK)) - { - break; - } - } - } - - return _ProcessLocalContacts(cData); -} -#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); - - // Main data holder - sData cData; - - // Assign ODE stuff - cData.gCylinder = o1; - cData.gTrimesh = (dxTriMesh*)o2; - cData.iFlags = flags; - cData.iSkip = skip; - cData.gContact = contact; - cData.nContacts = 0; - - _InitCylinderTrimeshData(cData); - -//*****at first , collide box aabb******// - - aabb3f test_aabb; - - test_aabb.minX = o1->aabb[0]; - test_aabb.maxX = o1->aabb[1]; - test_aabb.minY = o1->aabb[2]; - test_aabb.maxY = o1->aabb[3]; - test_aabb.minZ = o1->aabb[4]; - test_aabb.maxZ = o1->aabb[5]; - - - GDYNAMIC_ARRAY collision_result; - GIM_CREATE_BOXQUERY_LIST(collision_result); - - gim_aabbset_box_collision(&test_aabb, &cData.gTrimesh->m_collision_trimesh.m_aabbset , &collision_result); - - if(collision_result.m_size==0) - { - GIM_DYNARRAY_DESTROY(collision_result); - return 0; - } -//*****Set globals for box collision******// - - int ctContacts0 = 0; - cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK)); - - GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result); - GIM_TRIMESH * ptrimesh = &cData.gTrimesh->m_collision_trimesh; - - gim_trimesh_locks_work_data(ptrimesh); - - - for(unsigned int i=0;i= (cData.iFlags & NUMC_MASK)) - { - break; - } - } - - gim_trimesh_unlocks_work_data(ptrimesh); - GIM_DYNARRAY_DESTROY(collision_result); - - return _ProcessLocalContacts(cData); -} -#endif - -#endif // dTRIMESH_ENABLED - - -- cgit v1.1