mgcmatrix3.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 "MgcMatrix3.h"
#include "MgcRTLib.h"

const MgcReal MgcMatrix3::EPSILON = 1e-06;
const MgcMatrix3 MgcMatrix3::ZERO(0,0,0,0,0,0,0,0,0);
const MgcMatrix3 MgcMatrix3::IDENTITY(1,0,0,0,1,0,0,0,1);
const MgcReal MgcMatrix3::ms_fSvdEpsilon = 1e-04;
const int MgcMatrix3::ms_iSvdMaxIterations = 32;

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

    return true;
}
//----------------------------------------------------------------------------
bool MgcMatrix3::operator!= (const MgcMatrix3& rkMatrix) const
{
    return !operator==(rkMatrix);
}
//----------------------------------------------------------------------------
MgcMatrix3 MgcMatrix3::operator+ (const MgcMatrix3& rkMatrix) const
{
    MgcMatrix3 kSum;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
        {
            kSum.m_aafEntry[iRow][iCol] = m_aafEntry[iRow][iCol] +
                rkMatrix.m_aafEntry[iRow][iCol];
        }
    }
    return kSum;
}
//----------------------------------------------------------------------------
MgcMatrix3 MgcMatrix3::operator- (const MgcMatrix3& rkMatrix) const
{
    MgcMatrix3 kDiff;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
        {
            kDiff.m_aafEntry[iRow][iCol] = m_aafEntry[iRow][iCol] -
                rkMatrix.m_aafEntry[iRow][iCol];
        }
    }
    return kDiff;
}
//----------------------------------------------------------------------------
MgcMatrix3 MgcMatrix3::operator* (const MgcMatrix3& rkMatrix) const
{
    MgcMatrix3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
        {
            kProd.m_aafEntry[iRow][iCol] =
                m_aafEntry[iRow][0]*rkMatrix.m_aafEntry[0][iCol] +
                m_aafEntry[iRow][1]*rkMatrix.m_aafEntry[1][iCol] +
                m_aafEntry[iRow][2]*rkMatrix.m_aafEntry[2][iCol];
        }
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcVector3 MgcMatrix3::operator* (const MgcVector3& rkPoint) const
{
    MgcVector3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        kProd[iRow] =
            m_aafEntry[iRow][0]*rkPoint[0] +
            m_aafEntry[iRow][1]*rkPoint[1] +
            m_aafEntry[iRow][2]*rkPoint[2];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcVector3 operator* (const MgcVector3& rkPoint, const MgcMatrix3& rkMatrix)
{
    MgcVector3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        kProd[iRow] =
            rkPoint[0]*rkMatrix.m_aafEntry[0][iRow] +
            rkPoint[1]*rkMatrix.m_aafEntry[1][iRow] +
            rkPoint[2]*rkMatrix.m_aafEntry[2][iRow];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcMatrix3 MgcMatrix3::operator- () const
{
    MgcMatrix3 kNeg;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            kNeg[iRow][iCol] = -m_aafEntry[iRow][iCol];
    }
    return kNeg;
}
//----------------------------------------------------------------------------
MgcMatrix3 MgcMatrix3::operator* (MgcReal fScalar) const
{
    MgcMatrix3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            kProd[iRow][iCol] = fScalar*m_aafEntry[iRow][iCol];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcMatrix3 operator* (MgcReal fScalar, const MgcMatrix3& rkMatrix)
{
    MgcMatrix3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            kProd[iRow][iCol] = fScalar*rkMatrix.m_aafEntry[iRow][iCol];
    }
    return kProd;
}
//----------------------------------------------------------------------------
MgcMatrix3 MgcMatrix3::Transpose () const
{
    MgcMatrix3 kTranspose;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            kTranspose[iRow][iCol] = m_aafEntry[iCol][iRow];
    }
    return kTranspose;
}
//----------------------------------------------------------------------------
bool MgcMatrix3::Inverse (MgcMatrix3& rkInverse, MgcReal fTolerance) const
{
    // Invert a 3x3 using cofactors.  This is about 8 times faster than
    // the Numerical Recipes code which uses Gaussian elimination.

    rkInverse[0][0] = m_aafEntry[1][1]*m_aafEntry[2][2] -
        m_aafEntry[1][2]*m_aafEntry[2][1];
    rkInverse[0][1] = m_aafEntry[0][2]*m_aafEntry[2][1] -
        m_aafEntry[0][1]*m_aafEntry[2][2];
    rkInverse[0][2] = m_aafEntry[0][1]*m_aafEntry[1][2] -
        m_aafEntry[0][2]*m_aafEntry[1][1];
    rkInverse[1][0] = m_aafEntry[1][2]*m_aafEntry[2][0] -
        m_aafEntry[1][0]*m_aafEntry[2][2];
    rkInverse[1][1] = m_aafEntry[0][0]*m_aafEntry[2][2] -
        m_aafEntry[0][2]*m_aafEntry[2][0];
    rkInverse[1][2] = m_aafEntry[0][2]*m_aafEntry[1][0] -
        m_aafEntry[0][0]*m_aafEntry[1][2];
    rkInverse[2][0] = m_aafEntry[1][0]*m_aafEntry[2][1] -
        m_aafEntry[1][1]*m_aafEntry[2][0];
    rkInverse[2][1] = m_aafEntry[0][1]*m_aafEntry[2][0] -
        m_aafEntry[0][0]*m_aafEntry[2][1];
    rkInverse[2][2] = m_aafEntry[0][0]*m_aafEntry[1][1] -
        m_aafEntry[0][1]*m_aafEntry[1][0];

    MgcReal fDet =
        m_aafEntry[0][0]*rkInverse[0][0] +
        m_aafEntry[0][1]*rkInverse[1][0]+
        m_aafEntry[0][2]*rkInverse[2][0];

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

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

    return true;
}
//----------------------------------------------------------------------------
MgcMatrix3 MgcMatrix3::Inverse (MgcReal fTolerance) const
{
    MgcMatrix3 kInverse = MgcMatrix3::ZERO;
    Inverse(kInverse,fTolerance);
    return kInverse;
}
//----------------------------------------------------------------------------
MgcReal MgcMatrix3::Determinant () const
{
    MgcReal fCofactor00 = m_aafEntry[1][1]*m_aafEntry[2][2] -
        m_aafEntry[1][2]*m_aafEntry[2][1];
    MgcReal fCofactor10 = m_aafEntry[1][2]*m_aafEntry[2][0] -
        m_aafEntry[1][0]*m_aafEntry[2][2];
    MgcReal fCofactor20 = m_aafEntry[1][0]*m_aafEntry[2][1] -
        m_aafEntry[1][1]*m_aafEntry[2][0];

    MgcReal fDet =
        m_aafEntry[0][0]*fCofactor00 +
        m_aafEntry[0][1]*fCofactor10 +
        m_aafEntry[0][2]*fCofactor20;

    return fDet;
}
//----------------------------------------------------------------------------
void MgcMatrix3::Bidiagonalize (MgcMatrix3& kA, MgcMatrix3& kL,
    MgcMatrix3& kR)
{
    MgcReal afV[3], afW[3];
    MgcReal fLength, fSign, fT1, fInvT1, fT2;
    bool bIdentity;

    // map first column to (*,0,0)
    fLength = MgcMath::Sqrt(kA[0][0]*kA[0][0] + kA[1][0]*kA[1][0] +
        kA[2][0]*kA[2][0]);
    if ( fLength > 0.0 )
    {
        fSign = (kA[0][0] > 0.0 ? 1.0 : -1.0);
        fT1 = kA[0][0] + fSign*fLength;
        fInvT1 = 1.0/fT1;
        afV[1] = kA[1][0]*fInvT1;
        afV[2] = kA[2][0]*fInvT1;

        fT2 = -2.0/(1.0+afV[1]*afV[1]+afV[2]*afV[2]);
        afW[0] = fT2*(kA[0][0]+kA[1][0]*afV[1]+kA[2][0]*afV[2]);
        afW[1] = fT2*(kA[0][1]+kA[1][1]*afV[1]+kA[2][1]*afV[2]);
        afW[2] = fT2*(kA[0][2]+kA[1][2]*afV[1]+kA[2][2]*afV[2]);
        kA[0][0] += afW[0];
        kA[0][1] += afW[1];
        kA[0][2] += afW[2];
        kA[1][1] += afV[1]*afW[1];
        kA[1][2] += afV[1]*afW[2];
        kA[2][1] += afV[2]*afW[1];
        kA[2][2] += afV[2]*afW[2];

        kL[0][0] = 1.0+fT2;
        kL[0][1] = kL[1][0] = fT2*afV[1];
        kL[0][2] = kL[2][0] = fT2*afV[2];
        kL[1][1] = 1.0+fT2*afV[1]*afV[1];
        kL[1][2] = kL[2][1] = fT2*afV[1]*afV[2];
        kL[2][2] = 1.0+fT2*afV[2]*afV[2];
        bIdentity = false;
    }
    else
    {
        kL = MgcMatrix3::IDENTITY;
        bIdentity = true;
    }

    // map first row to (*,*,0)
    fLength = MgcMath::Sqrt(kA[0][1]*kA[0][1]+kA[0][2]*kA[0][2]);
    if ( fLength > 0.0 )
    {
        fSign = (kA[0][1] > 0.0 ? 1.0 : -1.0);
        fT1 = kA[0][1] + fSign*fLength;
        afV[2] = kA[0][2]/fT1;

        fT2 = -2.0/(1.0+afV[2]*afV[2]);
        afW[0] = fT2*(kA[0][1]+kA[0][2]*afV[2]);
        afW[1] = fT2*(kA[1][1]+kA[1][2]*afV[2]);
        afW[2] = fT2*(kA[2][1]+kA[2][2]*afV[2]);
        kA[0][1] += afW[0];
        kA[1][1] += afW[1];
        kA[1][2] += afW[1]*afV[2];
        kA[2][1] += afW[2];
        kA[2][2] += afW[2]*afV[2];
        
        kR[0][0] = 1.0;
        kR[0][1] = kR[1][0] = 0.0;
        kR[0][2] = kR[2][0] = 0.0;
        kR[1][1] = 1.0+fT2;
        kR[1][2] = kR[2][1] = fT2*afV[2];
        kR[2][2] = 1.0+fT2*afV[2]*afV[2];
    }
    else
    {
        kR = MgcMatrix3::IDENTITY;
    }

    // map second column to (*,*,0)
    fLength = MgcMath::Sqrt(kA[1][1]*kA[1][1]+kA[2][1]*kA[2][1]);
    if ( fLength > 0.0 )
    {
        fSign = (kA[1][1] > 0.0 ? 1.0 : -1.0);
        fT1 = kA[1][1] + fSign*fLength;
        afV[2] = kA[2][1]/fT1;

        fT2 = -2.0/(1.0+afV[2]*afV[2]);
        afW[1] = fT2*(kA[1][1]+kA[2][1]*afV[2]);
        afW[2] = fT2*(kA[1][2]+kA[2][2]*afV[2]);
        kA[1][1] += afW[1];
        kA[1][2] += afW[2];
        kA[2][2] += afV[2]*afW[2];

        MgcReal fA = 1.0+fT2;
        MgcReal fB = fT2*afV[2];
        MgcReal fC = 1.0+fB*afV[2];

        if ( bIdentity )
        {
            kL[0][0] = 1.0;
            kL[0][1] = kL[1][0] = 0.0;
            kL[0][2] = kL[2][0] = 0.0;
            kL[1][1] = fA;
            kL[1][2] = kL[2][1] = fB;
            kL[2][2] = fC;
        }
        else
        {
            for (int iRow = 0; iRow < 3; iRow++)
            {
                MgcReal fTmp0 = kL[iRow][1];
                MgcReal fTmp1 = kL[iRow][2];
                kL[iRow][1] = fA*fTmp0+fB*fTmp1;
                kL[iRow][2] = fB*fTmp0+fC*fTmp1;