mgcquadraticfit3.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 "MgcEigen.h"
#include "MgcQuadraticFit3.h"

//----------------------------------------------------------------------------
MgcReal MgcQuadraticFit (int iQuantity, const MgcVector3* akPoint,
    MgcReal afCoeff[10])
{
    MgcEigen kES(10);
    int iRow, iCol;
    for (iRow = 0; iRow < 10; iRow++)
    {
        for (iCol = 0; iCol < 10; iCol++)
            kES.Matrix(iRow,iCol) = 0.0;
    }

    for (int i = 0; i < iQuantity; i++)
    {
        MgcReal fX = akPoint[i].x;
        MgcReal fY = akPoint[i].y;
        MgcReal fZ = akPoint[i].z;
        MgcReal fX2 = fX*fX;
        MgcReal fY2 = fY*fY;
        MgcReal fZ2 = fZ*fZ;
        MgcReal fXY = fX*fY;
        MgcReal fXZ = fX*fZ;
        MgcReal fYZ = fY*fZ;
        MgcReal fX3 = fX*fX2;
        MgcReal fXY2 = fX*fY2;
        MgcReal fXZ2 = fX*fZ2;
        MgcReal fX2Y = fX*fXY;
        MgcReal fX2Z = fX*fXZ;
        MgcReal fXYZ = fX*fY*fZ;
        MgcReal fY3 = fY*fY2;
        MgcReal fYZ2 = fY*fZ2;
        MgcReal fY2Z = fY*fYZ;
        MgcReal fZ3 = fZ*fZ2;
        MgcReal fX4 = fX*fX3;
        MgcReal fX2Y2 = fX*fXY2;
        MgcReal fX2Z2 = fX*fXZ2;
        MgcReal fX3Y = fX*fX2Y;
        MgcReal fX3Z = fX*fX2Z;
        MgcReal fX2YZ = fX*fXYZ;
        MgcReal fY4 = fY*fY3;
        MgcReal fY2Z2 = fY*fYZ2;
        MgcReal fXY3 = fX*fY3;
        MgcReal fXY2Z = fX*fY2Z;
        MgcReal fY3Z = fY*fY2Z;
        MgcReal fZ4 = fZ*fZ3;
        MgcReal fXYZ2 = fX*fYZ2;
        MgcReal fXZ3 = fX*fZ3;
        MgcReal fYZ3 = fY*fZ3;

        kES.Matrix(0,1) += fX;
        kES.Matrix(0,2) += fY;
        kES.Matrix(0,3) += fZ;
        kES.Matrix(0,4) += fX2;
        kES.Matrix(0,5) += fY2;
        kES.Matrix(0,6) += fZ2;
        kES.Matrix(0,7) += fXY;
        kES.Matrix(0,8) += fXZ;
        kES.Matrix(0,9) += fYZ;
        kES.Matrix(1,4) += fX3;
        kES.Matrix(1,5) += fXY2;
        kES.Matrix(1,6) += fXZ2;
        kES.Matrix(1,7) += fX2Y;
        kES.Matrix(1,8) += fX2Z;
        kES.Matrix(1,9) += fXYZ;
        kES.Matrix(2,5) += fY3;
        kES.Matrix(2,6) += fYZ2;
        kES.Matrix(2,9) += fY2Z;
        kES.Matrix(3,6) += fZ3;
        kES.Matrix(4,4) += fX4;
        kES.Matrix(4,5) += fX2Y2;
        kES.Matrix(4,6) += fX2Z2;
        kES.Matrix(4,7) += fX3Y;
        kES.Matrix(4,8) += fX3Z;
        kES.Matrix(4,9) += fX2YZ;
        kES.Matrix(5,5) += fY4;
        kES.Matrix(5,6) += fY2Z2;
        kES.Matrix(5,7) += fXY3;
        kES.Matrix(5,8) += fXY2Z;
        kES.Matrix(5,9) += fY3Z;
        kES.Matrix(6,6) += fZ4;
        kES.Matrix(6,7) += fXYZ2;
        kES.Matrix(6,8) += fXZ3;
        kES.Matrix(6,9) += fYZ3;
        kES.Matrix(9,9) += fY2Z2;
    }

    kES.Matrix(0,0) = iQuantity;

    kES.Matrix(1,1) = kES.Matrix(0,4);
    kES.Matrix(1,2) = kES.Matrix(0,7);
    kES.Matrix(1,3) = kES.Matrix(0,8);
    kES.Matrix(2,2) = kES.Matrix(0,5);
    kES.Matrix(2,3) = kES.Matrix(0,9);
    kES.Matrix(2,4) = kES.Matrix(1,7);
    kES.Matrix(2,7) = kES.Matrix(1,5);
    kES.Matrix(2,8) = kES.Matrix(1,9);
    kES.Matrix(3,3) = kES.Matrix(0,6);
    kES.Matrix(3,4) = kES.Matrix(1,8);
    kES.Matrix(3,5) = kES.Matrix(2,9);
    kES.Matrix(3,7) = kES.Matrix(1,9);
    kES.Matrix(3,8) = kES.Matrix(1,6);
    kES.Matrix(3,9) = kES.Matrix(2,6);
    kES.Matrix(7,7) = kES.Matrix(4,5);
    kES.Matrix(7,8) = kES.Matrix(4,9);
    kES.Matrix(7,9) = kES.Matrix(5,8);
    kES.Matrix(8,8) = kES.Matrix(4,6);
    kES.Matrix(8,9) = kES.Matrix(6,7);
    kES.Matrix(9,9) = kES.Matrix(5,6);

    for (iRow = 0; iRow < 10; iRow++)
    {
        for (iCol = 0; iCol < iRow; iCol++)
            kES.Matrix(iRow,iCol) = kES.Matrix(iCol,iRow);
    }

    kES.IncrSortEigenStuffN();

    for (iRow = 0; iRow < 10; iRow++)
        afCoeff[iRow] = kES.GetEigenvector(iRow,0);

    // For exact fit, numeric round-off errors may make the minimum
    // eigenvalue just slightly negative.  Return absolute value since
    // application may rely on the return value being nonnegative.
    return MgcMath::Abs(kES.GetEigenvalue(0));
}
//----------------------------------------------------------------------------
MgcReal MgcQuadraticSphereFit (int iQuantity, const MgcVector3* akPoint,
    MgcVector3& rkCenter, MgcReal& rfRadius)
{
    MgcEigen kES(5);
    int iRow, iCol;
    for (iRow = 0; iRow < 5; iRow++)
    {
        for (iCol = 0; iCol < 5; iCol++)
            kES.Matrix(iRow,iCol) = 0.0;
    }

    for (int i = 0; i < iQuantity; i++)
    {
        MgcReal fX = akPoint[i].x;
        MgcReal fY = akPoint[i].y;
        MgcReal fZ = akPoint[i].z;
        MgcReal fX2 = fX*fX;
        MgcReal fY2 = fY*fY;
        MgcReal fZ2 = fZ*fZ;
        MgcReal fXY = fX*fY;
        MgcReal fXZ = fX*fZ;
        MgcReal fYZ = fY*fZ;
        MgcReal fR2 = fX2+fY2;
        MgcReal fXR2 = fX*fR2;
        MgcReal fYR2 = fY*fR2;
        MgcReal fZR2 = fZ*fR2;
        MgcReal fR4 = fR2*fR2;

        kES.Matrix(0,1) += fX;
        kES.Matrix(0,2) += fY;
        kES.Matrix(0,3) += fZ;
        kES.Matrix(0,4) += fR2;
        kES.Matrix(1,1) += fX2;
        kES.Matrix(1,2) += fXY;
        kES.Matrix(1,3) += fXZ;
        kES.Matrix(1,4) += fXR2;
        kES.Matrix(2,2) += fY2;
        kES.Matrix(2,3) += fYZ;
        kES.Matrix(2,4) += fYR2;
        kES.Matrix(3,3) += fZ2;
        kES.Matrix(3,4) += fZR2;
        kES.Matrix(4,4) += fR4;
    }

    kES.Matrix(0,0) = iQuantity;

    for (iRow = 0; iRow < 5; iRow++)
    {
        for (iCol = 0; iCol < iRow; iCol++)
            kES.Matrix(iRow,iCol) = kES.Matrix(iCol,iRow);
    }

    kES.IncrSortEigenStuffN();

    MgcReal fInv = 1.0/kES.GetEigenvector(4,0);  // watch out for zero divide
    MgcReal afCoeff[4];
    for (iRow = 0; iRow < 4; iRow++)
        afCoeff[iRow] = fInv*kES.GetEigenvector(iRow,0);

    rkCenter.x = -0.5*afCoeff[1];
    rkCenter.y = -0.5*afCoeff[2];
    rkCenter.z = -0.5*afCoeff[3];
    rfRadius = MgcMath::Sqrt(MgcMath::Abs(rkCenter.x*rkCenter.x +
        rkCenter.y*rkCenter.y + rkCenter.z*rkCenter.z - afCoeff[0]));

    // For exact fit, numeric round-off errors may make the minimum
    // eigenvalue just slightly negative.  Return absolute value since
    // application may rely on the return value being nonnegative.
    return MgcMath::Abs(kES.GetEigenvalue(0));
}
//----------------------------------------------------------------------------