wmldistlin3box3.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// http://www.wild-magic.com
// Copyright (c) 2003.  All Rights Reserved
//
// The Wild Magic Library (WML) source code is supplied under the terms of
// the license agreement http://www.magic-software.com/License/WildMagic.pdf
// and may not be copied or disclosed except in accordance with the terms of
// that agreement.

#include "WmlDistLin3Box3.h"
#include "WmlDistVec3Box3.h"
using namespace Wml;

//----------------------------------------------------------------------------
template <class Real>
static void Face (int i0, int i1, int i2, Vector3<Real>& rkPnt,
    const Vector3<Real>& rkDir, const Box3<Real>& rkBox,
    const Vector3<Real>& rkPmE, Real* pfLParam, Real& rfSqrDistance)
{
    Vector3<Real> kPpE;
    Real fLSqr, fInv, fTmp, fParam, fT, fDelta;

    kPpE[i1] = rkPnt[i1] + rkBox.Extent(i1);
    kPpE[i2] = rkPnt[i2] + rkBox.Extent(i2);
    if ( rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0] )
    {
        if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
        {
            // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
            if ( pfLParam )
            {
                rkPnt[i0] = rkBox.Extent(i0);
                fInv = ((Real)1.0)/rkDir[i0];
                rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
                rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
                *pfLParam = -rkPmE[i0]*fInv;
            }
        }
        else
        {
            // v[i1] >= -e[i1], v[i2] < -e[i2]
            fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
            fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] +
                rkDir[i2]*kPpE[i2]);
            if ( fTmp <= ((Real)2.0)*fLSqr*rkBox.Extent(i1) )
            {
                fT = fTmp/fLSqr;
                fLSqr += rkDir[i1]*rkDir[i1];
                fTmp = kPpE[i1] - fT;
                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp +
                    rkDir[i2]*kPpE[i2];
                fParam = -fDelta/fLSqr;
                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp +
                    kPpE[i2]*kPpE[i2] + fDelta*fParam;

                if ( pfLParam )
                {
                    *pfLParam = fParam;
                    rkPnt[i0] = rkBox.Extent(i0);
                    rkPnt[i1] = fT - rkBox.Extent(i1);
                    rkPnt[i2] = -rkBox.Extent(i2);
                }
            }
            else
            {
                fLSqr += rkDir[i1]*rkDir[i1];
                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] +
                    rkDir[i2]*kPpE[i2];
                fParam = -fDelta/fLSqr;
                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] +
                    kPpE[i2]*kPpE[i2] + fDelta*fParam;

                if ( pfLParam )
                {
                    *pfLParam = fParam;
                    rkPnt[i0] = rkBox.Extent(i0);
                    rkPnt[i1] = rkBox.Extent(i1);
                    rkPnt[i2] = -rkBox.Extent(i2);
                }
            }
        }
    }
    else
    {
        if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
        {
            // v[i1] < -e[i1], v[i2] >= -e[i2]
            fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
            fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] +
                rkDir[i1]*kPpE[i1]);
            if ( fTmp <= ((Real)2.0)*fLSqr*rkBox.Extent(i2) )
            {
                fT = fTmp/fLSqr;
                fLSqr += rkDir[i2]*rkDir[i2];
                fTmp = kPpE[i2] - fT;
                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
                    rkDir[i2]*fTmp;
                fParam = -fDelta/fLSqr;
                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
                    fTmp*fTmp + fDelta*fParam;

                if ( pfLParam )
                {
                    *pfLParam = fParam;
                    rkPnt[i0] = rkBox.Extent(i0);
                    rkPnt[i1] = -rkBox.Extent(i1);
                    rkPnt[i2] = fT - rkBox.Extent(i2);
                }
            }
            else
            {
                fLSqr += rkDir[i2]*rkDir[i2];
                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
                    rkDir[i2]*rkPmE[i2];
                fParam = -fDelta/fLSqr;
                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
                    rkPmE[i2]*rkPmE[i2] + fDelta*fParam;

                if ( pfLParam )
                {
                    *pfLParam = fParam;
                    rkPnt[i0] = rkBox.Extent(i0);
                    rkPnt[i1] = -rkBox.Extent(i1);
                    rkPnt[i2] = rkBox.Extent(i2);
                }
            }
        }
        else
        {
            // v[i1] < -e[i1], v[i2] < -e[i2]
            fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
            fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] +
                rkDir[i2]*kPpE[i2]);
            if ( fTmp >= (Real)0.0 )
            {
                // v[i1]-edge is closest
                if ( fTmp <= ((Real)2.0)*fLSqr*rkBox.Extent(i1) )
                {
                    fT = fTmp/fLSqr;
                    fLSqr += rkDir[i1]*rkDir[i1];
                    fTmp = kPpE[i1] - fT;
                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp +
                        rkDir[i2]*kPpE[i2];
                    fParam = -fDelta/fLSqr;
                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp +
                        kPpE[i2]*kPpE[i2] + fDelta*fParam;

                    if ( pfLParam )
                    {
                        *pfLParam = fParam;
                        rkPnt[i0] = rkBox.Extent(i0);
                        rkPnt[i1] = fT - rkBox.Extent(i1);
                        rkPnt[i2] = -rkBox.Extent(i2);
                    }
                }
                else
                {
                    fLSqr += rkDir[i1]*rkDir[i1];
                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] +
                        rkDir[i2]*kPpE[i2];
                    fParam = -fDelta/fLSqr;
                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1]
                        + kPpE[i2]*kPpE[i2] + fDelta*fParam;

                    if ( pfLParam )
                    {
                        *pfLParam = fParam;
                        rkPnt[i0] = rkBox.Extent(i0);
                        rkPnt[i1] = rkBox.Extent(i1);
                        rkPnt[i2] = -rkBox.Extent(i2);
                    }
                }
                return;
            }

            fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
            fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] +
                rkDir[i1]*kPpE[i1]);
            if ( fTmp >= (Real)0.0 )
            {
                // v[i2]-edge is closest
                if ( fTmp <= ((Real)2.0)*fLSqr*rkBox.Extent(i2) )
                {
                    fT = fTmp/fLSqr;
                    fLSqr += rkDir[i2]*rkDir[i2];
                    fTmp = kPpE[i2] - fT;
                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
                        rkDir[i2]*fTmp;
                    fParam = -fDelta/fLSqr;
                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
                        fTmp*fTmp + fDelta*fParam;

                    if ( pfLParam )
                    {
                        *pfLParam = fParam;
                        rkPnt[i0] = rkBox.Extent(i0);
                        rkPnt[i1] = -rkBox.Extent(i1);
                        rkPnt[i2] = fT - rkBox.Extent(i2);
                    }
                }
                else
                {
                    fLSqr += rkDir[i2]*rkDir[i2];
                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
                        rkDir[i2]*rkPmE[i2];
                    fParam = -fDelta/fLSqr;
                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
                        rkPmE[i2]*rkPmE[i2] + fDelta*fParam;

                    if ( pfLParam )
                    {
                        *pfLParam = fParam;
                        rkPnt[i0] = rkBox.Extent(i0);
                        rkPnt[i1] = -rkBox.Extent(i1);
                        rkPnt[i2] = rkBox.Extent(i2);
                    }
                }
                return;
            }

            // (v[i1],v[i2])-corner is closest
            fLSqr += rkDir[i2]*rkDir[i2];
            fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
                rkDir[i2]*kPpE[i2];
            fParam = -fDelta/fLSqr;
            rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
                kPpE[i2]*kPpE[i2] + fDelta*fParam;

            if ( pfLParam )
            {
                *pfLParam = fParam;
                rkPnt[i0] = rkBox.Extent(i0);
                rkPnt[i1] = -rkBox.Extent(i1);
                rkPnt[i2] = -rkBox.Extent(i2);
            }
        }
    }
}
//----------------------------------------------------------------------------
template <class Real>
static void CaseNoZeros (Vector3<Real>& rkPnt, const Vector3<Real>& rkDir,
    const Box3<Real>& rkBox, Real* pfLParam, Real& rfSqrDistance)
{
    Vector3<Real> kPmE(
        rkPnt.X() - rkBox.Extent(0),
        rkPnt.Y() - rkBox.Extent(1),
        rkPnt.Z() - rkBox.Extent(2));

    Real fProdDxPy = rkDir.X()*kPmE.Y();
    Real fProdDyPx = rkDir.Y()*kPmE.X();
    Real fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;

    if ( fProdDyPx >= fProdDxPy )
    {
        fProdDzPx = rkDir.Z()*kPmE.X();
        fProdDxPz = rkDir.X()*kPmE.Z();
        if ( fProdDzPx >= fProdDxPz )
        {
            // line intersects x = e0
            Face(0,1,2,rkPnt,rkDir,rkBox,kPmE,pfLParam,rfSqrDistance);
        }
        else
        {
            // line intersects z = e2
            Face(2,0,1,rkPnt,rkDir,rkBox,kPmE,pfLParam,rfSqrDistance);
        }
    }
    else
    {
        fProdDzPy = rkDir.Z()*kPmE.Y();
        fProdDyPz = rkDir.Y()*kPmE.Z();
        if ( fProdDzPy >= fProdDyPz )
        {
            // line intersects y = e1
            Face(1,2,0,rkPnt,rkDir,rkBox,kPmE,pfLParam,rfSqrDistance);
        }
        else
        {
            // line intersects z = e2
            Face(2,0,1,rkPnt,rkDir,rkBox,kPmE,pfLParam,rfSqrDistance);
        }
    }
}
//----------------------------------------------------------------------------
template <class Real>
static void Case0 (int i0, int i1, int i2, Vector3<Real>& rkPnt,
    const Vector3<Real>& rkDir, const Box3<Real>& rkBox, Real* pfLParam,
    Real& rfSqrDistance)
{
    Real fPmE0 = rkPnt[i0] - rkBox.Extent(i0);
    Real fPmE1 = rkPnt[i1] - rkBox.Extent(i1);
    Real fProd0 = rkDir[i1]*fPmE0;
    Real fProd1 = rkDir[i0]*fPmE1;
    Real fDelta, fInvLSqr, fInv;

    if ( fProd0 >= fProd1 )
    {
        // line intersects P[i0] = e[i0]
        rkPnt[i0] = rkBox.Extent(i0);

        Real fPpE1 = rkPnt[i1] + rkBox.Extent(i1);
        fDelta = fProd0 - rkDir[i0]*fPpE1;
        if ( fDelta >= (Real)0.0 )
        {
            fInvLSqr = ((Real)1.0)/(rkDir[i0]*rkDir[i0]+rkDir[i1]*rkDir[i1]);
            rfSqrDistance += fDelta*fDelta*fInvLSqr;
            if ( pfLParam )
            {
                rkPnt[i1] = -rkBox.Extent(i1);
                *pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
            }
        }
        else
        {
            if ( pfLParam )
            {
                fInv = ((Real)1.0)/rkDir[i0];
                rkPnt[i1] -= fProd0*fInv;
                *pfLParam = -fPmE0*fInv;
            }
        }
    }
    else
    {
        // line intersects P[i1] = e[i1]
        rkPnt[i1] = rkBox.Extent(i1);

        Real fPpE0 = rkPnt[i0] + rkBox.Extent(i0);
        fDelta = fProd1 - rkDir[i1]*fPpE0;
        if ( fDelta >= (Real)0.0 )
        {
            fInvLSqr = ((Real)1.0)/(rkDir[i0]*rkDir[i0]+rkDir[i1]*rkDir[i1]);
            rfSqrDistance += fDelta*fDelta*fInvLSqr;
            if ( pfLParam )
            {
                rkPnt[i0] = -rkBox.Extent(i0);
                *pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
            }
        }
        else
        {
            if ( pfLParam )
            {
                fInv = ((Real)1.0)/rkDir[i1];
                rkPnt[i0] -= fProd1*fInv;
                *pfLParam = -fPmE1*fInv;
            }
        }
    }

    if ( rkPnt[i2] < -rkBox.Extent(i2) )
    {
        fDelta = rkPnt[i2] + rkBox.Extent(i2);