inline int _ProcessLocalContacts(sData& cData)
{
if (cData.nContacts == 0)
{
return 0;
}
#ifdef OPTIMIZE_CONTACTS
if (cData.nContacts > 1 && !(cData.iFlags & CONTACTS_UNIMPORTANT))
{
// Can be optimized...
_OptimizeLocalContacts(cData);
}
#endif
int iContact = 0;
dContactGeom* Contact = 0;
int nFinalContact = 0;
for (iContact = 0; iContact < cData.nContacts; iContact ++)
{
if (1 == cData.gLocalContacts[iContact].nFlags)
{
Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip);
Contact->depth = cData.gLocalContacts[iContact].fDepth;
dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal);
dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos);
Contact->g1 = cData.gCylinder;
Contact->g2 = cData.gTrimesh;
Contact->side2 = cData.gLocalContacts[iContact].triIndex;
dVector3Inv(Contact->normal);
nFinalContact++;
}
}
// debug
//if (nFinalContact != cData.nContacts)
//{
// printf("[Info] %d contacts generated,%d filtered.\n",cData.nContacts,cData.nContacts-nFinalContact);
//}
return nFinalContact;
}
bool _cldTestAxis(sData& cData,
const dVector3 &v0,
const dVector3 &v1,
const dVector3 &v2,
dVector3& vAxis,
int iAxis,
bool bNoFlip = false)
{
// calculate length of separating axis vector
dReal fL = dVector3Length(vAxis);
// if not long enough
if ( fL < REAL(1e-5) )
{
// do nothing
return true;
}
// otherwise normalize it
vAxis[0] /= fL;
vAxis[1] /= fL;
vAxis[2] /= fL;
dReal fdot1 = dVector3Dot(cData.vCylinderAxis,vAxis);
// project capsule on vAxis
dReal frc;
if (dFabs(fdot1) > REAL(1.0) )
{
// fdot1 = REAL(1.0);
frc = dFabs(cData.fCylinderSize* REAL(0.5));
}
else
{
frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1)
+ cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
}
dVector3 vV0;
dVector3Subtract(v0,cData.vCylinderPos,vV0);
dVector3 vV1;
dVector3Subtract(v1,cData.vCylinderPos,vV1);
dVector3 vV2;
dVector3Subtract(v2,cData.vCylinderPos,vV2);
// project triangle on vAxis
dReal afv[3];
afv[0] = dVector3Dot( vV0 , vAxis );
afv[1] = dVector3Dot( vV1 , vAxis );
afv[2] = dVector3Dot( vV2 , vAxis );
dReal fMin = MAX_REAL;
dReal fMax = -MAX_REAL;
// for each vertex
for(int i = 0; i < 3; i++)
{
// find minimum
if (afv[i]<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;
}
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;
}
// test for given separating axis
int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis )
{
// check length of input normal
dReal fL = dVector3Length(vInputNormal);
// if not long enough
if ( fL < 1e-5f )
{
// do nothing
return 1;
}
// otherwise make it unit for sure
dNormalize3(vInputNormal);
// project box and Cylinder on mAxis
dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal);
dReal frc;
if (fdot1 > REAL(1.0))
{
fdot1 = REAL(1.0);
frc = dFabs(cData.fCylinderSize*REAL(0.5));
}
// project box and capsule on iAxis
frc = dFabs( fdot1 * (cData.fCylinderSize*REAL(0.5))) + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
dVector3 vTemp1;
dReal frb = REAL(0.0);
dMat3GetCol(cData.mBoxRot,0,vTemp1);
frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[0];
dMat3GetCol(cData.mBoxRot,1,vTemp1);
frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[1];
dMat3GetCol(cData.mBoxRot,2,vTemp1);
frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[2];
// project their distance on separating axis
dReal fd = dVector3Dot(cData.vDiff,vInputNormal);
// if they do not overlap exit, we have no intersection
if ( dFabs(fd) > frc+frb )
{
return 0;
}
// get depth
dReal fDepth = - dFabs(fd) + (frc+frb);
// get maximum depth
if ( fDepth < cData.fBestDepth )
{
cData.fBestDepth = fDepth;
dVector3Copy(vInputNormal,cData.vNormal);
cData.iBestAxis = iAxis;
cData.fBestrb = frb;
cData.fBestrc = frc;
// flip normal if interval is wrong faced
if (fd > 0)
{
dVector3Inv(cData.vNormal);
}
}
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;
}
// test for given separating axis
int sCylinderBoxData::_cldTestAxis( dVector3& vInputNormal, int iAxis )
{
// check length of input normal
dReal fL = dVector3Length(vInputNormal);
// if not long enough
if ( fL < REAL(1e-5) )
{
// do nothing
return 1;
}
// otherwise make it unit for sure
dNormalize3(vInputNormal);
// project box and Cylinder on mAxis
dReal fdot1 = dVector3Dot(m_vCylinderAxis, vInputNormal);
dReal frc;
if (fdot1 > REAL(1.0))
{
// assume fdot1 = 1
frc = m_fCylinderSize*REAL(0.5);
}
else if (fdot1 < REAL(-1.0))
{
// assume fdot1 = -1
frc = m_fCylinderSize*REAL(0.5);
}
else
{
// project box and capsule on iAxis
frc = dFabs( fdot1 * (m_fCylinderSize*REAL(0.5))) + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
}
dVector3 vTemp1;
dMat3GetCol(m_mBoxRot,0,vTemp1);
dReal frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[0];
dMat3GetCol(m_mBoxRot,1,vTemp1);
frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[1];
dMat3GetCol(m_mBoxRot,2,vTemp1);
frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[2];
// project their distance on separating axis
dReal fd = dVector3Dot(m_vDiff,vInputNormal);
// get depth
dReal fDepth = frc + frb; // Calculate partial depth
// if they do not overlap exit, we have no intersection
if ( dFabs(fd) > fDepth )
{
return 0;
}
// Finalyze the depth calculation
fDepth -= dFabs(fd);
// get maximum depth
if ( fDepth < m_fBestDepth )
{
m_fBestDepth = fDepth;
dVector3Copy(vInputNormal,m_vNormal);
m_iBestAxis = iAxis;
m_fBestrb = frb;
m_fBestrc = frc;
// flip normal if interval is wrong faced
if (fd > 0)
{
dVector3Inv(m_vNormal);
}
}
return 1;
}
