matrix.h

/*************************************************************************************
 * MechSys - A C++ library to simulate (Continuum) Mechanical Systems                *
 * Copyright (C) 2005 Dorival de Moraes Pedroso <dorival.pedroso at gmail.com>       *
 * Copyright (C) 2005 Raul Dario Durand Farfan  <raul.durand at gmail.com>           *
 *                                                                                   *
 * This file is part of MechSys.                                                     *
 *                                                                                   *
 * MechSys is free software; you can redistribute it and/or modify it under the      *
 * terms of the GNU General Public License as published by the Free Software         *
 * Foundation; either version 2 of the License, or (at your option) any later        *
 * version.                                                                          *
 *                                                                                   *
 * MechSys is distributed in the hope that it will be useful, but WITHOUT ANY        *
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A   *
 * PARTICULAR PURPOSE. See the GNU General Public License for more details.          *
 *                                                                                   *
 * You should have received a copy of the GNU General Public License along with      *
 * MechSys; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, *
 * Fifth Floor, Boston, MA 02110-1301, USA                                           *
 *************************************************************************************/

// -- IMPORTANT --
// OBS.:
//   Internally, the values are stored as Col-Major, thus a pointer
//   can be passed to FORTRAN routines very easily.

#ifndef MECHSYS_LINALG_MATRIX_H
#define MECHSYS_LINALG_MATRIX_H

#ifndef ZERO
  #define ZERO 1.0e-10
#endif

#include <sstream>
#include <cassert>
#include <cmath>

#include "util/numstreams.h"

using Util::_8s;

namespace LinAlg
{

// Prototype for expression classes
template <class t_exp, class t_res>
class expression;

template<typename Type>
class Matrix
{
public:
    // Default constructor
    Matrix() : _rows(0), _cols(0), _values(NULL) {}
    // Alternative Constructor
    Matrix(int Rows, int Cols)                              : _values(NULL) { _set(Rows,Cols); }
    Matrix(int Rows, int Cols, std::string const & strVals) : _values(NULL) { _set(Rows,Cols,strVals); }
    Matrix(int N, std::string const & strDiagonal, std::string const & strUpperTriang) : _values(NULL) { _set_sym(N,strDiagonal,strUpperTriang); }
    // Destructor
    ~Matrix()
    {
        if (_values!=NULL) delete [] _values;
    }
    // Copy constructor
    Matrix(Matrix<Type> const & Other)
    {
          _rows = Other.Rows();
          _cols = Other.Cols();
        _values = new Type [_rows*_cols];
        for (int i=0; i<_rows*_cols; ++i)
            _values[i] = Other._values[i];
    }
    // Get
           int   Rows()   const { assert(_values!=NULL); return _rows;   }
           int   Cols()   const { assert(_values!=NULL); return _cols;   }
          Type * GetPtr()       { assert(_values!=NULL); return _values; }
    const Type * GetPtr() const { assert(_values!=NULL); return _values; }
    Type         Det()    const;
    void         Inv(Matrix<Type> & inv) const;
    void         Trn(Matrix<Type> & trn) const;
    // Set
    void Set(int Rows, int Cols)                              { _set(Rows,Cols); }
    void Set(int Rows, int Cols, std::string const & strVals) { _set(Rows,Cols,strVals); }
    void Set(int N, std::string const & strDiagonal, std::string const & strUpperTriang) { _set_sym(N,strDiagonal,strUpperTriang); }
    void SetValues(Type Value) // {{{
    {
        assert(_values!=NULL);
        for (int i=0; i<_rows*_cols; ++i)
            _values[i] = Value; 
    } // }}}
    void Reset(std::string const & strVals) // {{{
    {
        std::istringstream iss(strVals);
        for (int i=0; i<_rows; ++i)
        for (int j=0; j<_cols; ++j)
            iss >> _values[j*_rows+i]; // Col major
    } // }}}
    void Resize(int Rows, int Cols) { _resize(Rows,Cols); }
    // operators
    void operator= (const Matrix<Type> & R) // {{{
    {
        assert(&R!=this);
        if (R.Rows() != _rows || R.Cols() != _cols)
        {
            _rows = R.Rows();
            _cols = R.Cols();
            if (_values != NULL) delete [] _values;
            _values = new Type [_rows*_cols];
        }
        
        int _components=_rows*_cols;
        for (int i=0; i<_components; ++i)
            _values[i] = R._values[i];
    } // }}}

    // operator to assign values separated by commas {{{
    class CommaAssign
    {
    public:
        CommaAssign(Type * Values, int & Rows, int & Cols, Type const & FirstValue):_values(Values),_rows(Rows),_cols(Cols),_index(0) 
        { 
            assert(_values!=NULL);
            _values[0] = FirstValue;
            int _components = _rows*_cols;
            for (int i=1; i<_components; ++i)
                _values[i] = static_cast<Type>(0);
        }
        CommaAssign & operator, (Type const & Num) 
        {
            _index++;
            assert(_index < _rows*_cols);
            _values[_index/_cols + (_index % _cols) * _rows] = Num; // considering col major
            return *this;
        }
    private:
        Type * _values;
        int  & _rows;
        int  & _cols;
        int    _index;
    };

    CommaAssign operator= (Type const & Num) 
    {
        return CommaAssign(_values, _rows, _cols, Num);
    } // }}}

    void operator+= (const Matrix<Type> & R) // {{{
    {
        assert(_values!=NULL);
        assert(R.Rows()==_rows);
        assert(R.Cols()==_cols);
        int _components=_rows*_cols;
        for (int i=0; i<_components; ++i)
            _values[i] += R._values[i];
    } // }}}

    void operator-= (const Matrix<Type> & R) // {{{
    {
        assert(_values!=NULL);
        assert(R.Rows()==_rows);
        assert(R.Cols()==_cols);
        int _components=_rows*_cols;
        for (int i=0; i<_components; ++i)
            _values[i] -= R._values[i];
    } // }}}
    
    // Suport for expression evaluation {{{
    template<typename t_exp>
    void operator  = (const expression<t_exp, Matrix<Type> > & Exp) { Exp.Apply(*this); } 

    template<typename t_exp>
    void operator += (const expression<t_exp, Matrix<Type> > & Exp) { Exp.Apply_pe(*this); } 

    template<typename t_exp>
    void operator -= (const expression<t_exp, Matrix<Type> > & Exp) { Exp.Apply_me(*this); } // }}}

    Type & operator() (int i, int j) // {{{
    {
        assert(_values!=NULL);
        assert(i>=0 & i<_rows);
        assert(j>=0 & j<_cols);
        return _values[j*_rows+i]; // Col major
    } // }}}
    const Type & operator() (int i, int j) const // {{{
    {
        assert(_values!=NULL);
        assert(i>=0 & i<_rows);
        assert(j>=0 & j<_cols);
        return _values[j*_rows+i]; // Col major
    } // }}}
private:
    int    _rows;
    int    _cols;
    Type * _values;
    void _set(int Rows, int Cols) // {{{
    {
        assert(_values==NULL);
        assert(Rows>0);
        assert(Cols>0);
          _rows = Rows;
          _cols = Cols;
        _values = new Type [_rows*_cols];
        for (int i=0; i<_rows*_cols; ++i)
            _values[i] = static_cast<Type>(0);
    } // }}}
    void _set(int Rows, int Cols, std::string const & strVals) // {{{
    {
        assert(_values==NULL);
        assert(Rows>0);
        assert(Cols>0);
          _rows = Rows;
          _cols = Cols;
        _values = new Type [_rows*_cols];
        std::istringstream iss(strVals);
        for (int i=0; i<_rows; ++i)
        for (int j=0; j<_cols; ++j)
            iss >> _values[j*_rows+i]; // Col major
    } // }}}
    void _resize(int Rows, int Cols) // {{{
    {
        assert(Rows>0);
        assert(Cols>0);
        if (Rows==_rows && Cols==_cols && _values!=NULL) return;    
        if (_values!=NULL) delete [] _values;
        _rows = Rows;
        _cols = Cols;
        _values = new Type [_rows*_cols];
    } // }}}
    void _set_sym(int N, std::string const & strDiagonal, std::string const & strUpperTriang) // {{{
    {
        assert(N>0);
          _rows = N;
          _cols = N;
        _values = new Type [_rows*_cols];
        std::stringstream iss_D  (strDiagonal);
        std::stringstream iss_UT (strUpperTriang);
        // diagonal
        for (int i=0; i<N; ++i)
            iss_D >> _values[i*_rows+i]; // Col major
        // upper triangle (and lower triangle)
        for (int i=0; i<N-1; ++i)
        for (int j=i+1; j<N; ++j)
        {
            iss_UT >> _values[j*_rows+i]; // Col major
            _values[i*_rows+j] = _values[j*_rows+i];
        } // }}}
    }

}; // class Matrix<Type>

template<typename Type>
std::ostream & operator<< (std::ostream & os, const Matrix<Type> & M) // {{{
{
    os << std:: endl;
    for (int i=0; i<M.Rows(); ++i)
    {
        for (int j=0; j<M.Cols(); ++j)
            os << _8s()<< M(i,j);
        os << std::endl;
    }
    return os;
} // }}}

// determinant of a matrix
template<typename Type>
inline Type Matrix<Type>::Det() const // {{{
{
    Matrix<Type> const & M = (*this);
    Type _det;
    if (_rows==1)
    {
        _det = 0;
        for (int i=0; i<_cols; i++) _det += pow(M(0,i),2);
        _det = pow(_det, 0.5);
    } 
    else if (_rows==3 && _cols==3)
    {
        _det =   M(0,0)*(M(1,1)*M(2,2) - M(1,2)*M(2,1)) \
               - M(0,1)*(M(1,0)*M(2,2) - M(1,2)*M(2,0)) \
               + M(0,2)*(M(1,0)*M(2,1) - M(1,1)*M(2,0));
    }
    else if (_rows==2 && _cols==3)
    {
        Type d1 = M(0,0)*M(1,1) - M(0,1)*M(1,0);
        Type d2 = M(0,1)*M(1,2) - M(0,2)*M(1,1);
        Type d3 = M(0,2)*M(1,0) - M(0,0)*M(1,2);
        _det = sqrt(d1*d1 + d2*d2 + d3*d3);
    }
    else
    {
        std::ostringstream oss;
        oss << "Matrix::Det: Determinant for a (" << _rows << " x " << _cols << ") matrix is not available\n"; // TODO: ??? << check error
        throw oss.str().c_str();
    }
    return _det;
} // }}}

template<typename Type>
inline void Matrix<Type>::Inv(Matrix<Type> & inv) const // {{{
{
    inv.Resize(_rows, _cols);
    Matrix<Type> const & M = (*this);
    if (_rows==3 && _cols==3) // && _rows()==3 && Inv.Cols()==3)
    {
        inv.Resize(3,3);
        Type det = Det();
        if (fabs(det)<ZERO)
            throw "Matrix::Inv: Could not calculate inverse because determinant is equal to zero\n";
        
        inv(0,0) = (M(1,1)*M(2,2) - M(1,2)*M(2,1)) / det;
        inv(0,1) = (M(0,2)*M(2,1) - M(0,1)*M(2,2)) / det;
        inv(0,2) = (M(0,1)*M(1,2) - M(0,2)*M(1,1)) / det;
        
        inv(1,0) = (M(1,2)*M(2,0) - M(1,0)*M(2,2)) / det;
        inv(1,1) = (M(0,0)*M(2,2) - M(0,2)*M(2,0)) / det;
        inv(1,2) = (M(0,2)*M(1,0) - M(0,0)*M(1,2)) / det;
        
        inv(2,0) = (M(1,0)*M(2,1) - M(1,1)*M(2,0)) / det;
        inv(2,1) = (M(0,1)*M(2,0) - M(0,0)*M(2,1)) / det;
        inv(2,2) = (M(0,0)*M(1,1) - M(0,1)*M(1,0)) / det;
    }
    else
    {
        inv.Resize(_rows,_cols);
        Matrix<Type> extended(_rows, _cols*2);

        //extending Matrix
        for (int r = 0; r < _rows; r++) 
            for (int c = 0; c < _cols; c++) 
            {
                extended(r,c) = this->operator()(r,c);
                if (r == c) extended(r,c+_rows) = 1;
                else extended(r,c+_rows) = 0;
            }

        for (int p = 0; p < _cols; p++) //triangulation
        { 
            for (int r = p + 1; r < _rows; r++)
            {
                //pivoting
                if (fabs(extended(p,p))<1.e-10)
                { 
                    //lookin for a line with non zero value in pivot column
                    int rr;
                    for (rr = p; rr < _rows; rr++) if (fabs(extended(rr,p)) >= 1.e-10) break; 
                    if (rr==_rows) throw "Matrix::Inv: Error calculating inverse matrix): singular matrix";
                    for (int c = p; c < _cols*2; c++){
                        Type temp = extended(p,c);
                        extended(p,c) = extended(rr,c); 
                        extended(rr,c) = temp;
                    }
                }
                Type coef = extended(r,p) / extended(p,p);
                for (int c = p; c < _cols*2; c++)
                    extended(r,c) -= coef * extended(p,c);
            }
        }

        //Retro-Triangulation
        for (int p = _rows-1; p > 0; p--)
        { 
            for (int r = p-1; r >= 0; r--)
            {
                Type coef = extended(r,p) / extended(p,p);
                for (int c = _cols*2-1; c >= p; c--)  
                    extended(r,c) -= coef * extended(p,c);
            }
        }
        
        //copying inverse Matrix and dividing by diagonal value
        for (int r = 0; r < _rows; r++)
            for (int c = 0; c < _cols; c++)
                inv(r,c) = extended(r,c+_rows) / extended(r,r); 
        
    }
} // }}}

template<typename Type>
inline void Matrix<Type>::Trn(Matrix<Type> & trn) const // {{{
{
    trn.Resize(_cols, _rows);
    Matrix<Type> const & M = (*this);
    for (int r = 0; r < _rows; r++) 
        for (int c = 0; c < _cols; c++) 
            trn(c,r) = M(r,c);
    
} // }}}

}; // namespace LinAlg

#endif // MECHSYS_LINALG_MATRIX_H

// vim:fdm=marker

---