mgcdistvec2elp2.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 afExtent[0] 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 "MgcDistVec2Elp2.h"

//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcEllipseStandard& rkEllipse,
    const MgcVector2& rkPoint, MgcVector2& rkClosest)
{
    const MgcReal* afExtent = rkEllipse.Extents();

    MgcReal fA2 = afExtent[0]*afExtent[0];
    MgcReal fB2 = afExtent[1]*afExtent[1];
    MgcReal fU2 = rkPoint.x*rkPoint.x;
    MgcReal fV2 = rkPoint.y*rkPoint.y;
    MgcReal fA2U2 = fA2*fU2, fB2V2 = fB2*fV2;
    MgcReal fDx, fDy, fXDivA, fYDivB;

    // handle points near the coordinate axes
    const MgcReal fThreshold = 1e-12;
    if ( MgcMath::Abs(rkPoint.x) <= fThreshold )  // rkPoint.x == 0
    {
        if ( afExtent[0] >= afExtent[1] 
        ||   MgcMath::Abs(rkPoint.y) >= afExtent[1]-fA2/afExtent[1] )
        {
            rkClosest.x = 0.0;
            rkClosest.y = ( rkPoint.y >= 0.0 ? afExtent[1] : -afExtent[1] );
            fDy = rkClosest.y - rkPoint.y;
            return MgcMath::Abs(fDy);
        }
        else
        {
            rkClosest.y = fB2*rkPoint.y/(fB2-fA2);
            fDy = rkClosest.y - rkPoint.y;
            fYDivB = rkClosest.y/afExtent[1];
            rkClosest.x = afExtent[0]*MgcMath::Sqrt(MgcMath::Abs(
                1.0-fYDivB*fYDivB));
            return MgcMath::Sqrt(rkClosest.x*rkClosest.x+fDy*fDy);
        }
    }

    if ( MgcMath::Abs(rkPoint.y) <= fThreshold )  // rkPoint.y == 0
    {
        if ( afExtent[1] >= afExtent[0]
        ||   MgcMath::Abs(rkPoint.x) >= afExtent[0]-fB2/afExtent[0] )
        {
            rkClosest.x = ( rkPoint.x >= 0 ? afExtent[0] : -afExtent[0] );
            rkClosest.y = 0;
            fDx = rkClosest.x - rkPoint.x;
            return MgcMath::Abs(fDx);
        }
        else
        {
            rkClosest.x = fA2*rkPoint.x/(fA2-fB2);
            fDx = rkClosest.x - rkPoint.x;
            fXDivA = rkClosest.x/afExtent[0];
            rkClosest.y = afExtent[1]*MgcMath::Sqrt(MgcMath::Abs(
                1.0-fXDivA*fXDivA));
            return MgcMath::Sqrt(fDx*fDx+rkClosest.y*rkClosest.y);
        }
    }

    // initial guess
    MgcReal fURatio = rkPoint.x/afExtent[0];
    MgcReal fVRatio = rkPoint.y/afExtent[1];
    MgcReal fT;
    if ( fURatio*fURatio + fVRatio*fVRatio < 1.0 )
    {
        fT = 0.0;
    }
    else
    {
        MgcReal fMax = afExtent[0];
        if ( afExtent[1] > fMax )
            fMax = afExtent[1];

        fT = fMax*rkPoint.Length();
    }

    // Newton's method
    const int iMaxIteration = 64;
    const MgcReal fEpsilon = 1e-06;
    MgcReal fP, fQ;
    for (int i = 0; i < iMaxIteration; i++)
    {
        fP = fT+fA2;
        fQ = fT+fB2;
        MgcReal fP2 = fP*fP;
        MgcReal fQ2 = fQ*fQ;
        MgcReal fR = fP2*fQ2-fA2U2*fQ2-fB2V2*fP2;
        if ( MgcMath::Abs(fR) < fEpsilon )
            break;

        MgcReal fDR = 2.0*(fP*fQ*(fP+fQ)-fA2U2*fQ-fB2V2*fP);
        fT -= fR/fDR;
    }

    rkClosest.x = fA2*rkPoint.x/fP;
    rkClosest.y = fB2*rkPoint.y/fQ;
    MgcVector2 kDiff = rkClosest - rkPoint;
    return kDiff.Length();
}
//----------------------------------------------------------------------------
MgcReal MgcDistance (const MgcEllipseStandard& rkEllipse,
    const MgcVector2& rkPoint, MgcVector2& rkClosest)
{
    return MgcMath::Sqrt(MgcDistance(rkEllipse,rkPoint,rkClosest));
}
//----------------------------------------------------------------------------