mgclinearsystem.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 "MgcLinearSystem.h"
#include "MgcRTLib.h"

MgcReal MgcLinearSystem::ms_fTolerance = 1e-06;

//----------------------------------------------------------------------------
MgcReal** MgcLinearSystem::NewMatrix (int iSize)
{
    MgcReal** aafA = new MgcReal*[iSize];
    assert( aafA );

    for (int iRow = 0; iRow < iSize; iRow++)
    {
        aafA[iRow] = new MgcReal[iSize];
        memset(aafA[iRow],0,iSize*sizeof(MgcReal));
    }

    return aafA;
}
//----------------------------------------------------------------------------
void MgcLinearSystem::DeleteMatrix (int iSize, MgcReal** aafA)
{
    for (int iRow = 0; iRow < iSize; iRow++)
        delete[] aafA[iRow];
    delete[] aafA;
}
//----------------------------------------------------------------------------
MgcReal* MgcLinearSystem::NewVector (int iSize)
{
    MgcReal* afB = new MgcReal[iSize];
    assert( afB );
    memset(afB,0,iSize*sizeof(MgcReal));
    return afB;
}
//----------------------------------------------------------------------------
void MgcLinearSystem::DeleteVector (int iSize, MgcReal* afB)
{
    delete[] afB;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::Solve2 (const MgcReal aafA[2][2], const MgcReal afB[2],
    MgcReal afX[2])
{
    MgcReal fDet = aafA[0][0]*aafA[1][1]-aafA[0][1]*aafA[1][0];
    if ( MgcMath::Abs(fDet) < ms_fTolerance )
        return false;

    MgcReal fInvDet = 1.0/fDet;
    afX[0] = (aafA[1][1]*afB[0]-aafA[0][1]*afB[1])*fInvDet;
    afX[1] = (aafA[0][0]*afB[1]-aafA[1][0]*afB[0])*fInvDet;
    return true;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::Solve3 (const MgcReal aafA[3][3], const MgcReal afB[3],
    MgcReal afX[3])
{
    MgcReal aafAInv[3][3];
    aafAInv[0][0] = aafA[1][1]*aafA[2][2]-aafA[1][2]*aafA[2][1];
    aafAInv[0][1] = aafA[0][2]*aafA[2][1]-aafA[0][1]*aafA[2][2];
    aafAInv[0][2] = aafA[0][1]*aafA[1][2]-aafA[0][2]*aafA[1][1];
    aafAInv[1][0] = aafA[1][2]*aafA[2][0]-aafA[1][0]*aafA[2][2];
    aafAInv[1][1] = aafA[0][0]*aafA[2][2]-aafA[0][2]*aafA[2][0];
    aafAInv[1][2] = aafA[0][2]*aafA[1][0]-aafA[0][0]*aafA[1][2];
    aafAInv[2][0] = aafA[1][0]*aafA[2][1]-aafA[1][1]*aafA[2][0];
    aafAInv[2][1] = aafA[0][1]*aafA[2][0]-aafA[0][0]*aafA[2][1];
    aafAInv[2][2] = aafA[0][0]*aafA[1][1]-aafA[0][1]*aafA[1][0];
    MgcReal fDet = aafA[0][0]*aafAInv[0][0] + aafA[0][1]*aafAInv[1][0] +
        aafA[0][2]*aafAInv[2][0];
    if ( MgcMath::Abs(fDet) < ms_fTolerance )
        return false;

    MgcReal fInvDet = 1.0/fDet;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            aafAInv[iRow][iCol] *= fInvDet;
    }

    afX[0] = aafAInv[0][0]*afB[0]+aafAInv[0][1]*afB[1]+aafAInv[0][2]*afB[2];
    afX[1] = aafAInv[1][0]*afB[0]+aafAInv[1][1]*afB[1]+aafAInv[1][2]*afB[2];
    afX[2] = aafAInv[2][0]*afB[0]+aafAInv[2][1]*afB[1]+aafAInv[2][2]*afB[2];
    return true;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::Inverse (int iSize, MgcReal** aafA)
{
    int* aiColIndex = new int[iSize];
    assert( aiColIndex );

    int* aiRowIndex = new int[iSize];
    assert( aiRowIndex );

    bool* abPivoted = new bool[iSize];
    assert( abPivoted );
    memset(abPivoted,0,iSize*sizeof(bool));

    int i1, i2, iRow, iCol;
    MgcReal fSave;

    // elimination by full pivoting
    for (int i0 = 0; i0 < iSize; i0++)
    {
        // search matrix (excluding pivoted rows) for maximum absolute entry
        MgcReal fMax = 0.0;
        for (i1 = 0; i1 < iSize; i1++)
        {
            if ( !abPivoted[i1] )
            {
                for (i2 = 0; i2 < iSize; i2++)
                {
                    if ( !abPivoted[i2] )
                    {
                        MgcReal fAbs = MgcMath::Abs(aafA[i1][i2]);
                        if ( fAbs > fMax )
                        {
                            fMax = fAbs;
                            iRow = i1;
                            iCol = i2;
                        }
                    }
                }
            }
        }

        if ( fMax == 0.0 )
        {
            // matrix is not invertible
            delete[] aiColIndex;
            delete[] aiRowIndex;
            delete[] abPivoted;
            return false;
        }

        abPivoted[iCol] = true;

        // swap rows so that A[iCol][iCol] contains the pivot entry
        if ( iRow != iCol )
        {
            MgcReal* afRowPtr = aafA[iRow];
            aafA[iRow] = aafA[iCol];
            aafA[iCol] = afRowPtr;
        }

        // keep track of the permutations of the rows
        aiRowIndex[i0] = iRow;
        aiColIndex[i0] = iCol;

        // scale the row so that the pivot entry is 1
        MgcReal fInv = 1.0/aafA[iCol][iCol];
        aafA[iCol][iCol] = 1.0;
        for (i2 = 0; i2 < iSize; i2++)
            aafA[iCol][i2] *= fInv;

        // zero out the pivot column locations in the other rows
        for (i1 = 0; i1 < iSize; i1++)
        {
            if ( i1 != iCol )
            {
                fSave = aafA[i1][iCol];
                aafA[i1][iCol] = 0.0;
                for (i2 = 0; i2 < iSize; i2++)
                    aafA[i1][i2] -= aafA[iCol][i2]*fSave;
            }
        }
    }

    // reorder rows so that A[][] stores the inverse of the original matrix
    for (i1 = iSize-1; i1 >= 0; i1--)
    {
        if ( aiRowIndex[i1] != aiColIndex[i1] )
        {
            for (i2 = 0; i2 < iSize; i2++)
            {
                fSave = aafA[i2][aiRowIndex[i1]];
                aafA[i2][aiRowIndex[i1]] = aafA[i2][aiColIndex[i1]];
                aafA[i2][aiColIndex[i1]] = fSave;
            }
        }
    }

    delete[] aiColIndex;
    delete[] aiRowIndex;
    delete[] abPivoted;
    return true;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::Solve (int iSize, MgcReal** aafA, MgcReal* afB)
{
    int* aiColIndex = new int[iSize];
    assert( aiColIndex );

    int* aiRowIndex = new int[iSize];
    assert( aiRowIndex );

    bool* abPivoted = new bool[iSize];
    assert( abPivoted );
    memset(abPivoted,0,iSize*sizeof(bool));

    int i1, i2, iRow, iCol;
    MgcReal fSave;

    // elimination by full pivoting
    for (int i0 = 0; i0 < iSize; i0++)
    {
        // search matrix (excluding pivoted rows) for maximum absolute entry
        MgcReal fMax = 0.0;
        for (i1 = 0; i1 < iSize; i1++)
        {
            if ( !abPivoted[i1] )
            {
                for (i2 = 0; i2 < iSize; i2++)
                {
                    if ( !abPivoted[i2] )
                    {
                        MgcReal fAbs = MgcMath::Abs(aafA[i1][i2]);
                        if ( fAbs > fMax )
                        {
                            fMax = fAbs;
                            iRow = i1;
                            iCol = i2;
                        }
                    }
                }
            }
        }

        if ( fMax == 0.0 )
        {
            // matrix is not invertible
            delete[] aiColIndex;
            delete[] aiRowIndex;
            delete[] abPivoted;
            return false;
        }

        abPivoted[iCol] = true;

        // swap rows so that A[iCol][iCol] contains the pivot entry
        if ( iRow != iCol )
        {
            MgcReal* afRowPtr = aafA[iRow];
            aafA[iRow] = aafA[iCol];
            aafA[iCol] = afRowPtr;

            fSave = afB[iRow];
            afB[iRow] = afB[iCol];
            afB[iCol] = fSave;
        }

        // keep track of the permutations of the rows
        aiRowIndex[i0] = iRow;
        aiColIndex[i0] = iCol;

        // scale the row so that the pivot entry is 1
        MgcReal fInv = 1.0/aafA[iCol][iCol];
        aafA[iCol][iCol] = 1.0;
        for (i2 = 0; i2 < iSize; i2++)
            aafA[iCol][i2] *= fInv;
        afB[iCol] *= fInv;

        // zero out the pivot column locations in the other rows
        for (i1 = 0; i1 < iSize; i1++)
        {
            if ( i1 != iCol )
            {
                fSave = aafA[i1][iCol];
                aafA[i1][iCol] = 0.0;
                for (i2 = 0; i2 < iSize; i2++)
                    aafA[i1][i2] -= aafA[iCol][i2]*fSave;
                afB[i1] -= afB[iCol]*fSave;
            }
        }
    }

    // reorder rows so that A[][] stores the inverse of the original matrix
    for (i1 = iSize-1; i1 >= 0; i1--)
    {
        if ( aiRowIndex[i1] != aiColIndex[i1] )
        {
            for (i2 = 0; i2 < iSize; i2++)
            {
                fSave = aafA[i2][aiRowIndex[i1]];
                aafA[i2][aiRowIndex[i1]] = aafA[i2][aiColIndex[i1]];
                aafA[i2][aiColIndex[i1]] = fSave;
            }
        }
    }

    delete[] aiColIndex;
    delete[] aiRowIndex;
    delete[] abPivoted;
    return true;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::SolveTri (int iSize, MgcReal* afA, MgcReal* afB,
    MgcReal* afC, MgcReal* afR, MgcReal* afU)
{
    if ( afB[0] == 0.0 )
        return false;

    MgcReal* afD = new MgcReal[iSize-1];
    assert( afD );

    MgcReal fE = afB[0];
    MgcReal fInvE = 1.0/fE;
    afU[0] = afR[0]*fInvE;

    int i0, i1;
    for (i0 = 0, i1 = 1; i1 < iSize; i0++, i1++)
    {
        afD[i0] = afC[i0]*fInvE;
        fE = afB[i1] - afA[i0]*afD[i0];
        if ( fE == 0.0 )
        {
            delete[] afD;
            return false;
        }
        fInvE = 1.0/fE;
        afU[i1] = (afR[i1] - afA[i0]*afU[i0])*fInvE;
    }

    for (i0 = iSize-1, i1 = iSize-2; i1 >= 0; i0--, i1--)
        afU[i1] -= afD[i1]*afU[i0];

    delete[] afD;
    return true;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::SolveConstTri (int iSize, MgcReal fA, MgcReal fB,
    MgcReal fC, MgcReal* afR, MgcReal* afU)
{
    if ( fB == 0.0 )
        return false;

    MgcReal* afD = new MgcReal[iSize-1];
    assert( afD );

    MgcReal fE = fB;
    MgcReal fInvE = 1.0/fE;
    afU[0] = afR[0]*fInvE;

    int i0, i1;
    for (i0 = 0, i1 = 1; i1 < iSize; i0++, i1++)
    {
        afD[i0] = fC*fInvE;
        fE = fB - fA*afD[i0];
        if ( fE == 0.0 )
        {
            delete[] afD;
            return false;
        }
        fInvE = 1.0/fE;
        afU[i1] = (afR[i1] - fA*afU[i0])*fInvE;
    }
    for (i0 = iSize-1, i1 = iSize-2; i1 >= 0; i0--, i1--)
        afU[i1] -= afD[i1]*afU[i0];

    delete[] afD;
    return true;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::SolveSymmetric (int iSize, MgcReal** aafA, MgcReal* afB)
{
    // A = L D L^t decomposition with diagonal terms of L equal to 1.  The
    // algorithm stores D terms in A[i][i] and off-diagonal L terms in
    // A[i][j] for i > j.  (G. Golub and C. Van Loan, Matrix Computations)

    const MgcReal fTolerance = 1e-06;

    int i0, i1;
    MgcReal* afV = new MgcReal[iSize];
    assert( afV );

    for (i1 = 0; i1 < iSize; i1++)
    {
        for (i0 = 0; i0 < i1; i0++)
            afV[i0] = aafA[i1][i0]*aafA[i0][i0];

        afV[i1] = aafA[i1][i1];
        for (i0 = 0; i0 < i1; i0++)
            afV[i1] -= aafA[i1][i0]*afV[i0];

        aafA[i1][i1] = afV[i1];
        if ( MgcMath::Abs(afV[i1]) <= fTolerance )
        {
            delete[] afV;
            return false;
        }

        MgcReal fInv = 1.0/afV[i1];
        for (i0 = i1+1; i0 < iSize; i0++)
        {
            for (int i2 = 0; i2 < i1; i2++)
                aafA[i0][i1] -= aafA[i0][i2]*afV[i2];
            aafA[i0][i1] *= fInv;
        }
    }
    delete[] afV;

    // Solve Ax = B.

    // Forward substitution:  Let z = DL^t x, then Lz = B.  Algorithm
    // stores z terms in B vector.
    for (i0 = 0; i0 < iSize; i0++)
    {
        for (i1 = 0; i1 < i0; i1++)
            afB[i0] -= aafA[i0][i1]*afB[i1];

    }

    // Diagonal division:  Let y = L^t x, then Dy = z.  Algorithm stores
    // y terms in B vector.
    for (i0 = 0; i0 < iSize; i0++)
    {
        if ( MgcMath::Abs(aafA[i0][i0]) <= fTolerance )
            return false;
        afB[i0] /= aafA[i0][i0];
    }

    // Back substitution:  Solve L^t x = y.  Algorithm stores x terms in
    // B vector.
    for (i0 = iSize-2; i0 >= 0; i0--)
    {
        for (i1 = i0+1; i1 < iSize; i1++)
            afB[i0] -= aafA[i1][i0]*afB[i1];
    }

    return true;
}
//----------------------------------------------------------------------------
bool MgcLinearSystem::SymmetricInverse (int iSize, MgcReal** aafA,
    MgcReal** aafAInv)
{
    // Same algorithm as SolveSymmetric, but applied simultaneously to
    // columns of identity matrix.

    MgcReal* afV = new MgcReal[iSize];
    assert( afV );

    int i0, i1;
    for (i0 = 0; i0 < iSize; i0++)
    {
        for (i1 = 0; i1 < iSize; i1++)
            aafAInv[i0][i1] = ( i0 != i1 ? 0.0 : 1.0 );
    }

    for (i1 = 0; i1 < iSize; i1++)
    {
        for (i0 = 0; i0 < i1; i0++)
            afV[i0] = aafA[i1][i0]*aafA[i0][i0];

        afV[i1] = aafA[i1][i1];
        for (i0 = 0; i0 < i1; i0++)
            afV[i1] -= aafA[i1][i0]*afV[i0];

        aafA[i1][i1] = afV[i1];
        for (i0 = i1+1; i0 < iSize; i0++)
        {
            for (int i2 = 0; i2 < i1; i2++)
                aafA[i0][i1] -= aafA[i0][i2]*afV[i2];
            aafA[i0][i1] /= afV[i1];
        }
    }
    delete[] afV;

    for (int iCol = 0; iCol < iSize; iCol++)
    {
        // forward substitution
        for (i0 = 0; i0 < iSize; i0++)
        {
            for (i1 = 0; i1 < i0; i1++)
                aafAInv[i0][iCol] -= aafA[i0][i1]*aafAInv[i1][iCol];
        }

        // diagonal division
        const MgcReal fTolerance = 1e-06;
        for (i0 = 0; i0 < iSize; i0++)
        {
            if ( fabs(aafA[i0][i0]) <= fTolerance )
                return false;
            aafAInv[i0][iCol] /= aafA[i0][i0];
        }

        // back substitution
        for (i0 = iSize-2; i0 >= 0; i0--)
        {
            for (i1 = i0+1; i1 < iSize; i1++)
                aafAInv[i0][iCol] -= aafA[i1][i0]*aafAInv[i1][iCol];
        }
    }

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