mgceigen.cpp

3D Game Engine Design Source Code非常棒

    // re-ordering if MgcEigen::QLAlgorithm is used subsequently
    for (i0 = 1, i3 = 0; i0 < iSize; i0++, i3++)
        m_afSubd[i3] = m_afSubd[i0];
    m_afSubd[iSize-1] = 0;
}
//---------------------------------------------------------------------------
bool MgcEigen::QLAlgorithm (int iSize, MgcReal* m_afDiag, MgcReal* m_afSubd,
    MgcReal** m_aafMat)
{
    const int iMaxIter = 32;

    for (int i0 = 0; i0 < iSize; i0++)
    {
        int i1;
        for (i1 = 0; i1 < iMaxIter; i1++)
        {
            int i2;
            for (i2 = i0; i2 <= iSize-2; i2++)
            {
                MgcReal fTmp =
                    MgcMath::Abs(m_afDiag[i2])+MgcMath::Abs(m_afDiag[i2+1]);
                if ( MgcMath::Abs(m_afSubd[i2]) + fTmp == fTmp )
                    break;
            }
            if ( i2 == i0 )
                break;

            MgcReal fG = (m_afDiag[i0+1]-m_afDiag[i0])/(2.0*m_afSubd[i0]);
            MgcReal fR = MgcMath::Sqrt(fG*fG+1.0);
            if ( fG < 0.0 )
                fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
            else
                fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
            MgcReal fSin = 1.0, fCos = 1.0, fP = 0.0;
            for (int i3 = i2-1; i3 >= i0; i3--)
            {
                MgcReal fF = fSin*m_afSubd[i3];
                MgcReal fB = fCos*m_afSubd[i3];
                if ( MgcMath::Abs(fF) >= MgcMath::Abs(fG) )
                {
                    fCos = fG/fF;
                    fR = MgcMath::Sqrt(fCos*fCos+1.0);
                    m_afSubd[i3+1] = fF*fR;
                    fSin = 1.0/fR;
                    fCos *= fSin;
                }
                else
                {
                    fSin = fF/fG;
                    fR = MgcMath::Sqrt(fSin*fSin+1.0);
                    m_afSubd[i3+1] = fG*fR;
                    fCos = 1.0/fR;
                    fSin *= fCos;
                }
                fG = m_afDiag[i3+1]-fP;
                fR = (m_afDiag[i3]-fG)*fSin+2.0*fB*fCos;
                fP = fSin*fR;
                m_afDiag[i3+1] = fG+fP;
                fG = fCos*fR-fB;

                for (int i4 = 0; i4 < iSize; i4++)
                {
                    fF = m_aafMat[i4][i3+1];
                    m_aafMat[i4][i3+1] = fSin*m_aafMat[i4][i3]+fCos*fF;
                    m_aafMat[i4][i3] = fCos*m_aafMat[i4][i3]-fSin*fF;
                }
            }
            m_afDiag[i0] -= fP;
            m_afSubd[i0] = fG;
            m_afSubd[i2] = 0.0;
        }
        if ( i1 == iMaxIter )
            return false;
    }

    return true;
}
//---------------------------------------------------------------------------
void MgcEigen::DecreasingSort (int iSize, MgcReal* afEigval,
    MgcReal** aafEigvec)
{
    // sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
    for (int i0 = 0, i1; i0 <= iSize-2; i0++)
    {
        // locate maximum eigenvalue
        i1 = i0;
        MgcReal fMax = afEigval[i1];
        int i2;
        for (i2 = i0+1; i2 < iSize; i2++)
        {
            if ( afEigval[i2] > fMax )
            {
                i1 = i2;
                fMax = afEigval[i1];
            }
        }

        if ( i1 != i0 )
        {
            // swap eigenvalues
            afEigval[i1] = afEigval[i0];
            afEigval[i0] = fMax;

            // swap eigenvectors
            for (i2 = 0; i2 < iSize; i2++)
            {
                MgcReal fTmp = aafEigvec[i2][i0];
                aafEigvec[i2][i0] = aafEigvec[i2][i1];
                aafEigvec[i2][i1] = fTmp;
            }
        }
    }
}
//---------------------------------------------------------------------------
void MgcEigen::IncreasingSort (int iSize, MgcReal* afEigval,
    MgcReal** aafEigvec)
{
    // sort eigenvalues in increasing order, e[0] <= ... <= e[iSize-1]
    for (int i0 = 0, i1; i0 <= iSize-2; i0++)
    {
        // locate minimum eigenvalue
        i1 = i0;
        MgcReal fMin = afEigval[i1];
        int i2;
        for (i2 = i0+1; i2 < iSize; i2++)
        {
            if ( afEigval[i2] < fMin )
            {
                i1 = i1;
                fMin = afEigval[i1];
            }
        }

        if ( i1 != i0 )
        {
            // swap eigenvalues
            afEigval[i1] = afEigval[i0];
            afEigval[i0] = fMin;

            // swap eigenvectors
            for (i2 = 0; i2 < iSize; i2++)
            {
                MgcReal fTmp = aafEigvec[i2][i0];
                aafEigvec[i2][i0] = aafEigvec[i2][i1];
                aafEigvec[i2][i1] = fTmp;
            }
        }
    }
}
//---------------------------------------------------------------------------
void MgcEigen::SetMatrix (MgcReal** aafMat)
{
    for (int iRow = 0; iRow < m_iSize; iRow++)
    {
        for (int iCol = 0; iCol < m_iSize; iCol++)
            m_aafMat[iRow][iCol] = aafMat[iRow][iCol];
    }
}
//---------------------------------------------------------------------------
void MgcEigen::EigenStuff2 ()
{
    Tridiagonal2(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::EigenStuff3 ()
{
    Tridiagonal3(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::EigenStuff4 ()
{
    Tridiagonal4(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::EigenStuffN ()
{
    TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::EigenStuff ()
{
    switch ( m_iSize )
    {
        case 2:
            Tridiagonal2(m_aafMat,m_afDiag,m_afSubd);
            break;
        case 3:
            Tridiagonal3(m_aafMat,m_afDiag,m_afSubd);
            break;
        case 4:
            Tridiagonal4(m_aafMat,m_afDiag,m_afSubd);
            break;
        default:
            TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd);
            break;
    }
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::DecrSortEigenStuff2 ()
{
    Tridiagonal2(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    DecreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::DecrSortEigenStuff3 ()
{
    Tridiagonal3(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    DecreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::DecrSortEigenStuff4 ()
{
    Tridiagonal4(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    DecreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::DecrSortEigenStuffN ()
{
    TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    DecreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::DecrSortEigenStuff ()
{
    switch ( m_iSize )
    {
        case 2:
            Tridiagonal2(m_aafMat,m_afDiag,m_afSubd);
            break;
        case 3:
            Tridiagonal3(m_aafMat,m_afDiag,m_afSubd);
            break;
        case 4:
            Tridiagonal4(m_aafMat,m_afDiag,m_afSubd);
            break;
        default:
            TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd);
            break;
    }
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    DecreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::IncrSortEigenStuff2 ()
{
    Tridiagonal2(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    IncreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::IncrSortEigenStuff3 ()
{
    Tridiagonal3(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    IncreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::IncrSortEigenStuff4 ()
{
    Tridiagonal4(m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    IncreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::IncrSortEigenStuffN ()
{
    TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd);
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    IncreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------
void MgcEigen::IncrSortEigenStuff ()
{
    switch ( m_iSize )
    {
        case 2:
            Tridiagonal2(m_aafMat,m_afDiag,m_afSubd);
            break;
        case 3:
            Tridiagonal3(m_aafMat,m_afDiag,m_afSubd);
            break;
        case 4:
            Tridiagonal4(m_aafMat,m_afDiag,m_afSubd);
            break;
        default:
            TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd);
            break;
    }
    QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat);
    IncreasingSort(m_iSize,m_afDiag,m_aafMat);
}
//---------------------------------------------------------------------------

#ifdef EIGEN_TEST

int main ()
{
    MgcEigen kES(3);

    kES.Matrix(0,0) = 2.0;  kES.Matrix(0,1) = 1.0;  kES.Matrix(0,2) = 1.0;
    kES.Matrix(1,0) = 1.0;  kES.Matrix(1,1) = 2.0;  kES.Matrix(1,2) = 1.0;
    kES.Matrix(2,0) = 1.0;  kES.Matrix(2,1) = 1.0;  kES.Matrix(2,2) = 2.0;

    kES.IncrSortEigenStuff3();

    cout.setf(ios::fixed);

    cout << "eigenvalues = " << endl;
    int iRow;
    for (iRow = 0; iRow < 3; iRow++)
        cout << kES.GetEigenvalue(iRow) << ' ';
    cout << endl;

    cout << "eigenvectors = " << endl;
    for (iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            cout << kES.GetEigenvector(iRow,iCol) << ' ';
        cout << endl;
    }

    // eigenvalues =
    //    1.000000 1.000000 4.000000
    // eigenvectors =
    //    0.411953  0.704955 0.577350
    //    0.404533 -0.709239 0.577350
    //   -0.816485  0.004284 0.577350

    return 0;
}

#endif