// 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);
}
// 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 );
}
// 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);
}
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 _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<iTmpCounter2; i++)
{
dQuatTransform(cData.qCylinderRot,avTempArray2[i], vPoint);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
fTempDepth = cData.fBestrt - ftmpdot;
// Depth must be positive
if (fTempDepth > 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<iTmpCounter1; i++)
{
dQuatTransform(cData.qCylinderRot,avTempArray1[i], vPoint);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
fTempDepth = cData.fBestrt - ftmpdot;
// Depth must be positive
if (fTempDepth > 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;;
}
}
}
}
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;
}
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]<fMin)
{
fMin = afv[i];
}
// find maximum
if (afv[i]>fMax)
{
fMax = afv[i];
}
}
// find capsule's center of interval on axis
dReal fCenter = (fMin+fMax)* REAL(0.5);
// calculate triangles halfinterval
dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);
// if they do not overlap,
if( dFabs(fCenter) > (frc+fTriangleRadius) )
{
// exit, we have no intersection
return false;
}
// calculate depth
dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) );
// if greater then best found so far
if ( fDepth < 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;
}
// initialize collision data
void _cldInitCylinderBox(sCylinderBoxData& cData)
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(cData.gBox);
dMatrix3Copy(pRotBox,cData.mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
dVector3Copy(*pPosBox,cData.vBoxPos);
dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
cData.vBoxHalfSize[0] *= REAL(0.5);
cData.vBoxHalfSize[1] *= REAL(0.5);
cData.vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
// vertex 1
cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
// vertex 2
cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
// vertex 3
cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
// vertex 4
cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
// vertex 5
cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
// vertex 6
cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
// vertex 7
cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
}
// find relative position
dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
cData.fBestDepth = MAX_FLOAT;
cData.vNormal[0] = REAL(0.0);
cData.vNormal[1] = REAL(0.0);
cData.vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = M_PI/nCYLINDER_SEGMENT;
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
cData.fBestrb = 0;
cData.fBestrc = 0;
cData.iBestAxis = 0;
cData.nContacts = 0;
}
void _cldClipBoxToCylinder(sCylinderBoxData& cData )
{
dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
// check which circle from cylinder we take for clipping
if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > 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[0] = REAL(0.0);
vCylinderCircleNormal_Rel[1] = REAL(0.0);
vCylinderCircleNormal_Rel[2] = REAL(0.0);
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[0] = REAL(0.0);
vCylinderCircleNormal_Rel[1] = REAL(0.0);
vCylinderCircleNormal_Rel[2] = REAL(0.0);
vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
}
// vNr is normal in Box frame, pointing from Cylinder to Box
dVector3 vNr;
dMatrix3 mBoxInv;
// Find a way to use quaternion
dMatrix3Inv(cData.mBoxRot,mBoxInv);
dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr);
dVector3 vAbsNormal;
vAbsNormal[0] = dFabs( vNr[0] );
vAbsNormal[1] = dFabs( vNr[1] );
vAbsNormal[2] = dFabs( vNr[2] );
// find which face in box is closest to cylinder
int iB0, iB1, iB2;
// Different from Croteam's code
if (vAbsNormal[1] > vAbsNormal[0])
{
// 1 > 0
if (vAbsNormal[0]> vAbsNormal[2])
{
// 0 > 2 -> 1 > 0 >2
iB0 = 1; iB1 = 0; iB2 = 2;
}
else
{
// 2 > 0-> Must compare 1 and 2
if (vAbsNormal[1] > vAbsNormal[2])
{
// 1 > 2 -> 1 > 2 > 0
iB0 = 1; iB1 = 2; iB2 = 0;
}
else
{
// 2 > 1 -> 2 > 1 > 0;
iB0 = 2; iB1 = 1; iB2 = 0;
}
}
}
else
{
// 0 > 1
if (vAbsNormal[1] > vAbsNormal[2])
{
// 1 > 2 -> 0 > 1 > 2
iB0 = 0; iB1 = 1; iB2 = 2;
}
else
{
// 2 > 1 -> Must compare 0 and 2
if (vAbsNormal[0] > vAbsNormal[2])
{
// 0 > 2 -> 0 > 2 > 1;
iB0 = 0; iB1 = 2; iB2 = 1;
}
else
{
// 2 > 0 -> 2 > 0 > 1;
iB0 = 2; iB1 = 0; iB2 = 1;
}
}
}
dVector3 vCenter;
// find center of box polygon
dVector3 vTemp;
if (vNr[iB0] > 0)
{
dMat3GetCol(cData.mBoxRot,iB0,vTemp);
vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2];
}
else
{
dMat3GetCol(cData.mBoxRot,iB0,vTemp);
vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2];
}
// find the vertices of box polygon
dVector3 avPoints[4];
dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS];
dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS];
int i=0;
for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++)
{
avTempArray1[i][0] = REAL(0.0);
avTempArray1[i][1] = REAL(0.0);
avTempArray1[i][2] = REAL(0.0);
avTempArray2[i][0] = REAL(0.0);
avTempArray2[i][1] = REAL(0.0);
avTempArray2[i][2] = REAL(0.0);
}
dVector3 vAxis1, vAxis2;
dMat3GetCol(cData.mBoxRot,iB1,vAxis1);
dMat3GetCol(cData.mBoxRot,iB2,vAxis2);
avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
// transform box points to space of cylinder circle
dMatrix3 mCylinderInv;
dMatrix3Inv(cData.mCylinderRot,mCylinderInv);
for(i=0; i<4; i++)
{
dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp);
dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]);
}
int iTmpCounter1 = 0;
int iTmpCounter2 = 0;
dVector4 plPlane;
// plane of cylinder that contains circle for intersection
dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane);
// Body of base circle of Cylinder
int nCircleSegment = 0;
for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; 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 <= MAX_CYLBOX_CLIP_POINTS );
dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS );
}
// back transform clipped points to absolute space
dReal ftmpdot;
dReal fTempDepth;
dVector3 vPoint;
if (nCircleSegment %2)
{
for( i=0; i<iTmpCounter2; i++)
{
dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, cData.vNormal);
fTempDepth = cData.fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
}
}
}
else
{
for( i=0; i<iTmpCounter1; i++)
{
dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, cData.vNormal);
fTempDepth = cData.fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
}
}
}
}
// 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;
}
// initialize collision data
void sCylinderBoxData::_cldInitCylinderBox()
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(m_gCylinder);
dMatrix3Copy(pRotCyc,m_mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(m_gCylinder);
dVector3Copy(*pPosCyc,m_vCylinderPos);
dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(m_gCylinder,&m_fCylinderRadius,&m_fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(m_gBox);
dMatrix3Copy(pRotBox,m_mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(m_gBox);
dVector3Copy(*pPosBox,m_vBoxPos);
dGeomBoxGetLengths(m_gBox, m_vBoxHalfSize);
m_vBoxHalfSize[0] *= REAL(0.5);
m_vBoxHalfSize[1] *= REAL(0.5);
m_vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
m_avBoxVertices[0][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[0][1] = m_vBoxHalfSize[1];
m_avBoxVertices[0][2] = -m_vBoxHalfSize[2];
// vertex 1
m_avBoxVertices[1][0] = m_vBoxHalfSize[0];
m_avBoxVertices[1][1] = m_vBoxHalfSize[1];
m_avBoxVertices[1][2] = -m_vBoxHalfSize[2];
// vertex 2
m_avBoxVertices[2][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[2][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[2][2] = -m_vBoxHalfSize[2];
// vertex 3
m_avBoxVertices[3][0] = m_vBoxHalfSize[0];
m_avBoxVertices[3][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[3][2] = -m_vBoxHalfSize[2];
// vertex 4
m_avBoxVertices[4][0] = m_vBoxHalfSize[0];
m_avBoxVertices[4][1] = m_vBoxHalfSize[1];
m_avBoxVertices[4][2] = m_vBoxHalfSize[2];
// vertex 5
m_avBoxVertices[5][0] = m_vBoxHalfSize[0];
m_avBoxVertices[5][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[5][2] = m_vBoxHalfSize[2];
// vertex 6
m_avBoxVertices[6][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[6][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[6][2] = m_vBoxHalfSize[2];
// vertex 7
m_avBoxVertices[7][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[7][1] = m_vBoxHalfSize[1];
m_avBoxVertices[7][2] = m_vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(m_mBoxRot,m_avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], m_vBoxPos, m_avBoxVertices[i]);
}
// find relative position
dVector3Subtract(m_vCylinderPos,m_vBoxPos,m_vDiff);
m_fBestDepth = MAX_FLOAT;
m_vNormal[0] = REAL(0.0);
m_vNormal[1] = REAL(0.0);
m_vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = (dReal) (M_PI/nCYLINDER_SEGMENT);
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
m_avCylinderNormals[i][0] = -dCos(fAngle);
m_avCylinderNormals[i][1] = -dSin(fAngle);
m_avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
m_fBestrb = 0;
m_fBestrc = 0;
m_iBestAxis = 0;
m_nContacts = 0;
}
