mgcmatrix3.cpp

3D Game Engine Design Source Code非常棒

            }
        }
    }
}
//----------------------------------------------------------------------------
void MgcMatrix3::GolubKahanStep (MgcMatrix3& kA, MgcMatrix3& kL,
    MgcMatrix3& kR)
{
    MgcReal fT11 = kA[0][1]*kA[0][1]+kA[1][1]*kA[1][1];
    MgcReal fT22 = kA[1][2]*kA[1][2]+kA[2][2]*kA[2][2];
    MgcReal fT12 = kA[1][1]*kA[1][2];
    MgcReal fTrace = fT11+fT22;
    MgcReal fDiff = fT11-fT22;
    MgcReal fDiscr = MgcMath::Sqrt(fDiff*fDiff+4.0*fT12*fT12);
    MgcReal fRoot1 = 0.5*(fTrace+fDiscr);
    MgcReal fRoot2 = 0.5*(fTrace-fDiscr);

    // adjust right
    MgcReal fY = kA[0][0] - (MgcMath::Abs(fRoot1-fT22) <=
        MgcMath::Abs(fRoot2-fT22) ? fRoot1 : fRoot2);
    MgcReal fZ = kA[0][1];
    MgcReal fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
    MgcReal fSin = fZ*fInvLength;
    MgcReal fCos = -fY*fInvLength;

    MgcReal fTmp0 = kA[0][0];
    MgcReal fTmp1 = kA[0][1];
    kA[0][0] = fCos*fTmp0-fSin*fTmp1;
    kA[0][1] = fSin*fTmp0+fCos*fTmp1;
    kA[1][0] = -fSin*kA[1][1];
    kA[1][1] *= fCos;

    int iRow;
    for (iRow = 0; iRow < 3; iRow++)
    {
        fTmp0 = kR[0][iRow];
        fTmp1 = kR[1][iRow];
        kR[0][iRow] = fCos*fTmp0-fSin*fTmp1;
        kR[1][iRow] = fSin*fTmp0+fCos*fTmp1;
    }

    // adjust left
    fY = kA[0][0];
    fZ = kA[1][0];
    fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
    fSin = fZ*fInvLength;
    fCos = -fY*fInvLength;

    kA[0][0] = fCos*kA[0][0]-fSin*kA[1][0];
    fTmp0 = kA[0][1];
    fTmp1 = kA[1][1];
    kA[0][1] = fCos*fTmp0-fSin*fTmp1;
    kA[1][1] = fSin*fTmp0+fCos*fTmp1;
    kA[0][2] = -fSin*kA[1][2];
    kA[1][2] *= fCos;

    int iCol;
    for (iCol = 0; iCol < 3; iCol++)
    {
        fTmp0 = kL[iCol][0];
        fTmp1 = kL[iCol][1];
        kL[iCol][0] = fCos*fTmp0-fSin*fTmp1;
        kL[iCol][1] = fSin*fTmp0+fCos*fTmp1;
    }

    // adjust right
    fY = kA[0][1];
    fZ = kA[0][2];
    fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
    fSin = fZ*fInvLength;
    fCos = -fY*fInvLength;

    kA[0][1] = fCos*kA[0][1]-fSin*kA[0][2];
    fTmp0 = kA[1][1];
    fTmp1 = kA[1][2];
    kA[1][1] = fCos*fTmp0-fSin*fTmp1;
    kA[1][2] = fSin*fTmp0+fCos*fTmp1;
    kA[2][1] = -fSin*kA[2][2];
    kA[2][2] *= fCos;

    for (iRow = 0; iRow < 3; iRow++)
    {
        fTmp0 = kR[1][iRow];
        fTmp1 = kR[2][iRow];
        kR[1][iRow] = fCos*fTmp0-fSin*fTmp1;
        kR[2][iRow] = fSin*fTmp0+fCos*fTmp1;
    }

    // adjust left
    fY = kA[1][1];
    fZ = kA[2][1];
    fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
    fSin = fZ*fInvLength;
    fCos = -fY*fInvLength;

    kA[1][1] = fCos*kA[1][1]-fSin*kA[2][1];
    fTmp0 = kA[1][2];
    fTmp1 = kA[2][2];
    kA[1][2] = fCos*fTmp0-fSin*fTmp1;
    kA[2][2] = fSin*fTmp0+fCos*fTmp1;

    for (iCol = 0; iCol < 3; iCol++)
    {
        fTmp0 = kL[iCol][1];
        fTmp1 = kL[iCol][2];
        kL[iCol][1] = fCos*fTmp0-fSin*fTmp1;
        kL[iCol][2] = fSin*fTmp0+fCos*fTmp1;
    }
}
//----------------------------------------------------------------------------
void MgcMatrix3::SingularValueDecomposition (MgcMatrix3& kL, MgcVector3& kS,
    MgcMatrix3& kR) const
{
    const int iMax = 16;
    int iRow, iCol;

    MgcMatrix3 kA = *this;
    Bidiagonalize(kA,kL,kR);

    for (int i = 0; i < ms_iSvdMaxIterations; i++)
    {
        MgcReal fTmp, fTmp0, fTmp1;
        MgcReal fSin0, fCos0, fTan0;
        MgcReal fSin1, fCos1, fTan1;

        bool bTest1 = (MgcMath::Abs(kA[0][1]) <=
            ms_fSvdEpsilon*(MgcMath::Abs(kA[0][0])+MgcMath::Abs(kA[1][1])));
        bool bTest2 = (MgcMath::Abs(kA[1][2]) <=
            ms_fSvdEpsilon*(MgcMath::Abs(kA[1][1])+MgcMath::Abs(kA[2][2])));
        if ( bTest1 )
        {
            if ( bTest2 )
            {
                kS[0] = kA[0][0];
                kS[1] = kA[1][1];
                kS[2] = kA[2][2];
                break;
            }
            else
            {
                // 2x2 closed form factorization
                fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] +
                    kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]);
                fTan0 = 0.5*(fTmp+MgcMath::Sqrt(fTmp*fTmp + 4.0));
                fCos0 = 1.0/MgcMath::Sqrt(1.0+fTan0*fTan0);
                fSin0 = fTan0*fCos0;

                for (iCol = 0; iCol < 3; iCol++)
                {
                    fTmp0 = kL[iCol][1];
                    fTmp1 = kL[iCol][2];
                    kL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1;
                    kL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1;
                }
                
                fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1];
                fCos1 = 1.0/MgcMath::Sqrt(1.0+fTan1*fTan1);
                fSin1 = -fTan1*fCos1;

                for (iRow = 0; iRow < 3; iRow++)
                {
                    fTmp0 = kR[1][iRow];
                    fTmp1 = kR[2][iRow];
                    kR[1][iRow] = fCos1*fTmp0-fSin1*fTmp1;
                    kR[2][iRow] = fSin1*fTmp0+fCos1*fTmp1;
                }

                kS[0] = kA[0][0];
                kS[1] = fCos0*fCos1*kA[1][1] -
                    fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]);
                kS[2] = fSin0*fSin1*kA[1][1] +
                    fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]);
                break;
            }
        }
        else 
        {
            if ( bTest2 )
            {
                // 2x2 closed form factorization 
                fTmp = (kA[0][0]*kA[0][0] + kA[1][1]*kA[1][1] -
                    kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]);
                fTan0 = 0.5*(-fTmp+MgcMath::Sqrt(fTmp*fTmp + 4.0));
                fCos0 = 1.0/MgcMath::Sqrt(1.0+fTan0*fTan0);
                fSin0 = fTan0*fCos0;

                for (iCol = 0; iCol < 3; iCol++)
                {
                    fTmp0 = kL[iCol][0];
                    fTmp1 = kL[iCol][1];
                    kL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1;
                    kL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1;
                }
                
                fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0];
                fCos1 = 1.0/MgcMath::Sqrt(1.0+fTan1*fTan1);
                fSin1 = -fTan1*fCos1;

                for (iRow = 0; iRow < 3; iRow++)
                {
                    fTmp0 = kR[0][iRow];
                    fTmp1 = kR[1][iRow];
                    kR[0][iRow] = fCos1*fTmp0-fSin1*fTmp1;
                    kR[1][iRow] = fSin1*fTmp0+fCos1*fTmp1;
                }

                kS[0] = fCos0*fCos1*kA[0][0] -
                    fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]);
                kS[1] = fSin0*fSin1*kA[0][0] +
                    fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]);
                kS[2] = kA[2][2];
                break;
            }
            else
            {
                GolubKahanStep(kA,kL,kR);
            }
        }
    }

    // positize diagonal
    for (iRow = 0; iRow < 3; iRow++)
    {
        if ( kS[iRow] < 0.0 )
        {
            kS[iRow] = -kS[iRow];
            for (iCol = 0; iCol < 3; iCol++)
                kR[iRow][iCol] = -kR[iRow][iCol];
        }
    }
}
//----------------------------------------------------------------------------
void MgcMatrix3::SingularValueComposition (const MgcMatrix3& kL,
    const MgcVector3& kS, const MgcMatrix3& kR)
{
    int iRow, iCol;
    MgcMatrix3 kTmp;

    // product S*R
    for (iRow = 0; iRow < 3; iRow++)
    {
        for (iCol = 0; iCol < 3; iCol++)
            kTmp[iRow][iCol] = kS[iRow]*kR[iRow][iCol];
    }

    // product L*S*R
    for (iRow = 0; iRow < 3; iRow++)
    {
        for (iCol = 0; iCol < 3; iCol++)
        {
            m_aafEntry[iRow][iCol] = 0.0;
            for (int iMid = 0; iMid < 3; iMid++)
                m_aafEntry[iRow][iCol] += kL[iRow][iMid]*kTmp[iMid][iCol];
        }
    }
}
//----------------------------------------------------------------------------
void MgcMatrix3::Orthonormalize ()
{
    // Algorithm uses Gram-Schmidt orthogonalization.  If 'this' matrix is
    // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
    //
    //   q0 = m0/|m0|
    //   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
    //   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
    //
    // where |V| indicates length of vector V and A*B indicates dot
    // product of vectors A and B.

    // compute q0
    MgcReal fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][0]*m_aafEntry[0][0]
        + m_aafEntry[1][0]*m_aafEntry[1][0] +
        m_aafEntry[2][0]*m_aafEntry[2][0]);

    m_aafEntry[0][0] *= fInvLength;
    m_aafEntry[1][0] *= fInvLength;
    m_aafEntry[2][0] *= fInvLength;

    // compute q1
    MgcReal fDot0 =
        m_aafEntry[0][0]*m_aafEntry[0][1] +
        m_aafEntry[1][0]*m_aafEntry[1][1] +
        m_aafEntry[2][0]*m_aafEntry[2][1];

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

    fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][1]*m_aafEntry[0][1] +
        m_aafEntry[1][1]*m_aafEntry[1][1] +
        m_aafEntry[2][1]*m_aafEntry[2][1]);

    m_aafEntry[0][1] *= fInvLength;
    m_aafEntry[1][1] *= fInvLength;
    m_aafEntry[2][1] *= fInvLength;

    // compute q2
    MgcReal fDot1 =
        m_aafEntry[0][1]*m_aafEntry[0][2] +
        m_aafEntry[1][1]*m_aafEntry[1][2] +
        m_aafEntry[2][1]*m_aafEntry[2][2];

    fDot0 =
        m_aafEntry[0][0]*m_aafEntry[0][2] +
        m_aafEntry[1][0]*m_aafEntry[1][2] +
        m_aafEntry[2][0]*m_aafEntry[2][2];

    m_aafEntry[0][2] -= fDot0*m_aafEntry[0][0] + fDot1*m_aafEntry[0][1];
    m_aafEntry[1][2] -= fDot0*m_aafEntry[1][0] + fDot1*m_aafEntry[1][1];
    m_aafEntry[2][2] -= fDot0*m_aafEntry[2][0] + fDot1*m_aafEntry[2][1];

    fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][2]*m_aafEntry[0][2] +
        m_aafEntry[1][2]*m_aafEntry[1][2] +
        m_aafEntry[2][2]*m_aafEntry[2][2]);

    m_aafEntry[0][2] *= fInvLength;
    m_aafEntry[1][2] *= fInvLength;
    m_aafEntry[2][2] *= fInvLength;
}
//----------------------------------------------------------------------------
void MgcMatrix3::QDUDecomposition (MgcMatrix3& kQ,
    MgcVector3& kD, MgcVector3& kU) const
{
    // Factor M = QR = QDU where Q is orthogonal, D is diagonal, 
    // and U is upper triangular with ones on its diagonal.  Algorithm uses
    // Gram-Schmidt orthogonalization (the QR algorithm).
    //
    // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
    //
    //   q0 = m0/|m0|
    //   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
    //   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
    //
    // where |V| indicates length of vector V and A*B indicates dot
    // product of vectors A and B.  The matrix R has entries
    //
    //   r00 = q0*m0  r01 = q0*m1  r02 = q0*m2
    //   r10 = 0      r11 = q1*m1  r12 = q1*m2
    //   r20 = 0      r21 = 0      r22 = q2*m2
    //
    // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
    // u02 = r02/r00, and u12 = r12/r11.

    // Q = rotation
    // D = scaling
    // U = shear

    // D stores the three diagonal entries r00, r11, r22
    // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12

    // build orthogonal matrix Q
    MgcReal fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][0]*m_aafEntry[0][0]
        + m_aafEntry[1][0]*m_aafEntry[1][0] +
        m_aafEntry[2][0]*m_aafEntry[2][0]);
    kQ[0][0] = m_aafEntry[0][0]*fInvLength;
    kQ[1][0] = m_aafEntry[1][0]*fInvLength;
    kQ[2][0] = m_aafEntry[2][0]*fInvLength;

    MgcReal fDot = kQ[0][0]*m_aafEntry[0][1] + kQ[1][0]*m_aafEntry[1][1] +
        kQ[2][0]*m_aafEntry[2][1];
    kQ[0][1] = m_aafEntry[0][1]-fDot*kQ[0][0];
    kQ[1][1] = m_aafEntry[1][1]-fDot*kQ[1][0];
    kQ[2][1] = m_aafEntry[2][1]-fDot*kQ[2][0];
    fInvLength = 1.0/MgcMath::Sqrt(kQ[0][1]*kQ[0][1] + kQ[1][1]*kQ[1][1] +
        kQ[2][1]*kQ[2][1]);
    kQ[0][1] *= fInvLength;
    kQ[1][1] *= fInvLength;
    kQ[2][1] *= fInvLength;

    fDot = kQ[0][0]*m_aafEntry[0][2] + kQ[1][0]*m_aafEntry[1][2] +
        kQ[2][0]*m_aafEntry[2][2];
    kQ[0][2] = m_aafEntry[0][2]-fDot*kQ[0][0];
    kQ[1][2] = m_aafEntry[1][2]-fDot*kQ[1][0];
    kQ[2][2] = m_aafEntry[2][2]-fDot*kQ[2][0];
    fDot = kQ[0][1]*m_aafEntry[0][2] + kQ[1][1]*m_aafEntry[1][2] +
        kQ[2][1]*m_aafEntry[2][2];
    kQ[0][2] -= fDot*kQ[0][1];
    kQ[1][2] -= fDot*kQ[1][1];
    kQ[2][2] -= fDot*kQ[2][1];
    fInvLength = 1.0/MgcMath::Sqrt(kQ[0][2]*kQ[0][2] + kQ[1][2]*kQ[1][2] +
        kQ[2][2]*kQ[2][2]);
    kQ[0][2] *= fInvLength;
    kQ[1][2] *= fInvLength;
    kQ[2][2] *= fInvLength;