// 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); }
// check for separation between box edge and cylinder circle edge int _cldTestEdgeCircleAxis( sCylinderBoxData& cData, const dVector3 &vCenterPoint, const dVector3 &vVx0, const dVector3 &vVx1, int iAxis ) { // calculate direction of edge dVector3 vDirEdge; dVector3Subtract(vVx1,vVx0,vDirEdge); dNormalize3(vDirEdge); // starting point of edge dVector3 vEStart; dVector3Copy(vVx0,vEStart);; // calculate angle cosine between cylinder axis and edge dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis); // if edge is perpendicular to cylinder axis if(dFabs(fdot2) < 1e-5f) { // this can't be separating axis, because edge is parallel to circle plane return 1; } // find point of intersection between edge line and circle plane dVector3 vTemp1; dVector3Subtract(vCenterPoint,vEStart,vTemp1); dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis); dVector3 vpnt; vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2); vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2); vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2); // find tangent vector on circle with same center (vCenterPoint) that // touches point of intersection (vpnt) dVector3 vTangent; dVector3Subtract(vCenterPoint,vpnt,vTemp1); dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent); // find vector orthogonal both to tangent and edge direction dVector3 vAxis; dVector3Cross(vTangent,vDirEdge,vAxis); // use that vector as separating axis return _cldTestAxis( cData, vAxis, iAxis ); }
// 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)<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( cData ,v0, v1, v2, vAxis, iAxis ); }
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); } }
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<REAL(0.0)) { fDepth1 = REAL(0.0); } // Generate contact 0 { cData.gLocalContacts[cData.nContacts].fDepth = fDepth0; dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); dVector3Copy(vCEdgePoint0,cData.gLocalContacts[cData.nContacts].vPos); cData.gLocalContacts[cData.nContacts].nFlags = 1; cData.nContacts++; if(cData.nContacts >= (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; }
bool _cldTestSeparatingAxes(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { // calculate edge vectors dVector3Subtract(v1 ,v0 , cData.vE0); // cData.vE1 has been calculated before -> 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; }
// Test separating axis for collision int _cldTestSeparatingAxes(sCylinderBoxData& cData) { // reset best axis cData.fBestDepth = MAX_FLOAT; cData.iBestAxis = 0; cData.fBestrb = 0; cData.fBestrc = 0; cData.nContacts = 0; dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; // Epsilon value for checking axis vector length const dReal fEpsilon = 1e-6f; // axis A0 dMat3GetCol(cData.mBoxRot, 0 , vAxis); if (!_cldTestAxis( cData, vAxis, 1 )) { return 0; } // axis A1 dMat3GetCol(cData.mBoxRot, 1 , vAxis); if (!_cldTestAxis( cData, vAxis, 2 )) { return 0; } // axis A2 dMat3GetCol(cData.mBoxRot, 2 , vAxis); if (!_cldTestAxis( cData, vAxis, 3 )) { return 0; } // axis C - Cylinder Axis //vAxis = vCylinderAxis; dVector3Copy(cData.vCylinderAxis , vAxis); if (!_cldTestAxis( cData, vAxis, 4 )) { return 0; } // axis CxA0 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 )); dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 5 )) { return 0; } } // axis CxA1 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 )); dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 6 )) { return 0; } } // axis CxA2 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 )); dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 7 )) { return 0; } } int i = 0; dVector3 vTemp1; dVector3 vTemp2; // here we check box's vertices axis for(i=0; i< 8; i++) { //vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos)); dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1); dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2); //vAxis = ( vCylinderAxis cross vAxis ); dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 8 + i )) { return 0; } } } // ************************************ // this is defined for first 12 axes // normal of plane that contains top circle of cylinder // center of top circle of cylinder dVector3 vcc; vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); // ************************************ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27)) { return 0; } // ************************************ // this is defined for second 12 axes // normal of plane that contains bottom circle of cylinder // center of bottom circle of cylinder // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5)); vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); // ************************************ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39)) { return 0; } return 1; }
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (Cylinder->type == dCylinderClass); dIASSERT (Plane->type == dPlaneClass); dIASSERT ((flags & NUMC_MASK) >= 1); int GeomCount = 0; // count of used contactgeoms #ifdef dSINGLE const dReal toleranz = REAL(0.0001); #endif #ifdef dDOUBLE const dReal toleranz = REAL(0.0000001); #endif // Get the properties of the cylinder (length+radius) dReal radius, length; dGeomCylinderGetParams(Cylinder, &radius, &length); dVector3 &cylpos = Cylinder->final_posr->pos; // and the plane dVector4 planevec; dGeomPlaneGetParams(Plane, planevec); dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]}; dVector3 G1Pos1, G1Pos2, vDir1; vDir1[0] = Cylinder->final_posr->R[2]; vDir1[1] = Cylinder->final_posr->R[6]; vDir1[2] = Cylinder->final_posr->R[10]; dReal s; s = length * REAL(0.5); G1Pos2[0] = vDir1[0] * s + cylpos[0]; G1Pos2[1] = vDir1[1] * s + cylpos[1]; G1Pos2[2] = vDir1[2] * s + cylpos[2]; G1Pos1[0] = vDir1[0] * -s + cylpos[0]; G1Pos1[1] = vDir1[1] * -s + cylpos[1]; G1Pos1[2] = vDir1[2] * -s + cylpos[2]; dVector3 C; // parallel-check s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2]; if(s < 0) s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel else s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel if(s < toleranz && s > (-toleranz)) { // discs are parallel to the plane // 1.compute if, and where contacts are dVector3 P; s = planevec[3] - dVector3Dot(planevec, G1Pos1); dReal t; t = planevec[3] - dVector3Dot(planevec, G1Pos2); if(s >= t) // s == t does never happen, { if(s >= 0) { // 1. Disc dVector3Copy(G1Pos1, P); } else return GeomCount; // no contacts } else { if(t >= 0) { // 2. Disc dVector3Copy(G1Pos2, P); } else return GeomCount; // no contacts } // 2. generate a coordinate-system on the disc dVector3 V1, V2; if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) { // not x-axis V1[0] = vDir1[0] + REAL(1.0); // random value V1[1] = vDir1[1]; V1[2] = vDir1[2]; } else { // maybe x-axis V1[0] = vDir1[0]; V1[1] = vDir1[1] + REAL(1.0); // random value V1[2] = vDir1[2]; } // V1 is now another direction than vDir1 // Cross-product dVector3Cross(V1, vDir1, V2); // make unit V2 t = dVector3Length(V2); t = radius / t; dVector3Scale(V2, t); // cross again dVector3Cross(V2, vDir1, V1); // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' // V1 = first axis // V2 = second axis // 3. generate contactpoints // Potential contact 1 dVector3Add(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 2 dVector3Subtract(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 3 dVector3Add(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 4 dVector3Subtract(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } else { dReal t = dVector3Dot(PlaneNormal, vDir1); C[0] = vDir1[0] * t - PlaneNormal[0]; C[1] = vDir1[1] * t - PlaneNormal[1]; C[2] = vDir1[2] * t - PlaneNormal[2]; s = dVector3Length(C); // move C onto the circle s = radius / s; dVector3Scale(C, s); // deepest point of disc 1 dVector3Add(C, G1Pos1, contact->pos); // depth of the deepest point contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // C is still computed // deepest point of disc 2 dVector3Add(C, G1Pos2, contact->pos); // depth of the deepest point contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } return GeomCount; }