mgcmatrix2.cpp

《3D游戏引擎设计》的源码

// 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 "MgcMatrix2.h"
#include "MgcRTLib.h"

const MgcReal MgcMatrix2::EPSILON = 1e-06;
const MgcMatrix2 MgcMatrix2::ZERO(0,0,0,0);
const MgcMatrix2 MgcMatrix2::IDENTITY(1,0,0,1);

//----------------------------------------------------------------------------
MgcMatrix2::MgcMatrix2 ()
{
    // For efficiency reasons, do not initialize matrix.
}
//----------------------------------------------------------------------------
MgcMatrix2::MgcMatrix2 (const MgcReal aafEntry[2][2])
{
    memcpy(m_aafEntry,aafEntry,4*sizeof(MgcReal));
}
//----------------------------------------------------------------------------
MgcMatrix2::MgcMatrix2 (const MgcMatrix2& rkMatrix)
{
    memcpy(m_aafEntry,rkMatrix.m_aafEntry,4*sizeof(MgcReal));
}
//----------------------------------------------------------------------------
MgcMatrix2::MgcMatrix2 (MgcReal fEntry00, MgcReal fEntry01, MgcReal fEntry10,
    MgcReal fEntry11)
{
    m_aafEntry[0][0] = fEntry00;
    m_aafEntry[0][1] = fEntry01;
    m_aafEntry[1][0] = fEntry10;
    m_aafEntry[1][1] = fEntry11;
}
//----------------------------------------------------------------------------
MgcReal* MgcMatrix2::operator[] (int iRow) const
{
    return (MgcReal*)&m_aafEntry[iRow][0];
}
//----------------------------------------------------------------------------
MgcMatrix2::operator MgcReal* ()
{
    return &m_aafEntry[0][0];
}
//----------------------------------------------------------------------------
MgcVector2 MgcMatrix2::GetColumn (int iCol) const
{
    assert( 0 <= iCol && iCol < 2 );
    return MgcVector2(m_aafEntry[0][iCol],m_aafEntry[1][iCol]);
}
//----------------------------------------------------------------------------
MgcMatrix2& MgcMatrix2::operator= (const MgcMatrix2& rkMatrix)
{
    memcpy(m_aafEntry,rkMatrix.m_aafEntry,4*sizeof(MgcReal));
    return *this;
}
//----------------------------------------------------------------------------
bool MgcMatrix2::operator== (const MgcMatrix2& rkMatrix) const
{
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
        {
            if ( m_aafEntry[iRow][iCol] != rkMatrix.m_aafEntry[iRow][iCol] )
                return false;
        }
    }

    return true;
}
//----------------------------------------------------------------------------
bool MgcMatrix2::operator!= (const MgcMatrix2& rkMatrix) const
{
    return !operator==(rkMatrix);
}
//----------------------------------------------------------------------------
MgcMatrix2 MgcMatrix2::operator+ (const MgcMatrix2& rkMatrix) const
{
    MgcMatrix2 kSum;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
        {
            kSum.m_aafEntry[iRow][iCol] = m_aafEntry[iRow][iCol] +
                rkMatrix.m_aafEntry[iRow][iCol];
        }
    }
    return kSum;
}
//----------------------------------------------------------------------------
MgcMatrix2 MgcMatrix2::operator- (const MgcMatrix2& rkMatrix) const
{
    MgcMatrix2 kDiff;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
        {
            kDiff.m_aafEntry[iRow][iCol] = m_aafEntry[iRow][iCol] -
                rkMatrix.m_aafEntry[iRow][iCol];
        }
    }
    return kDiff;
}
//----------------------------------------------------------------------------
MgcMatrix2 MgcMatrix2::operator* (const MgcMatrix2& rkMatrix) const
{
    MgcMatrix2 kProd;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
        {
            kProd.m_aafEntry[iRow][iCol] =
                m_aafEntry[iRow][0]*rkMatrix.m_aafEntry[0][iCol] +
                m_aafEntry[iRow][1]*rkMatrix.m_aafEntry[1][iCol];
        }
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcVector2 MgcMatrix2::operator* (const MgcVector2& rkVector) const
{
    MgcVector2 kProd;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        kProd[iRow] =
            m_aafEntry[iRow][0]*rkVector[0] +
            m_aafEntry[iRow][1]*rkVector[1];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcVector2 operator* (const MgcVector2& rkVector, const MgcMatrix2& rkMatrix)
{
    MgcVector2 kProd;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        kProd[iRow] =
            rkVector[0]*rkMatrix.m_aafEntry[0][iRow] +
            rkVector[1]*rkMatrix.m_aafEntry[1][iRow];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcMatrix2 MgcMatrix2::operator- () const
{
    MgcMatrix2 kNeg;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
            kNeg[iRow][iCol] = -m_aafEntry[iRow][iCol];
    }
    return kNeg;
}
//----------------------------------------------------------------------------
MgcMatrix2 MgcMatrix2::operator* (MgcReal fScalar) const
{
    MgcMatrix2 kProd;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
            kProd[iRow][iCol] = fScalar*m_aafEntry[iRow][iCol];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcMatrix2 operator* (MgcReal fScalar, const MgcMatrix2& rkMatrix)
{
    MgcMatrix2 kProd;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
            kProd[iRow][iCol] = fScalar*rkMatrix.m_aafEntry[iRow][iCol];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcMatrix2 MgcMatrix2::Transpose () const
{
    MgcMatrix2 kTranspose;
    for (int iRow = 0; iRow < 2; iRow++)
    {
        for (int iCol = 0; iCol < 2; iCol++)
            kTranspose[iRow][iCol] = m_aafEntry[iCol][iRow];
    }
    return kTranspose;
}
//----------------------------------------------------------------------------
bool MgcMatrix2::Inverse (MgcMatrix2& rkInverse, MgcReal fTolerance) const
{
    MgcReal fDet = m_aafEntry[0][0]*m_aafEntry[1][1] -
        m_aafEntry[0][1]*m_aafEntry[1][0];

    if ( MgcMath::Abs(fDet) <= fTolerance )
        return false;

    MgcReal fInvDet = 1.0/fDet;
    rkInverse[0][0] = m_aafEntry[1][1]*fInvDet;
    rkInverse[1][0] = -m_aafEntry[1][0]*fInvDet;
    rkInverse[0][1] = -m_aafEntry[0][1]*fInvDet;
    rkInverse[1][1] = m_aafEntry[0][0]*fInvDet;

    return true;
}
//----------------------------------------------------------------------------
MgcMatrix2 MgcMatrix2::Inverse (MgcReal fTolerance) const
{
    MgcMatrix2 fInverse = MgcMatrix2::ZERO;
    Inverse(fInverse,fTolerance);
    return fInverse;
}
//----------------------------------------------------------------------------
MgcReal MgcMatrix2::Determinant () const
{
    return m_aafEntry[0][0]*m_aafEntry[1][1] -
           m_aafEntry[0][1]*m_aafEntry[1][0];
}
//----------------------------------------------------------------------------
void MgcMatrix2::SingularValueDecomposition (MgcMatrix2& rkL,
    MgcVector2& rkS, MgcMatrix2& rkR) const
{
    // compute M*M^t and M^t*M where M^t = Transpose(M)
    MgcReal fM00Sqr = m_aafEntry[0][0]*m_aafEntry[0][0];
    MgcReal fM01Sqr = m_aafEntry[0][1]*m_aafEntry[0][1];
    MgcReal fM10Sqr = m_aafEntry[1][0]*m_aafEntry[1][0];
    MgcReal fM11Sqr = m_aafEntry[1][1]*m_aafEntry[1][1];

    MgcReal afMMT[3], afMTM[3];
    afMMT[0] = fM00Sqr + fM01Sqr;
    afMMT[1] = m_aafEntry[0][0]*m_aafEntry[1][0] +
        m_aafEntry[0][1]*m_aafEntry[1][1];
    afMMT[2] = fM10Sqr + fM11Sqr;
    afMTM[0] = fM00Sqr + fM10Sqr;
    afMTM[1] = m_aafEntry[0][0]*m_aafEntry[0][1] +
        m_aafEntry[1][0]*m_aafEntry[1][1];
    afMTM[2] = fM01Sqr + fM11Sqr;

    // factor M*M^t = L*S^2*L^t
    MgcReal fCos, fSin, fInvLen;
    if ( MgcMath::Abs(afMMT[1]) > 0.0 )
    {
        MgcReal fTrace = afMMT[0] + afMMT[2];
        MgcReal fDet = afMMT[0]*afMMT[2] - afMMT[1]*afMMT[1];
        MgcReal fDiscr = fTrace*fTrace-4.0*fDet;
        MgcReal fRoot = MgcMath::Sqrt(MgcMath::Abs(fDiscr));
        rkS[0] = 0.5*(fTrace + fRoot);
        rkS[1] = 0.5*(fTrace - fRoot);

        fCos = afMMT[1];
        fSin = rkS[0] - afMMT[0];
        fInvLen = 1.0/MgcMath::Sqrt(fCos*fCos + fSin*fSin);
        fCos *= fInvLen;
        fSin *= fInvLen;

        rkL[0][0] = fCos;
        rkL[1][0] = fSin;
        rkL[0][1] = -fSin;
        rkL[1][1] = fCos;
    }
    else
    {
        rkS[0] = afMMT[0];
        rkS[1] = afMMT[2];
        rkL = IDENTITY;
    }

    // factor M^t*M = R^t*S^2*R
    if ( MgcMath::Abs(afMTM[1]) > 0.0 )
    {
        fCos = afMTM[1];
        fSin = rkS[0] - afMTM[0];
        fInvLen = 1.0/MgcMath::Sqrt(fCos*fCos + fSin*fSin);
        fCos *= fInvLen;
        fSin *= fInvLen;

        rkR[0][0] = fCos;
        rkR[1][0] = -fSin;
        rkR[0][1] = +fSin;
        rkR[1][1] = fCos;
    }
    else
    {
        rkR = IDENTITY;
    }

    rkS[0] = MgcMath::Sqrt(MgcMath::Abs(rkS[0]));
    rkS[1] = MgcMath::Sqrt(MgcMath::Abs(rkS[1]));

    MgcVector2 kDiag = rkS;

    MgcMatrix2 kTest;
    kTest.SingularValueComposition(rkL,kDiag,rkR);
    kTest = *this - kTest;
    int i = 0;
    MgcReal fMinNormSqr = kTest.L2NormSqr();

    kDiag[0] = -kDiag[0];
    kTest.SingularValueComposition(rkL,kDiag,rkR);
    kTest = *this - kTest;
    MgcReal fNormSqr = kTest.L2NormSqr();
    if ( fNormSqr < fMinNormSqr )
    {
        i = 1;
        fMinNormSqr = fNormSqr;
    }

    kDiag[1] = -kDiag[1];
    kTest.SingularValueComposition(rkL,kDiag,rkR);
    kTest = *this - kTest;
    fNormSqr = kTest.L2NormSqr();
    if ( fNormSqr < fMinNormSqr )
    {
        i = 2;
        fMinNormSqr = fNormSqr;
    }

    kDiag[0] = -kDiag[0];
    kTest.SingularValueComposition(rkL,kDiag,rkR);
    kTest = *this - kTest;
    fNormSqr = kTest.L2NormSqr();
    if ( fNormSqr < fMinNormSqr )
    {
        i = 3;
        fMinNormSqr = fNormSqr;