libmatricxx/src/matrix.hxx

/*! @file

    @id $Id$
*/
//       1         2         3         4         5         6         7         8
// 45678901234567890123456789012345678901234567890123456789012345678901234567890

#include <iostream>
#include <cstring>
#include <cassert>
#include <type_traits>

template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
    
    //..............................................................constants
  public:

    const size_t ROWS;
    const size_t COLUMNS;
    const size_t SIZE;
    const size_t MEM_SIZE;

    //...............................................................typedefs
  public: 

    /// @name Auxiliary Classes
    ///{

    /// Return One Row as Vector, internally used for element access
    /** Only used to access values:

        @code
        Matrix<int,4,4> m;
        m[2][2] = 1;
        @endcode */
    class RowVector {
      public:
        TYPE& operator[](size_t column) {
          assert(column<_m.COLUMNS);
          return _v[column];
        }
      protected:
        friend class MatrixBase;
        RowVector() = delete; // forbidden
        RowVector(const MatrixBase& m, TYPE c[]): _m(m), _v(c) {}
        const MatrixBase& _m;
        TYPE *_v;
    };

    /// Same as RowVector, but in a constant environment.
    class ConstRowVector {
      public:
        const TYPE& operator[](size_t column) const {
          assert(column<_m.COLUMNS);
          return _v[column];
        }
      protected:
        friend class MatrixBase;
        ConstRowVector() = delete; // forbidden
        ConstRowVector(const MatrixBase& m, const TYPE c[]): _m(m), _v(c) {}
        const MatrixBase& _m;
        const TYPE *_v;
    };
    
    ///}

    //................................................................methods
  public:

    /// @name construction
    ///{
    
    MatrixBase(size_t rows, size_t columns):
        ROWS(rows), COLUMNS(columns),
        SIZE(ROWS*COLUMNS), MEM_SIZE(SIZE*sizeof(TYPE)) {
    }

    template<typename ...ARGS>
        MatrixBase(size_t rows, size_t columns, ARGS...t):
            ROWS(rows), COLUMNS(columns),
            SIZE(ROWS*COLUMNS), MEM_SIZE(SIZE*sizeof(TYPE)),
            _c{std::forward<TYPE>(t)...} {
    }

    ///}

    /// @name element access
    ///{
    
    TYPE& at(size_t row, size_t column) {
      assert(row<ROWS);
      assert(column<COLUMNS);
      return *((TYPE*)_c+row*COLUMNS+column);
    }
    
    const TYPE& at(size_t row, size_t column) const {
      assert(row<ROWS);
      assert(column<COLUMNS);
      return *((TYPE*)_c+row*COLUMNS+column);
    }
    
    TYPE& operator()(size_t row, size_t column) {
      return at(row, column);
    }
    
    const TYPE& operator()(size_t row, size_t column) const {
      return at(row, column);
    }
    
    RowVector operator[](size_t row) {
      assert(row<ROWS);
      return RowVector(*this, (TYPE*)_c+row*COLUMNS);
    }
    
    const ConstRowVector operator[](size_t row) const {
      assert(row<ROWS);
      return ConstRowVector(*this, (TYPE*)_c+row*COLUMNS);
    }
    
    ///}

    /// @name operators
    ///{
    
    MatrixBase& operator=(TYPE oc[]) {
      assert(sizeof(oc)==MEM_SIZE);
      memcpy(_c, oc, MEM_SIZE);
      return *this;
    }
    
    MatrixBase& operator=(const MatrixBase& o) {
      assert_check(o);
      memcpy(_c, o._c, MEM_SIZE);
      return *this;
    }

    bool operator==(const MatrixBase& o) const {
      if (!check(o)) return false;
      TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE);
      while (to>(TYPE*)(_c)) if (*--to != *--from) return false;
      return true;
    }

    bool operator!=(const MatrixBase& o) const {
      return !operator==(o);
    }

    MatrixBase& operator+=(const MatrixBase& o) {
      assert_check(o);
      TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE);
      while (to>(TYPE*)(_c)) *--to += *--from;
      return *this;
    }

    MatrixBase& operator-=(const MatrixBase& o) {
      assert_check(o);
      TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE);
      while (to>(TYPE*)(_c)) *--to -= *--from;
      return *this;
    }

    MatrixBase& operator*=(const TYPE& o) {
      TYPE *res((TYPE*)(_c)+SIZE);
      while (res>(TYPE*)(_c)) *--res *= o;
      return *this;
    }

    MatrixBase& operator/=(const TYPE& o) {
      TYPE *res((TYPE*)(_c)+SIZE);
      while (res>(TYPE*)(_c)) *--res /= o;
      return *this;
    }

    ///}

    /// @name special operations
    ///{

    TYPE det() {
      TYPE res(gauss());
      for (TYPE *p((TYPE*)(_c)+SIZE); --p>=(TYPE*)(_c); p-=COLUMNS) res *= *p;
      return res;
    }

    TYPE gauss() {
      /// calculate using gauss algorithmus
      /// @see http://www.mathebibel.de/determinante-berechnen-nach-gauss
      /// 1. normalize first line to first value
      TYPE lambda(at(0, 0));
      at(0, 0) = 1;
      for (TYPE *p((TYPE*)(_c)+COLUMNS); p>(TYPE*)(_c)+1;) *--p/=lambda;
      /// 2. nullify lower triangle
      for (size_t column(0); column<COLUMNS-1; ++column) {
        for (size_t row(column+1); row<ROWS; ++row) {
          TYPE pivot(at(row, column));
          if (pivot!=0) {
            at(row, column) = 0;
            for (size_t pos(column+1); pos<COLUMNS; ++pos)
              at(row, pos) -= pivot*at(0, pos);
          }
        }
      }
      return lambda;
    }

    ///}
    
    //................................................................methods
  protected:

    virtual void assert_check(const MatrixBase& o) const {}
    virtual bool check(const MatrixBase& o) const {
      return true;
    }

    //..............................................................variables
  protected:
    
    ARRAY _c;

};

//==============================================================================

template<typename TYPE, size_t TROWS=0, size_t TCOLUMNS=0> class Matrix:
  public MatrixBase<TYPE, TYPE[TROWS][TCOLUMNS]> {

    //...............................................................typedefs
  private:

    typedef MatrixBase<TYPE, TYPE[TROWS][TCOLUMNS]> Parent;

    //................................................................methods
  public:

    /// @name construction
    ///{
    
    Matrix(): Parent(TROWS, TCOLUMNS) {
      memset(Parent::_c, 0, Parent::MEM_SIZE);
    }

    Matrix(const Matrix& o): Matrix() {
      memcpy(Parent::_c, o._c, Parent::MEM_SIZE);
    }

    template<typename ...ARGS>
        Matrix(ARGS...t): Parent(TROWS, TCOLUMNS, t...) {
      static_assert(sizeof...(t)==TROWS*TCOLUMNS, "wrong array size");
    }

    ///}

    /// @name operators
    ///{
    
    Matrix& operator=(TYPE oc[TROWS][TCOLUMNS]) {
      memcpy(Parent::_c, oc, Parent::MEM_SIZE);
      return *this;
    }
    
    Matrix operator-() const {
      Matrix res;
      for (TYPE *to((TYPE*)(res._c)+Parent::SIZE); to>(TYPE*)(Parent::_c); --to)
        *to = -*to;
      return res;
    }

    template<size_t NEWCOLUMNS>
        Matrix<TYPE, TROWS, NEWCOLUMNS>
        operator*(const Matrix<TYPE, TCOLUMNS, NEWCOLUMNS>& o) const {
      Matrix<TYPE, TROWS, NEWCOLUMNS> res;
      for (size_t i(0); i<TROWS; ++i)
        for (size_t k(0); k<NEWCOLUMNS; ++k)
          for (size_t j(0); j<TCOLUMNS; ++j)
            res(i, k) += this->at(i, j) * o(j, k);
      return res;
    }

    ///}
                    
    /// @name special operations
    ///{

    Matrix<TYPE, TCOLUMNS, TROWS> t() const {
      Matrix<TYPE, TCOLUMNS, TROWS> res;
      for (size_t row(0); row<TROWS; ++row)
        for (size_t column(0); column<TCOLUMNS; ++column)
          res(column, row) = this->at(row, column);
      return res;
    }

    static Matrix i() {
      Matrix res;
      for (size_t row(0); row<TROWS&&row<TCOLUMNS; ++row) res(row, row) = 1;
      return res;
    }

    Matrix& inv() {
      /// calculate using gauss-jordan algorithmus
      /// @see http://www.mathebibel.de/inverse-matrix-berechnen-nach-gauss-jordan
      Matrix o(*this); // left side
      *this = i(); // right side
      /// 1. lower left part
      for (size_t row(0); row<this->ROWS; ++row)
        if (rows<this->COLUMNS) {
          /// 2. normalize first line to first value
          TYPE pivot(o(row, row));
          if (pivot!=1) {
            o(row, row) = 1;
            for (size_t column(row+1); column<this->COLUMNS; ++column) o(row, column)/=pivot;
            for (size_t column(row); column<this->COLUMNS; ++column) this->at(row, column)/=pivot;
          }
          /// 3. nullify lower triangle
          for (size_t row(column+1); row<this->ROWS; ++row) {
            TYPE pivot(o(row, column));
            if (pivot!=0) {
              o(row, column) = 0;
              for (size_t pos(column+1); pos<this->COLUMNS; ++pos) o(row, pos) -= pivot*o(0, pos);
              for (size_t pos(column); pos<this->COLUMNS; ++pos) this->at(row, pos) -= pivot*this->at(0, pos);
            }
          }
        }
      /// 4. nullify the upper triangle
      
      return *this;
    }

    ///}
};

//==============================================================================

template<typename TYPE> class Matrix<TYPE, 0, 0>: public MatrixBase<TYPE> {

    //...............................................................typedefs
  private:

    typedef MatrixBase<TYPE> Parent;

    //................................................................methods
  public:
    
    /// @name construction
    ///{

    Matrix() = delete;
    
    Matrix(size_t rows, size_t columns):
        Parent(rows, columns) {
      assert(rows>0);
      assert(columns>0);
      Parent::_c = new TYPE[rows*columns];
      memset(Parent::_c, 0, Parent::MEM_SIZE);
    }

    Matrix(const Matrix& o): Matrix(o.ROWS, o.Parent::COLUMNS) {
      memcpy(Parent::_c, o.Parent::_c, Parent::MEM_SIZE);
    }
    
    template<typename ...ARGS>
        Matrix(size_t rows, size_t columns, ARGS...t):
            Matrix(rows, columns) {
      assert(sizeof...(t)==Parent::SIZE);
      copy_args(Parent::_c, t...);
    }

    ///}

    /// @name destruction
    ///{

    virtual ~Matrix() {
      delete[] Parent::_c;
    }
    
    ///}
    
    /// @name operators
    ///{

    Matrix& operator=(const Matrix& o) {
      if (o.ROWS!=Parent::ROWS&&o.COLUMNS!=Parent::COLUMNS) {
        delete[] Parent::_c;
        Parent::ROWS = o.ROWS;
        Parent::COLUMNS = o.COLUMNS;
        Parent::SIZE = o.SIZE;
        Parent::MEM_SIZE = o.MEM_SIZE;
        Parent::_c = new TYPE[Parent::SIZE];
      }
      return Parent::operator=(o);
    }

    Matrix operator-() const {
      Matrix res(Parent::COLUMNS, Parent::ROWS);
      for (TYPE *to((TYPE*)(res._c)+Parent::SIZE); to>(TYPE*)(Parent::_c); --to)
        *to = -*to;
      return res;
    }

    Matrix operator*(const Matrix& o) const {
      Matrix<TYPE> res(this->ROWS, o.COLUMNS);
      assert(this->COLUMNS==o.ROWS);
      for (size_t i(0); i<this->ROWS; ++i)
        for (size_t k(0); k<o.COLUMNS; ++k)
          for (size_t j(0); j<this->COLUMNS; ++j)
            res(i, k) += this->at(i, j) * o(j, k);
      return res;
    }

    ///}

    ///@name special operations
    ///{
    
    Matrix t() const {
      Matrix res(this->COLUMNS, this->ROWS);
      for (size_t row(0); row<this->ROWS; ++row)
        for (size_t column(0); column<this->COLUMNS; ++column)
          res(column, row) = this->at(row, column);
      return res;
    }

    Matrix i() const {
      Matrix res(this->ROWS, this->COLUMNS);
      for (size_t row(0); row<this->ROWS&&row<this->COLUMNS; ++row)
        res(row, row) = 1;
      return res;
    }

    ///}

    //................................................................methods
  protected:

    virtual void assert_check(const Matrix& o) const {
      assert(o.ROWS==this->ROWS);
      assert(o.COLUMNS==this->COLUMNS);
    }

    virtual bool check(const Matrix& o) const {
      return o.ROWS==this->ROWS && o.COLUMNS==this->COLUMNS;
    }

    void copy_args(TYPE*) {}
    template<typename ...ARGS>
        void copy_args(TYPE* to, TYPE t1, ARGS...t) {
      *to = t1;
      copy_args(++to, t...);
    }

};

//==============================================================================

template<typename TYPE, size_t ROWS, size_t COLUMNS>
    Matrix<TYPE, ROWS, COLUMNS> operator+(const Matrix<TYPE, ROWS, COLUMNS>& a,
                                          const Matrix<TYPE, ROWS, COLUMNS>& b) {
  Matrix<TYPE, ROWS, COLUMNS> res(a);
  res += b;
   return res;
}

template<typename TYPE, size_t ROWS, size_t COLUMNS>
    Matrix<TYPE, ROWS, COLUMNS> operator-(const Matrix<TYPE, ROWS, COLUMNS>& a,
                                          const Matrix<TYPE, ROWS, COLUMNS>& b) {
  Matrix<TYPE, ROWS, COLUMNS> res(a);
  res -= b;
   return res;
}

template<typename TYPE, size_t ROWS, size_t COLUMNS>
    Matrix<TYPE, ROWS, COLUMNS> operator*(const TYPE& v,
                                          const Matrix<TYPE, ROWS, COLUMNS>& m) {
  Matrix<TYPE, ROWS, COLUMNS> res(m);
  res *= v;
   return res;
}

template<typename TYPE, size_t ROWS, size_t COLUMNS>
    Matrix<TYPE, ROWS, COLUMNS> operator*(const Matrix<TYPE, ROWS, COLUMNS>& m,
                                          const TYPE& v) {
  Matrix<TYPE, ROWS, COLUMNS> res(m);
  res *= v;
   return res;
}

template<typename TYPE, size_t ROWS, size_t COLUMNS>
    std::ostream& operator<<(std::ostream& s, const Matrix<TYPE, ROWS, COLUMNS>& m) {
  for (size_t w = 0; w < m.ROWS; ++w) {
    for (size_t h = 0; h < m.COLUMNS;++h) {
      s<<m[w][h]<<' ';
    }
    s<<'\n';
  }
}