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;
}
Example #2
0
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;
}