mgcintrlin3cap.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcDistLin3Lin3.h"
#include "MgcIntrLin3Cap.h"

static const MgcReal gs_fEpsilon = 1e-12;

//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcSegment3& rkSegment,
    const MgcCapsule& rkCapsule)
{
    MgcReal fSqrDist = MgcSqrDistance(rkSegment,rkCapsule.Segment());
    return fSqrDist <= rkCapsule.Radius();
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcRay3& rkRay, const MgcCapsule& rkCapsule)
{
    MgcReal fSqrDist = MgcSqrDistance(rkRay,rkCapsule.Segment());
    return fSqrDist <= rkCapsule.Radius();
}
//----------------------------------------------------------------------------
bool MgcTestIntersection (const MgcLine3& rkLine, const MgcCapsule& rkCapsule)
{
    MgcReal fSqrDist = MgcSqrDistance(rkLine,rkCapsule.Segment());
    return fSqrDist <= rkCapsule.Radius();
}
//----------------------------------------------------------------------------
static int FindIntersection (const MgcVector3& rkOrigin,
    const MgcVector3& rkDirection, const MgcCapsule& rkCapsule,
    MgcReal afT[2])
{
    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    MgcVector3 kU, kV, kW = rkCapsule.Direction();
    MgcReal fWLength = kW.Unitize();
    MgcReal fInvWLength = 1.0/fWLength;
    MgcVector3::GenerateOrthonormalBasis(kU,kV,kW);
    MgcVector3 kD(kU.Dot(rkDirection),kV.Dot(rkDirection),
        kW.Dot(rkDirection));
    MgcReal fDLength = kD.Unitize();
    MgcReal fInvDLength = 1.0/fDLength;
    MgcVector3 kDiff = rkOrigin - rkCapsule.Origin();
    MgcVector3 kP(kU.Dot(kDiff),kV.Dot(kDiff),kW.Dot(kDiff));
    MgcReal fRadiusSqr = rkCapsule.Radius()*rkCapsule.Radius();

    MgcReal fInv, fA, fB, fC, fDiscr, fRoot, fT, fTmp;

    if ( MgcMath::Abs(kD.z) >= 1.0 - gs_fEpsilon )
    {
        // line is parallel to capsule axis
        fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
        if ( fDiscr >= 0.0 )
        {
            fRoot = MgcMath::Sqrt(fDiscr);
            afT[0] = -(kP.z + fRoot)*fInvDLength;
            afT[1] = (fWLength - kP.z + fRoot)*fInvDLength;
            return 2;
        }
        else
        {
            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.0 )
    {
        // line does not intersect infinite cylinder
        return 0;
    }

    int iQuantity = 0;

    if ( fDiscr > 0.0 )
    {
        // line intersects infinite cylinder in two places
        fRoot = MgcMath::Sqrt(fDiscr);
        fInv = 1.0/fA;
        fT = (-fB - fRoot)*fInv;
        fTmp = kP.z + fT*kD.z;
        if ( 0.0 <= fTmp && fTmp <= fWLength )
            afT[iQuantity++] = fT*fInvDLength;

        fT = (-fB + fRoot)*fInv;
        fTmp = kP.z + fT*kD.z;
        if ( 0.0 <= fTmp && fTmp <= fWLength )
            afT[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.0 <= fTmp && fTmp <= fWLength )
        {
            afT[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.0 )
    {
        fRoot = MgcMath::Sqrt(fDiscr);
        fT = -fB - fRoot;
        fTmp = kP.z + fT*kD.z;
        if ( fTmp <= 0.0 )
        {
            afT[iQuantity++] = fT*fInvDLength;
            if ( iQuantity == 2 )
                return 2;
        }

        fT = -fB + fRoot;
        fTmp = kP.z + fT*kD.z;
        if ( fTmp <= 0.0 )
        {
            afT[iQuantity++] = fT*fInvDLength;
            if ( iQuantity == 2 )
                return 2;
        }
    }
    else if ( fDiscr == 0.0 )
    {
        fT = -fB;
        fTmp = kP.z + fT*kD.z;
        if ( fTmp <= 0.0 )
        {
            afT[iQuantity++] = fT*fInvDLength;
            if ( iQuantity == 2 )
                return 2;
        }
    }

    // test intersection with top hemisphere
    // fA = 1
    fB -= kD.z*fWLength;
    fC += fWLength*(fWLength - 2.0*kP.z);

    fDiscr = fB*fB - fC;
    if ( fDiscr > 0.0 )
    {
        fRoot = MgcMath::Sqrt(fDiscr);
        fT = -fB - fRoot;
        fTmp = kP.z + fT*kD.z;
        if ( fTmp >= fWLength )
        {
            afT[iQuantity++] = fT*fInvDLength;
            if ( iQuantity == 2 )
                return 2;
        }

        fT = -fB + fRoot;
        fTmp = kP.z + fT*kD.z;
        if ( fTmp >= fWLength )
        {
            afT[iQuantity++] = fT*fInvDLength;
            if ( iQuantity == 2 )
                return 2;
        }
    }
    else if ( fDiscr == 0.0 )
    {
        fT = -fB;
        fTmp = kP.z + fT*kD.z;
        if ( fTmp >= fWLength )
        {
            afT[iQuantity++] = fT*fInvDLength;
            if ( iQuantity == 2 )
                return 2;
        }
    }

    return iQuantity;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcSegment3& rkSegment,
    const MgcCapsule& rkCapsule, int& riQuantity, MgcVector3 akPoint[2])
{
    MgcReal afT[2];
    riQuantity = FindIntersection(rkSegment.Origin(),
        rkSegment.Direction(),rkCapsule,afT);

    int iClipQuantity = 0;
    for (int i = 0; i < riQuantity; i++)
    {
        if ( 0.0 <= afT[i] && afT[i] <= 1.0 )
        {
            akPoint[iClipQuantity++] = rkSegment.Origin() +
                afT[i]*rkSegment.Direction();
        }
    }

    riQuantity = iClipQuantity;
    return riQuantity > 0;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcRay3& rkRay, const MgcCapsule& rkCapsule,
    int& riQuantity, MgcVector3 akPoint[2])
{
    MgcReal afT[2];
    riQuantity = FindIntersection(rkRay.Origin(),rkRay.Direction(),
        rkCapsule,afT);

    int iClipQuantity = 0;
    for (int i = 0; i < riQuantity; i++)
    {
        if ( afT[i] >= 0.0 )
        {
            akPoint[iClipQuantity++] = rkRay.Origin() +
                afT[i]*rkRay.Direction();
        }
    }

    riQuantity = iClipQuantity;
    return riQuantity > 0;
}
//----------------------------------------------------------------------------
bool MgcFindIntersection (const MgcLine3& rkLine, const MgcCapsule& rkCapsule,
    int& riQuantity, MgcVector3 akPoint[2])
{
    MgcReal afT[2];
    riQuantity = FindIntersection(rkLine.Origin(),rkLine.Direction(),
        rkCapsule,afT);

    for (int i = 0; i < riQuantity; i++)
        akPoint[i] = rkLine.Origin() + afT[i]*rkLine.Direction();

    return riQuantity > 0;
}
//----------------------------------------------------------------------------