From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- .../ode-0.9/ode/src/collision_cylinder_trimesh.cpp | 1145 ++++++++++++++++++++ 1 file changed, 1145 insertions(+) create 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 new file mode 100644 index 0000000..342be04 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp @@ -0,0 +1,1145 @@ +/************************************************************************* +* * +* 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