mgcellipsefit.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 "MgcContBox.h"
#include "MgcDistVec2Elp2.h"
#include "MgcEllipseFit.h"
#include "MgcMinimizeND.h"


class PointArray
{
public:
    PointArray (int iQuantity, const MgcVector2* akPoint)
        :
        m_akPoint(akPoint)
    {
        m_iQuantity = iQuantity;
        m_akTemp = new MgcVector2[iQuantity];
    }

    ~PointArray ()
    {
        delete[] m_akTemp;
    }

    int m_iQuantity;
    const MgcVector2* m_akPoint;
    MgcVector2* m_akTemp;
};

//----------------------------------------------------------------------------
static MgcReal Energy (const MgcReal* afV, void* pvUserData)
{
    int iQuantity = ((PointArray*)pvUserData)->m_iQuantity;
    const MgcVector2* akPoint = ((PointArray*)pvUserData)->m_akPoint;
    MgcVector2* akTemp = ((PointArray*)pvUserData)->m_akTemp;

    MgcReal fEnergy = 0.0;

    // build rotation matrix
    MgcMatrix2 kRot;
    kRot.FromAngle(afV[4]);

    MgcEllipseStandard kEllipse;
    kEllipse.Extent(0) = afV[0];
    kEllipse.Extent(1) = afV[1];
    MgcVector2 kClosest;

    // transform the points to the coordinate system of U and R
    for (int i = 0; i < iQuantity; i++)
    {
        MgcVector2 kDiff(akPoint[i].x - afV[2],akPoint[i].y - afV[3]);
        akTemp[i] = kDiff*kRot;
        MgcReal fDist = MgcDistance(kEllipse,akTemp[i],kClosest);
        fEnergy += fDist;
    }

    return fEnergy;
}
//----------------------------------------------------------------------------
static void InitialGuess (int iQuantity, const MgcVector2* akPoint,
    MgcVector2& rkU, MgcMatrix2& rkR, MgcReal afD[2])
{
    MgcBox2 kBox = MgcContOrientedBox(iQuantity,akPoint);

    rkU = kBox.Center();
    rkR[0][0] = kBox.Axis(0).x;
    rkR[0][1] = kBox.Axis(0).y;
    rkR[1][0] = kBox.Axis(1).x;
    rkR[1][1] = kBox.Axis(1).y;
    afD[0] = kBox.Extent(0);
    afD[1] = kBox.Extent(1);
}
//----------------------------------------------------------------------------
MgcReal MgcEllipseFit (int iQuantity, const MgcVector2* akPoint,
    MgcVector2& rkU, MgcMatrix2& rkR, MgcReal afD[2])
{
    // Energy function is E : R^5 -> R where
    // V = (V0,V1,V2,V3,V4)
    //   = (D[0],D[1],U.x,U,y,atan2(R[1][0],R[1][1])).

    unsigned int uiMaxLevel = 8;
    unsigned int uiMaxBracket = 8;
    unsigned int uiMaxIterations = 32;
    PointArray kPA(iQuantity,akPoint);
    MgcMinimizeND kMinimizer(5,Energy,uiMaxLevel,uiMaxBracket,
        uiMaxIterations,&kPA);

    InitialGuess(iQuantity,akPoint,rkU,rkR,afD);
    MgcReal fAngle = MgcMath::ACos(rkR[0][0]);
    MgcReal fE0 = afD[0]*MgcMath::Abs(rkR[0][0]) +
        afD[1]*MgcMath::Abs(rkR[0][1]);
    MgcReal fE1 = afD[0]*MgcMath::Abs(rkR[1][0]) +
        afD[1]*MgcMath::Abs(rkR[1][1]);

    MgcReal afV0[5] =
    {
        0.5*afD[0],
        0.5*afD[1],
        rkU.x - fE0,
        rkU.y - fE1,
        0.0
    };

    MgcReal afV1[5] =
    {
        2.0*afD[0],
        2.0*afD[1],
        rkU.x + fE0,
        rkU.y + fE1,
        MgcMath::PI
    };

    MgcReal afVInitial[5] =
    {
        afD[0],
        afD[1],
        rkU.x,
        rkU.y,
        fAngle
    };

    MgcReal afVMin[5], fEMin;
    kMinimizer.GetMinimum(afV0,afV1,afVInitial,afVMin,fEMin);

    afD[0] = afVMin[0];
    afD[1] = afVMin[1];
    rkU.x = afVMin[2];
    rkU.y = afVMin[3];
    rkR.FromAngle(afVMin[4]);

    return fEMin;
}
//----------------------------------------------------------------------------