int IntrLine3Cylinder3<Real>::Find (const Vector3<Real>& rkOrigin,
const Vector3<Real>& rkDir, const Cylinder3<Real>& rkCylinder,
Real afT[2])
{
// Create a coordinate system for the cylinder. In this system, the
// cylinder segment center C is the origin and the cylinder axis direction
// W is the z-axis. U and V are the other coordinate axis directions.
// If P = x*U+y*V+z*W, the cylinder is x^2 + y^2 = r^2, where r is the
// cylinder radius. The end caps are |z| = h/2, where h is the cylinder
// height.
Vector3<Real> kU, kV, kW = rkCylinder.Segment.Direction;
Vector3<Real>::GenerateComplementBasis(kU,kV,kW);
Real fHalfHeight = ((Real)0.5)*rkCylinder.Height;
Real fRSqr = rkCylinder.Radius*rkCylinder.Radius;
// convert incoming line origin to cylinder coordinates
Vector3<Real> kDiff = rkOrigin - rkCylinder.Segment.Origin;
Vector3<Real> kP(kU.Dot(kDiff),kV.Dot(kDiff),kW.Dot(kDiff));
// Get the z-value, in cylinder coordinates, of the incoming line's
// unit-length direction.
Real fDz = kW.Dot(rkDir);
if (Math<Real>::FAbs(fDz) >= (Real)1.0 - Math<Real>::ZERO_TOLERANCE)
{
// The line is parallel to the cylinder axis. Determine if the line
// intersects the cylinder end disks.
Real fRadialSqrDist = fRSqr - kP.X()*kP.X() - kP.Y()*kP.Y();
if (fRadialSqrDist < (Real)0.0)
{
// line outside the cylinder, no intersection
return 0;
}
// line intersects the cylinder end disks
if (fDz > (Real)0.0)
{
afT[0] = -kP.Z() - fHalfHeight;
afT[1] = -kP.Z() + fHalfHeight;
}
else
{
afT[0] = kP.Z() - fHalfHeight;
afT[1] = kP.Z() + fHalfHeight;
}
return 2;
}
// convert incoming line unit-length direction to cylinder coordinates
Vector3<Real> kD(kU.Dot(rkDir),kV.Dot(rkDir),fDz);
Real fA0, fA1, fA2, fDiscr, fRoot, fInv, fT;
if (Math<Real>::FAbs(kD.Z()) <= Math<Real>::ZERO_TOLERANCE)
{
// The line is perpendicular to the cylinder axis.
if (Math<Real>::FAbs(kP.Z()) > fHalfHeight)
{
// line is outside the planes of the cylinder end disks
return 0;
}
// Test intersection of line P+t*D with infinite cylinder
// x^2+y^2 = r^2. This reduces to computing the roots of a
// quadratic equation. If P = (px,py,pz) and D = (dx,dy,dz),
// then the quadratic equation is
// (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
fA0 = kP.X()*kP.X() + kP.Y()*kP.Y() - fRSqr;
fA1 = kP.X()*kD.X() + kP.Y()*kD.Y();
fA2 = kD.X()*kD.X() + kD.Y()*kD.Y();
fDiscr = fA1*fA1 - fA0*fA2;
if (fDiscr < (Real)0.0)
{
// line does not intersect cylinder
return 0;
}
else if (fDiscr > Math<Real>::ZERO_TOLERANCE)
{
// line intersects cylinder in two places
fRoot = Math<Real>::Sqrt(fDiscr);
fInv = ((Real)1.0)/fA2;
afT[0] = (-fA1 - fRoot)*fInv;
afT[1] = (-fA1 + fRoot)*fInv;
return 2;
}
else
{
// line is tangent to the cylinder
afT[0] = -fA1/fA2;
return 1;
}
}
// test plane intersections first
int iQuantity = 0;
fInv = ((Real)1.0)/kD.Z();
Real fT0 = (-fHalfHeight - kP.Z())*fInv;
Real fXTmp = kP.X() + fT0*kD.X();
Real fYTmp = kP.Y() + fT0*kD.Y();
if (fXTmp*fXTmp + fYTmp*fYTmp <= fRSqr)
{
// planar intersection inside the top cylinder end disk
afT[iQuantity++] = fT0;
}
Real fT1 = (+fHalfHeight - kP.Z())*fInv;
fXTmp = kP.X() + fT1*kD.X();
fYTmp = kP.Y() + fT1*kD.Y();
if (fXTmp*fXTmp + fYTmp*fYTmp <= fRSqr)
{
// planar intersection inside the bottom cylinder end disk
afT[iQuantity++] = fT1;
}
if (iQuantity == 2)
{
// line intersects both top and bottom cylinder end disks
if (afT[0] > afT[1])
{
Real fSave = afT[0];
afT[0] = afT[1];
afT[1] = fSave;
}
return 2;
}
// If iQuantity == 1, then the line must intersect cylinder wall in a
// single point somewhere between the end disks. This case is detected
// in the following code that tests for intersection between line and
// cylinder wall.
fA0 = kP.X()*kP.X() + kP.Y()*kP.Y() - fRSqr;
fA1 = kP.X()*kD.X() + kP.Y()*kD.Y();
fA2 = kD.X()*kD.X() + kD.Y()*kD.Y();
fDiscr = fA1*fA1 - fA0*fA2;
if (fDiscr < (Real)0.0)
{
// line does not intersect cylinder wall
assert( iQuantity == 0 );
return 0;
}
else if (fDiscr > Math<Real>::ZERO_TOLERANCE)
{
fRoot = Math<Real>::Sqrt(fDiscr);
fInv = ((Real)1.0)/fA2;
fT = (-fA1 - fRoot)*fInv;
if (fT0 <= fT1)
{
if (fT0 <= fT && fT <= fT1)
{
afT[iQuantity++] = fT;
}
}
else
{
if (fT1 <= fT && fT <= fT0)
{
afT[iQuantity++] = fT;
}
}
if (iQuantity == 2)
{
// Line intersects one of the cylinder end disks and once on the
// cylinder wall.
if (afT[0] > afT[1])
{
Real fSave = afT[0];
afT[0] = afT[1];
afT[1] = fSave;
}
return 2;
}
fT = (-fA1 + fRoot)*fInv;
if (fT0 <= fT1)
{
if (fT0 <= fT && fT <= fT1)
{
afT[iQuantity++] = fT;
}
}
else
{
if (fT1 <= fT && fT <= fT0)
{
afT[iQuantity++] = fT;
}
}
}
else
{
fT = -fA1/fA2;
if (fT0 <= fT1)
{
if (fT0 <= fT && fT <= fT1)
{
afT[iQuantity++] = fT;
}
}
else
{
if (fT1 <= fT && fT <= fT0)
{
afT[iQuantity++] = fT;
}
}
}
if (iQuantity == 2)
{
if (afT[0] > afT[1])
{
Real fSave = afT[0];
afT[0] = afT[1];
afT[1] = fSave;
}
}
return iQuantity;
}
udword /*Ctc::*/RayCapsuleOverlap(const Point& origin, const Point& dir, const LSS& capsule, float s[2])
{
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
Point kU, kV, kW, capsDir;
capsule.ComputeDirection(capsDir);
kW = capsDir;
float fWLength = kW.Magnitude();
kW.Normalize();
// generate orthonormal basis
float fInvLength;
if ( fabsf(kW.x) >= fabsf(kW.y) )
{
// W.x or W.z is the largest magnitude component, swap them
fInvLength = 1.0f/sqrtf(kW.x*kW.x + kW.z*kW.z);
kU.x = -kW.z*fInvLength;
kU.y = 0.0f;
kU.z = +kW.x*fInvLength;
}
else
{
// W.y or W.z is the largest magnitude component, swap them
fInvLength = 1.0f/sqrtf(kW.y*kW.y + kW.z*kW.z);
kU.x = 0.0f;
kU.y = +kW.z*fInvLength;
kU.z = -kW.y*fInvLength;
}
kV = kW^kU;
kU.Normalize();
if(gNormalize)
kV.Normalize();
// compute intersection
Point kD(kU|dir, kV|dir, kW|dir);
float fDLength = kD.Magnitude();
kD.Normalize();
float fInvDLength = 1.0f/fDLength;
Point kDiff = origin - capsule.mP0;
Point kP(kU|kDiff, kV|kDiff, kW|kDiff);
float fRadiusSqr = capsule.mRadius*capsule.mRadius;
float fInv, fA, fB, fC, fDiscr, fRoot, fT, fTmp;
// Is the velocity parallel to the capsule direction? (or zero)
if ( fabsf(kD.z) >= 1.0f - FLT_EPSILON || fDLength < FLT_EPSILON )
{
float fAxisDir = dir|capsDir;
fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
if ( fAxisDir < 0 && fDiscr >= 0.0f )
{
// Velocity anti-parallel to the capsule direction
fRoot = sqrtf(fDiscr);
s[0] = (kP.z + fRoot)*fInvDLength;
s[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
return 2;
}
else if ( fAxisDir > 0 && fDiscr >= 0.0f )
{
// Velocity parallel to the capsule direction
fRoot = sqrtf(fDiscr);
s[0] = -(kP.z + fRoot)*fInvDLength;
s[1] = (fWLength - kP.z + fRoot)*fInvDLength;
return 2;
}
else
{
// sphere heading wrong direction, or no velocity at all
return 0;
}
}
// test intersection with infinite cylinder
fA = kD.x*kD.x + kD.y*kD.y;
fB = kP.x*kD.x + kP.y*kD.y;
fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
fDiscr = fB*fB - fA*fC;
if ( fDiscr < 0.0f )
{
// line does not intersect infinite cylinder
return 0;
}
int iQuantity = 0;
if ( fDiscr > 0.0f )
{
// line intersects infinite cylinder in two places
fRoot = sqrtf(fDiscr);
fInv = 1.0f/fA;
fT = (-fB - fRoot)*fInv;
fTmp = kP.z + fT*kD.z;
if ( 0.0f <= fTmp && fTmp <= fWLength )
s[iQuantity++] = fT*fInvDLength;
fT = (-fB + fRoot)*fInv;
fTmp = kP.z + fT*kD.z;
if ( 0.0f <= fTmp && fTmp <= fWLength )
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
{
// line intersects capsule wall in two places
return 2;
}
}
else
{
// line is tangent to infinite cylinder
fT = -fB/fA;
fTmp = kP.z + fT*kD.z;
if ( 0.0f <= fTmp && fTmp <= fWLength )
{
s[0] = fT*fInvDLength;
return 1;
}
}
// test intersection with bottom hemisphere
// fA = 1
fB += kP.z*kD.z;
fC += kP.z*kP.z;
fDiscr = fB*fB - fC;
if ( fDiscr > 0.0f )
{
fRoot = sqrtf(fDiscr);
fT = -fB - fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
fT = -fB + fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
else if ( fDiscr == 0.0f )
{
fT = -fB;
fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
// test intersection with top hemisphere
// fA = 1
fB -= kD.z*fWLength;
fC += fWLength*(fWLength - 2.0f*kP.z);
fDiscr = fB*fB - fC;
if ( fDiscr > 0.0f )
{
fRoot = sqrtf(fDiscr);
fT = -fB - fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
fT = -fB + fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
else if ( fDiscr == 0.0f )
{
fT = -fB;
fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
return iQuantity;
}