mgcdistlin3box3.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 "MgcDistLin3Box3.h"
#include "MgcDistVec3Box3.h"

//----------------------------------------------------------------------------
static void Face (int i0, int i1, int i2, MgcVector3& rkPnt,
    const MgcVector3& rkDir, const MgcBox3& rkBox, const MgcVector3& rkPmE,
    MgcReal* pfLParam, MgcReal& rfSqrDistance)
{
    MgcVector3 kPpE;
    MgcReal 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 = 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 <= 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 <= 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 >= 0.0 )
            {
                // v[i1]-edge is closest
                if ( fTmp <= 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 >= 0.0 )
            {
                // v[i2]-edge is closest
                if ( fTmp <= 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);
            }
        }
    }
}
//----------------------------------------------------------------------------
static void CaseNoZeros (MgcVector3& rkPnt, const MgcVector3& rkDir,
    const MgcBox3& rkBox, MgcReal* pfLParam, MgcReal& rfSqrDistance)
{
    MgcVector3 kPmE(rkPnt.x - rkBox.Extent(0), rkPnt.y - rkBox.Extent(1),
        rkPnt.z - rkBox.Extent(2));

    MgcReal fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;

    fProdDxPy = rkDir.x*kPmE.y;
    fProdDyPx = rkDir.y*kPmE.x;
    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);
        }
    }
}
//----------------------------------------------------------------------------
static void Case0 (int i0, int i1, int i2, MgcVector3& rkPnt,
    const MgcVector3& rkDir, const MgcBox3& rkBox, MgcReal* pfLParam,
    MgcReal& rfSqrDistance)
{
    MgcReal fPmE0 = rkPnt[i0] - rkBox.Extent(i0);
    MgcReal fPmE1 = rkPnt[i1] - rkBox.Extent(i1);
    MgcReal fProd0 = rkDir[i1]*fPmE0;
    MgcReal fProd1 = rkDir[i0]*fPmE1;
    MgcReal fDelta, fInvLSqr, fInv;

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

        MgcReal fPpE1 = rkPnt[i1] + rkBox.Extent(i1);
        fDelta = fProd0 - rkDir[i0]*fPpE1;
        if ( fDelta >= 0.0 )
        {
            fInvLSqr = 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 = 1.0/rkDir[i0];
                rkPnt[i1] -= fProd0*fInv;
                *pfLParam = -fPmE0*fInv;
            }
        }
    }
    else
    {
        // line intersects P[i1] = e[i1]
        rkPnt[i1] = rkBox.Extent(i1);

        MgcReal fPpE0 = rkPnt[i0] + rkBox.Extent(i0);
        fDelta = fProd1 - rkDir[i1]*fPpE0;
        if ( fDelta >= 0.0 )
        {
            fInvLSqr = 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 = 1.0/rkDir[i1];
                rkPnt[i0] -= fProd1*fInv;
                *pfLParam = -fPmE1*fInv;
            }
        }
    }

    if ( rkPnt[i2] < -rkBox.Extent(i2) )