TAPsSparseSymmetricMatrix_Matrix3x3.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsSparseSymmetricMatrix_Matrix3x3.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (12/30/2009)
UPDATE          (04/10/2010)
******************************************************************************/
#include "TAPsSparseSymmetricMatrix_Matrix3x3.hpp"
// Using Inclusion Model (i.e. definitions are included in declarations)
//                       (this name.cpp is included in name.hpp)
// Each friend is defined directly inside its declaration.

BEGIN_NAMESPACE_TAPs
//=============================================================================
// Constructors
//-----------------------------------------------------------------------------
template <typename T>
SparseSymmetricMatrix_Matrix3x3<T>::SparseSymmetricMatrix_Matrix3x3 ( unsigned int numOfRows )
{
    m_Rows.resize( numOfRows );
}
//-----------------------------------------------------------------------------
template <typename T>
SparseSymmetricMatrix_Matrix3x3<T>::SparseSymmetricMatrix_Matrix3x3 ( SparseSymmetricMatrix_Matrix3x3<T> const &orig )
{
    //m_Rows = orig.m_Rows;
    *this = orig;
}
//-----------------------------------------------------------------------------
template <typename T>
SparseSymmetricMatrix_Matrix3x3<T>::~SparseSymmetricMatrix_Matrix3x3 ()
{}  
//-----------------------------------------------------------------------------
template <typename T>
std::string SparseSymmetricMatrix_Matrix3x3<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "SparseSymmetricMatrix_Matrix3x3<" << typeid(T).name() << "> has " << m_Rows.size() << " rows:\n";
    std::list< SparseVector_Matrix3x3<T> >::const_iterator it = m_Rows.begin();

    // string of matrix (the symmetry part are shown as zeroes)
    it = m_Rows.begin();
    Matrix3x3<T> *M = new Matrix3x3<T>[m_Rows.size()];
    Matrix3x3<T> const *N;
    for ( unsigned int i = 0; i < m_Rows.size(); ++i ) {
        for ( unsigned int j = 0; j < m_Rows.size(); ++j ) {
            if ( N = it->ReturnMatrix3x3( j ) ) {
                M[j] = *N;
            }
            else {
                M[j].MakeZero();
            }
        }
        unsigned int j;
        for ( unsigned int r = 0; r < 9; r+=3 ) {
            ss << "[ ";
            for ( j = 0; j < m_Rows.size()-1; ++j ) {
                ss.width(12);
                ss << M[j][0+r] << ", ";
                ss.width(12);
                ss << M[j][1+r] << ", ";
                ss.width(12);
                ss << M[j][2+r] << ", ";
            }
            ss.width(12);
            ss << M[j][0+r] << ", ";
            ss.width(12);
            ss << M[j][1+r] << ", ";
            ss.width(12);
            ss << M[j][2+r];
            ss << " ]\n";
        }
        ++it;
    }
    delete [] M;

    return ss.str();
}
//-----------------------------------------------------------------------------
//=============================================================================
// Assignment Operator
//-----------------------------------------------------------------------------
template <typename T>
inline SparseSymmetricMatrix_Matrix3x3<T> & SparseSymmetricMatrix_Matrix3x3<T>::operator= ( SparseSymmetricMatrix_Matrix3x3<T> const &orig )
{   
    m_Rows = orig.m_Rows;
    if ( orig.m_DirectAccessors.size() > 0 ) {
        this->SetDirectAccessors();
    }
    return *this;
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Operations
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::MulWith (
    std::vector< Vector3<T> > const &I, 
    std::vector< Vector3<T> > &O        
) const
{
    for ( unsigned int i = 0; i < O.size(); ++i ) {
        O[i].SetXYZ( 0, 0, 0 );
    }
    unsigned int r = 0;
    std::list< SparseVector_Matrix3x3<T> >::const_iterator it = m_Rows.begin();
    while ( it != m_Rows.end() ) {
        it->MulForSparseSymmetricMatrix( r, I, O );
        ++r;
        ++it;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::MulWith_wSkippedElements (
    std::vector< Vector3<T> > const &I, 
    std::vector< Vector3<T> > &O,       
    std::vector< bool > const &Skip     
) const
{
    for ( unsigned int i = 0; i < O.size(); ++i ) {
        O[i].SetXYZ( 0, 0, 0 );
    }
    unsigned int r = 0;
    std::list< SparseVector_Matrix3x3<T> >::const_iterator it = m_Rows.begin();
    while ( it != m_Rows.end() ) {
        if ( Skip[r] == false ) {
            it->MulForFEMSparseSymmetricMatrix_wSkippedElements( r, I, O, Skip );
        }
        else {
            O[r] = I[r];
        }
        ++r;
        ++it;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::ChangeNumOfRows ( unsigned int numOfRows )
{   
    m_Rows.resize( numOfRows );
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::DiagonalizeRowCol ( unsigned int ii, T diagonalValue )
{   
    std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data;
    // From row 0 to row (ii-1)
    for ( unsigned int m = 0; m < ii; ++m ) {
        data = DirectAccess( m )->RefToElements().begin();
        while ( data != DirectAccess( m )->RefToElements().end() && data->idx < ii ) {
            ++data;
        }
        if ( data != DirectAccess( m )->RefToElements().end() && data->idx == ii ) {
            data->M.MakeZero();
        }
    }

    // Row ii
    data = DirectAccess( ii )->RefToElements().begin();
    while ( data != DirectAccess( ii )->RefToElements().end() ) {
        if ( data->idx == ii ) {
            data->M.MakeDiagonal( diagonalValue );
        }
        else {
            data->M.MakeZero();
        }
        ++data;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::DiagonalizeRowsCols (
    std::vector< bool > const & ii, 
    T diagonalValue = 1             
)
{   
    std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data;
    for ( unsigned int r = 0; r < GetNumOfRows(); ++r ) {
        data = DirectAccess( r )->RefToElements().begin();
        while ( data != DirectAccess( r )->RefToElements().end() ) {
            if ( ii[data->idx] ) {
                if ( data->idx == r ) {
                    data->M.MakeDiagonal( diagonalValue );
                }
                else {
                    data->M.MakeZero();
                }
            }
            ++data;
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::DiagonalizeRowsCols (
    std::vector< unsigned int > const & ii, 
    T diagonalValue = 1                     
)
{   
    std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data;
    for ( unsigned int r = 0; r < ii.size(); ++r ) {
        // From row 0 to row ii[r]
        unsigned int row = ii[r];
        for ( unsigned int m = 0; m < row; ++m ) {
            data = DirectAccess( m )->RefToElements().begin();
            while ( data != DirectAccess( m )->RefToElements().end() && data->idx < row ) {
                ++data;
            }
            if ( data != DirectAccess( m )->RefToElements().end() && data->idx == row ) {
                data->M.MakeZero();
            }
        }

        // Row r
        data = DirectAccess( row )->RefToElements().begin();
        while ( data != DirectAccess( row )->RefToElements().end() ) {
            if ( data->idx == row ) {
                data->M.MakeDiagonal( diagonalValue );
            }
            else {
                data->M.MakeZero();
            }
            ++data;
        }
    }
    /*
    for ( unsigned int r = 0; r < ii.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( ii[r] )->RefToElements().begin();
        while ( data != DirectAccess( ii[r] )->RefToElements().end() ) {
            if ( data->idx == ii[r] ) {
                data->M.MakeDiagonal( diagonalValue );
            }
            else {
                data->M.MakeZero();
            }
            ++data;
        }
    }
    //*/
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::SetDirectAccessors ()
{   
    m_DirectAccessors.resize( m_Rows.size() );
    unsigned int n = 0;
    std::list< SparseVector_Matrix3x3<T> >::iterator it = m_Rows.begin();
    while ( it != m_Rows.end() ) {
        m_DirectAccessors[n++] = &(*it);
        ++it;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
SparseVector_Matrix3x3<T> * const SparseSymmetricMatrix_Matrix3x3<T>::DirectAccess ( unsigned int row )
{
    return m_DirectAccessors[row];
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::MPCGSolver ( 
    SparseSymmetricMatrix_Matrix3x3<T> const &A,    
    std::vector< Vector3<T> > const &x,     
    std::vector< Vector3<T> > const &b,     
    std::vector< Vector3<T> > &x_out,       
    std::vector< Vector3<T> > const &ft,    
    std::vector< Vector3<T> > const &P,     
    int iterationLimit = 100,               
    T errorThreshold = 1E-10                
)
{
    //std::cout << "START MPCGSolver() ------------------------------------------" << std::endl;

    unsigned int size = b.size();
    
    // inputs
    // A, x, b, x_out, ft, P
    // temps
    std::vector< Vector3<T> > v( size );
    T D0, Dold, Dnew, alpha;
    std::vector< Vector3<T> > Pinv( size );
    std::vector< Vector3<T> > r( size );
    std::vector< Vector3<T> > s( size );
    std::vector< Vector3<T> > c( size );
    std::vector< Vector3<T> > q( size );

    // x_out = x
    // D0 = Filter(b)^T.P.Filter(b)
    D0 = 0;
    Vector3<T> filterB;
    for ( unsigned int i = 0; i < size; ++i ) {
        // x_out
        x_out[i][0] = x[i][0];
        x_out[i][1] = x[i][1];
        x_out[i][2] = x[i][2];
        // D0
        filterB[0] = ft[i][0]*b[i][0];
        filterB[1] = ft[i][1]*b[i][1];
        filterB[2] = ft[i][2]*b[i][2];
        D0 += filterB[0]*filterB[0]*P[i][0] + filterB[1]*filterB[1]*P[i][1] + filterB[2]*filterB[2]*P[i][2];
        //std::cout << "D0: " << D0 << "\n";
    }
    // r = Filter(b - A.x_out)
    // c = Filter(P^{-1}.r)
    // Dnew = r^{T}.c
    Dnew = 0;
    A.MulWith( x_out, v );      // v = A.x_out

    // DEBUG
    //std::cout << "x_out:\n" << DEBUG_ToStr(x_out) << "\n";
    //std::cout << "v = A.x_out:\n" << DEBUG_ToStr(v) << "\n";

    for ( unsigned int i = 0; i < size; ++i ) {
        // r
        r[i][0] = b[i][0] - v[i][0];
        r[i][1] = b[i][1] - v[i][1];
        r[i][2] = b[i][2] - v[i][2];
        r[i][0] = ft[i][0]*r[i][0];
        r[i][1] = ft[i][1]*r[i][1];
        r[i][2] = ft[i][2]*r[i][2];
        // c
        Pinv[i][0] = T(1) / P[i][0];
        Pinv[i][1] = T(1) / P[i][1];
        Pinv[i][2] = T(1) / P[i][2];
        c[i][0] = Pinv[i][0] * r[i][0];
        c[i][1] = Pinv[i][1] * r[i][1];
        c[i][2] = Pinv[i][2] * r[i][2];
        c[i][0] = ft[i][0]*c[i][0];
        c[i][1] = ft[i][1]*c[i][1];
        c[i][2] = ft[i][2]*c[i][2];
        // Dnew
        Dnew += r[i] * c[i];
    }

    //D0 = Dnew;

    int count = 0;
    T err = 1;
    T ths = errorThreshold * errorThreshold * D0 * D0;

    /*
    std::cout << "FT P PINV DO X_OUT R C DNEW COUNT ERROR\n";
    std::cout << "BEFORE ENTERING THE LOOP\n";
    std::cout << "ft:\n" << DEBUG_ToStr(ft) << "\n";
    std::cout << "P:\n" << DEBUG_ToStr(P) << "\n";
    std::cout << "Pinv:\n" << DEBUG_ToStr(Pinv) << "\n";
    std::cout << "D0:\n" << D0 << "\n";
    std::cout << "x_out:\n" << DEBUG_ToStr(x_out) << "\n";
    std::cout << "r:\n" << DEBUG_ToStr(r) << "\n";
    std::cout << "c:\n" << DEBUG_ToStr(c) << "\n";
    std::cout << "Dnew:\n" << Dnew << "\n";

    std::cout << "count: " << count << ";\t";
    std::cout << "error: " << err << "\n";
    //*/

    while ( err > 0 && count < iterationLimit ) {

        // q = Filter(A.c)
        // alpha = Dnew / (c^T.q)
        T tmp = 0;
        A.MulWith( c, v );  // v = A.c

        // DEBUG
        //std::cout << "c:\n" << DEBUG_ToStr(c) << "\n";
        //std::cout << "v = A.c:\n" << DEBUG_ToStr(v) << "\n";

        for ( unsigned int i = 0; i < size; ++i ) {
            // q
            q[i][0] = ft[i][0]*v[i][0];
            q[i][1] = ft[i][1]*v[i][1];
            q[i][2] = ft[i][2]*v[i][2];

            //std::cout << "ft["<<i<<"]: " << ft[i] << "\n";
            //std::cout << "v["<<i<<"]: " << v[i] << "\n";
            //std::cout << "q["<<i<<"]: " << q[i] << "\n";

            // alpha
            tmp += c[i]*q[i];

            //std::cout << "tmp: " << tmp << "\n";
        }
        if ( tmp != 0 ) {
            alpha = Dnew / tmp;

            //std::cout << "alpha: " << alpha << "\n";
        }
        else {
            break;
        }
        // x_out += alpha.c
        // r -= alpha.q
        // s = P^{-1}.r
        for ( unsigned int i = 0; i < size; ++i ) {
            // x_out
            x_out[i][0] += alpha * c[i][0];
            x_out[i][1] += alpha * c[i][1];
            x_out[i][2] += alpha * c[i][2];
            // r
            r[i][0] -= alpha * q[i][0];
            r[i][1] -= alpha * q[i][1];
            r[i][2] -= alpha * q[i][2];
            // s
            s[i][0] = Pinv[i][0]*r[i][0];
            s[i][1] = Pinv[i][1]*r[i][1];
            s[i][2] = Pinv[i][2]*r[i][2];
        }
        // Dold = Dnew
        Dold = Dnew;
        // Dnew = r^T.s
        Dnew = 0;
        for ( unsigned int i = 0; i < size; ++i ) {
            Dnew += r[i] * s[i];
        }
        // c = Filter( s + (Dnew/Dold)*c )
        tmp = Dnew/Dold;
        for ( unsigned int i = 0; i < size; ++i ) {
            c[i][0] = s[i][0] + tmp*c[i][0];
            c[i][1] = s[i][1] + tmp*c[i][1];
            c[i][2] = s[i][2] + tmp*c[i][2];
            c[i][0] = ft[i][0]*c[i][0];
            c[i][1] = ft[i][1]*c[i][1];
            c[i][2] = ft[i][2]*c[i][2];
        }

        // update values for stop conditions
        err = Dnew*Dnew - ths;
        ++count;

        /*
        std::cout << "MPCGSolver: count: " << count << "\n";
        std::cout << "q:\n" << DEBUG_ToStr(q) << "\n";
        std::cout << "alpha:\n" << alpha << "\n";
        std::cout << "x_out:\n" << DEBUG_ToStr(x_out) << "\n";
        std::cout << "r:\n" << DEBUG_ToStr(r) << "\n";
        std::cout << "s:\n" << DEBUG_ToStr(s) << "\n";
        std::cout << "Dold:\n" << Dold << "\n";
        std::cout << "Dnew:\n" << Dnew << "\n";
        std::cout << "c:\n" << DEBUG_ToStr(c) << "\n";

        std::cout << "count: " << count << ";\t";
        std::cout << "error: " << err << "\n";
        //*/
    }

    /*
    std::cout << "count: " << count << ";\t";
    std::cout << "error: " << err << "\n";
    std::cout << "A:\n" << A << "\n";
    std::cout << "x:\n" << DEBUG_ToStr(x) << "\n";
    std::cout << "x_out:\n" << DEBUG_ToStr(x_out) << "\n";
    std::cout << "b:\n" << DEBUG_ToStr(b) << "\n";
    A.MulWith( x_out, v );
    std::cout << "v:\n" << DEBUG_ToStr(v) << "\n";
    //*/

    //std::cout << "END MPCGSolver() --------------------------------------------" << std::endl;
}


//-----------------------------------------------------------------------------
template <typename T>
void SparseSymmetricMatrix_Matrix3x3<T>::MPCGSolverSpecificForFEM (
    std::vector< bool > const &fixedNodes,  
    SparseSymmetricMatrix_Matrix3x3<T> const &A,    
    std::vector< Vector3<T> > const &x,     
    std::vector< Vector3<T> > const &b,     
    std::vector< Vector3<T> > &x_out,       
    std::vector< Vector3<T> > const &ft,    
    std::vector< Vector3<T> > const &P,     
    int iterationLimit = 100,               
    T errorThreshold = 1E-10                
)
{
    //std::cout << "START MPCGSolverSpecificForFEM() ------------------------------------------" << std::endl;

    unsigned int size = b.size();
    
    // inputs
    // A, x, b, x_out, ft, P
    // temps
    std::vector< Vector3<T> > v( size );
    T D0, Dold, Dnew, alpha;
    std::vector< Vector3<T> > Pinv( size );
    std::vector< Vector3<T> > r( size );
    std::vector< Vector3<T> > s( size );
    std::vector< Vector3<T> > c( size );
    std::vector< Vector3<T> > q( size );

    // x_out = x
    // D0 = Filter(b)^T.P.Filter(b)
    D0 = 0;
    Vector3<T> filterB;
    for ( unsigned int i = 0; i < size; ++i ) {
        // x_out
        x_out[i][0] = x[i][0];
        x_out[i][1] = x[i][1];
        x_out[i][2] = x[i][2];
        // D0
        filterB[0] = ft[i][0]*b[i][0];
        filterB[1] = ft[i][1]*b[i][1];
        filterB[2] = ft[i][2]*b[i][2];
        D0 += filterB[0]*filterB[0]*P[i][0] + filterB[1]*filterB[1]*P[i][1] + filterB[2]*filterB[2]*P[i][2];
        //std::cout << "D0: " << D0 << "\n";
    }
    // r = Filter(b - A.x_out)
    // c = Filter(P^{-1}.r)
    // Dnew = r^{T}.c
    Dnew = 0;
    A.MulWith_wSkippedElements( x_out, v, fixedNodes );     // v = A.x_out

    for ( unsigned int i = 0; i < size; ++i ) {
        // r
        r[i][0] = b[i][0] - v[i][0];
        r[i][1] = b[i][1] - v[i][1];
        r[i][2] = b[i][2] - v[i][2];
        r[i][0] = ft[i][0]*r[i][0];
        r[i][1] = ft[i][1]*r[i][1];
        r[i][2] = ft[i][2]*r[i][2];
        // c
        Pinv[i][0] = T(1) / P[i][0];
        Pinv[i][1] = T(1) / P[i][1];
        Pinv[i][2] = T(1) / P[i][2];
        c[i][0] = Pinv[i][0] * r[i][0];
        c[i][1] = Pinv[i][1] * r[i][1];
        c[i][2] = Pinv[i][2] * r[i][2];
        c[i][0] = ft[i][0]*c[i][0];
        c[i][1] = ft[i][1]*c[i][1];
        c[i][2] = ft[i][2]*c[i][2];
        // Dnew
        Dnew += r[i] * c[i];
    }

    //D0 = Dnew;

    int count = 0;
    T err = 1;
    T ths = errorThreshold * errorThreshold * D0 * D0;

    while ( err > 0 && count < iterationLimit ) {

        // q = Filter(A.c)
        // alpha = Dnew / (c^T.q)
        T tmp = 0;
        A.MulWith_wSkippedElements( c, v, fixedNodes ); // v = A.c

        for ( unsigned int i = 0; i < size; ++i ) {
            // q
            q[i][0] = ft[i][0]*v[i][0];
            q[i][1] = ft[i][1]*v[i][1];
            q[i][2] = ft[i][2]*v[i][2];
            // alpha
            tmp += c[i]*q[i];
        }
        if ( tmp != 0 ) {
            alpha = Dnew / tmp;
        }
        else {
            break;
        }
        // x_out += alpha.c
        // r -= alpha.q
        // s = P^{-1}.r
        for ( unsigned int i = 0; i < size; ++i ) {
            // x_out
            x_out[i][0] += alpha * c[i][0];
            x_out[i][1] += alpha * c[i][1];
            x_out[i][2] += alpha * c[i][2];
            // r
            r[i][0] -= alpha * q[i][0];
            r[i][1] -= alpha * q[i][1];
            r[i][2] -= alpha * q[i][2];
            // s
            s[i][0] = Pinv[i][0]*r[i][0];
            s[i][1] = Pinv[i][1]*r[i][1];
            s[i][2] = Pinv[i][2]*r[i][2];
        }
        // Dold = Dnew
        Dold = Dnew;
        // Dnew = r^T.s
        Dnew = 0;
        for ( unsigned int i = 0; i < size; ++i ) {
            Dnew += r[i] * s[i];
        }
        // c = Filter( s + (Dnew/Dold)*c )
        tmp = Dnew/Dold;
        for ( unsigned int i = 0; i < size; ++i ) {
            c[i][0] = s[i][0] + tmp*c[i][0];
            c[i][1] = s[i][1] + tmp*c[i][1];
            c[i][2] = s[i][2] + tmp*c[i][2];
            c[i][0] = ft[i][0]*c[i][0];
            c[i][1] = ft[i][1]*c[i][1];
            c[i][2] = ft[i][2]*c[i][2];
        }

        // update values for stop conditions
        err = Dnew*Dnew - ths;
        ++count;
    }

    //*
    std::cout << "count: " << count << ";\t";
    std::cout << "error: " << err << "\n";
    std::cout << "x:\n" << DEBUG_ToStr(x) << "\n";
    std::cout << "x_out:\n" << DEBUG_ToStr(x_out) << "\n";
    std::cout << "b:\n" << DEBUG_ToStr(b) << "\n";
    //*/

    //std::cout << "END MPCGSolverSpecificForFEM() --------------------------------------------" << std::endl;
}
//-----------------------------------------------------------------------------
//=============================================================================

//-----------------------------------------------------------------------------
//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----