mgcspherefit.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 "MgcSphereFit.h"

//----------------------------------------------------------------------------
bool MgcSphereFit (int iQuantity, const MgcVector3* akPoint,
    MgcVector3& rkCenter, MgcReal& rfRadius)
{
    // user-selected parameters
    const int iMaxIterations = 64;
    const MgcReal fTolerance = 1e-06;

    // compute the average of the data points
    MgcVector3 kAverage = akPoint[0];
    int i0;
    for (i0 = 1; i0 < iQuantity; i0++)
        kAverage += akPoint[i0];
    MgcReal fInvQuantity = 1.0/iQuantity;
    kAverage *= fInvQuantity;

    // initial guess
    rkCenter = kAverage;

    int i1;
    for (i1 = 0; i1 < iMaxIterations; i1++)
    {
        // update the iterates
        MgcVector3 kCurrent = rkCenter;

        // compute average L, dL/da, dL/db, dL/dc
        MgcReal fLAverage = 0.0;
        MgcVector3 kDerLAverage = MgcVector3::ZERO;
        for (i0 = 0; i0 < iQuantity; i0++)
        {
            MgcVector3 kDiff = akPoint[i0] - rkCenter;
            MgcReal fLength = kDiff.Length();
            if ( fLength > fTolerance )
            {
                fLAverage += fLength;
                MgcReal fInvLength = 1.0/fLength;
                kDerLAverage -= fInvLength*kDiff;
            }
        }
        fLAverage *= fInvQuantity;
        kDerLAverage *= fInvQuantity;

        rkCenter = kAverage + fLAverage*kDerLAverage;
        rfRadius = fLAverage;

        if ( MgcMath::Abs(rkCenter.x - kCurrent.x) <= fTolerance
        &&   MgcMath::Abs(rkCenter.y - kCurrent.y) <= fTolerance
        &&   MgcMath::Abs(rkCenter.z - kCurrent.z) <= fTolerance )
        {
            break;
        }
    }

    return i1 < iMaxIterations;
}
//----------------------------------------------------------------------------