// 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;
}
