mgcmatrix3.cpp

3D Game Engine Design Source Code非常棒

            rfRAngle = 0.0;  // any angle works
            rfYAngle = rfRAngle - fRmY;
            return false;
        }
    }
    else
    {
        // WARNING.  Not a unique solution.
        float fRpY = MgcMath::ATan2(m_aafEntry[2][1],m_aafEntry[2][2]);
        rfRAngle = 0.0;  // any angle works
        rfYAngle = fRpY - rfRAngle;
        return false;
    }
}
//----------------------------------------------------------------------------
bool MgcMatrix3::ToEulerAnglesZXY (float& rfYAngle, float& rfPAngle,
    float& rfRAngle)
{
    // rot =  cy*cz-sx*sy*sz -cx*sz           cz*sy+cy*sx*sz
    //        cz*sx*sy+cy*sz  cx*cz          -cy*cz*sx+sy*sz
    //       -cx*sy           sx              cx*cy

    rfPAngle = MgcMath::ASin(m_aafEntry[2][1]);
    if ( rfPAngle < MgcMath::HALF_PI )
    {
        if ( rfPAngle > -MgcMath::HALF_PI )
        {
            rfYAngle = MgcMath::ATan2(-m_aafEntry[0][1],m_aafEntry[1][1]);
            rfRAngle = MgcMath::ATan2(-m_aafEntry[2][0],m_aafEntry[2][2]);
            return true;
        }
        else
        {
            // WARNING.  Not a unique solution.
            float fRmY = MgcMath::ATan2(m_aafEntry[0][2],m_aafEntry[0][0]);
            rfRAngle = 0.0;  // any angle works
            rfYAngle = rfRAngle - fRmY;
            return false;
        }
    }
    else
    {
        // WARNING.  Not a unique solution.
        float fRpY = MgcMath::ATan2(m_aafEntry[0][2],m_aafEntry[0][0]);
        rfRAngle = 0.0;  // any angle works
        rfYAngle = fRpY - rfRAngle;
        return false;
    }
}
//----------------------------------------------------------------------------
bool MgcMatrix3::ToEulerAnglesZYX (float& rfYAngle, float& rfPAngle,
    float& rfRAngle)
{
    // rot =  cy*cz           cz*sx*sy-cx*sz  cx*cz*sy+sx*sz
    //        cy*sz           cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
    //       -sy              cy*sx           cx*cy

    rfPAngle = MgcMath::ASin(-m_aafEntry[2][0]);
    if ( rfPAngle < MgcMath::HALF_PI )
    {
        if ( rfPAngle > -MgcMath::HALF_PI )
        {
            rfYAngle = MgcMath::ATan2(m_aafEntry[1][0],m_aafEntry[0][0]);
            rfRAngle = MgcMath::ATan2(m_aafEntry[2][1],m_aafEntry[2][2]);
            return true;
        }
        else
        {
            // WARNING.  Not a unique solution.
            float fRmY = MgcMath::ATan2(-m_aafEntry[0][1],m_aafEntry[0][2]);
            rfRAngle = 0.0;  // any angle works
            rfYAngle = rfRAngle - fRmY;
            return false;
        }
    }
    else
    {
        // WARNING.  Not a unique solution.
        float fRpY = MgcMath::ATan2(-m_aafEntry[0][1],m_aafEntry[0][2]);
        rfRAngle = 0.0;  // any angle works
        rfYAngle = fRpY - rfRAngle;
        return false;
    }
}
//----------------------------------------------------------------------------
void MgcMatrix3::FromEulerAnglesXYZ (float fYAngle, float fPAngle,
    float fRAngle)
{
    MgcReal fCos, fSin;

    fCos = MgcMath::Cos(fYAngle);
    fSin = MgcMath::Sin(fYAngle);
    MgcMatrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);

    fCos = MgcMath::Cos(fPAngle);
    fSin = MgcMath::Sin(fPAngle);
    MgcMatrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);

    fCos = MgcMath::Cos(fRAngle);
    fSin = MgcMath::Sin(fRAngle);
    MgcMatrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);

    *this = kXMat*(kYMat*kZMat);
}
//----------------------------------------------------------------------------
void MgcMatrix3::FromEulerAnglesXZY (float fYAngle, float fPAngle,
    float fRAngle)
{
    MgcReal fCos, fSin;

    fCos = MgcMath::Cos(fYAngle);
    fSin = MgcMath::Sin(fYAngle);
    MgcMatrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);

    fCos = MgcMath::Cos(fPAngle);
    fSin = MgcMath::Sin(fPAngle);
    MgcMatrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);

    fCos = MgcMath::Cos(fRAngle);
    fSin = MgcMath::Sin(fRAngle);
    MgcMatrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);

    *this = kXMat*(kZMat*kYMat);
}
//----------------------------------------------------------------------------
void MgcMatrix3::FromEulerAnglesYXZ (float fYAngle, float fPAngle,
    float fRAngle)
{
    MgcReal fCos, fSin;

    fCos = MgcMath::Cos(fYAngle);
    fSin = MgcMath::Sin(fYAngle);
    MgcMatrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);

    fCos = MgcMath::Cos(fPAngle);
    fSin = MgcMath::Sin(fPAngle);
    MgcMatrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);

    fCos = MgcMath::Cos(fRAngle);
    fSin = MgcMath::Sin(fRAngle);
    MgcMatrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);

    *this = kYMat*(kXMat*kZMat);
}
//----------------------------------------------------------------------------
void MgcMatrix3::FromEulerAnglesYZX (float fYAngle, float fPAngle,
    float fRAngle)
{
    MgcReal fCos, fSin;

    fCos = MgcMath::Cos(fYAngle);
    fSin = MgcMath::Sin(fYAngle);
    MgcMatrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);

    fCos = MgcMath::Cos(fPAngle);
    fSin = MgcMath::Sin(fPAngle);
    MgcMatrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);

    fCos = MgcMath::Cos(fRAngle);
    fSin = MgcMath::Sin(fRAngle);
    MgcMatrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);

    *this = kYMat*(kZMat*kXMat);
}
//----------------------------------------------------------------------------
void MgcMatrix3::FromEulerAnglesZXY (float fYAngle, float fPAngle,
    float fRAngle)
{
    MgcReal fCos, fSin;

    fCos = MgcMath::Cos(fYAngle);
    fSin = MgcMath::Sin(fYAngle);
    MgcMatrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);

    fCos = MgcMath::Cos(fPAngle);
    fSin = MgcMath::Sin(fPAngle);
    MgcMatrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);

    fCos = MgcMath::Cos(fRAngle);
    fSin = MgcMath::Sin(fRAngle);
    MgcMatrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);

    *this = kZMat*(kXMat*kYMat);
}
//----------------------------------------------------------------------------
void MgcMatrix3::FromEulerAnglesZYX (float fYAngle, float fPAngle,
    float fRAngle)
{
    MgcReal fCos, fSin;

    fCos = MgcMath::Cos(fYAngle);
    fSin = MgcMath::Sin(fYAngle);
    MgcMatrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);

    fCos = MgcMath::Cos(fPAngle);
    fSin = MgcMath::Sin(fPAngle);
    MgcMatrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);

    fCos = MgcMath::Cos(fRAngle);
    fSin = MgcMath::Sin(fRAngle);
    MgcMatrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);

    *this = kZMat*(kYMat*kXMat);
}
//----------------------------------------------------------------------------
void MgcMatrix3::Tridiagonal (MgcReal afDiag[3], MgcReal afSubDiag[3])
{
    // Householder reduction T = Q^t M Q
    //   Input:   
    //     mat, symmetric 3x3 matrix M
    //   Output:  
    //     mat, orthogonal matrix Q
    //     diag, diagonal entries of T
    //     subd, subdiagonal entries of T (T is symmetric)

    MgcReal fA = m_aafEntry[0][0];
    MgcReal fB = m_aafEntry[0][1];
    MgcReal fC = m_aafEntry[0][2];
    MgcReal fD = m_aafEntry[1][1];
    MgcReal fE = m_aafEntry[1][2];
    MgcReal fF = m_aafEntry[2][2];

    afDiag[0] = fA;
    afSubDiag[2] = 0.0;
    if ( MgcMath::Abs(fC) >= EPSILON )
    {
        MgcReal fLength = MgcMath::Sqrt(fB*fB+fC*fC);
        MgcReal fInvLength = 1.0/fLength;
        fB *= fInvLength;
        fC *= fInvLength;
        MgcReal fQ = 2.0*fB*fE+fC*(fF-fD);
        afDiag[1] = fD+fC*fQ;
        afDiag[2] = fF-fC*fQ;
        afSubDiag[0] = fLength;
        afSubDiag[1] = fE-fB*fQ;
        m_aafEntry[0][0] = 1.0;
        m_aafEntry[0][1] = 0.0;
        m_aafEntry[0][2] = 0.0;
        m_aafEntry[1][0] = 0.0;
        m_aafEntry[1][1] = fB;
        m_aafEntry[1][2] = fC;
        m_aafEntry[2][0] = 0.0;
        m_aafEntry[2][1] = fC;
        m_aafEntry[2][2] = -fB;
    }
    else
    {
        afDiag[1] = fD;
        afDiag[2] = fF;
        afSubDiag[0] = fB;
        afSubDiag[1] = fE;
        m_aafEntry[0][0] = 1.0;
        m_aafEntry[0][1] = 0.0;
        m_aafEntry[0][2] = 0.0;
        m_aafEntry[1][0] = 0.0;
        m_aafEntry[1][1] = 1.0;
        m_aafEntry[1][2] = 0.0;
        m_aafEntry[2][0] = 0.0;
        m_aafEntry[2][1] = 0.0;
        m_aafEntry[2][2] = 1.0;
    }
}
//----------------------------------------------------------------------------
bool MgcMatrix3::QLAlgorithm (MgcReal afDiag[3], MgcReal afSubDiag[3])
{
    // QL iteration with implicit shifting to reduce matrix from tridiagonal
    // to diagonal

    for (int i0 = 0; i0 < 3; i0++)
    {
        const int iMaxIter = 32;
        int iIter;
        for (iIter = 0; iIter < iMaxIter; iIter++)
        {
            int i1;
            for (i1 = i0; i1 <= 1; i1++)
            {
                MgcReal fSum = MgcMath::Abs(afDiag[i1]) +
                    MgcMath::Abs(afDiag[i1+1]);
                if ( MgcMath::Abs(afSubDiag[i1]) + fSum == fSum )
                    break;
            }
            if ( i1 == i0 )
                break;

            MgcReal fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0*afSubDiag[i0]);
            MgcReal fTmp1 = MgcMath::Sqrt(fTmp0*fTmp0+1.0);
            if ( fTmp0 < 0.0 )
                fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
            else
                fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
            MgcReal fSin = 1.0;
            MgcReal fCos = 1.0;
            MgcReal fTmp2 = 0.0;
            for (int i2 = i1-1; i2 >= i0; i2--)
            {
                MgcReal fTmp3 = fSin*afSubDiag[i2];
                MgcReal fTmp4 = fCos*afSubDiag[i2];
                if ( fabs(fTmp3) >= fabs(fTmp0) )
                {
                    fCos = fTmp0/fTmp3;
                    fTmp1 = MgcMath::Sqrt(fCos*fCos+1.0);
                    afSubDiag[i2+1] = fTmp3*fTmp1;
                    fSin = 1.0/fTmp1;
                    fCos *= fSin;
                }
                else
                {
                    fSin = fTmp3/fTmp0;
                    fTmp1 = MgcMath::Sqrt(fSin*fSin+1.0);
                    afSubDiag[i2+1] = fTmp0*fTmp1;
                    fCos = 1.0/fTmp1;
                    fSin *= fCos;
                }
                fTmp0 = afDiag[i2+1]-fTmp2;
                fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0*fTmp4*fCos;
                fTmp2 = fSin*fTmp1;
                afDiag[i2+1] = fTmp0+fTmp2;
                fTmp0 = fCos*fTmp1-fTmp4;

                for (int iRow = 0; iRow < 3; iRow++)
                {
                    fTmp3 = m_aafEntry[iRow][i2+1];
                    m_aafEntry[iRow][i2+1] = fSin*m_aafEntry[iRow][i2] +
                        fCos*fTmp3;
                    m_aafEntry[iRow][i2] = fCos*m_aafEntry[iRow][i2] -
                        fSin*fTmp3;
                }
            }
            afDiag[i0] -= fTmp2;
            afSubDiag[i0] = fTmp0;
            afSubDiag[i1] = 0.0;
        }

        if ( iIter == iMaxIter )
        {
            // should not get here under normal circumstances
            return false;
        }
    }

    return true;
}
//----------------------------------------------------------------------------
void MgcMatrix3::EigenSolveSymmetric (MgcReal afEigenvalue[3],
    MgcVector3 akEigenvector[3]) const
{
    MgcMatrix3 kMatrix = *this;
    MgcReal afSubDiag[3];
    kMatrix.Tridiagonal(afEigenvalue,afSubDiag);
    kMatrix.QLAlgorithm(afEigenvalue,afSubDiag);

    for (int i = 0; i < 3; i++)
    {
        akEigenvector[i][0] = kMatrix[0][i];
        akEigenvector[i][1] = kMatrix[1][i];
        akEigenvector[i][2] = kMatrix[2][i];
    }

    // make eigenvectors form a right--handed system
    MgcVector3 kCross = akEigenvector[1].Cross(akEigenvector[2]);
    MgcReal fDet = akEigenvector[0].Dot(kCross);
    if ( fDet < 0.0 )
    {
        akEigenvector[2][0] = - akEigenvector[2][0];
        akEigenvector[2][1] = - akEigenvector[2][1];
        akEigenvector[2][2] = - akEigenvector[2][2];
    }
}
//----------------------------------------------------------------------------
void MgcMatrix3::TensorProduct (const MgcVector3& rkU, const MgcVector3& rkV,
    MgcMatrix3& rkProduct)
{
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            rkProduct[iRow][iCol] = rkU[iRow]*rkV[iCol];
    }
}
//----------------------------------------------------------------------------