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::TestIntersection (const Triangle3<Real>& rkTri,
const Sphere3<Real>& rkSphere)
{
Real fSqrDist = SqrDistance(rkSphere.Center(),rkTri);
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fSqrDist < fRSqr;
}
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 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;
}
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 Wml::FindIntersection (const Box3<Real>& rkBox,
const Vector3<Real>& rkBoxVelocity, const Sphere3<Real>& rkSphere,
const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real>& rkP)
{
// Find intersections relative to the coordinate system of the box.
// The sphere is transformed to the box coordinates and the velocity of
// the sphere is relative to the box.
Vector3<Real> kCDiff = rkSphere.Center() - rkBox.Center();
Vector3<Real> kVel = rkSphVelocity - rkBoxVelocity;
Real fAx = kCDiff.Dot(rkBox.Axis(0));
Real fAy = kCDiff.Dot(rkBox.Axis(1));
Real fAz = kCDiff.Dot(rkBox.Axis(2));
Real fVx = kVel.Dot(rkBox.Axis(0));
Real fVy = kVel.Dot(rkBox.Axis(1));
Real fVz = kVel.Dot(rkBox.Axis(2));
// flip coordinate frame into the first octant
int iSignX = 1;
if ( fAx < (Real)0.0 )
{
fAx = -fAx;
fVx = -fVx;
iSignX = -1;
}
int iSignY = 1;
if ( fAy < (Real)0.0 )
{
fAy = -fAy;
fVy = -fVy;
iSignY = -1;
}
int iSignZ = 1;
if ( fAz < (Real)0.0 )
{
fAz = -fAz;
fVz = -fVz;
iSignZ = -1;
}
// intersection coordinates
Real fIx, fIy, fIz;
int iRetVal;
if ( fAx <= rkBox.Extent(0) )
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere center inside box
rfTFirst = (Real)0.0;
riQuantity = 0;
return false;
}
else
{
// sphere above face on axis Z
iRetVal = FindFaceRegionIntersection(rkBox.Extent(0),
rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
rfTFirst,rkSphere.Radius(),fIx,fIy,fIz,true);
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere above face on axis Y
iRetVal = FindFaceRegionIntersection(rkBox.Extent(0),
rkBox.Extent(2),rkBox.Extent(1),fAx,fAz,fAy,fVx,fVz,fVy,
rfTFirst,rkSphere.Radius(),fIx,fIz,fIy,true);
}
else
{
// sphere is above the edge formed by faces y and z
iRetVal = FindEdgeRegionIntersection(rkBox.Extent(1),
rkBox.Extent(0),rkBox.Extent(2),fAy,fAx,fAz,fVy,fVx,fVz,
rfTFirst,rkSphere.Radius(),fIy,fIx,fIz,true);
}
}
}
else
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere above face on axis X
iRetVal = FindFaceRegionIntersection(rkBox.Extent(1),
rkBox.Extent(2),rkBox.Extent(0),fAy,fAz,fAx,fVy,fVz,fVx,
rfTFirst,rkSphere.Radius(),fIy,fIz,fIx,true);
}
else
{
// sphere is above the edge formed by faces x and z
iRetVal = FindEdgeRegionIntersection(rkBox.Extent(0),
rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
rfTFirst,rkSphere.Radius(),fIx,fIy,fIz,true);
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere is above the edge formed by faces x and y
iRetVal = FindEdgeRegionIntersection(rkBox.Extent(0),
rkBox.Extent(2),rkBox.Extent(1),fAx,fAz,fAy,fVx,fVz,fVy,
rfTFirst,rkSphere.Radius(),fIx,fIz,fIy,true);
}
else
{
// sphere is above the corner formed by faces x,y,z
iRetVal = FindVertexRegionIntersection(rkBox.Extent(0),
rkBox.Extent(1),rkBox.Extent(2),fAx,fAy,fAz,fVx,fVy,fVz,
rfTFirst,rkSphere.Radius(),fIx,fIy,fIz);
}
}
}
if ( iRetVal != 1 || rfTFirst > fTMax )
{
riQuantity = 0;
return false;
}
// calculate actual intersection (move point back into world coordinates)
riQuantity = 1;
rkP = rkBox.Center() + (iSignX*fIx)*rkBox.Axis(0) +
(iSignY*fIy)*rkBox.Axis(1) + (iSignZ*fIz)*rkBox.Axis(2);
return true;
}
bool Wml::TestIntersection (const Box3<Real>& rkBox,
const Sphere3<Real>& rkSphere)
{
// Test for intersection in the coordinate system of the box by
// transforming the sphere into that coordinate system.
Vector3<Real> kCDiff = rkSphere.Center() - rkBox.Center();
Real fAx = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(0)));
Real fAy = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(1)));
Real fAz = Math<Real>::FAbs(kCDiff.Dot(rkBox.Axis(2)));
Real fDx = fAx - rkBox.Extent(0);
Real fDy = fAy - rkBox.Extent(1);
Real fDz = fAz - rkBox.Extent(2);
if ( fAx <= rkBox.Extent(0) )
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// sphere center inside box
return true;
}
else
{
// potential sphere-face intersection with face z
return fDz <= rkSphere.Radius();
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// potential sphere-face intersection with face y
return fDy <= rkSphere.Radius();
}
else
{
// potential sphere-edge intersection with edge formed
// by faces y and z
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDy*fDy + fDz*fDz <= fRSqr;
}
}
}
else
{
if ( fAy <= rkBox.Extent(1) )
{
if ( fAz <= rkBox.Extent(2) )
{
// potential sphere-face intersection with face x
return fDx <= rkSphere.Radius();
}
else
{
// potential sphere-edge intersection with edge formed
// by faces x and z
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDx*fDx + fDz*fDz <= fRSqr;
}
}
else
{
if ( fAz <= rkBox.Extent(2) )
{
// potential sphere-edge intersection with edge formed
// by faces x and y
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDx*fDx + fDy*fDy <= fRSqr;
}
else
{
// potential sphere-vertex intersection at corner formed
// by faces x,y,z
Real fRSqr = rkSphere.Radius()*rkSphere.Radius();
return fDx*fDx + fDy*fDy + fDz*fDz <= fRSqr;
}
}
}
}
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;
}
}
}
}
}
bool Wml::FindIntersection (const Sphere3<Real>& rkS0,
const Sphere3<Real>& rkS1, Real fTime, const Vector3<Real>& rkV0,
const Vector3<Real>& rkV1, Real& rfFirstTime,
Vector3<Real>& rkFirstPoint)
{
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
|| fTime*(fTime*fA + ((Real)2.0)*fB) + fC <= fRSumSqr )
{
Real fCDiff = fC - fRSumSqr;
Real fDiscr = fB*fB - fA*fCDiff;
if ( fDiscr >= (Real)0.0 )
{
if ( fCDiff <= (Real)0.0 )
{
// The spheres are initially intersecting. Estimate a
// point of contact by using the midpoint of the line
// segment connecting the sphere centers.
rfFirstTime = (Real)0.0;
rkFirstPoint = ((Real)0.5)*(rkS0.Center() +
rkS1.Center());
}
else
{
// The first time of contact is in [0,fTime].
rfFirstTime = -(fB + Math<Real>::Sqrt(fDiscr))/fA;
if ( rfFirstTime < (Real)0.0 )
rfFirstTime = (Real)0.0;
else if ( rfFirstTime > fTime )
rfFirstTime = fTime;
Vector3<Real> kNewCDiff = kCDiff + rfFirstTime*kVDiff;
rkFirstPoint = rkS0.Center() + rfFirstTime*rkV0 +
(rkS0.Radius()/fRSum)*kNewCDiff;
}
return true;
}
}
return false;
}
}
if ( fC <= fRSumSqr )
{
// The spheres are initially intersecting. Estimate a point of
// contact by using the midpoint of the line segment connecting the
// sphere centers.
rfFirstTime = (Real)0.0;
rkFirstPoint = ((Real)0.5)*(rkS0.Center() + rkS1.Center());
return true;
}
return false;
}