mgcplanefit.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 "MgcPlaneFit.h"

//----------------------------------------------------------------------------
MgcReal MgcOrthogonalPlaneFit (int iQuantity, MgcVector3* akPoint,
    MgcVector3& rkOffset, MgcVector3& rkNormal)
{
    // compute average of points
    rkOffset = akPoint[0];
    int i;
    for (i = 1; i < iQuantity; i++)
        rkOffset += akPoint[i];
    MgcReal fInvQuantity = 1.0/iQuantity;
    rkOffset *= fInvQuantity;

    // compute sums of products
    MgcReal fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
    MgcReal fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
    for (i = 0; i < iQuantity; i++) 
    {
        MgcVector3 kDiff = akPoint[i] - rkOffset;
        fSumXX += kDiff.x*kDiff.x;
        fSumXY += kDiff.x*kDiff.y;
        fSumXZ += kDiff.x*kDiff.z;
        fSumYY += kDiff.y*kDiff.y;
        fSumYZ += kDiff.y*kDiff.z;
        fSumZZ += kDiff.z*kDiff.z;
    }

    // setup the eigensolver
    MgcEigen kES(3);
    kES.Matrix(0,0) = fSumXX;
    kES.Matrix(0,1) = fSumXY;
    kES.Matrix(0,2) = fSumXZ;
    kES.Matrix(1,0) = kES.Matrix(0,1);
    kES.Matrix(1,1) = fSumYY;
    kES.Matrix(1,2) = fSumYZ;
    kES.Matrix(2,0) = kES.Matrix(0,2);
    kES.Matrix(2,1) = kES.Matrix(1,2);
    kES.Matrix(2,2) = fSumZZ;

    // compute eigenstuff, smallest eigenvalue is in last position
    kES.DecrSortEigenStuff3();

    // unit-length direction for best-fit line
    rkNormal.x = kES.GetEigenvector(0,2);
    rkNormal.y = kES.GetEigenvector(1,2);
    rkNormal.z = kES.GetEigenvector(2,2);

    // the minimum energy
    return kES.GetEigenvalue(2);
}
//----------------------------------------------------------------------------