//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Line3& rkLine, const Circle3& rkCircle,
Vector3* pkLineClosest, Vector3* pkCircleClosest)
{
Vector3 kDiff = rkLine.Origin() - rkCircle.Center();
Real fDSqrLen = kDiff.SquaredLength();
Real fMdM = rkLine.Direction().SquaredLength();
Real fDdM = kDiff.Dot(rkLine.Direction());
Real fNdM = rkCircle.N().Dot(rkLine.Direction());
Real fDdN = kDiff.Dot(rkCircle.N());
Real fA0 = fDdM;
Real fA1 = fMdM;
Real fB0 = fDdM - fNdM*fDdN;
Real fB1 = fMdM - fNdM*fNdM;
Real fC0 = fDSqrLen - fDdN*fDdN;
Real fC1 = fB0;
Real fC2 = fB1;
Real fRSqr = rkCircle.Radius()*rkCircle.Radius();
Real fA0Sqr = fA0*fA0;
Real fA1Sqr = fA1*fA1;
Real fTwoA0A1 = 2.0f*fA0*fA1;
Real fB0Sqr = fB0*fB0;
Real fB1Sqr = fB1*fB1;
Real fTwoB0B1 = 2.0f*fB0*fB1;
Real fTwoC1 = 2.0f*fC1;
// The minimum point B+t*M occurs when t is a root of the quartic
// equation whose coefficients are defined below.
Polynomial kPoly(5);
kPoly[0] = fA0Sqr*fC0 - fB0Sqr*fRSqr;
kPoly[1] = fTwoA0A1*fC0 + fA0Sqr*fTwoC1 - fTwoB0B1*fRSqr;
kPoly[2] = fA1Sqr*fC0 + fTwoA0A1*fTwoC1 + fA0Sqr*fC2 - fB1Sqr*fRSqr;
kPoly[3] = fA1Sqr*fTwoC1 + fTwoA0A1*fC2;
kPoly[4] = fA1Sqr*fC2;
int iNumRoots;
Real afRoot[4];
kPoly.GetAllRoots(iNumRoots,afRoot);
Real fMinSqrDist = Math::MAX_REAL;
for (int i = 0; i < iNumRoots; i++)
{
// compute distance from P(t) to circle
Vector3 kP = rkLine.Origin() + afRoot[i]*rkLine.Direction();
Vector3 kCircleClosest;
Real fSqrDist = SqrDistance(kP,rkCircle,&kCircleClosest);
if ( fSqrDist < fMinSqrDist )
{
fMinSqrDist = fSqrDist;
if ( pkLineClosest )
*pkLineClosest = kP;
if ( pkCircleClosest )
*pkCircleClosest = kCircleClosest;
}
}
return fMinSqrDist;
}
Real Mgc::SqrDistance (const Vector3& rkPoint, const Line3& rkLine,
Real* pfParam)
{
Vector3 kDiff = rkPoint - rkLine.Origin();
Real fSqrLen = rkLine.Direction().SquaredLength();
Real fT = kDiff.Dot(rkLine.Direction())/fSqrLen;
kDiff -= fT*rkLine.Direction();
if ( pfParam )
*pfParam = fT;
return kDiff.SquaredLength();
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Line3& rkLine, const Box3& rkBox)
{
Real fAWdU[3], fAWxDdU[3], fRhs;
Vector3 kDiff = rkLine.Origin() - rkBox.Center();
Vector3 kWxD = rkLine.Direction().Cross(kDiff);
fAWdU[1] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(1)));
fAWdU[2] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(2)));
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;
fAWdU[0] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(0)));
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 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;
}
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line3& rkLine, const Triangle3& rkTriangle,
Vector3& rkPoint)
{
Real fLinP;
if ( SqrDistance(rkLine,rkTriangle,&fLinP) <= gs_fEpsilon )
{
rkPoint = rkLine.Origin() + fLinP*rkLine.Direction();
return true;
}
return false;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line3& rkLine, const Box3& rkBox,
int& riQuantity, Vector3 akPoint[2])
{
// convert line to box coordinates
Vector3 kDiff = rkLine.Origin() - rkBox.Center();
Vector3 kOrigin(
kDiff.Dot(rkBox.Axis(0)),
kDiff.Dot(rkBox.Axis(1)),
kDiff.Dot(rkBox.Axis(2))
);
Vector3 kDirection(
rkLine.Direction().Dot(rkBox.Axis(0)),
rkLine.Direction().Dot(rkBox.Axis(1)),
rkLine.Direction().Dot(rkBox.Axis(2))
);
Real fT0 = -Math::MAX_REAL, fT1 = Math::MAX_REAL;
bool bIntersects = FindIntersection(kOrigin,kDirection,rkBox.Extents(),
fT0,fT1);
if ( bIntersects )
{
if ( fT0 != fT1 )
{
riQuantity = 2;
akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
akPoint[1] = rkLine.Origin() + fT1*rkLine.Direction();
}
else
{
riQuantity = 1;
akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
}
}
else
{
riQuantity = 0;
}
return bIntersects;
}
bool Wml::FindIntersection (const Line3<Real>& rkLine,
const Cylinder3<Real>& rkCylinder, int& riQuantity,
Vector3<Real> akPoint[2])
{
Real afT[2];
if ( rkCylinder.Capped() )
{
riQuantity = Find(rkLine.Origin(),rkLine.Direction(),rkCylinder,afT);
}
else
{
riQuantity = FindHollow(rkLine.Origin(),rkLine.Direction(),
rkCylinder,afT);
}
for (int i = 0; i < riQuantity; i++)
akPoint[i] = rkLine.Origin() + afT[i]*rkLine.Direction();
return riQuantity > 0;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Plane& rkPlane0, const Plane& rkPlane1,
Line3& rkLine)
{
// If Cross(N0,N1) is zero, then either planes are parallel and separated
// or the same plane. In both cases, 'false' is returned. Otherwise,
// the intersection line is
//
// L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
//
// for some coefficients c0 and c1 and for t any real number (the line
// parameter). Taking dot products with the normals,
//
// d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
// d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
//
// which are two equations in two unknowns. The solution is
//
// c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
// c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
//
// where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
Real fN00 = rkPlane0.Normal().SquaredLength();
Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
Real fN11 = rkPlane1.Normal().SquaredLength();
Real fDet = fN00*fN11 - fN01*fN01;
if ( Math::FAbs(fDet) < gs_fEpsilon )
return false;
Real fInvDet = 1.0f/fDet;
Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;
rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
return true;
}
bool Mgc::FindIntersection (const Line3& rkLine, const Sphere& rkSphere,
int& riQuantity, Vector3 akPoint[2])
{
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
Vector3 kDiff = rkLine.Origin() - rkSphere.Center();
Real fA = rkLine.Direction().SquaredLength();
Real fB = kDiff.Dot(rkLine.Direction());
Real fC = kDiff.SquaredLength() -
rkSphere.Radius()*rkSphere.Radius();
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;
riQuantity = 2;
afT[0] = (-fB - fRoot)*fInvA;
afT[1] = (-fB + fRoot)*fInvA;
akPoint[0] = rkLine.Origin() + afT[0]*rkLine.Direction();
akPoint[1] = rkLine.Origin() + afT[1]*rkLine.Direction();
return true;
}
else
{
riQuantity = 1;
afT[0] = -fB/fA;
akPoint[0] = rkLine.Origin() + afT[0]*rkLine.Direction();
return true;
}
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Line3& rkLine, 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
// compute coordinates of line in box coordinate system
Vector3 kDiff = rkLine.Origin() - rkBox.Center();
Vector3 kPnt(kDiff.Dot(rkBox.Axis(0)),kDiff.Dot(rkBox.Axis(1)),
kDiff.Dot(rkBox.Axis(2)));
Vector3 kDir(rkLine.Direction().Dot(rkBox.Axis(0)),
rkLine.Direction().Dot(rkBox.Axis(1)),
rkLine.Direction().Dot(rkBox.Axis(2)));
// Apply reflections so that direction vector has nonnegative components.
bool bReflect[3];
int i;
for (i = 0; i < 3; i++)
{
if ( kDir[i] < 0.0f )
{
kPnt[i] = -kPnt[i];
kDir[i] = -kDir[i];
bReflect[i] = true;
}
else
{
bReflect[i] = false;
}
}
Real fSqrDistance = 0.0f;
if ( kDir.x > 0.0f )
{
if ( kDir.y > 0.0f )
{
if ( kDir.z > 0.0f )
{
// (+,+,+)
CaseNoZeros(kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (+,+,0)
Case0(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
}
else
{
if ( kDir.z > 0.0f )
{
// (+,0,+)
Case0(0,2,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (+,0,0)
Case00(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
}
}
else
{
if ( kDir.y > 0.0f )
{
if ( kDir.z > 0.0f )
{
// (0,+,+)
Case0(1,2,0,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (0,+,0)
Case00(1,0,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
}
else
{
if ( kDir.z > 0.0f )
{
// (0,0,+)
Case00(2,0,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
}
else
{
// (0,0,0)
Case000(kPnt,rkBox,fSqrDistance);
if ( pfLParam )
*pfLParam = 0.0f;
}
}
}
if ( pfLParam )
{
// undo reflections
for (i = 0; i < 3; i++)
{
if ( bReflect[i] )
kPnt[i] = -kPnt[i];
}
*pfBParam0 = kPnt.x;
*pfBParam1 = kPnt.y;
*pfBParam2 = kPnt.z;
}
return fSqrDistance;
}