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