mgcellipsoidfit.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 "MgcDistVec3Elp3.h"
#include "MgcEllipsoidFit.h"
#include "MgcMinimizeND.h"


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

    ~PointArray ()
    {
        delete[] m_akTemp;
    }

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

//----------------------------------------------------------------------------
static void MatrixToAngles (const MgcMatrix3& rkR, MgcReal* afAngle)
{
    // rotation axis = (cos(a0)sin(a1),sin(a0)sin(a1),cos(a1))
    // a0 in [-pi,pi], a1 in [0,pi], a2 in [0,pi]

    MgcVector3 kAxis;
    rkR.ToAxisAngle(kAxis,afAngle[2]);

    if ( -1.0 < kAxis.z )
    {
        if ( kAxis.x < 1.0 )
        {
            afAngle[0] = MgcMath::ATan2(kAxis.y,kAxis.x);
            afAngle[1] = MgcMath::ACos(kAxis.z);
        }
        else
        {
            afAngle[0] = 0.0;
            afAngle[1] = 0.0;
        }
    }
    else
    {
        afAngle[0] = 0.0;
        afAngle[1] = MgcMath::PI;
    }
}
//----------------------------------------------------------------------------
static void AnglesToMatrix (const MgcReal* afAngle, MgcMatrix3& rkR)
{
    // rotation axis = (cos(a0)sin(a1),sin(a0)sin(a1),cos(a1))
    // a0 in [-pi,pi], a1 in [0,pi], a2 in [0,pi]

    MgcReal fCos0 = MgcMath::Cos(afAngle[0]);
    MgcReal fSin0 = MgcMath::Sin(afAngle[0]);
    MgcReal fCos1 = MgcMath::Cos(afAngle[1]);
    MgcReal fSin1 = MgcMath::Sin(afAngle[1]);
    MgcVector3 kAxis(fCos0*fSin1,fSin0*fSin1,fCos1);
    rkR.FromAxisAngle(kAxis,afAngle[2]);
}
//----------------------------------------------------------------------------
static MgcReal Energy (const MgcReal* afV, void* pvUserData)
{
    int iQuantity = ((PointArray*)pvUserData)->m_iQuantity;
    const MgcVector3* akPoint = ((PointArray*)pvUserData)->m_akPoint;
    MgcVector3* akTemp = ((PointArray*)pvUserData)->m_akTemp;

    MgcReal fEnergy = 0.0;

    // build rotation matrix
    MgcMatrix3 kRot;
    AnglesToMatrix(&afV[6],kRot);

    // Uniformly scale the extents to keep reasonable floating point values
    // in the distance calculations.
    MgcReal fMax = afV[0];
    if ( afV[1] > fMax )
        fMax = afV[1];
    if ( afV[2] > fMax )
        fMax = afV[2];

    MgcReal fInvMax = 1.0/fMax;
    MgcEllipsoidStandard kEllipsoid;
    kEllipsoid.Extent(0) = fInvMax*afV[0];
    kEllipsoid.Extent(1) = fInvMax*afV[1];
    kEllipsoid.Extent(2) = fInvMax*afV[2];
    MgcVector3 kClosest;

    // transform the points to the coordinate system of U and R
    for (int i = 0; i < iQuantity; i++)
    {
        MgcVector3 kDiff(
            akPoint[i].x - afV[3],
            akPoint[i].y - afV[4],
            akPoint[i].z - afV[5]);

        akTemp[i] = fInvMax*(kDiff*kRot);

        MgcReal fDist = MgcDistance(kEllipsoid,akTemp[i],kClosest);
        fEnergy += fMax*fDist;
    }

    return fEnergy;
}
//----------------------------------------------------------------------------
static void InitialGuess (int iQuantity, const MgcVector3* akPoint,
    MgcVector3& rkU, MgcMatrix3& rkR, MgcReal afD[3])
{
    MgcBox3 kBox = MgcContOrientedBox(iQuantity,akPoint);

    rkU = kBox.Center();
    rkR[0][0] = kBox.Axis(0).x;
    rkR[0][1] = kBox.Axis(0).y;
    rkR[0][2] = kBox.Axis(0).z;
    rkR[1][0] = kBox.Axis(1).x;
    rkR[1][1] = kBox.Axis(1).y;
    rkR[1][2] = kBox.Axis(1).z;
    rkR[2][0] = kBox.Axis(2).x;
    rkR[2][1] = kBox.Axis(2).y;
    rkR[2][2] = kBox.Axis(2).z;
    afD[0] = kBox.Extent(0);
    afD[1] = kBox.Extent(1);
    afD[2] = kBox.Extent(2);
}
//----------------------------------------------------------------------------
MgcReal MgcEllipsoidFit (int iQuantity, const MgcVector3* akPoint,
    MgcVector3& rkU, MgcMatrix3& rkR, MgcReal afD[3])
{
    // Energy function is E : R^9 -> R where
    // V = (V0,V1,V2,V3,V4,V5,V6,V7,V8)
    //   = (D[0],D[1],D[2],U.x,U,y,U.z,A0,A1,A2). 
    // For really scattered data, you might need a search function

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

    InitialGuess(iQuantity,akPoint,rkU,rkR,afD);

    MgcReal afAngle[3];
    MatrixToAngles(rkR,afAngle);

    MgcReal afExtent[3] =
    {
        afD[0]*MgcMath::Abs(rkR[0][0]) + afD[1]*MgcMath::Abs(rkR[0][1])
            + afD[2]*MgcMath::Abs(rkR[0][2]),
        afD[0]*MgcMath::Abs(rkR[1][0]) + afD[1]*MgcMath::Abs(rkR[1][1])
            + afD[2]*MgcMath::Abs(rkR[1][2]),
        afD[0]*MgcMath::Abs(rkR[2][0]) + afD[1]*MgcMath::Abs(rkR[2][1])
            + afD[2]*MgcMath::Abs(rkR[2][2])
    };

    MgcReal afV0[9] =
    {
        0.5*afD[0],
        0.5*afD[1],
        0.5*afD[2],
        rkU.x - afExtent[0],
        rkU.y - afExtent[1],
        rkU.z - afExtent[2],
        -MgcMath::PI,
        0.0,
        0.0
    };

    MgcReal afV1[9] =
    {
        2.0*afD[0],
        2.0*afD[1],
        2.0*afD[2],
        rkU.x + afExtent[0],
        rkU.y + afExtent[1],
        rkU.z + afExtent[2],
        MgcMath::PI,
        MgcMath::PI,
        MgcMath::PI
    };

    MgcReal afVInitial[9] =
    {
        afD[0],
        afD[1],
        afD[2],
        rkU.x,
        rkU.y,
        rkU.z,
        afAngle[0],
        afAngle[1],
        afAngle[2]
    };

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

    afD[0] = afVMin[0];
    afD[1] = afVMin[1];
    afD[2] = afVMin[2];
    rkU.x = afVMin[3];
    rkU.y = afVMin[4];
    rkU.z = afVMin[5];

    AnglesToMatrix(&afVMin[6],rkR);

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