// clip polygon with plane and generate new polygon points
static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn,
dVector3 avArrayOut[], int &ctOut,
const dVector4 &plPlane )
{
// start with no output points
ctOut = 0;
int i0 = ctIn-1;
// for each edge in input polygon
for (int i1=0; i1<ctIn; i0=i1, i1++) {
// calculate distance of edge points to plane
dReal fDistance0 = POINTDISTANCE( plPlane ,avArrayIn[i0] );
dReal fDistance1 = POINTDISTANCE( plPlane ,avArrayIn[i1] );
// if first point is in front of plane
if( fDistance0 >= 0 ) {
// emit point
avArrayOut[ctOut][0] = avArrayIn[i0][0];
avArrayOut[ctOut][1] = avArrayIn[i0][1];
avArrayOut[ctOut][2] = avArrayIn[i0][2];
ctOut++;
}
// if points are on different sides
if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
// find intersection point of edge and plane
dVector3 vIntersectionPoint;
vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
// emit intersection point
avArrayOut[ctOut][0] = vIntersectionPoint[0];
avArrayOut[ctOut][1] = vIntersectionPoint[1];
avArrayOut[ctOut][2] = vIntersectionPoint[2];
ctOut++;
}
}
}
BOOL sTrimeshCapsuleColliderData::_cldClipEdgeToPlane(
dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
{
// calculate distance of edge points to plane
dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 );
dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 );
// if both points are behind the plane
if ( fDistance0 < 0 && fDistance1 < 0 )
{
// do nothing
return FALSE;
// if both points in front of the plane
} else if ( fDistance0 > 0 && fDistance1 > 0 )
{
// accept them
return TRUE;
// if we have edge/plane intersection
} else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0))
{
// find intersection point of edge and plane
dVector3 vIntersectionPoint;
vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
// clamp correct edge to intersection point
if ( fDistance0 < 0 )
{
SET(vEpnt0,vIntersectionPoint);
} else
{
SET(vEpnt1,vIntersectionPoint);
}
return TRUE;
}
return TRUE;
}
// test one mesh triangle on intersection with capsule
void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule(
const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
uint8 flags)
{
// calculate edges
SUBTRACT(v1,v0,m_vE0);
SUBTRACT(v2,v1,m_vE1);
SUBTRACT(v0,v2,m_vE2);
dVector3 _minus_vE0;
SUBTRACT(v0,v1,_minus_vE0);
// calculate poly normal
dCalcVectorCross3(m_vN,m_vE1,_minus_vE0);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(m_vN))
{
return;
}
// create plane from triangle
dReal plDistance = -dCalcVectorDot3(v0,m_vN);
dVector4 plTrianglePlane;
CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance);
// calculate capsule distance to plane
dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition);
// Capsule must be over positive side of triangle
if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
{
// if not don't generate contacts
return;
}
dVector3 vPnt0;
SET (vPnt0,v0);
dVector3 vPnt1;
SET (vPnt1,v1);
dVector3 vPnt2;
SET (vPnt2,v2);
if (fDistanceCapsuleCenterToPlane < 0 )
{
SET (vPnt0,v0);
SET (vPnt1,v2);
SET (vPnt2,v1);
}
// do intersection test and find best separating axis
if (!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags))
{
// if not found do nothing
return;
}
// if best separation axis is not found
if (m_iBestAxis == 0 )
{
// this should not happen (we should already exit in that case)
dIASSERT(FALSE);
// do nothing
return;
}
// calculate caps centers in absolute space
dVector3 vCposTrans;
vCposTrans[0] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius;
vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius;
vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius;
dVector3 vCEdgePoint0;
vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCEdgePoint1;
vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// transform capsule edge points into triangle space
vCEdgePoint0[0] -= vPnt0[0];
vCEdgePoint0[1] -= vPnt0[1];
vCEdgePoint0[2] -= vPnt0[2];
vCEdgePoint1[0] -= vPnt0[0];
vCEdgePoint1[1] -= vPnt0[1];
vCEdgePoint1[2] -= vPnt0[2];
dVector4 plPlane;
dVector3 _minus_vN;
_minus_vN[0] = -m_vN[0];
_minus_vN[1] = -m_vN[1];
_minus_vN[2] = -m_vN[2];
// triangle plane
CONSTRUCTPLANE(plPlane,_minus_vN,0);
//plPlane = Plane4f( -m_vN, 0);
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
// plane with edge 0
dVector3 vTemp;
dCalcVectorCross3(vTemp,m_vN,m_vE0);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE1);
CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5)));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE2);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
return;
}
// return capsule edge points into absolute space
vCEdgePoint0[0] += vPnt0[0];
vCEdgePoint0[1] += vPnt0[1];
vCEdgePoint0[2] += vPnt0[2];
vCEdgePoint1[0] += vPnt0[0];
vCEdgePoint1[1] += vPnt0[1];
vCEdgePoint1[2] += vPnt0[2];
// calculate depths for both contact points
SUBTRACT(vCEdgePoint0,m_vCapsulePosition,vTemp);
dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp);
dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
// clamp depths to zero
if (fDepth0 < 0)
{
fDepth0 = 0.0f;
}
if (fDepth1 < 0 )
{
fDepth1 = 0.0f;
}
// Cached contacts's data
// contact 0
dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
m_gLocalContacts[m_ctContacts].fDepth = fDepth0;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
if (m_ctContacts < (m_iFlags & NUMC_MASK)) {
// contact 1
m_gLocalContacts[m_ctContacts].fDepth = fDepth1;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
}
}
