mgclinefit.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 "MgcLineFit.h"

//----------------------------------------------------------------------------
void MgcOrthogonalLineFit (int iQuantity, const MgcVector2* akPoint,
    MgcVector2& rkOffset, MgcVector2& rkDirection)
{
    // 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, fSumYY = 0.0;
    for (i = 0; i < iQuantity; i++) 
    {
        MgcVector2 kDiff = akPoint[i] - rkOffset;
        fSumXX += kDiff.x*kDiff.x;
        fSumXY += kDiff.x*kDiff.y;
        fSumYY += kDiff.y*kDiff.y;
    }

    // setup the eigensolver
    MgcEigen kES(2);
    kES.Matrix(0,0) = fSumYY;
    kES.Matrix(0,1) = -fSumXY;
    kES.Matrix(1,0) = kES.Matrix(0,1);
    kES.Matrix(1,1) = fSumXX;

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

    // unit-length direction for best-fit line
    rkDirection.x = kES.GetEigenvector(0,1);
    rkDirection.y = kES.GetEigenvector(1,1);
}
//----------------------------------------------------------------------------
void MgcOrthogonalLineFit (int iQuantity, const MgcVector3* akPoint,
    MgcVector3& rkOffset, MgcVector3& rkDirection)
{
    // 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) = fSumYY+fSumZZ;
    kES.Matrix(0,1) = -fSumXY;
    kES.Matrix(0,2) = -fSumXZ;
    kES.Matrix(1,0) = kES.Matrix(0,1);
    kES.Matrix(1,1) = fSumXX+fSumZZ;
    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) = fSumXX+fSumYY;

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

    // unit-length direction for best-fit line
    rkDirection.x = kES.GetEigenvector(0,2);
    rkDirection.y = kES.GetEigenvector(1,2);
    rkDirection.z = kES.GetEigenvector(2,2);
}
//----------------------------------------------------------------------------
bool MgcOrthogonalLineFit (int iQuantity, const MgcVector2* akPoint,
    const bool* abValid, MgcVector2& rkOffset, MgcVector2& rkDirection)
{
    // compute average of points
    rkOffset = MgcVector2::ZERO;
    int i, iValidQuantity = 0;
    for (i = 0; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            rkOffset += akPoint[i];
            iValidQuantity++;
        }
    }
    if ( iValidQuantity == 0 )
        return false;

    MgcReal fInvQuantity = 1.0/iValidQuantity;
    rkOffset *= fInvQuantity;

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

    // setup the eigensolver
    MgcEigen kES(2);
    kES.Matrix(0,0) = fSumYY;
    kES.Matrix(0,1) = -fSumXY;
    kES.Matrix(1,0) = kES.Matrix(0,1);
    kES.Matrix(1,1) = fSumXX;

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

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

    return true;
}
//----------------------------------------------------------------------------
bool MgcOrthogonalLineFit (int iQuantity, const MgcVector3* akPoint,
    const bool* abValid, MgcVector3& rkOffset, MgcVector3& rkDirection)
{
    // compute average of points
    rkOffset = MgcVector3::ZERO;
    int i, iValidQuantity = 0;
    for (i = 0; i < iQuantity; i++)
    {
        if ( abValid[i] )
        {
            rkOffset += akPoint[i];
            iValidQuantity++;
        }
    }
    if ( iValidQuantity == 0 )
        return false;

    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++) 
    {
        if ( abValid[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) = fSumYY+fSumZZ;
    kES.Matrix(0,1) = -fSumXY;
    kES.Matrix(0,2) = -fSumXZ;
    kES.Matrix(1,0) = kES.Matrix(0,1);
    kES.Matrix(1,1) = fSumXX+fSumZZ;
    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) = fSumXX+fSumYY;

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

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

    return true;
}
//----------------------------------------------------------------------------