wmldistlin3box3.cpp

3D Game Engine Design Source Code非常棒

        rfSqrDistance += fDelta*fDelta;
        rkPnt[i2] = -rkBox.Extent(i2);
    }
    else if ( rkPnt[i2] > rkBox.Extent(i2) )
    {
        fDelta = rkPnt[i2] - rkBox.Extent(i2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i2] = rkBox.Extent(i2);
    }
}
//----------------------------------------------------------------------------
template <class Real>
static void Case00 (int i0, int i1, int i2, Vector3<Real>& rkPnt,
    const Vector3<Real>& rkDir, const Box3<Real>& rkBox, Real* pfLParam,
    Real& rfSqrDistance)
{
    Real fDelta;

    if ( pfLParam )
        *pfLParam = (rkBox.Extent(i0) - rkPnt[i0])/rkDir[i0];

    rkPnt[i0] = rkBox.Extent(i0);

    if ( rkPnt[i1] < -rkBox.Extent(i1) )
    {
        fDelta = rkPnt[i1] + rkBox.Extent(i1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i1] = -rkBox.Extent(i1);
    }
    else if ( rkPnt[i1] > rkBox.Extent(i1) )
    {
        fDelta = rkPnt[i1] - rkBox.Extent(i1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i1] = rkBox.Extent(i1);
    }

    if ( rkPnt[i2] < -rkBox.Extent(i2) )
    {
        fDelta = rkPnt[i2] + rkBox.Extent(i2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i1] = -rkBox.Extent(i2);
    }
    else if ( rkPnt[i2] > rkBox.Extent(i2) )
    {
        fDelta = rkPnt[i2] - rkBox.Extent(i2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt[i2] = rkBox.Extent(i2);
    }
}
//----------------------------------------------------------------------------
template <class Real>
static void Case000 (Vector3<Real>& rkPnt, const Box3<Real>& rkBox,
    Real& rfSqrDistance)
{
    Real fDelta;

    if ( rkPnt.X() < -rkBox.Extent(0) )
    {
        fDelta = rkPnt.X() + rkBox.Extent(0);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.X() = -rkBox.Extent(0);
    }
    else if ( rkPnt.X() > rkBox.Extent(0) )
    {
        fDelta = rkPnt.X() - rkBox.Extent(0);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.X() = rkBox.Extent(0);
    }

    if ( rkPnt.Y() < -rkBox.Extent(1) )
    {
        fDelta = rkPnt.Y() + rkBox.Extent(1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.Y() = -rkBox.Extent(1);
    }
    else if ( rkPnt.Y() > rkBox.Extent(1) )
    {
        fDelta = rkPnt.Y() - rkBox.Extent(1);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.Y() = rkBox.Extent(1);
    }

    if ( rkPnt.Z() < -rkBox.Extent(2) )
    {
        fDelta = rkPnt.Z() + rkBox.Extent(2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.Z() = -rkBox.Extent(2);
    }
    else if ( rkPnt.Z() > rkBox.Extent(2) )
    {
        fDelta = rkPnt.Z() - rkBox.Extent(2);
        rfSqrDistance += fDelta*fDelta;
        rkPnt.Z() = rkBox.Extent(2);
    }
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::SqrDistance (const Line3<Real>& rkLine, const Box3<Real>& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
    // compute coordinates of line in box coordinate system
    Vector3<Real> kDiff = rkLine.Origin() - rkBox.Center();
    Vector3<Real> kPnt(kDiff.Dot(rkBox.Axis(0)),kDiff.Dot(rkBox.Axis(1)),
        kDiff.Dot(rkBox.Axis(2)));
    Vector3<Real> kDir(rkLine.Direction().Dot(rkBox.Axis(0)),
        rkLine.Direction().Dot(rkBox.Axis(1)),
        rkLine.Direction().Dot(rkBox.Axis(2)));

    // Apply reflections so that direction vector has nonnegative components.
    bool bReflect[3];
    int i;
    for (i = 0; i < 3; i++)
    {
        if ( kDir[i] < (Real)0.0 )
        {
            kPnt[i] = -kPnt[i];
            kDir[i] = -kDir[i];
            bReflect[i] = true;
        }
        else
        {
            bReflect[i] = false;
        }
    }

    Real fSqrDistance = (Real)0.0;

    if ( kDir.X() > (Real)0.0 )
    {
        if ( kDir.Y() > (Real)0.0 )
        {
            if ( kDir.Z() > (Real)0.0 )
            {
                // (+,+,+)
                CaseNoZeros(kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (+,+,0)
                Case0(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
        else
        {
            if ( kDir.Z() > (Real)0.0 )
            {
                // (+,0,+)
                Case0(0,2,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (+,0,0)
                Case00(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
    }
    else
    {
        if ( kDir.Y() > (Real)0.0 )
        {
            if ( kDir.Z() > (Real)0.0 )
            {
                // (0,+,+)
                Case0(1,2,0,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (0,+,0)
                Case00(1,0,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
        else
        {
            if ( kDir.Z() > (Real)0.0 )
            {
                // (0,0,+)
                Case00(2,0,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (0,0,0)
                Case000(kPnt,rkBox,fSqrDistance);
                if ( pfLParam )
                    *pfLParam = (Real)0.0;
            }
        }
    }

    // undo reflections
    for (i = 0; i < 3; i++)
    {
        if ( bReflect[i] )
            kPnt[i] = -kPnt[i];
    }

    if ( pfBParam0 )
        *pfBParam0 = kPnt.X();

    if ( pfBParam1 )
        *pfBParam1 = kPnt.Y();

    if ( pfBParam2 )
        *pfBParam2 = kPnt.Z();

    return fSqrDistance;
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::SqrDistance (const Ray3<Real>& rkRay, const Box3<Real>& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
    Line3<Real> kLine;
    kLine.Origin() = rkRay.Origin();
    kLine.Direction() = rkRay.Direction();

    Real fLP, fBP0, fBP1, fBP2;
    Real fSqrDistance = SqrDistance(kLine,rkBox,&fLP,&fBP0,&fBP1,&fBP2);
    if ( fLP >= (Real)0.0 )
    {
        if ( pfLParam )
            *pfLParam = fLP;

        if ( pfBParam0 )
            *pfBParam0 = fBP0;

        if ( pfBParam1 )
            *pfBParam1 = fBP1;

        if ( pfBParam2 )
            *pfBParam2 = fBP2;

        return fSqrDistance;
    }
    else
    {
        fSqrDistance = SqrDistance(rkRay.Origin(),rkBox,pfBParam0,
            pfBParam1,pfBParam2);

        if ( pfLParam )
            *pfLParam = (Real)0.0;

        return fSqrDistance;
    }
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::SqrDistance (const Segment3<Real>& rkSeg, const Box3<Real>& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
    Line3<Real> kLine;
    kLine.Origin() = rkSeg.Origin();
    kLine.Direction() = rkSeg.Direction();

    Real fLP, fBP0, fBP1, fBP2;
    Real fSqrDistance = SqrDistance(kLine,rkBox,&fLP,&fBP0,&fBP1,&fBP2);
    if ( fLP >= (Real)0.0 )
    {
        if ( fLP <= (Real)1.0 )
        {
            if ( pfLParam )
                *pfLParam = fLP;

            if ( pfBParam0 )
                *pfBParam0 = fBP0;

            if ( pfBParam1 )
                *pfBParam1 = fBP1;

            if ( pfBParam2 )
                *pfBParam2 = fBP2;

            return fSqrDistance;
        }
        else
        {
            fSqrDistance = SqrDistance<Real>(rkSeg.Origin()+rkSeg.Direction(),
                rkBox,pfBParam0,pfBParam1,pfBParam2);

            if ( pfLParam )
                *pfLParam = (Real)1.0;

            return fSqrDistance;
        }
    }
    else
    {
        fSqrDistance = SqrDistance(rkSeg.Origin(),rkBox,pfBParam0,
            pfBParam1,pfBParam2);

        if ( pfLParam )
            *pfLParam = (Real)0.0;

        return fSqrDistance;
    }
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::Distance (const Line3<Real>& rkLine, const Box3<Real>& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
    return Math<Real>::Sqrt(SqrDistance(rkLine,rkBox,pfLParam,pfBParam0,
        pfBParam1,pfBParam2));
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::Distance (const Ray3<Real>& rkRay, const Box3<Real>& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
    return Math<Real>::Sqrt(SqrDistance(rkRay,rkBox,pfLParam,pfBParam0,
        pfBParam1,pfBParam2));
}
//----------------------------------------------------------------------------
template <class Real>
Real Wml::Distance (const Segment3<Real>& rkSeg, const Box3<Real>& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
    return Math<Real>::Sqrt(SqrDistance(rkSeg,rkBox,pfLParam,pfBParam0,
        pfBParam1,pfBParam2));
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// explicit instantiation
//----------------------------------------------------------------------------
namespace Wml
{
template WML_ITEM float SqrDistance<float> (const Line3<float>&,
    const Box3<float>&, float*, float*, float*, float*);
template WML_ITEM float SqrDistance<float> (const Ray3<float>&,
    const Box3<float>&, float*, float*, float*, float*);
template WML_ITEM float SqrDistance<float> (const Segment3<float>&,
    const Box3<float>&, float*, float*, float*, float*);
template WML_ITEM float Distance<float> (const Line3<float>&,
    const Box3<float>&, float*, float*, float*, float*);
template WML_ITEM float Distance<float> (const Ray3<float>&,
    const Box3<float>&, float*, float*, float*, float*);
template WML_ITEM float Distance<float> (const Segment3<float>&,
    const Box3<float>&, float*, float*, float*, float*);

template WML_ITEM double SqrDistance<double> (const Line3<double>&,
    const Box3<double>&, double*, double*, double*, double*);
template WML_ITEM double SqrDistance<double> (const Ray3<double>&,
    const Box3<double>&, double*, double*, double*, double*);
template WML_ITEM double SqrDistance<double> (const Segment3<double>&,
    const Box3<double>&, double*, double*, double*, double*);
template WML_ITEM double Distance<double> (const Line3<double>&,
    const Box3<double>&, double*, double*, double*, double*);
template WML_ITEM double Distance<double> (const Ray3<double>&,
    const Box3<double>&, double*, double*, double*, double*);
template WML_ITEM double Distance<double> (const Segment3<double>&,
    const Box3<double>&, double*, double*, double*, double*);
}
//----------------------------------------------------------------------------