mgcdistvec3elp3.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 "MgcDistVec3Elp3.h"

//----------------------------------------------------------------------------
MgcReal MgcSqrDistance (const MgcEllipsoidStandard& rkEllipsoid,
    const MgcVector3& rkPoint, MgcVector3& rkClosest)
{
    const MgcReal* afExtent = rkEllipsoid.Extents();

    MgcReal fA2 = afExtent[0]*afExtent[0];
    MgcReal fB2 = afExtent[1]*afExtent[1];
    MgcReal fC2 = afExtent[2]*afExtent[2];
    MgcReal fU2 = rkPoint.x*rkPoint.x;
    MgcReal fV2 = rkPoint.y*rkPoint.y;
    MgcReal fW2 = rkPoint.z*rkPoint.z;
    MgcReal fA2U2 = fA2*fU2, fB2V2 = fB2*fV2, fC2W2 = fC2*fW2;

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

        fT = fMax*rkPoint.Length();
    }

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

        MgcReal fPQ = fP*fQ, fPR = fP*fR, fQR = fQ*fR, fPQR = fP*fQ*fR;
        MgcReal fDS = 2.0*(fPQR*(fQR+fPR+fPQ)-fA2U2*fQR*(fQ+fR)-
            fB2V2*fPR*(fP+fR)-fC2W2*fPQ*(fP+fQ));
        fT -= fS/fDS;
    }

    rkClosest.x = fA2*rkPoint.x/fP;
    rkClosest.y = fB2*rkPoint.y/fQ;
    rkClosest.z = fC2*rkPoint.z/fR;
    MgcVector3 kDiff = rkClosest - rkPoint;
    return kDiff.SquaredLength();
}
//----------------------------------------------------------------------------
MgcReal MgcDistance (const MgcEllipsoidStandard& rkEllipsoid,
    const MgcVector3& rkPoint, MgcVector3& rkClosest)
{
    return MgcMath::Sqrt(MgcSqrDistance(rkEllipsoid,rkPoint,rkClosest));
}
//----------------------------------------------------------------------------