mgcdistlin3rct3.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 "MgcDistVec3Rct3.h"
#include "MgcDistLin3Rct3.h"
#include "MgcRTLib.h"

static const MgcReal gs_fTolerance = 1e-05;

//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcLine3& rkLine, const MgcRectangle3& rkRct,
    MgcReal* pfLinP, MgcReal* pfRctP0, MgcReal* pfRctP1)
{
    MgcVector3 kDiff = rkRct.Origin() - rkLine.Origin();
    MgcReal fA00 = rkLine.Direction().SquaredLength();
    MgcReal fA01 = -rkLine.Direction().Dot(rkRct.Edge0());
    MgcReal fA02 = -rkLine.Direction().Dot(rkRct.Edge1());
    MgcReal fA11 = rkRct.Edge0().SquaredLength();
    MgcReal fA22 = rkRct.Edge1().SquaredLength();
    MgcReal fB0  = -kDiff.Dot(rkLine.Direction());
    MgcReal fB1  = kDiff.Dot(rkRct.Edge0());
    MgcReal fB2  = kDiff.Dot(rkRct.Edge1());
    MgcReal fCof00 = fA11*fA22;
    MgcReal fCof01 = -fA01*fA22;
    MgcReal fCof02 = -fA02*fA11;
    MgcReal fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;

    MgcSegment3 kSegPgm;
    MgcVector3 kPt;
    MgcReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;

    if ( MgcMath::Abs(fDet) >= gs_fTolerance )
    {
        MgcReal fCof11 = fA00*fA22-fA02*fA02;
        MgcReal fCof12 = fA02*fA01;
        MgcReal fCof22 = fA00*fA11-fA01*fA01;
        MgcReal fInvDet = 1.0/fDet;
        MgcReal fRhs0 = -fB0*fInvDet;
        MgcReal fRhs1 = -fB1*fInvDet;
        MgcReal fRhs2 = -fB2*fInvDet;

        fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
        fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
        fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;

        if ( fS < 0.0 )
        {
            if ( fT < 0.0 )  // region 6
            {
                // min on face s=0 or t=0
                kSegPgm.Origin() = rkRct.Origin();
                kSegPgm.Direction() = rkRct.Edge1();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
                fS = 0.0;
                kSegPgm.Origin() = rkRct.Origin();
                kSegPgm.Direction() = rkRct.Edge0();
                fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
                fT0 = 0.0;
                if ( fSqrDist0 < fSqrDist )
                {
                    fSqrDist = fSqrDist0;
                    fS = fS0;
                    fT = fT0;
                }
            }
            else if ( fT <= 1.0 )  // region 5
            {
                // min on face s=0
                kSegPgm.Origin() = rkRct.Origin();
                kSegPgm.Direction() = rkRct.Edge1();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
                fS = 0.0;
            }
            else  // region 4
            {
                // min on face s=0 or t=1
                kSegPgm.Origin() = rkRct.Origin();
                kSegPgm.Direction() = rkRct.Edge1();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
                fS = 0.0;
                kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
                kSegPgm.Direction() = rkRct.Edge0();
                fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
                fT0 = 1.0;
                if ( fSqrDist0 < fSqrDist )
                {
                    fSqrDist = fSqrDist0;
                    fS = fS0;
                    fT = fT0;
                }
            }
        }
        else if ( fS <= 1.0 )
        {
            if ( fT < 0.0 )  // region 7
            {
                // min on face t=0
                kSegPgm.Origin() = rkRct.Origin();
                kSegPgm.Direction() = rkRct.Edge0();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fS);
                fT = 0.0;
            }
            else if ( fT <= 1.0 )  // region 0
            {
                // line intersects rectangle
                fSqrDist = 0.0;
            }
            else  // region 3
            {
                // min on face t=1
                kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
                kSegPgm.Direction() = rkRct.Edge0();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fS);
                fT = 1.0;
            }
        }
        else
        {
            if ( fT < 0.0 )  // region 8
            {
                // min on face s=1 or t=0
                kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
                kSegPgm.Direction() = rkRct.Edge1();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
                fS = 1.0;
                kSegPgm.Origin() = rkRct.Origin();
                kSegPgm.Direction() = rkRct.Edge0();
                fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
                fT0 = 0.0;
                if ( fSqrDist0 < fSqrDist )
                {
                    fSqrDist = fSqrDist0;
                    fS = fS0;
                    fT = fT0;
                }
            }
            else if ( fT <= 1.0 )  // region 1
            {
                // min on face s=1
                kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
                kSegPgm.Direction() = rkRct.Edge1();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
                fS = 1.0;
            }
            else  // region 2
            {
                // min on face s=1 or t=1
                kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
                kSegPgm.Direction() = rkRct.Edge1();
                fSqrDist = MgcSqrDistance(rkLine,kSegPgm,0,&fT);
                fS = 1.0;
                kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
                kSegPgm.Direction() = rkRct.Edge0();
                fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,0,&fS0);
                fT0 = 1.0;
                if ( fSqrDist0 < fSqrDist )
                {
                    fSqrDist = fSqrDist0;
                    fS = fS0;
                    fT = fT0;
                }
            }
        }
    }
    else
    {
        // line and rectangle are parallel
        kSegPgm.Origin() = rkRct.Origin();
        kSegPgm.Direction() = rkRct.Edge0();
        fSqrDist = MgcSqrDistance(rkLine,kSegPgm,&fR,&fS);
        fT = 0.0;

        kSegPgm.Direction() = rkRct.Edge1();
        fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,&fR0,&fT0);
        fS0 = 0.0;
        if ( fSqrDist0 < fSqrDist )
        {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
        }

        kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
        kSegPgm.Direction() = rkRct.Edge0();
        fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,&fR0,&fS0);
        fT0 = 1.0;
        if ( fSqrDist0 < fSqrDist )
        {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
        }

        kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge0();
        kSegPgm.Direction() = rkRct.Edge1();
        fSqrDist0 = MgcSqrDistance(rkLine,kSegPgm,&fR0,&fT0);
        fS0 = 1.0;
        if ( fSqrDist0 < fSqrDist )
        {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
        }
    }

    if ( pfLinP )
        *pfLinP = fR;

    if ( pfRctP0 )
        *pfRctP0 = fS;

    if ( pfRctP1 )
        *pfRctP1 = fT;

    return MgcMath::Abs(fSqrDist);
}
//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcRay3& rkRay, const MgcRectangle3& rkRct,
    MgcReal* pfRayP, MgcReal* pfRctP0, MgcReal* pfRctP1)
{
    MgcVector3 kDiff = rkRct.Origin() - rkRay.Origin();
    MgcReal fA00 = rkRay.Direction().SquaredLength();
    MgcReal fA01 = -rkRay.Direction().Dot(rkRct.Edge0());
    MgcReal fA02 = -rkRay.Direction().Dot(rkRct.Edge1());
    MgcReal fA11 = rkRct.Edge0().SquaredLength();
    MgcReal fA22 = rkRct.Edge1().SquaredLength();
    MgcReal fB0  = -kDiff.Dot(rkRay.Direction());
    MgcReal fB1  = kDiff.Dot(rkRct.Edge0());
    MgcReal fB2  = kDiff.Dot(rkRct.Edge1());
    MgcReal fCof00 = fA11*fA22;
    MgcReal fCof01 = -fA01*fA22;
    MgcReal fCof02 = -fA02*fA11;
    MgcReal fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;

    MgcSegment3 kSegPgm;
    MgcVector3 kPt;
    MgcReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;

    if ( MgcMath::Abs(fDet) >= gs_fTolerance )
    {
        MgcReal fCof11 = fA00*fA22-fA02*fA02;
        MgcReal fCof12 = fA02*fA01;
        MgcReal fCof22 = fA00*fA11-fA01*fA01;
        MgcReal fInvDet = 1.0/fDet;
        MgcReal fRhs0 = -fB0*fInvDet;
        MgcReal fRhs1 = -fB1*fInvDet;
        MgcReal fRhs2 = -fB2*fInvDet;

        fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
        fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
        fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;

        if ( fR <= 0.0 )
        {
            if ( fS < 0.0 )
            {
                if ( fT < 0.0 )  // region 6m
                {
                    // min on face s=0 or t=0 or r=0
                    kSegPgm.Origin() = rkRct.Origin();
                    kSegPgm.Direction() = rkRct.Edge1();
                    fSqrDist = MgcSqrDistance(rkRay,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    kSegPgm.Origin() = rkRct.Origin();
                    kSegPgm.Direction() = rkRct.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkRay,kSegPgm,&fR0,&fS0);
                    fT0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                    fSqrDist0 = MgcSqrDistance(rkRay.Origin(),rkRct,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else if ( fT <= 1.0 )  // region 5m
                {
                    // min on face s=0 or r=0
                    kSegPgm.Origin() = rkRct.Origin();
                    kSegPgm.Direction() = rkRct.Edge1();
                    fSqrDist = MgcSqrDistance(rkRay,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    fSqrDist0 = MgcSqrDistance(rkRay.Origin(),rkRct,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else  // region 4m
                {
                    // min on face s=0 or t=1 or r=0
                    kSegPgm.Origin() = rkRct.Origin();
                    kSegPgm.Direction() = rkRct.Edge1();
                    fSqrDist = MgcSqrDistance(rkRay,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    kSegPgm.Origin() = rkRct.Origin()+rkRct.Edge1();
                    kSegPgm.Direction() = rkRct.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkRay,kSegPgm,&fR0,&fS0);
                    fT0 = 1.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }