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;
}
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++;
}
}
}
}
int _cldClipCylinderToBox(sCylinderBoxData& cData)
{
// calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
dVector3 vN;
dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal);
vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1;
vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1;
vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1;
// normalize that vector
dNormalize3(vN);
// translate cylinder end points by the vector
dVector3 vCposTrans;
vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius;
vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius;
vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius;
cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
// transform edge points in box space
cData.vEp0[0] -= cData.vBoxPos[0];
cData.vEp0[1] -= cData.vBoxPos[1];
cData.vEp0[2] -= cData.vBoxPos[2];
cData.vEp1[0] -= cData.vBoxPos[0];
cData.vEp1[1] -= cData.vBoxPos[1];
cData.vEp1[2] -= cData.vBoxPos[2];
dVector3 vTemp1;
// clip the edge to box
dVector4 plPlane;
// plane 0 +x
dMat3GetCol(cData.mBoxRot,0,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 1 +y
dMat3GetCol(cData.mBoxRot,1,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 2 +z
dMat3GetCol(cData.mBoxRot,2,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 3 -x
dMat3GetCol(cData.mBoxRot,0,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 4 -y
dMat3GetCol(cData.mBoxRot,1,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 5 -z
dMat3GetCol(cData.mBoxRot,2,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// calculate depths for both contact points
cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal);
cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal);
// clamp depths to 0
if(cData.fDepth0<0)
{
cData.fDepth0 = REAL(0.0);
}
if(cData.fDepth1<0)
{
cData.fDepth1 = REAL(0.0);
}
// back transform edge points from box to absolute space
cData.vEp0[0] += cData.vBoxPos[0];
cData.vEp0[1] += cData.vBoxPos[1];
cData.vEp0[2] += cData.vBoxPos[2];
cData.vEp1[0] += cData.vBoxPos[0];
cData.vEp1[1] += cData.vBoxPos[1];
cData.vEp1[2] += cData.vBoxPos[2];
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = cData.fDepth0;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(cData.vEp0,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact1->depth = cData.fDepth1;
dVector3Copy(cData.vNormal,Contact1->normal);
dVector3Copy(cData.vEp1,Contact1->pos);
Contact1->g1 = cData.gCylinder;
Contact1->g2 = cData.gBox;
dVector3Inv(Contact1->normal);
cData.nContacts++;
return 1;
}
int sCylinderBoxData::_cldClipCylinderToBox()
{
dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK));
// calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
dVector3 vN;
dReal fTemp1 = dVector3Dot(m_vCylinderAxis,m_vNormal);
vN[0] = m_vNormal[0] - m_vCylinderAxis[0]*fTemp1;
vN[1] = m_vNormal[1] - m_vCylinderAxis[1]*fTemp1;
vN[2] = m_vNormal[2] - m_vCylinderAxis[2]*fTemp1;
// normalize that vector
dNormalize3(vN);
// translate cylinder end points by the vector
dVector3 vCposTrans;
vCposTrans[0] = m_vCylinderPos[0] + vN[0] * m_fCylinderRadius;
vCposTrans[1] = m_vCylinderPos[1] + vN[1] * m_fCylinderRadius;
vCposTrans[2] = m_vCylinderPos[2] + vN[2] * m_fCylinderRadius;
m_vEp0[0] = vCposTrans[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
m_vEp0[1] = vCposTrans[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
m_vEp0[2] = vCposTrans[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));
m_vEp1[0] = vCposTrans[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
m_vEp1[1] = vCposTrans[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
m_vEp1[2] = vCposTrans[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));
// transform edge points in box space
m_vEp0[0] -= m_vBoxPos[0];
m_vEp0[1] -= m_vBoxPos[1];
m_vEp0[2] -= m_vBoxPos[2];
m_vEp1[0] -= m_vBoxPos[0];
m_vEp1[1] -= m_vBoxPos[1];
m_vEp1[2] -= m_vBoxPos[2];
dVector3 vTemp1;
// clip the edge to box
dVector4 plPlane;
// plane 0 +x
dMat3GetCol(m_mBoxRot,0,vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 1 +y
dMat3GetCol(m_mBoxRot,1,vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 2 +z
dMat3GetCol(m_mBoxRot,2,vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 3 -x
dMat3GetCol(m_mBoxRot,0,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 4 -y
dMat3GetCol(m_mBoxRot,1,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 5 -z
dMat3GetCol(m_mBoxRot,2,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// calculate depths for both contact points
m_fDepth0 = m_fBestrb + dVector3Dot(m_vEp0, m_vNormal);
m_fDepth1 = m_fBestrb + dVector3Dot(m_vEp1, m_vNormal);
// clamp depths to 0
if(m_fDepth0<0)
{
m_fDepth0 = REAL(0.0);
}
if(m_fDepth1<0)
{
m_fDepth1 = REAL(0.0);
}
// back transform edge points from box to absolute space
m_vEp0[0] += m_vBoxPos[0];
m_vEp0[1] += m_vBoxPos[1];
m_vEp0[2] += m_vBoxPos[2];
m_vEp1[0] += m_vBoxPos[0];
m_vEp1[1] += m_vBoxPos[1];
m_vEp1[2] += m_vBoxPos[2];
dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip);
Contact0->depth = m_fDepth0;
dVector3Copy(m_vNormal,Contact0->normal);
dVector3Copy(m_vEp0,Contact0->pos);
Contact0->g1 = m_gCylinder;
Contact0->g2 = m_gBox;
Contact0->side1 = -1;
Contact0->side2 = -1;
dVector3Inv(Contact0->normal);
m_nContacts++;
if (m_nContacts != (m_iFlags & NUMC_MASK))
{
dContactGeom* Contact1 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip);
Contact1->depth = m_fDepth1;
dVector3Copy(m_vNormal,Contact1->normal);
dVector3Copy(m_vEp1,Contact1->pos);
Contact1->g1 = m_gCylinder;
Contact1->g2 = m_gBox;
Contact1->side1 = -1;
Contact1->side2 = -1;
dVector3Inv(Contact1->normal);
m_nContacts++;
}
return 1;
}
