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 _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;
}
int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (Cylinder->type == dCylinderClass);
dIASSERT (Sphere->type == dSphereClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
//unsigned char* pContactData = (unsigned char*)contact;
int GeomCount = 0; // count of used contacts
#ifdef dSINGLE
const dReal toleranz = REAL(0.0001);
#endif
#ifdef dDOUBLE
const dReal toleranz = REAL(0.0000001);
#endif
// get the data from the geoms
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
//const dReal* pfRot1 = dGeomGetRotation(Cylinder);
dReal radius2;
radius2 = dGeomSphereGetRadius(Sphere);
const dReal* SpherePos = dGeomGetPosition(Sphere);
// G1Pos1 is the middle of the first disc
// G1Pos2 is the middle of the second disc
// vDir1 is the unit direction of the cylinderaxis
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); // just a precomputed factor
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;
dReal t;
// Step 1: compute the two distances 's' and 't'
// 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
if(s < (-radius2) || s > (length + radius2) )
{
// Sphere is too far away from the discs
// no collision
return 0;
}
// C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
// t is the distance from the Sphere-middle to the cylinder-axis!
t = dVector3Length(C);
if(t > (radius + radius2) )
{
// Sphere is too far away from the cylinder axis!
// no collision
return 0;
}
// decide which kind of collision we have:
if(t > radius && (s < 0 || s > length) )
{
// 3. collision
if(s <= 0)
{
contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
else
{
// now s is bigger than length here!
contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
else if( (radius - t) <= s && (radius - t) <= (length - s) )
{
// 1. collsision
if(t > (radius2 + toleranz))
{
// cylinder-axis is outside the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
C[0] /= t;
C[1] /= t;
C[2] /= t;
contact->pos[0] = C[0] * radius2 + SpherePos[0];
contact->pos[1] = C[1] * radius2 + SpherePos[1];
contact->pos[2] = C[2] * radius2 + SpherePos[2];
contact->normal[0] = C[0];
contact->normal[1] = C[1];
contact->normal[2] = C[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
else
{
// cylinder-axis is outside of the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
contact->pos[0] = C[0] + SpherePos[0];
contact->pos[1] = C[1] + SpherePos[1];
contact->pos[2] = C[2] + SpherePos[2];
contact->normal[0] = C[0] / t;
contact->normal[1] = C[1] / t;
contact->normal[2] = C[2] / t;
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
}
else
{
// 2. collision
if(s <= (length * REAL(0.5)) )
{
// collsision with the first disc
contact->depth = s + radius2;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
contact->normal[0] = vDir1[0];
contact->normal[1] = vDir1[1];
contact->normal[2] = vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
else
{
// collsision with the second disc
contact->depth = (radius2 + length - s);
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
contact->normal[0] = -vDir1[0];
contact->normal[1] = -vDir1[1];
contact->normal[2] = -vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
return GeomCount;
}
// 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 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;
}
// 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;
}
