TAPsSparseMatrix_Matrix3x3.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsSparseMatrix_Matrix3x3.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (12/22/2009)
UPDATE          (04/10/2010)
******************************************************************************/
#include "TAPsSparseMatrix_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>
SparseMatrix_Matrix3x3<T>::SparseMatrix_Matrix3x3 ( unsigned int numOfRows )
{
    m_Rows.resize( numOfRows );
}
//-----------------------------------------------------------------------------
template <typename T>
SparseMatrix_Matrix3x3<T>::SparseMatrix_Matrix3x3 ( SparseMatrix_Matrix3x3<T> const &orig )
{
    *this = orig;
}
//-----------------------------------------------------------------------------
template <typename T>
SparseMatrix_Matrix3x3<T>::~SparseMatrix_Matrix3x3 ()
{}  
//-----------------------------------------------------------------------------
template <typename T>
std::string SparseMatrix_Matrix3x3<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "SparseMatrix_Matrix3x3<" << typeid(T).name() << "> has " << m_Rows.size() << " rows:\n";
    std::list< SparseVector_Matrix3x3<T> >::const_iterator it = m_Rows.begin();
    unsigned int r = 0;

    //while ( it != m_Rows.end() ) {
    //  ss << "Row# " << r++ << ":\t" << *it;
    //  ++it;
    //}

    // Print in matrix format
    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>
SparseMatrix_Matrix3x3<T> & SparseMatrix_Matrix3x3<T>::operator= ( SparseMatrix_Matrix3x3<T> const &orig )
{   
    m_Rows = orig.m_Rows;
    if ( orig.m_DirectAccessors.size() > 0 ) {
        this->SetDirectAccessors();
    }
    return *this;
}
//template <typename T>
//SparseMatrix_Matrix3x3<T> & SparseMatrix_Matrix3x3<T>::operator= ( SparseSymmetricMatrix_Matrix3x3<T> &orig )
//{ 
//  if ( orig.GetNumOfRows() == 0 ) {
//      m_Rows.clear();
//      return *this;
//  }
//
//  //orig.SetDirectAccessors();
//
//  m_Rows.resize( orig.GetNumOfRows() );
//  for ( unsigned int r = 0; r < GetNumOfRows(); ++r ) {
//  }
//
//  this->SetDirectAccessors();
//
//  std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator it;
//  for ( unsigned int r = 0; r < GetNumOfRows(); ++r ) {
//      it = orig.DirectAccess( r )->RefToElements().begin();
//      while ( it != (orig.DirectAccess( r ))->RefToElements().end() ) {
//          unsigned int c = it->idx;
//          if ( c == r ) {
//              this->DirectAccess( r )->Insert( c, it->M );
//          }
//          else {
//              this->DirectAccess( c )->Insert( c, it->M );
//              this->DirectAccess( r )->Insert( c, it->M );
//          }
//          ++it;
//      }
//  }
//
//  this->SetDirectAccessors();
//
//    return *this;
//  RESULT IS NOT CORRECT DUE TO THE CHANGING OF STD DATA LOCATION!!!
//}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::SetTo ( SparseMatrix_Matrix3x3<T> const &orig )
{   
    assert( m_Rows.size() == orig.m_Rows.size() );
    std::list< SparseVector_Matrix3x3<T> >::const_iterator A = orig.m_Rows.begin();
    std::list< SparseVector_Matrix3x3<T> >::iterator B = m_Rows.begin();
    while ( B != m_Rows.end() ) {
        B->SetTo( *A );
        ++A;
        ++B;
    }
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Operations
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::MulWith (
    std::vector< Vector3<T> > const &I, 
    std::vector< Vector3<T> > &O        
) const
{
    unsigned int r = 0;
    std::list< SparseVector_Matrix3x3<T> >::const_iterator it = m_Rows.begin();
    while ( it != m_Rows.end() ) {
        O[r] = it->Mul( I );
        ++r;
        ++it;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::MulWith_wSkippedRows (
    std::vector< Vector3<T> > const &I,         
    std::vector< Vector3<T> > &O,               
    std::vector< bool > const &rowsFixStatus,   
    T diagval                                   
) const
{
    unsigned int r = 0;
    std::list< SparseVector_Matrix3x3<T> >::const_iterator it = m_Rows.begin();
    while ( it != m_Rows.end() ) {
        if ( rowsFixStatus[r] ) {
            O[r][0] = diagval * I[r][0];
            O[r][1] = diagval * I[r][1];
            O[r][2] = diagval * I[r][2];
        }
        else {
            O[r] = it->Mul( I );
        }
        ++r;
        ++it;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::MulWith_wSkippedCols (
    std::vector< Vector3<T> > const &I,         
    std::vector< Vector3<T> > &O,               
    std::vector< bool > const &colsFixStatus,   
    T diagval                                   
) const
{
    // Since all diagonal is one therefore, at least O will be I.
    for ( unsigned int i = 0; i < I.size(); ++i ) {
        O[i].Clear();
    }
    unsigned int r = 0;
    std::list< SparseVector_Matrix3x3<T> >::const_iterator row = m_Rows.begin();
    while ( row != m_Rows.end() ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::const_iterator vec = row->RefToElements().begin();
        while ( vec != row->RefToElements().end() ) {
            if ( !colsFixStatus[vec->idx] ) {
                O[r] += vec->M * I[vec->idx];
            }
            else if ( vec->idx == r ) {
                O[r][0] += diagval * I[vec->idx][0];
                O[r][1] += diagval * I[vec->idx][1];
                O[r][2] += diagval * I[vec->idx][2];
            }
            ++vec;
        }
        ++r;
        ++row;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::MulWith_wSkippedRowsCols (
    std::vector< Vector3<T> > const &I,             
    std::vector< Vector3<T> > &O,                   
    std::vector< bool > const &rowscolsFixStatus,   
    T diagval                                       
) const
{
    // Since all diagonal is one therefore, at least O will be I.
    for ( unsigned int i = 0; i < I.size(); ++i ) {
        O[i].Clear();
    }
    unsigned int r = 0;
    std::list< SparseVector_Matrix3x3<T> >::const_iterator row = m_Rows.begin();
    while ( row != m_Rows.end() ) {
        if ( !rowscolsFixStatus[r] ) 
        {
            std::list< SparseVector_Matrix3x3_ElementData<T> >::const_iterator vec = row->RefToElements().begin();
            while ( vec != row->RefToElements().end() ) {
                if ( !rowscolsFixStatus[vec->idx] ) 
                {
                    O[r] += vec->M * I[vec->idx];
                }
                ++vec;
            }
        }
        else {
            O[r][0] = diagval * I[r][0];
            O[r][1] = diagval * I[r][1];
            O[r][2] = diagval * I[r][2];
        }
        ++r;
        ++row;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::ChangeNumOfRows ( unsigned int numOfRows )
{   
    m_Rows.resize( numOfRows );
}




//=============================================================================
// START of Diagonalizes WITH A SPECIFIED VALUE
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRow ( unsigned int row, T diagonalValue )
{   
    std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( row )->RefToElements().begin();
    while ( data != DirectAccess( row )->RefToElements().end() ) {
        if ( data->idx == row ) {
            data->M.MakeDiagonal( diagonalValue );
        }
        else {
            data->M.MakeZero();
        }
        ++data;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRows (
    std::vector< bool > const & rows,   
    T diagonalValue = 1                 
)
{   
    for ( unsigned int r = 0; r < rows.size(); ++r ) {
        if ( rows[r] ) {
            std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == r ) {
                    data->M.MakeDiagonal( diagonalValue );
                }
                else {
                    data->M.MakeZero();
                }
                ++data;
            }
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRows (
    std::vector< unsigned int > const & rows,   
    T diagonalValue = 1                         
)
{   
    for ( unsigned int r = 0; r < rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( rows[r] )->RefToElements().begin();
        while ( data != DirectAccess( rows[r] )->RefToElements().end() ) {
            if ( data->idx == rows[r] ) {
                data->M.MakeDiagonal( diagonalValue );
            }
            else {
                data->M.MakeZero();
            }
            ++data;
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeCol ( unsigned int col, T diagonalValue )
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        while ( data != DirectAccess( r )->RefToElements().end() ) {
            if ( data->idx == col ) {
                if ( data->idx == r ) {
                    data->M.MakeDiagonal( diagonalValue );
                }
                else {
                    data->M.MakeZero();
                }
            }
            ++data;
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeCols (
    std::vector< bool > const & cols,   
    T diagonalValue = 1                 
)
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        while ( data != DirectAccess( r )->RefToElements().end() ) {
            if ( cols[ data->idx ] ) {
                if ( data->idx == r ) {
                    data->M.MakeDiagonal( diagonalValue );
                }
                else {
                    data->M.MakeZero();
                }
            }
            ++data;
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRowCol ( unsigned int rowcol, T diagonalValue )
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        if ( rowcol == r ) {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == r ) {
                    data->M.MakeDiagonal( diagonalValue );
                }
                else {
                    data->M.MakeZero();
                }
                ++data;
            }
        }
        else {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == rowcol ) {
                    if ( data->idx == r ) {
                        data->M.MakeDiagonal( diagonalValue );
                    }
                    else {
                        data->M.MakeZero();
                    }
                }
                ++data;
            }
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRowsCols (
    std::vector< bool > const & rowscols,   
    T diagonalValue = 1                     
)
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        if ( rowscols[r] ) {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == r ) {
                    data->M.MakeDiagonal( diagonalValue );
                }
                else {
                    data->M.MakeZero();
                }
                ++data;
            }
        }
        else {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( rowscols[ data->idx ] ) {
                    if ( data->idx == r ) {
                        data->M.MakeDiagonal( diagonalValue );
                    }
                    else {
                        data->M.MakeZero();
                    }
                }
                ++data;
            }
        }
    }
}
//-----------------------------------------------------------------------------
// END of Diagonalizes WITH A SPECIFIED VALUE
//=============================================================================




//=============================================================================
// START of Diagonalizes
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRow ( unsigned int row )
{   
    std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( row )->RefToElements().begin();
    while ( data != DirectAccess( row )->RefToElements().end() ) {
        if ( data->idx == row ) {
            data->M[1] = data->M[2] = data->M[3] = 0;
            data->M[5] = data->M[6] = data->M[7] = 0;
        }
        else {
            data->M.MakeZero();
        }
        ++data;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRows (
    std::vector< bool > const & rows    
)
{   
    for ( unsigned int r = 0; r < rows.size(); ++r ) {
        if ( rows[r] ) {
            std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == r ) {
                    data->M[1] = data->M[2] = data->M[3] = 0;
                    data->M[5] = data->M[6] = data->M[7] = 0;
                }
                else {
                    data->M.MakeZero();
                }
                ++data;
            }
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRows (
    std::vector< unsigned int > const & rows    
)
{   
    for ( unsigned int r = 0; r < rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( rows[r] )->RefToElements().begin();
        while ( data != DirectAccess( rows[r] )->RefToElements().end() ) {
            if ( data->idx == rows[r] ) {
                data->M[1] = data->M[2] = data->M[3] = 0;
                data->M[5] = data->M[6] = data->M[7] = 0;
            }
            else {
                data->M.MakeZero();
            }
            ++data;
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeCol ( unsigned int col )
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        while ( data != DirectAccess( r )->RefToElements().end() ) {
            if ( data->idx == col ) {
                if ( data->idx == r ) {
                    data->M[1] = data->M[2] = data->M[3] = 0;
                    data->M[5] = data->M[6] = data->M[7] = 0;
                }
                else {
                    data->M.MakeZero();
                }
            }
            ++data;
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeCols (
    std::vector< bool > const & cols    
)
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        while ( data != DirectAccess( r )->RefToElements().end() ) {
            if ( cols[ data->idx ] ) {
                if ( data->idx == r ) {
                    data->M[1] = data->M[2] = data->M[3] = 0;
                    data->M[5] = data->M[6] = data->M[7] = 0;
                }
                else {
                    data->M.MakeZero();
                }
            }
            ++data;
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRowCol ( unsigned int rowcol )
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        if ( rowcol == r ) {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == r ) {
                    data->M[1] = data->M[2] = data->M[3] = 0;
                    data->M[5] = data->M[6] = data->M[7] = 0;
                }
                else {
                    data->M.MakeZero();
                }
                ++data;
            }
        }
        else {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == rowcol ) {
                    if ( data->idx == r ) {
                        data->M[1] = data->M[2] = data->M[3] = 0;
                        data->M[5] = data->M[6] = data->M[7] = 0;
                    }
                    else {
                        data->M.MakeZero();
                    }
                }
                ++data;
            }
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::DiagonalizeRowsCols (
    std::vector< bool > const & rowscols    
)
{   
    for ( unsigned int r = 0; r < m_Rows.size(); ++r ) {
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator data = DirectAccess( r )->RefToElements().begin();
        if ( rowscols[r] ) {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( data->idx == r ) {
                    data->M[1] = data->M[2] = data->M[3] = 0;
                    data->M[5] = data->M[6] = data->M[7] = 0;
                }
                else {
                    data->M.MakeZero();
                }
                ++data;
            }
        }
        else {
            while ( data != DirectAccess( r )->RefToElements().end() ) {
                if ( rowscols[ data->idx ] ) {
                    if ( data->idx == r ) {
                        data->M[1] = data->M[2] = data->M[3] = 0;
                        data->M[5] = data->M[6] = data->M[7] = 0;
                    }
                    else {
                        data->M.MakeZero();
                    }
                }
                ++data;
            }
        }
    }
}
//-----------------------------------------------------------------------------
// END of Diagonalizes WITH A SPECIFIED VALUE
//=============================================================================




//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_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 SparseMatrix_Matrix3x3<T>::DirectAccess ( unsigned int row )
{
    return m_DirectAccessors[row];
}
//-----------------------------------------------------------------------------
template <typename T>
void SparseMatrix_Matrix3x3<T>::MPCGSolver ( 
    SparseMatrix_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];
    }
    // 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 (from nonsym):\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:\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 (from nonsym):\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];
            // 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 << "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;
}
//-----------------------------------------------------------------------------
//=============================================================================

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