bool IntrSphere3Sphere3<Real>::Test ( Real tmax,
const Vector3<Real>& velocity0, const Vector3<Real>& velocity1 )
{
Vector3<Real> relVelocity = velocity1 - velocity0;
Real a = relVelocity.SquaredLength();
Vector3<Real> CDiff = mSphere1->Center - mSphere0->Center;
Real c = CDiff.SquaredLength();
Real rSum = mSphere0->Radius + mSphere1->Radius;
Real rSumSqr = rSum * rSum;
if ( a > ( Real )0 )
{
Real b = CDiff.Dot( relVelocity );
if ( b <= ( Real )0 )
{
if ( -tmax * a <= b )
{
return a * c - b * b <= a * rSumSqr;
}
else
{
return tmax * ( tmax * a + ( ( Real )2 ) * b ) + c <= rSumSqr;
}
}
}
return c <= rSumSqr;
}
bool IntrSphere3Sphere3<Real>::Test (Real fTMax,
const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
{
Vector3<Real> kVDiff = rkVelocity1 - rkVelocity0;
Real fA = kVDiff.SquaredLength();
Vector3<Real> kCDiff = m_rkSphere1.Center - m_rkSphere0.Center;
Real fC = kCDiff.SquaredLength();
Real fRSum = m_rkSphere0.Radius + m_rkSphere1.Radius;
Real fRSumSqr = fRSum*fRSum;
if (fA > (Real)0.0)
{
Real fB = kCDiff.Dot(kVDiff);
if (fB <= (Real)0.0)
{
if (-fTMax*fA <= fB)
{
return fA*fC - fB*fB <= fA*fRSumSqr;
}
else
{
return fTMax*(fTMax*fA + ((Real)2.0)*fB) + fC <= fRSumSqr;
}
}
}
return fC <= fRSumSqr;
}
bool Wml::TestIntersection (const Sphere3<Real>& rkS0,
const Sphere3<Real>& rkS1, Real fTime, const Vector3<Real>& rkV0,
const Vector3<Real>& rkV1)
{
Vector3<Real> kVDiff = rkV1 - rkV0;
Real fA = kVDiff.SquaredLength();
Vector3<Real> kCDiff = rkS1.Center() - rkS0.Center();
Real fC = kCDiff.SquaredLength();
Real fRSum = rkS0.Radius() + rkS1.Radius();
Real fRSumSqr = fRSum*fRSum;
if ( fA > (Real)0.0 )
{
Real fB = kCDiff.Dot(kVDiff);
if ( fB <= (Real)0.0 )
{
if ( -fTime*fA <= fB )
return fA*fC - fB*fB <= fA*fRSumSqr;
else
return fTime*(fTime*fA + ((Real)2.0)*fB) + fC <= fRSumSqr;
}
}
return fC <= fRSumSqr;
}
bool Wml::FindIntersection (const Sphere3<Real>& rkS0,
const Sphere3<Real>& rkS1, Vector3<Real>& rkU, Vector3<Real>& rkV,
Vector3<Real>& rkC, Real& rfR)
{
// plane of intersection must have N as its normal
Vector3<Real> kN = rkS1.Center() - rkS0.Center();
Real fNSqrLen = kN.SquaredLength();
Real fRSum = rkS0.Radius() + rkS1.Radius();
if ( fNSqrLen > fRSum*fRSum )
{
// sphere centers are too far apart for intersection
return false;
}
Real fR0Sqr = rkS0.Radius()*rkS0.Radius();
Real fR1Sqr = rkS1.Radius()*rkS1.Radius();
Real fInvNSqrLen = ((Real)1.0)/fNSqrLen;
Real fT = ((Real)0.5)*((Real)1.0+(fR0Sqr-fR1Sqr)*fInvNSqrLen);
if ( fT < (Real)0.0 || fT > (Real)1.0 )
return false;
Real fRSqr = fR0Sqr - fT*fT*fNSqrLen;
if ( fRSqr < (Real)0.0 )
return false;
// center and radius of circle of intersection
rkC = rkS0.Center() + fT*kN;
rfR = Math<Real>::Sqrt(fRSqr);
// compute U and V for plane of circle
kN *= Math<Real>::Sqrt(fInvNSqrLen);
Vector3<Real>::GenerateOrthonormalBasis(rkU,rkV,kN,true);
return true;
}
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();
}
Sphere3<Real> MergeSpheres (const Sphere3<Real>& sphere0,
const Sphere3<Real>& sphere1)
{
Vector3<Real> cenDiff = sphere1.Center - sphere0.Center;
Real lenSqr = cenDiff.SquaredLength();
Real rDiff = sphere1.Radius - sphere0.Radius;
Real rDiffSqr = rDiff*rDiff;
if (rDiffSqr >= lenSqr)
{
return (rDiff >= (Real)0 ? sphere1 : sphere0);
}
Real length = Math<Real>::Sqrt(lenSqr);
Sphere3<Real> sphere;
if (length > Math<Real>::ZERO_TOLERANCE)
{
Real coeff = (length + rDiff)/(((Real)2)*length);
sphere.Center = sphere0.Center + coeff*cenDiff;
}
else
{
sphere.Center = sphere0.Center;
}
sphere.Radius = ((Real)0.5)*(length + sphere0.Radius + sphere1.Radius);
return sphere;
}
Real DistPoint3Segment3<Real>::GetSquared ()
{
Vector3<Real> diff = *mPoint - mSegment->Center;
mSegmentParameter = mSegment->Direction.Dot(diff);
if (-mSegment->Extent < mSegmentParameter)
{
if (mSegmentParameter < mSegment->Extent)
{
mClosestPoint1 = mSegment->Center +
mSegmentParameter*mSegment->Direction;
}
else
{
mClosestPoint1 = mSegment->P1;
mSegmentParameter = mSegment->Extent;
}
}
else
{
mClosestPoint1 = mSegment->P0;
mSegmentParameter = -mSegment->Extent;
}
mClosestPoint0 = *mPoint;
diff = mClosestPoint1 - mClosestPoint0;
return diff.SquaredLength();
}
//----------------------------------------------------------------------------
Sphere Mgc::MergeSpheres (const Sphere& rkSphere0, const Sphere& rkSphere1)
{
Vector3 kCDiff = rkSphere1.Center() - rkSphere0.Center();
Real fLSqr = kCDiff.SquaredLength();
Real fRDiff = rkSphere1.Radius() - rkSphere0.Radius();
Real fRDiffSqr = fRDiff*fRDiff;
if ( fRDiffSqr >= fLSqr )
return ( fRDiff >= 0.0f ? rkSphere1 : rkSphere0 );
Real fLength = Math::Sqrt(fLSqr);
const Real fTolerance = 1e-06f;
Sphere kSphere;
if ( fLength > fTolerance )
{
Real fCoeff = (fLength + fRDiff)/(2.0f*fLength);
kSphere.Center() = rkSphere0.Center() + fCoeff*kCDiff;
}
else
{
kSphere.Center() = rkSphere0.Center();
}
kSphere.Radius() = 0.5f*(fLength + rkSphere0.Radius() +
rkSphere1.Radius());
return kSphere;
}
Real DistLine3Line3<Real>::GetSquared ()
{
Vector3<Real> kDiff = m_pkLine0->Origin - m_pkLine1->Origin;
Real fA01 = -m_pkLine0->Direction.Dot(m_pkLine1->Direction);
Real fB0 = kDiff.Dot(m_pkLine0->Direction);
Real fC = kDiff.SquaredLength();
Real fDet = Math<Real>::FAbs((Real)1.0 - fA01*fA01);
Real fB1, fS0, fS1, fSqrDist;
if (fDet >= Math<Real>::ZERO_TOLERANCE)
{
// lines are not parallel
fB1 = -kDiff.Dot(m_pkLine1->Direction);
Real fInvDet = ((Real)1.0)/fDet;
fS0 = (fA01*fB1-fB0)*fInvDet;
fS1 = (fA01*fB0-fB1)*fInvDet;
fSqrDist = fS0*(fS0+fA01*fS1+((Real)2.0)*fB0) +
fS1*(fA01*fS0+fS1+((Real)2.0)*fB1)+fC;
}
else
{
// lines are parallel, select any closest pair of points
fS0 = -fB0;
fS1 = (Real)0.0;
fSqrDist = fB0*fS0+fC;
}
m_kClosestPoint0 = m_pkLine0->Origin + fS0*m_pkLine0->Direction;
m_kClosestPoint1 = m_pkLine1->Origin + fS1*m_pkLine1->Direction;
m_fLine0Parameter = fS0;
m_fLine1Parameter = fS1;
return Math<Real>::FAbs(fSqrDist);
}
Real DistVector3Segment3<Real>::GetSquared ()
{
Vector3<Real> kDiff = m_rkVector - m_rkSegment.Origin;
m_fSegmentParameter = m_rkSegment.Direction.Dot(kDiff);
if (-m_rkSegment.Extent < m_fSegmentParameter)
{
if (m_fSegmentParameter < m_rkSegment.Extent)
{
m_kClosestPoint1 = m_rkSegment.Origin +
m_fSegmentParameter*m_rkSegment.Direction;
}
else
{
m_kClosestPoint1 = m_rkSegment.Origin +
m_rkSegment.Extent*m_rkSegment.Direction;
}
}
else
{
m_kClosestPoint1 = m_rkSegment.Origin -
m_rkSegment.Extent*m_rkSegment.Direction;
}
m_kClosestPoint0 = m_rkVector;
kDiff = m_kClosestPoint1 - m_kClosestPoint0;
return kDiff.SquaredLength();
}
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();
}
Sphere3<Real> ContSphereAverage (int numPoints, const Vector3<Real>* points)
{
Sphere3<Real> sphere;
sphere.Center = points[0];
int i;
for (i = 1; i < numPoints; ++i)
{
sphere.Center += points[i];
}
sphere.Center /= (Real)numPoints;
for (i = 0; i < numPoints; ++i)
{
Vector3<Real> diff = points[i] - sphere.Center;
Real radiusSqr = diff.SquaredLength();
if (radiusSqr > sphere.Radius)
{
sphere.Radius = radiusSqr;
}
}
sphere.Radius = Math<Real>::Sqrt(sphere.Radius);
return sphere;
}
//----------------------------------------------------------------------------
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 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::InSphere (const Vector3& rkPoint, const Sphere& rkSphere,
Real fEpsilon)
{
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
Vector3 kDiff = rkPoint - rkSphere.Center();
Real fSqrDist = kDiff.SquaredLength();
return fSqrDist <= fRSqr + fEpsilon;
}
Real DistPoint3Line3<Real>::GetSquared ()
{
Vector3<Real> diff = *mPoint - mLine->Origin;
mLineParameter = mLine->Direction.Dot(diff);
mClosestPoint0 = *mPoint;
mClosestPoint1 = mLine->Origin + mLineParameter*mLine->Direction;
diff = mClosestPoint1 - mClosestPoint0;
return diff.SquaredLength();
}
Sphere3<Real> MinSphere3<Real>::ExactSphere2 ( const Vector3<Real>& P0,
const Vector3<Real>& P1 )
{
Sphere3<Real> minimal;
minimal.Center = ( ( Real )0.5 ) * ( P0 + P1 );
Vector3<Real> diff = P1 - P0;
minimal.Radius = ( ( Real )0.25 ) * diff.SquaredLength();
return minimal;
}
Real DistLine3Circle3<Real>::GetSquared ()
{
Vector3<Real> diff = mLine->Origin - mCircle->Center;
Real diffSqrLen = diff.SquaredLength();
Real MdM = mLine->Direction.SquaredLength();
Real DdM = diff.Dot(mLine->Direction);
Real NdM = mCircle->Normal.Dot(mLine->Direction);
Real DdN = diff.Dot(mCircle->Normal);
Real a0 = DdM;
Real a1 = MdM;
Real b0 = DdM - NdM*DdN;
Real b1 = MdM - NdM*NdM;
Real c0 = diffSqrLen - DdN*DdN;
Real c1 = b0;
Real c2 = b1;
Real rsqr = mCircle->Radius*mCircle->Radius;
Real a0sqr = a0*a0;
Real a1sqr = a1*a1;
Real twoA0A1 = ((Real)2)*a0*a1;
Real b0sqr = b0*b0;
Real b1Sqr = b1*b1;
Real twoB0B1 = ((Real)2)*b0*b1;
Real twoC1 = ((Real)2)*c1;
// The minimum point B+t*M occurs when t is a root of the quartic
// equation whose coefficients are defined below.
Polynomial1<Real> poly(4);
poly[0] = a0sqr*c0 - b0sqr*rsqr;
poly[1] = twoA0A1*c0 + a0sqr*twoC1 - twoB0B1*rsqr;
poly[2] = a1sqr*c0 + twoA0A1*twoC1 + a0sqr*c2 - b1Sqr*rsqr;
poly[3] = a1sqr*twoC1 + twoA0A1*c2;
poly[4] = a1sqr*c2;
PolynomialRoots<Real> polyroots(Math<Real>::ZERO_TOLERANCE);
polyroots.FindB(poly, 6);
int count = polyroots.GetCount();
const Real* roots = polyroots.GetRoots();
Real minSqrDist = Math<Real>::MAX_REAL;
for (int i = 0; i < count; ++i)
{
// Compute distance from P(t) to circle.
Vector3<Real> P = mLine->Origin + roots[i]*mLine->Direction;
DistPoint3Circle3<Real> query(P, *mCircle);
Real sqrDist = query.GetSquared();
if (sqrDist < minSqrDist)
{
minSqrDist = sqrDist;
mClosestPoint0 = query.GetClosestPoint0();
mClosestPoint1 = query.GetClosestPoint1();
}
}
return minSqrDist;
}
bool Wml::TestIntersection (const Sphere3<Real>& rkS0,
const Sphere3<Real>& rkS1)
{
Vector3<Real> kDiff = rkS1.Center() - rkS0.Center();
Real fSqrLen = kDiff.SquaredLength();
Real fRSum = rkS0.Radius() + rkS1.Radius();
Real fRSumSqr = fRSum*fRSum;
return fSqrLen <= fRSumSqr;
}
bool MinSphere3<Real>::Contains ( const Vector3<Real>& point,
const Sphere3<Real>& sphere, Real& distDiff )
{
Vector3<Real> diff = point - sphere.Center;
Real test = diff.SquaredLength();
// NOTE: In this algorithm, Sphere3 is storing the *squared radius*,
// so the next line of code is not in error.
distDiff = test - sphere.Radius;
return distDiff <= ( Real )0;
}
bool MinSphere3<Real>::Support::Contains ( int index, Vector3<Real>** points,
Real epsilon )
{
for ( int i = 0; i < Quantity; ++i )
{
Vector3<Real> diff = *points[index] - *points[Index[i]];
if ( diff.SquaredLength() < epsilon )
{
return true;
}
}
return false;
}
Real DistLine3Ray3<Real>::GetSquared ()
{
Vector3<Real> kDiff = mLine->Origin - mRay->Origin;
Real a01 = -mLine->Direction.Dot(mRay->Direction);
Real b0 = kDiff.Dot(mLine->Direction);
Real c = kDiff.SquaredLength();
Real det = Math<Real>::FAbs((Real)1 - a01*a01);
Real b1, s0, s1, sqrDist;
if (det >= Math<Real>::ZERO_TOLERANCE)
{
b1 = -kDiff.Dot(mRay->Direction);
s1 = a01*b0 - b1;
if (s1 >= (Real)0)
{
// Two interior points are closest, one on line and one on ray.
Real invDet = ((Real)1)/det;
s0 = (a01*b1 - b0)*invDet;
s1 *= invDet;
sqrDist = s0*(s0 + a01*s1 + ((Real)2)*b0) +
s1*(a01*s0 + s1 + ((Real)2)*b1) + c;
}
else
{
// Origin of ray and interior point of line are closest.
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
}
}
else
{
// Lines are parallel, closest pair with one point at ray origin.
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
}
mClosestPoint0 = mLine->Origin + s0*mLine->Direction;
mClosestPoint1 = mRay->Origin + s1*mRay->Direction;
mLineParameter = s0;
mRayParameter = s1;
// Account for numerical round-off errors.
if (sqrDist < (Real)0)
{
sqrDist = (Real)0;
}
return sqrDist;
}
static bool FindSphereVertexIntersection (const Vector3<Real>& rkVertex,
const Sphere3<Real>& rkSphere, const Vector3<Real>& rkSphVelocity,
const Vector3<Real>& rkTriVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real> akP[6])
{
// Finds the time and place (and possible occurance, it may miss) of an
// intersection between a sphere of fRadius at rkOrigin moving in rkDir
// towards a vertex at rkVertex.
Vector3<Real> kVel = rkSphVelocity - rkTriVelocity;
Vector3<Real> kD = rkSphere.Center() - rkVertex;
Vector3<Real> kCross = kD.Cross(kVel);
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
Real fVSqr = kVel.SquaredLength();
if ( kCross.SquaredLength() > fRSqr*fVSqr )
{
// ray overshoots the sphere
return false;
}
// find time of intersection
Real fDot = kD.Dot(kVel);
Real fDiff = kD.SquaredLength() - fRSqr;
Real fInv = Math<Real>::InvSqrt(Math<Real>::FAbs(fDot*fDot-fVSqr*fDiff));
rfTFirst = fDiff*fInv/((Real)1.0-fDot*fInv);
if ( rfTFirst > fTMax )
{
// intersection after max time
return false;
}
// place of intersection is triangle vertex
riQuantity = 1;
akP[0] = rkVertex + rfTFirst*rkTriVelocity;
return true;
}
bool IntrSphere3Cone3<Real>::Find ()
{
// test if cone vertex is in sphere
Vector3<Real> kDiff = m_pkSphere->Center - m_pkCone->Vertex;
Real fRSqr = m_pkSphere->Radius*m_pkSphere->Radius;
Real fLSqr = kDiff.SquaredLength();
if (fLSqr <= fRSqr)
{
return true;
}
// test if sphere center is in cone
Real fDot = kDiff.Dot(m_pkCone->Axis);
Real fDotSqr = fDot*fDot;
Real fCosSqr = m_pkCone->CosAngle*m_pkCone->CosAngle;
if (fDotSqr >= fLSqr*fCosSqr && fDot > (Real)0.0)
{
// sphere center is inside cone, so sphere and cone intersect
return true;
}
// Sphere center is outside cone. Problem now reduces to looking for
// an intersection between circle and ray in the plane containing
// cone vertex and spanned by cone axis and vector from vertex to
// sphere center.
// Ray is t*D+V (t >= 0) where |D| = 1 and dot(A,D) = cos(angle).
// Also, D = e*A+f*(C-V). Plugging ray equation into sphere equation
// yields R^2 = |t*D+V-C|^2, so the quadratic for intersections is
// t^2 - 2*dot(D,C-V)*t + |C-V|^2 - R^2 = 0. An intersection occurs
// if and only if the discriminant is nonnegative. This test becomes
//
// dot(D,C-V)^2 >= dot(C-V,C-V) - R^2
//
// Note that if the right-hand side is nonpositive, then the inequality
// is true (the sphere contains V). I have already ruled this out in
// the first block of code in this function.
Real fULen = Math<Real>::Sqrt(Math<Real>::FAbs(fLSqr-fDotSqr));
Real fTest = m_pkCone->CosAngle*fDot + m_pkCone->SinAngle*fULen;
Real fDiscr = fTest*fTest - fLSqr + fRSqr;
// compute point of intersection closest to vertex V
Real fT = fTest - Math<Real>::Sqrt(fDiscr);
Vector3<Real> kB = kDiff - fDot*m_pkCone->Axis;
Real fTmp = m_pkCone->SinAngle/fULen;
m_kPoint = fT*(m_pkCone->CosAngle*m_pkCone->Axis + fTmp*kB);
return fDiscr >= (Real)0.0 && fTest >= (Real)0.0;
}
bool IntrSphere3Cone3<Real>::Test ()
{
Real fInvSin = ((Real)1.0)/m_pkCone->SinAngle;
Real fCosSqr = m_pkCone->CosAngle*m_pkCone->CosAngle;
Vector3<Real> kCmV = m_pkSphere->Center - m_pkCone->Vertex;
Vector3<Real> kD = kCmV + (m_pkSphere->Radius*fInvSin)*m_pkCone->Axis;
Real fDSqrLen = kD.SquaredLength();
Real fE = kD.Dot(m_pkCone->Axis);
if (fE > (Real)0.0 && fE*fE >= fDSqrLen*fCosSqr)
{
Real fSinSqr = m_pkCone->SinAngle*m_pkCone->SinAngle;
fDSqrLen = kCmV.SquaredLength();
fE = -kCmV.Dot(m_pkCone->Axis);
if (fE > (Real)0.0 && fE*fE >= fDSqrLen*fSinSqr)
{
Real fRSqr = m_pkSphere->Radius*m_pkSphere->Radius;
return fDSqrLen <= fRSqr;
}
return true;
}
return false;
}
bool IntrSphere3Cone3<Real>::Test ()
{
Real invSin = ((Real)1)/mCone->SinAngle;
Real cosSqr = mCone->CosAngle*mCone->CosAngle;
Vector3<Real> CmV = mSphere->Center - mCone->Vertex;
Vector3<Real> D = CmV + (mSphere->Radius*invSin)*mCone->Axis;
Real DSqrLen = D.SquaredLength();
Real e = D.Dot(mCone->Axis);
if (e > (Real)0 && e*e >= DSqrLen*cosSqr)
{
Real sinSqr = mCone->SinAngle*mCone->SinAngle;
DSqrLen = CmV.SquaredLength();
e = -CmV.Dot(mCone->Axis);
if (e > (Real)0 && e*e >= DSqrLen*sinSqr)
{
Real rSqr = mSphere->Radius*mSphere->Radius;
return DSqrLen <= rSqr;
}
return true;
}
return false;
}
bool IntrSphere3Cone3<Real>::Find ()
{
// Test whether cone vertex is in sphere.
Vector3<Real> diff = mSphere->Center - mCone->Vertex;
Real rSqr = mSphere->Radius*mSphere->Radius;
Real lenSqr = diff.SquaredLength();
if (lenSqr <= rSqr)
{
return true;
}
// Test whether sphere center is in cone.
Real dot = diff.Dot(mCone->Axis);
Real dotSqr = dot*dot;
Real cosSqr = mCone->CosAngle*mCone->CosAngle;
if (dotSqr >= lenSqr*cosSqr && dot > (Real)0)
{
// Sphere center is inside cone, so sphere and cone intersect.
return true;
}
// Sphere center is outside cone. Problem now reduces to looking for
// an intersection between circle and ray in the plane containing
// cone vertex and spanned by cone axis and vector from vertex to
// sphere center.
// Ray is t*D+V (t >= 0) where |D| = 1 and dot(A,D) = cos(angle).
// Also, D = e*A+f*(C-V). Plugging ray equation into sphere equation
// yields R^2 = |t*D+V-C|^2, so the quadratic for intersections is
// t^2 - 2*dot(D,C-V)*t + |C-V|^2 - R^2 = 0. An intersection occurs
// if and only if the discriminant is nonnegative. This test becomes
//
// dot(D,C-V)^2 >= dot(C-V,C-V) - R^2
//
// Note that if the right-hand side is nonpositive, then the inequality
// is true (the sphere contains V). I have already ruled this out in
// the first block of code in this function.
Real uLen = Math<Real>::Sqrt(Math<Real>::FAbs(lenSqr-dotSqr));
Real test = mCone->CosAngle*dot + mCone->SinAngle*uLen;
Real discr = test*test - lenSqr + rSqr;
// compute point of intersection closest to vertex V
Real t = test - Math<Real>::Sqrt(discr);
Vector3<Real> B = diff - dot*mCone->Axis;
Real tmp = mCone->SinAngle/uLen;
mPoint = t*(mCone->CosAngle*mCone->Axis + tmp*B);
return discr >= (Real)0 && test >= (Real)0;
}
bool IntrTriangle3Sphere3<Real>::FindSphereVertexIntersection (
const Vector3<Real>& vertex, Real tmax,
const Vector3<Real>& velocity0, const Vector3<Real>& velocity1)
{
// Finds the time and place (and possible occurrence it may miss) of an
// intersection between a sphere of fRadius at rkOrigin moving in rkDir
// towards a vertex at vertex.
Vector3<Real> relVelocity = velocity1 - velocity0;
Vector3<Real> D = mSphere->Center - vertex;
Vector3<Real> cross = D.Cross(relVelocity);
Real rSqr = mSphere->Radius*mSphere->Radius;
Real vsqr = relVelocity.SquaredLength();
if (cross.SquaredLength() > rSqr*vsqr)
{
// The ray overshoots the sphere.
return false;
}
// Find the time of intersection.
Real dot = D.Dot(relVelocity);
Real diff = D.SquaredLength() - rSqr;
Real inv = Math<Real>::InvSqrt(Math<Real>::FAbs(dot*dot - vsqr*diff));
mContactTime = diff*inv/((Real)1 - dot*inv);
if (mContactTime > tmax)
{
// The intersection occurs after max time.
return false;
}
// The intersection is a triangle vertex.
mPoint = vertex + mContactTime*velocity0;
return true;
}
Sphere Mgc::ContSphereAverage (int iQuantity, const Vector3* akPoint)
{
Vector3 kCenter = akPoint[0];
int i;
for (i = 1; i < iQuantity; i++)
kCenter += akPoint[i];
Real fInvQuantity = 1.0f/iQuantity;
kCenter *= fInvQuantity;
Real fMaxRadiusSqr = 0.0f;
for (i = 0; i < iQuantity; i++)
{
Vector3 kDiff = akPoint[i] - kCenter;
Real fRadiusSqr = kDiff.SquaredLength();
if ( fRadiusSqr > fMaxRadiusSqr )
fMaxRadiusSqr = fRadiusSqr;
}
Sphere kSphere;
kSphere.Center() = kCenter;
kSphere.Radius() = Math::Sqrt(fMaxRadiusSqr);
return kSphere;
}
static Real UpdateInvRSqr (int iQuantity, const Vector3<Real>* akPoint,
const Vector3<Real>& rkC, const Vector3<Real>& rkU, Real& rfInvRSqr)
{
Real fASum = (Real)0.0, fAASum = (Real)0.0;
for (int i = 0; i < iQuantity; i++)
{
Vector3<Real> kDelta = akPoint[i] - rkC;
Vector3<Real> kDxU = kDelta.Cross(rkU);
Real fL2 = kDxU.SquaredLength();
fASum += fL2;
fAASum += fL2*fL2;
}
rfInvRSqr = fASum/fAASum;
Real fMin = (Real)1.0 - rfInvRSqr*fASum/(Real)iQuantity;
return fMin;
}