Real Wml::SqrDistance (const Vector3<Real>& rkPoint,
const Segment3<Real>& rkSegment, Real* pfParam)
{
Vector3<Real> kDiff = rkPoint - rkSegment.Origin();
Real fT = kDiff.Dot(rkSegment.Direction());
if ( fT <= (Real)0.0 )
{
fT = (Real)0.0;
}
else
{
Real fSqrLen= rkSegment.Direction().SquaredLength();
if ( fT >= fSqrLen )
{
fT = (Real)1.0;
kDiff -= rkSegment.Direction();
}
else
{
fT /= fSqrLen;
kDiff -= fT*rkSegment.Direction();
}
}
if ( pfParam )
*pfParam = fT;
return kDiff.SquaredLength();
}
Segment3<Real> Cylinder3<Real>::GetSegment () const
{
Segment3<Real> kSegment;
kSegment.Direction() = m_fHeight*m_kDirection;
kSegment.Origin() = m_kCenter - ((Real)0.5)*kSegment.Direction();
return kSegment;
}
bool Wml::FindIntersection (const Segment3<Real>& rkSegment,
const Cylinder3<Real>& rkCylinder, int& riQuantity,
Vector3<Real> akPoint[2])
{
Real afT[2];
if ( rkCylinder.Capped() )
{
riQuantity = Find(rkSegment.Origin(),rkSegment.Direction(),
rkCylinder,afT);
}
else
{
riQuantity = FindHollow(rkSegment.Origin(),rkSegment.Direction(),
rkCylinder,afT);
}
int iClipQuantity = 0;
for (int i = 0; i < riQuantity; i++)
{
if ( (Real)0.0 <= afT[i] && afT[i] <= (Real)1.0 )
{
akPoint[iClipQuantity++] = rkSegment.Origin() +
afT[i]*rkSegment.Direction();
}
}
riQuantity = iClipQuantity;
return riQuantity > 0;
}
Real Mgc::SqrDistance (const Vector3& rkPoint, const Segment3& rkSegment,
Real* pfParam)
{
Vector3 kDiff = rkPoint - rkSegment.Origin();
Real fT = kDiff.Dot(rkSegment.Direction());
if ( fT <= 0.0f )
{
fT = 0.0f;
}
else
{
Real fSqrLen= rkSegment.Direction().SquaredLength();
if ( fT >= fSqrLen )
{
fT = 1.0f;
kDiff -= rkSegment.Direction();
}
else
{
fT /= fSqrLen;
kDiff -= fT*rkSegment.Direction();
}
}
if ( pfParam )
*pfParam = fT;
return kDiff.SquaredLength();
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Segment3& rkSegment, const Box3& rkBox,
int& riQuantity, Vector3 akPoint[2])
{
// convert segment to box coordinates
Vector3 kDiff = rkSegment.Origin() - rkBox.Center();
Vector3 kOrigin(
kDiff.Dot(rkBox.Axis(0)),
kDiff.Dot(rkBox.Axis(1)),
kDiff.Dot(rkBox.Axis(2))
);
Vector3 kDirection(
rkSegment.Direction().Dot(rkBox.Axis(0)),
rkSegment.Direction().Dot(rkBox.Axis(1)),
rkSegment.Direction().Dot(rkBox.Axis(2))
);
Real fT0 = 0.0f, fT1 = 1.0f;
bool bIntersects = FindIntersection(kOrigin,kDirection,rkBox.Extents(),
fT0,fT1);
if ( bIntersects )
{
if ( fT0 > 0.0f )
{
if ( fT1 < 1.0f )
{
riQuantity = 2;
akPoint[0] = rkSegment.Origin() + fT0*rkSegment.Direction();
akPoint[1] = rkSegment.Origin() + fT1*rkSegment.Direction();
}
else
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin() + fT0*rkSegment.Direction();
}
}
else // fT0 == 0
{
if ( fT1 < 1.0f )
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin() + fT1*rkSegment.Direction();
}
else // fT1 == 1
{
// segment entirely in box
riQuantity = 0;
}
}
}
else
{
riQuantity = 0;
}
return bIntersects;
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Segment3& rkSeg, const Box3& rkBox,
Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
#ifdef _DEBUG
// The four parameters pointers are either all non-null or all null.
if ( pfLParam )
{
assert( pfBParam0 && pfBParam1 && pfBParam2 );
}
else
{
assert( !pfBParam0 && !pfBParam1 && !pfBParam2 );
}
#endif
Line3 kLine;
kLine.Origin() = rkSeg.Origin();
kLine.Direction() = rkSeg.Direction();
Real fLP, fBP0, fBP1, fBP2;
Real fSqrDistance = SqrDistance(kLine,rkBox,&fLP,&fBP0,&fBP1,&fBP2);
if ( fLP >= 0.0f )
{
if ( fLP <= 1.0f )
{
if ( pfLParam )
{
*pfLParam = fLP;
*pfBParam0 = fBP0;
*pfBParam1 = fBP1;
*pfBParam2 = fBP2;
}
return fSqrDistance;
}
else
{
fSqrDistance = SqrDistance(rkSeg.Origin()+rkSeg.Direction(),
rkBox,pfBParam0,pfBParam1,pfBParam2);
if ( pfLParam )
*pfLParam = 1.0f;
return fSqrDistance;
}
}
else
{
fSqrDistance = SqrDistance(rkSeg.Origin(),rkBox,pfBParam0,
pfBParam1,pfBParam2);
if ( pfLParam )
*pfLParam = 0.0f;
return fSqrDistance;
}
}
//----------------------------------------------------------------------------
float VertexCollapse::GetWeight (int iM, int iZ, int iP, Vector3* akVertex)
{
Segment3 kSegment;
kSegment.Origin() = akVertex[iM];
kSegment.Direction() = akVertex[iP] - akVertex[iM];
float fSqrDist = SqrDistance(akVertex[iZ],kSegment);
float fSqrLen = kSegment.Direction().SquaredLength();
return ( fSqrLen > 0.0f ? fSqrDist/fSqrLen : FLT_MAX );
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Segment3& rkSegment,
const Triangle3& rkTriangle, Vector3& rkPoint)
{
Real fSegP;
if ( SqrDistance(rkSegment,rkTriangle,&fSegP) <= gs_fEpsilon )
{
rkPoint = rkSegment.Origin() + fSegP*rkSegment.Direction();
return true;
}
return false;
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Segment3& rkSegment, const Box3& rkBox)
{
Real fAWdU[3], fADdU[3], fAWxDdU[3], fRhs;
Vector3 kSDir = 0.5f*rkSegment.Direction();
Vector3 kSCen = rkSegment.Origin() + kSDir;
Vector3 kDiff = kSCen - rkBox.Center();
fAWdU[0] = Math::FAbs(kSDir.Dot(rkBox.Axis(0)));
fADdU[0] = Math::FAbs(kDiff.Dot(rkBox.Axis(0)));
fRhs = rkBox.Extent(0) + fAWdU[0];
if ( fADdU[0] > fRhs )
return false;
fAWdU[1] = Math::FAbs(kSDir.Dot(rkBox.Axis(1)));
fADdU[1] = Math::FAbs(kDiff.Dot(rkBox.Axis(1)));
fRhs = rkBox.Extent(1) + fAWdU[1];
if ( fADdU[1] > fRhs )
return false;
fAWdU[2] = Math::FAbs(kSDir.Dot(rkBox.Axis(2)));
fADdU[2] = Math::FAbs(kDiff.Dot(rkBox.Axis(2)));
fRhs = rkBox.Extent(2) + fAWdU[2];
if ( fADdU[2] > fRhs )
return false;
Vector3 kWxD = kSDir.Cross(kDiff);
fAWxDdU[0] = Math::FAbs(kWxD.Dot(rkBox.Axis(0)));
fRhs = rkBox.Extent(1)*fAWdU[2] + rkBox.Extent(2)*fAWdU[1];
if ( fAWxDdU[0] > fRhs )
return false;
fAWxDdU[1] = Math::FAbs(kWxD.Dot(rkBox.Axis(1)));
fRhs = rkBox.Extent(0)*fAWdU[2] + rkBox.Extent(2)*fAWdU[0];
if ( fAWxDdU[1] > fRhs )
return false;
fAWxDdU[2] = Math::FAbs(kWxD.Dot(rkBox.Axis(2)));
fRhs = rkBox.Extent(0)*fAWdU[1] + rkBox.Extent(1)*fAWdU[0];
if ( fAWxDdU[2] > fRhs )
return false;
return true;
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Triangle3& rkTri0, const Triangle3& rkTri1,
Real* pfTri0P0, Real* pfTri0P1, Real* pfTri1P0, Real* pfTri1P1)
{
Real fS, fT, fU, fV, fS0, fT0, fU0, fV0, fSqrDist, fSqrDist0;
Segment3 kSeg;
// compare edges of tri0 against all of tri1
kSeg.Origin() = rkTri0.Origin();
kSeg.Direction() = rkTri0.Edge0();
fSqrDist = SqrDistance(kSeg,rkTri1,&fS,&fU,&fV);
fT = 0.0f;
kSeg.Direction() = rkTri0.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
fS0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = kSeg.Origin() + rkTri0.Edge0();
kSeg.Direction() = kSeg.Direction() - rkTri0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
fS0 = 1.0f-fT0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
// compare edges of tri1 against all of tri0
kSeg.Origin() = rkTri1.Origin();
kSeg.Direction() = rkTri1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fU0,&fS0,&fT0);
fV0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Direction() = rkTri1.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
fU0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = kSeg.Origin() + rkTri1.Edge0();
kSeg.Direction() = kSeg.Direction() - rkTri1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
fU0 = 1.0f-fV0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
if ( pfTri0P0 )
*pfTri0P0 = fS;
if ( pfTri0P1 )
*pfTri0P1 = fT;
if ( pfTri1P0 )
*pfTri1P0 = fU;
if ( pfTri1P1 )
*pfTri1P1 = fV;
return Math::FAbs(fSqrDist);
}
Real Wml::SqrDistance (const Rectangle3<Real>& rkRct0,
const Rectangle3<Real>& rkRct1, Real* pfRct0P0, Real* pfRct0P1,
Real* pfRct1P0, Real* pfRct1P1)
{
Real fS, fT, fS0, fT0; // rct0 parameters
Real fU, fV, fU0, fV0; // rct1 parameters
Real fSqrDist, fSqrDist0;
Segment3<Real> kSeg;
// compare edges of rct0 against all of rct1
kSeg.Origin() = rkRct0.Origin();
kSeg.Direction() = rkRct0.Edge0();
fSqrDist = SqrDistance(kSeg,rkRct1,&fS,&fU,&fV);
fT = (Real)0.0;
kSeg.Direction() = rkRct0.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkRct1,&fT0,&fU0,&fV0);
fS0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct0.Origin() + rkRct0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct1,&fT0,&fU0,&fV0);
fS0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct0.Origin() + rkRct0.Edge1();
kSeg.Direction() = rkRct0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct1,&fS0,&fU0,&fV0);
fT0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
// compare edges of pgm1 against all of pgm0
kSeg.Origin() = rkRct1.Origin();
kSeg.Direction() = rkRct1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fU0,&fS0,&fT0);
fV0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Direction() = rkRct1.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fV0,&fS0,&fT0);
fU0 = (Real)0.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct1.Origin() + rkRct1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fV0,&fS0,&fT0);
fU0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkRct1.Origin() + rkRct1.Edge1();
kSeg.Direction() = rkRct1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkRct0,&fU0,&fS0,&fT0);
fV0 = (Real)1.0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
if ( pfRct0P0 )
*pfRct0P0 = fS;
if ( pfRct0P1 )
*pfRct0P1 = fT;
if ( pfRct1P0 )
*pfRct1P0 = fU;
if ( pfRct1P1 )
*pfRct1P1 = fV;
return fSqrDist;
}
bool Mgc::FindIntersection (const Segment3& rkSegment, const Sphere& rkSphere,
int& riQuantity, Vector3 akPoint[2])
{
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
Vector3 kDiff = rkSegment.Origin() - rkSphere.Center();
Real fA = rkSegment.Direction().SquaredLength();
Real fB = kDiff.Dot(rkSegment.Direction());
Real fC = kDiff.SquaredLength() -
rkSphere.Radius()*rkSphere.Radius();
// no intersection if Q(t) has no real roots
Real afT[2];
Real fDiscr = fB*fB - fA*fC;
if ( fDiscr < 0.0f )
{
riQuantity = 0;
return false;
}
else if ( fDiscr > 0.0f )
{
Real fRoot = Math::Sqrt(fDiscr);
Real fInvA = 1.0f/fA;
afT[0] = (-fB - fRoot)*fInvA;
afT[1] = (-fB + fRoot)*fInvA;
// assert: t0 < t1 since A > 0
if ( afT[0] > 1.0f || afT[1] < 0.0f )
{
riQuantity = 0;
return false;
}
else if ( afT[0] >= 0.0f )
{
if ( afT[1] > 1.0f )
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();
return true;
}
else
{
riQuantity = 2;
akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();
akPoint[1] = rkSegment.Origin()+afT[1]*rkSegment.Direction();
return true;
}
}
else // afT[1] >= 0
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin()+afT[1]*rkSegment.Direction();
return true;
}
}
else
{
afT[0] = -fB/fA;
if ( 0.0f <= afT[0] && afT[0] <= 1.0f )
{
riQuantity = 1;
akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();
return true;
}
else
{
riQuantity = 0;
return false;
}
}
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Parallelogram3& rkPgm0,
const Parallelogram3& rkPgm1, Real* pfPgm0P0, Real* pfPgm0P1,
Real* pfPgm1P0, Real* pfPgm1P1)
{
Real fS, fT, fS0, fT0; // pgm0 parameters
Real fU, fV, fU0, fV0; // pgm1 parameters
Real fSqrDist, fSqrDist0;
Segment3 kSeg;
// compare edges of pgm0 against all of pgm1
kSeg.Origin() = rkPgm0.Origin();
kSeg.Direction() = rkPgm0.Edge0();
fSqrDist = SqrDistance(kSeg,rkPgm1,&fS,&fU,&fV);
fT = 0.0f;
kSeg.Direction() = rkPgm0.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fT0,&fU0,&fV0);
fS0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm0.Origin() + rkPgm0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fT0,&fU0,&fV0);
fS0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm0.Origin() + rkPgm0.Edge1();
kSeg.Direction() = rkPgm0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fS0,&fU0,&fV0);
fT0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
// compare edges of pgm1 against all of pgm0
kSeg.Origin() = rkPgm1.Origin();
kSeg.Direction() = rkPgm1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fU0,&fS0,&fT0);
fV0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Direction() = rkPgm1.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fV0,&fS0,&fT0);
fU0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm1.Origin() + rkPgm1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fV0,&fS0,&fT0);
fU0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = rkPgm1.Origin() + rkPgm1.Edge1();
kSeg.Direction() = rkPgm1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fU0,&fS0,&fT0);
fV0 = 1.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
if ( pfPgm0P0 )
*pfPgm0P0 = fS;
if ( pfPgm0P1 )
*pfPgm0P1 = fT;
if ( pfPgm1P0 )
*pfPgm1P0 = fU;
if ( pfPgm1P1 )
*pfPgm1P1 = fV;
return Math::FAbs(fSqrDist);
}
static bool FindTriSphrCoplanarIntersection (int iVertex,
const Vector3<Real> akV[3], const Vector3<Real>& /* rkNormal */,
const Vector3<Real>& rkSideNorm, const Vector3<Real>& rkSide,
const Sphere3<Real>& rkSphere, const Vector3<Real>& rkTriVelocity,
const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real> akP[6])
{
// TO DO. The parameter rkNormal is not used here. Is this an error?
// Or does the caller make some adjustments to the other inputs to
// account for the normal?
// iVertex is the "hinge" vertex that the two potential edges that can
// be intersected by the sphere connect to, and it indexes into akV.
// rkSideNorm is the normal of the plane formed by (iVertex,iVertex+1)
// and the tri norm, passed so as not to recalculate
// check for intersections at time 0
Vector3<Real> kDist = akV[iVertex] - rkSphere.Center();
if ( kDist.SquaredLength() < rkSphere.Radius()*rkSphere.Radius() )
{
// already intersecting that vertex
rfTFirst = (Real)0.0;
return false;
}
// Tri stationary, sphere moving
Vector3<Real> kVel = rkSphVelocity - rkTriVelocity;
// check for easy out
if ( kVel.Dot(kDist) <= (Real)0.0 )
{
// moving away
return false;
}
// find intersection of velocity ray and side normal
// project ray and plane onto the plane normal
Real fPlane = rkSideNorm.Dot(akV[iVertex]);
Real fCenter = rkSideNorm.Dot(rkSphere.Center());
Real fVel = rkSideNorm.Dot(kVel);
Real fFactor = (fPlane - fCenter)/fVel;
Vector3<Real> kPoint = rkSphere.Center() + fFactor*kVel;
// now, find which side of the hinge vertex this lies by projecting
// both the vertex and this new point onto the triangle edge (the same
// edge whose "normal" was used to find this point)
Real fVertex = rkSide.Dot(akV[iVertex]);
Real fPoint = rkSide.Dot(kPoint);
Segment3<Real> kSeg;
if ( fPoint >= fVertex )
{
// intersection with edge (iVertex,iVertex+1)
kSeg.Origin() = akV[iVertex];
kSeg.Direction() = akV[(iVertex+1)%3] - akV[iVertex];
}
else
{
// intersection with edge (iVertex-1,iVertex)
if ( iVertex != 0 )
kSeg.Origin() = akV[iVertex-1];
else
kSeg.Origin() = akV[2];
kSeg.Direction() = akV[iVertex] - kSeg.Origin();
}
// This could be either an sphere-edge or a sphere-vertex intersection
// (this test isn't enough to differentiate), so use the full-on
// line-sphere test.
return FindIntersection(kSeg,rkTriVelocity,rkSphere,rkSphVelocity,
rfTFirst,fTMax,riQuantity,akP);
}
bool Wml::FindIntersection (const Triangle3<Real>& rkTri,
const Vector3<Real>& rkTriVelocity, const Sphere3<Real>& rkSphere,
const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real> akP[6])
{
// triangle vertices
Vector3<Real> akV[3] =
{
rkTri.Origin(),
rkTri.Origin() + rkTri.Edge0(),
rkTri.Origin() + rkTri.Edge1()
};
// triangle edges
Vector3<Real> akE[3] =
{
akV[1] - akV[0],
akV[2] - akV[1],
akV[0] - akV[2]
};
// triangle normal
Vector3<Real> kN = akE[1].Cross(akE[0]);
// sphere center projection on triangle normal
Real fNdC = kN.Dot(rkSphere.Center());
// Radius projected length in normal direction. This defers the square
// root to normalize kN until absolutely needed.
Real fNormRadiusSqr =
kN.SquaredLength()*rkSphere.Radius()*rkSphere.Radius();
// triangle projection on triangle normal
Real fNdT = kN.Dot(akV[0]);
// Distance from sphere to triangle along the normal
Real fDist = fNdC - fNdT;
// normals for the plane formed by edge i and the triangle normal
Vector3<Real> akExN[3] =
{
akE[0].Cross(kN),
akE[1].Cross(kN),
akE[2].Cross(kN)
};
Segment3<Real> kSeg;
if ( fDist*fDist <= fNormRadiusSqr )
{
// sphere currently intersects the plane of the triangle
// see which edges the sphere center is inside/outside of
bool bInside[3];
for (int i = 0; i < 3; i++ )
{
bInside[i] = ( akExN[i].Dot(rkSphere.Center()) >=
akExN[i].Dot(akV[i]) );
}
if ( bInside[0] )
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// triangle inside sphere
return false;
}
else // !bInside[2]
{
// potential intersection with edge 2
kSeg.Origin() = akV[2];
kSeg.Direction() = akE[2];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edge 1
kSeg.Origin() = akV[1];
kSeg.Direction() = akE[1];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with edges 1,2
return FindTriSphrCoplanarIntersection(2,akV,kN,akExN[2],
akE[2],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
}
}
else // !bInside[0]
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// potential intersection with edge 0
kSeg.Origin() = akV[0];
kSeg.Direction() = akE[0];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with edges 2,0
return FindTriSphrCoplanarIntersection(0,akV,kN,akExN[0],
akE[0],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edges 0,1
return FindTriSphrCoplanarIntersection(1,akV,kN,akExN[1],
akE[1],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// we should not get here
assert( false );
return false;
}
}
}
}
else
{
// sphere does not currently intersect the plane of the triangle
// sphere moving, triangle stationary
Vector3<Real> kVel = rkSphVelocity - rkTriVelocity;
// Find point of intersection of the sphere and the triangle
// plane. Where this point occurs on the plane relative to the
// triangle determines the potential kind of intersection.
kN.Normalize();
// Point on sphere we care about intersecting the triangle plane
Vector3<Real> kSpherePoint;
// Which side of the triangle is the sphere on?
if ( fNdC > fNdT )
{
// positive side
if ( kVel.Dot(kN) >= (Real)0.0 )
{
// moving away, easy out
return false;
}
kSpherePoint = rkSphere.Center() - rkSphere.Radius()*kN;
}
else
{
// negative side
if ( kVel.Dot(kN) <= (Real)0.0 )
{
// moving away, easy out
return false;
}
kSpherePoint = rkSphere.Center() + rkSphere.Radius()*kN;
}
// find intersection of velocity ray and triangle plane
// project ray and plane onto the plane normal
Real fPlane = kN.Dot(akV[0]);
Real fPoint = kN.Dot(kSpherePoint);
Real fVel = kN.Dot(kVel);
Real fTime = (fPlane - fPoint)/fVel;
// where this intersects
Vector3<Real> kIntrPoint = kSpherePoint + fTime*kVel;
// see which edges this intersection point is inside/outside of
bool bInside[3];
for (int i = 0; i < 3; i++ )
bInside[i] = (akExN[i].Dot(kIntrPoint) >= akExN[i].Dot(akV[i]));
if ( bInside[0] )
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// intersects face at time fTime
if ( fTime > fTMax )
{
// intersection after tMax
return false;
}
else
{
rfTFirst = fTime;
riQuantity = 1;
// kIntrPoint is the point in space, assuming that
// TriVel is 0. Re-adjust the point to where it
// should be, were it not.
akP[0] = kIntrPoint + fTime*rkTriVelocity;
return true;
}
}
else // !bInside[2]
{
// potential intersection with edge 2
kSeg.Origin() = akV[2];
kSeg.Direction() = akE[2];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edge 1
kSeg.Origin() = akV[1];
kSeg.Direction() = akE[1];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with vertex 2
return FindSphereVertexIntersection(akV[2],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
}
}
else // !bInside[0]
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// potential intersection with edge 0
kSeg.Origin() = akV[0];
kSeg.Direction() = akE[0];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with vertex 0
return FindSphereVertexIntersection(akV[0],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with vertex 1
return FindSphereVertexIntersection(akV[1],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
else // !bInside[2]
{
// we should not get here
assert( false );
return false;
}
}
}
}
}