FixedMatrix.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLMath/src/ICLMath/FixedMatrix.h                      **
** Module : ICLMath                                                **
** Authors: Christof Elbrechter                                    **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.GPL  **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <iterator>
#include <algorithm>
#include <numeric>
#include <functional>
#include <iostream>
#include <vector>
#include <cmath>

#include <ICLUtils/Exception.h>
#include <ICLMath/DynMatrix.h>
#include <ICLUtils/ClippedCast.h>
#include <ICLMath/MatrixSubRectIterator.h>
#include <ICLUtils/FixedArray.h>

#ifdef HAVE_IPP
#include <ippm.h>
#endif

namespace icl{
  namespace math{
  
    struct FixedMatrixBase{
      template<class SrcIterator, class DstIterator, unsigned int N>
      static void optimized_copy(SrcIterator srcBegin, SrcIterator srcEnd, DstIterator dstBegin){
        if(N>30){
          std::copy(srcBegin,srcEnd,dstBegin);
        }else{
          while(srcBegin != srcEnd){
            //          *dstBegin++ = *srcBegin++;
            *dstBegin =
            *srcBegin;
  
            dstBegin++;
            srcBegin++;
          }
        }
      }
    };
      

    template<class T,unsigned int COLS,unsigned int ROWS> class FixedMatrix;


    template<class T,unsigned int N, class Iterator>
    class FixedMatrixPart : public FixedMatrixBase{
  
      public:
      Iterator begin;
      
      Iterator end;
    
      FixedMatrixPart(Iterator begin, Iterator end):begin(begin),end(end){}
      
      FixedMatrixPart& operator=(const T &value){
        std::fill(begin,end,value);
        return *this;
      }

      FixedMatrixPart& operator=(const FixedMatrixPart &other){
        FixedMatrixBase::optimized_copy<Iterator,Iterator,N>(other.begin,other.end,begin);
        return *this;
      }
      

      template<class OtherIterator, class OtherT>
      FixedMatrixPart& operator=(const FixedMatrixPart<OtherT,N,OtherIterator> &other){
        std::transform(other.begin,other.end,begin,utils::clipped_cast<OtherT,T>);
        return *this;
      }
      

      template<unsigned int COLS>
      FixedMatrixPart& operator=(const FixedMatrix<T,COLS,N/COLS> &m);
  

      template<class T2, unsigned int COLS>
      FixedMatrixPart& operator=(const FixedMatrix<T2,COLS,N/COLS> &m);
      
    };
  
  
  

    template<class T,unsigned int COLS,unsigned int ROWS>
    class FixedMatrix : public utils::FixedArray<T,COLS*ROWS>, public FixedMatrixBase{
      public:
      

      DynMatrix<T> dyn() { return DynMatrix<T>(COLS,ROWS,begin(),false); }
  
      const DynMatrix<T> dyn() const { return DynMatrix<T>(COLS,ROWS,const_cast<T*>(begin()),false); }
      
      static const FixedMatrix &null(){
        static FixedMatrix null_matrix(T(0));
        return null_matrix;
      }
  
      static const unsigned int DIM = COLS*ROWS;
      
      public:
      

      FixedMatrix(){}
      
      FixedMatrix(const T &initValue){
        std::fill(begin(),end(),initValue);
      }
  

      FixedMatrix(const T *srcdata){
        FixedMatrixBase::optimized_copy<const T*,T*,DIM>(srcdata,srcdata+dim(),begin());
        //std::copy(srcdata,srcdata+dim(),begin());
      }
  

      FixedMatrix(const T& v0,const T& v1,const T& v2=0,const T& v3=0,
                  const T& v4=0,const T& v5=0,const T& v6=0,const T& v7=0,  
                  const T& v8=0,const T& v9=0,const T& v10=0,const T& v11=0,  
                  const T& v12=0,const T& v13=0,const T& v14=0,const T& v15=0){
  #define C1(N) if(DIM>N) utils::FixedArray<T,COLS*ROWS>::m_data[N]=v##N
  #define C4(A,B,C,D) C1(A);C1(B);C1(C);C1(D)
        C4(0,1,2,3);C4(4,5,6,7);C4(8,9,10,11);C4(12,13,14,15);
  #undef C1
  #undef C2
      } 
  

      template<class OtherIterator>
      FixedMatrix(OtherIterator begin, OtherIterator end){
        FixedMatrixBase::optimized_copy<OtherIterator,T*,DIM>(begin,end,this->begin());
        //      std::copy(begin,end,begin());
      }
  
      // Explicit Copy template based constructor (deep copy)
      FixedMatrix(const FixedMatrix &other){
        FixedMatrixBase::optimized_copy<const T*,T*,DIM>(other.begin(),other.end(),begin());
        //std::copy(other.begin(),other.end(),begin());
      }
  
      // Explicit Copy template based constructor (deep copy)
      template<class otherT>
      FixedMatrix(const FixedMatrix<otherT,COLS,ROWS> &other){
        std::transform(other.begin(),other.end(),begin(),utils::clipped_cast<otherT,T>);
      }
  
      template<class Iterator>
      FixedMatrix (const FixedMatrixPart<T,DIM,Iterator> &r){ 
        FixedMatrixBase::optimized_copy<Iterator,T*,DIM>(r.begin,r.end,begin());
      }
  
      template<class otherT, class Iterator>
      FixedMatrix(const FixedMatrixPart<otherT,DIM,Iterator> &r){ 
        std::transform(r.begin,r.end,begin(),utils::clipped_cast<otherT,T>);
      }
      
      FixedMatrix &operator=(const FixedMatrix &other){
        if(this == &other) return *this;
        FixedMatrixBase::optimized_copy<const T*,T*,DIM>(other.begin(),other.end(),begin());
        //std::copy(other.begin(),other.end(),begin());
        return *this;
      }
      

      template<class otherT>
      FixedMatrix &operator=(const FixedMatrix<otherT,COLS,ROWS> &other){
        if(this == &other) return *this;
        std::transform(other.begin(),other.end(),begin(),utils::clipped_cast<otherT,T>);
        return *this;
      }
      
      FixedMatrix &operator=(const T &t){
        std::fill(begin(),end(),t);
        return *this;
      }
  
      template<class Iterator>
      FixedMatrix &operator=(const FixedMatrixPart<T,DIM,Iterator> &r){ 
        //      std::copy(r.begin,r.end,begin());
        FixedMatrixBase::optimized_copy<Iterator,T*,DIM>(r.begin,r.end,begin());
        return *this;
      }
  
      template<class otherT, class Iterator>
      FixedMatrix &operator=(const FixedMatrixPart<otherT,DIM,Iterator> &r){ 
        std::transform(r.begin,r.end,begin(),utils::clipped_cast<otherT,T>);
        return *this;
      }
  

      FixedMatrix operator/(const FixedMatrix &m) const 
        throw (IncompatibleMatrixDimensionException,
               InvalidMatrixDimensionException,
               SingularMatrixException){
        return this->operator*(m.inv());
      }
  
      FixedMatrix &operator/=(const FixedMatrix &m) const 
        throw (IncompatibleMatrixDimensionException,
               InvalidMatrixDimensionException,
               SingularMatrixException){
        return *this = this->operator*(m.inv());
      }
  
      FixedMatrix operator*(T f) const{
        FixedMatrix d;
        std::transform(begin(),end(),d.begin(),std::bind2nd(std::multiplies<T>(),f));
        return d;
      }
  
      FixedMatrix &operator*=(T f){
        std::transform(begin(),end(),begin(),std::bind2nd(std::multiplies<T>(),f));
        return *this;
      }
  
      FixedMatrix operator/(T f) const{
        FixedMatrix d;
        std::transform(begin(),end(),d.begin(),std::bind2nd(std::divides<T>(),f));
        return d;
      }
  
      FixedMatrix &operator/=(T f){
        std::transform(begin(),end(),begin(),std::bind2nd(std::divides<T>(),f));
        return *this;
      }
  
      FixedMatrix operator+(const T &t) const{
        FixedMatrix d;
        std::transform(begin(),end(),d.begin(),std::bind2nd(std::plus<T>(),t));
        return d;
      }
  
      FixedMatrix &operator+=(const T &t){
        std::transform(begin(),end(),begin(),std::bind2nd(std::plus<T>(),t));
        return *this;
      }
  
  
      FixedMatrix operator-(const T &t) const{
        FixedMatrix d;
        std::transform(begin(),end(),d.begin(),std::bind2nd(std::minus<T>(),t));
        return d;
      }
  
      FixedMatrix &operator-=(const T &t){
        std::transform(begin(),end(),begin(),std::bind2nd(std::minus<T>(),t));
        return *this;
      }
  
      FixedMatrix operator+(const FixedMatrix &m) const{
        FixedMatrix d;
        std::transform(begin(),end(),m.begin(),d.begin(),std::plus<T>());
        return d;
      }
  
      FixedMatrix &operator+=(const FixedMatrix &m){
        std::transform(begin(),end(),m.begin(),begin(),std::plus<T>());
        return *this;
      }
  
      FixedMatrix operator-(const FixedMatrix &m) const{
        FixedMatrix d;
        std::transform(begin(),end(),m.begin(),d.begin(),std::minus<T>());
        return d;
      }
      FixedMatrix &operator-=(const FixedMatrix &m){
        std::transform(begin(),end(),m.begin(),begin(),std::minus<T>());
        return *this;
      }
  

      FixedMatrix operator-() const {
        FixedMatrix cpy(*this);
        std::transform(cpy.begin(),cpy.end(),cpy.begin(),std::negate<T>());
        return cpy;
      }
  
  
      T &operator()(unsigned int col,unsigned int row){
        return begin()[col+cols()*row];
      }
  
      const T &operator() (unsigned int col,unsigned int row) const{
        return begin()[col+cols()*row];
      }
  
      T &at(unsigned int col,unsigned int row) throw (InvalidIndexException){
        if(col>=cols() || row >= rows()) throw InvalidIndexException("row or col index too large");
        return begin()[col+cols()*row];
      }
  
      const T &at(unsigned int col,unsigned int row) const throw (InvalidIndexException){
        return const_cast<FixedMatrix*>(this)->at(col,row);
      }
      

      T &operator[](unsigned int idx){
        return begin()[idx];
      }

      const T &operator[](unsigned int idx) const{
        return begin()[idx];
      }
      
  
      typedef T* iterator;
      
      typedef const T* const_iterator;
  
      typedef T* row_iterator;
  
      typedef const T* const_row_iterator;
    
      static unsigned int rows(){ return ROWS; }
    
      static unsigned int cols(){ return COLS; }
  
      T *data() { return utils::FixedArray<T,COLS*ROWS>::m_data; }
  
      const T *data() const { return utils::FixedArray<T,COLS*ROWS>::m_data; }
  
      static unsigned int dim() { return DIM; }
    
      struct col_iterator : public std::iterator<std::random_access_iterator_tag,T>{
  
        typedef unsigned int difference_type;
        
        mutable T *p;
        
        static const unsigned int STRIDE = COLS;
        
        col_iterator(T *col_begin):p(col_begin){}
        
        col_iterator &operator++(){
          p+=STRIDE;
          return *this;
        }
        const col_iterator &operator++() const{
          p+=STRIDE;
          return *this;
        }
        col_iterator operator++(int){
          col_iterator tmp = *this;
          ++(*this);
          return tmp;
        }
        const col_iterator operator++(int) const{
          col_iterator tmp = *this;
          ++(*this);
          return tmp;
        }
  
        col_iterator &operator--(){
          p-=STRIDE;
          return *this;
        }
  
        const col_iterator &operator--() const{
          p-=STRIDE;
          return *this;
        }
  
        col_iterator operator--(int){
          col_iterator tmp = *this;
          --(*this);
          return tmp;
        }
  
        const col_iterator operator--(int) const{
          col_iterator tmp = *this;
          --(*this);
          return tmp;
        }
  
        col_iterator &operator+=(difference_type n){
          p += n * STRIDE;
          return *this;
        }
  
        const col_iterator &operator+=(difference_type n) const{
          p += n * STRIDE;
          return *this;
        }
  
  
        col_iterator &operator-=(difference_type n){
          p -= n * STRIDE;
          return *this;
        }
  
        const col_iterator &operator-=(difference_type n) const{
          p -= n * STRIDE;
          return *this;
        }
  
  
        col_iterator operator+(difference_type n) {
          col_iterator tmp = *this;
          tmp+=n;
          return tmp;
        }
  
        const col_iterator operator+(difference_type n) const{
          col_iterator tmp = *this;
          tmp+=n;
          return tmp;
        }
  
        col_iterator operator-(difference_type n) {
          col_iterator tmp = *this;
          tmp-=n;
          return tmp;
        }
  
        const col_iterator operator-(difference_type n) const {
          col_iterator tmp = *this;
          tmp-=n;
          return tmp;
        }
  
        difference_type operator-(const col_iterator &i) const{
          return (p-i.p)/STRIDE;
        }
        
        T &operator*(){
          return *p;
        }
  
        const T &operator*() const{
          return *p;
        }
  
        bool operator==(const col_iterator &i) const{ return p == i.p; }
  
        bool operator!=(const col_iterator &i) const{ return p != i.p; }
  
        bool operator<(const col_iterator &i) const{ return p < i.p; }
  
        bool operator<=(const col_iterator &i) const{ return p <= i.p; }
  
        bool operator>=(const col_iterator &i) const{ return p >= i.p; }
  
        bool operator>(const col_iterator &i) const{ return p > i.p; }
  
      };
    
      // const column operator typedef
      typedef const col_iterator const_col_iterator;
  
      iterator begin() { return utils::FixedArray<T,COLS*ROWS>::m_data; }
  
      iterator end() { return utils::FixedArray<T,COLS*ROWS>::m_data+dim(); }
    
      const_iterator begin() const { return utils::FixedArray<T,COLS*ROWS>::m_data; }
  
      const_iterator end() const { return utils::FixedArray<T,COLS*ROWS>::m_data+dim(); }
    
      col_iterator col_begin(unsigned int col) { return col_iterator(begin()+col); }
  
      col_iterator col_end(unsigned int col) { return col_iterator(begin()+col+dim()); }
    
      const_col_iterator col_begin(unsigned int col) const { return col_iterator(const_cast<T*>(begin())+col); }
  
      const_col_iterator col_end(unsigned int col) const { return col_iterator(const_cast<T*>(begin())+col+dim()); }
  
      row_iterator row_begin(unsigned int row) { return begin()+row*cols(); }
  
      row_iterator row_end(unsigned int row) { return begin()+(row+1)*cols(); }
  
      const_row_iterator row_begin(unsigned int row) const { return begin()+row*cols(); }
  
      const_row_iterator row_end(unsigned int row) const { return begin()+(row+1)*cols(); }
  

      template<unsigned int MCOLS>
      void mult(const FixedMatrix<T,MCOLS,COLS> &m,  FixedMatrix<T,MCOLS,ROWS> &dst) const{
        for(unsigned int c=0;c<MCOLS;++c){
          for(unsigned int r=0;r<ROWS;++r){
            //          std::cout << "calling inner_product" << std::endl;
            dst(c,r) = std::inner_product(m.col_begin(c),m.col_end(c),row_begin(r),T(0));
          }
        }      
      }

      template<unsigned int MCOLS>
      FixedMatrix<T,MCOLS,ROWS> operator*(const FixedMatrix<T,MCOLS,COLS> &m) const{
        FixedMatrix<T,MCOLS,ROWS> d;
        mult(m,d);
        return d;
      }
   
  
  

      FixedMatrix inv() const throw (InvalidMatrixDimensionException,SingularMatrixException){
        return FixedMatrix(dyn().inv().data());
      }
      

      T det() const throw(InvalidMatrixDimensionException){
        return dyn().det();
      }
    
      FixedMatrix<T,ROWS,COLS> transp() const{
        FixedMatrix<T,ROWS,COLS> d;
        for(unsigned int i=0;i<cols();++i){
          FixedMatrixBase::optimized_copy<const_col_iterator, typename FixedMatrix<T,ROWS,COLS>::row_iterator,DIM>(col_begin(i),col_end(i),d.row_begin(i));
          //        std::copy(col_begin(i),col_end(i),d.row_begin(i));
        }
        return d;
      }
  

      template<unsigned int OTHER_COLS>
      T element_wise_inner_product(const FixedMatrix<T,OTHER_COLS,DIM/OTHER_COLS> &other) const {
        return std::inner_product(begin(),end(),other.begin(),T(0));
      }
  
  

      template<unsigned int OTHER_COLS>
      FixedMatrix<T,OTHER_COLS,COLS> dot(const FixedMatrix<T,OTHER_COLS,ROWS> &M) const{
        return this->transp() * M;
      }
      
      double cond(const double p=2) const {
        DynMatrix<T> mat = this->dyn();
        return mat.cond();
      }
  
      T trace() const{
        ICLASSERT_RETURN_VAL(COLS==ROWS,0);
        double accu = 0;
        for(int i=0;i<DIM;i+=COLS+1){
          accu += begin()[i];
        }
        return accu;
      }
  
      FixedMatrixPart<T,COLS,row_iterator> row(unsigned int idx){
        return FixedMatrixPart<T,COLS,row_iterator>(row_begin(idx),row_end(idx));
      }
  
      FixedMatrixPart<T,COLS,const_row_iterator> row(unsigned int idx) const{
        return FixedMatrixPart<T,COLS,const_row_iterator>(row_begin(idx),row_end(idx));
      }
  
      FixedMatrixPart<T,ROWS,col_iterator> col(unsigned int idx){
        return FixedMatrixPart<T,ROWS,col_iterator>(col_begin(idx),col_end(idx));
      }
  
      FixedMatrixPart<T,ROWS,const_col_iterator> col(unsigned int idx) const{
        return FixedMatrixPart<T,ROWS,const_col_iterator>(col_begin(idx),col_end(idx));
      }
  
      template<unsigned int X,unsigned int Y,unsigned int WIDTH,unsigned int HEIGHT>
      FixedMatrixPart<T,WIDTH*HEIGHT,MatrixSubRectIterator<T> > part(){
        return FixedMatrixPart<T,WIDTH*HEIGHT,MatrixSubRectIterator<T> >( 
                   MatrixSubRectIterator<T>(begin(),COLS,X,Y,WIDTH,HEIGHT),
                   MatrixSubRectIterator<T>::create_end_iterator(begin(),COLS,X,Y,WIDTH,HEIGHT));
      }
  
      template<unsigned int X,unsigned int Y,unsigned int WIDTH,unsigned int HEIGHT>
      const FixedMatrixPart<T,WIDTH*HEIGHT,MatrixSubRectIterator<T> > part() const{
        return const_cast<FixedMatrix<T,COLS,ROWS>*>(this)->part<X,Y,WIDTH,HEIGHT>();
      }
      

      template<unsigned int NEW_WIDTH,unsigned int NEW_HEIGHT>
      inline FixedMatrix<T,NEW_WIDTH,NEW_HEIGHT> resize(const T &init=T(0)) const {
        FixedMatrix<T,NEW_WIDTH,NEW_HEIGHT> M(init);
        for(unsigned int x=0;x<COLS && x < NEW_WIDTH; ++x){
          for(unsigned int y=0;y<ROWS && y < NEW_HEIGHT; ++y){
            M(x,y) = (*this)(x,y);
          }
        }
        return M;
      }
      

      static FixedMatrix<T,ROWS,COLS> id(){
        FixedMatrix<T,ROWS,COLS> m(T(0));
        for(unsigned int i=0;i<ROWS && i<COLS;++i){
          m(i,i) = 1;
        }
        return m;
      }
  
      inline double length(T norm=2) const{ 
        double sumSquares = 0;
        for(unsigned int i=0;i<DIM;++i){
          sumSquares += ::pow((*this)[i],(double)norm);
        }
        return ::pow( sumSquares, 1.0/norm);
      }
  
      inline void normalize(T norm=2){
        T l = (T)length(norm);
        if(l) (*this) /= l;
      }
      
      inline FixedMatrix<T,COLS,ROWS> normalized(T norm=2) const{
        T l = (T)length(norm);
        return l ?  (*this)/l : *this;
      }
  
      template<class otherT>
      bool operator==(const FixedMatrix<otherT,COLS,ROWS> &m) const{
        for(unsigned int i=0;i<DIM;++i){
          if(begin()[i] != m[i]) return false;
        }
        return true;
      }    
      template<class otherT>
      bool operator!=(const FixedMatrix<otherT,COLS,ROWS> &m) const{
        return !this->operator==(m);
      }   
  
  
      FixedMatrix<T,1,ROWS> diag() const throw (InvalidMatrixDimensionException){
        if(ROWS != COLS) throw InvalidMatrixDimensionException("trace is only possible for sqaure matrices");
        FixedMatrix<T,1,ROWS> t;
        for(unsigned int i=0;i<ROWS;++i){
          t[i] = (*this)(i,i);
        }
        return t;
      }
  

      void decompose_QR(FixedMatrix<T,COLS,ROWS> &Q, FixedMatrix<T,COLS,COLS> &R) const{
        DynMatrix<T> Qd = Q.dyn(), Rd = R.dyn();
        dyn().decompose_QR(Qd,Rd);
      }
      

      void decompose_RQ(FixedMatrix<T,ROWS,ROWS> &R, FixedMatrix<T,ROWS,ROWS> &Q) const{
        DynMatrix<T> Rd = R.dyn(), Qd = Q.dyn();
        dyn().decompose_RQ(Rd,Qd);
      }
      

      void svd(FixedMatrix<T,COLS,ROWS> &U, FixedMatrix<T,1,COLS> &s, FixedMatrix<T,COLS,COLS> &V) const{
        DynMatrix<T> Ud = U.dyn(), sd = s.dyn(), Vd = V.dyn();
        return dyn().svd(Ud,sd,Vd);
      }
      

      FixedMatrix<T,ROWS,COLS> pinv(bool useSVD=0, float zeroThreshold=0.00000000000000001) const {
        return FixedMatrix<T,ROWS,COLS>(dyn().pinv(useSVD,zeroThreshold).begin());
      }
      

      void eigen(FixedMatrix &eigenvectors, FixedMatrix<T,1,COLS> &eigenvalues) const{
        if(ROWS != COLS) throw InvalidMatrixDimensionException("eigenvalue decomposition is only possible for sqaure matrices (use svd instead!)");
        DynMatrix<T> evecs = eigenvectors.dyn(), evals = eigenvalues.dyn();
        return dyn().eigen(evecs,evals);
      }
      
      
    };
  

    template<class T,unsigned  int WIDTH,unsigned  int HEIGHT, unsigned int HEIGHT2>
    inline FixedMatrix<T,WIDTH,HEIGHT+HEIGHT2> operator%(const FixedMatrix<T,WIDTH,HEIGHT> &a,
                                                         const FixedMatrix<T,WIDTH,HEIGHT2> &b){
      FixedMatrix<T,WIDTH,HEIGHT+HEIGHT2> M;
      for(unsigned int i=0;i<HEIGHT;++i) M.row(i) = a.row(i);
      for(unsigned int i=0;i<HEIGHT2;++i) M.row(i+HEIGHT) = b.row(i);
      return M;
    }
  

    template<class T,unsigned  int WIDTH,unsigned  int HEIGHT, unsigned int WIDTH2>
    inline FixedMatrix<T,WIDTH+WIDTH2,HEIGHT> operator,(const FixedMatrix<T,WIDTH,HEIGHT> &a,
                                                         const FixedMatrix<T,WIDTH2,HEIGHT> &b){
      FixedMatrix<T,WIDTH+WIDTH2,HEIGHT> M;
      for(unsigned int i=0;i<WIDTH;++i) M.col(i) = a.col(i);
      for(unsigned int i=0;i<WIDTH2;++i) M.col(i+WIDTH) = b.col(i);
      return M;
    }
   
  
  
  

    template<class T, unsigned int M_ROWS_AND_COLS,unsigned int V_COLS>
    inline FixedMatrix<T,V_COLS,M_ROWS_AND_COLS> &operator*=(FixedMatrix<T,V_COLS,M_ROWS_AND_COLS> &v,
                                                             const FixedMatrix<T,M_ROWS_AND_COLS,M_ROWS_AND_COLS> &m){
      return v = (m*v);
    } 
  

    template<class T, unsigned int COLS, unsigned int ROWS>
    inline std::ostream &operator<<(std::ostream &s,const FixedMatrix<T,COLS,ROWS> &m){
      return s << m.dyn();
    }
  

    template<class T, unsigned int COLS, unsigned int ROWS>
    inline std::istream &operator>>(std::istream &s,FixedMatrix<T,COLS,ROWS> &m){
      DynMatrix<T> dyn = m.dyn();
      return s >> dyn;
    }
  
  
    template<class T>
    FixedMatrix<T,2,2> create_rot_2D(T angle);
  
    template<class T>
    FixedMatrix<T,3,3> create_hom_3x3(T angle, T dx=0, T dy=0, T v0=0, T v1=0);
  
    template<class T>
    inline FixedMatrix<T,3,3> create_hom_3x3_trans(T dx, T dy){
      FixedMatrix<T,3,3> m = FixedMatrix<T,3,3>::id();
      m(2,0)=dx;
      m(2,1)=dy;
      return m;
    }
    
  
    template<class T>
    FixedMatrix<T,3,3> create_rot_3D(T axisX, T axisY, T axisZ, T angle);
    
    template<class T>
    FixedMatrix<T,3,3> create_rot_3D(T rx,T ry,T rz);
  
    template<class T>
    FixedMatrix<T,4,4> create_hom_4x4(T rx, T ry, T rz, T dx=0, T dy=0, T dz=0, T v0=0, T v1=0, T v2=0);
  
    template<class T>
    FixedMatrix<T,4,4> create_rot_4x4(T axisX, T axisY, T axisZ, T angle);
  
  
    template<class T>
    inline FixedMatrix<T,4,4> create_hom_4x4_trans(T dx, T dy, T dz){
      FixedMatrix<T,4,4> m = FixedMatrix<T,4,4>::id();
      m(3,0)=dx;
      m(3,1)=dy;
      m(3,2)=dy;
      return m;
    }
  

    template<class T,unsigned int COLS, unsigned int ROWS>
    FixedMatrix<T,1,3> extract_euler_angles(const FixedMatrix<T,COLS,ROWS> &m) throw (InvalidMatrixDimensionException){
      ICLASSERT_THROW(COLS>2 && ROWS>2,InvalidMatrixDimensionException("extract_euler_angles needs a matrix that has at least 3 rows and columns"));
      if( m(1,2) > -0.999999 && m(1,2) < 0.999999){ //avoid Gimbal lock
        return FixedMatrix<T,1,3>(asin(m(1,2)),atan2(m(0,2),m(2,2)),atan2(m(1,0),m(1,1)));
      }else{
        return FixedMatrix<T,1,3>(-M_PI/2,0,-atan2(m(0,1),m(0,0)));
      }
    }
  
    
    template<class T,unsigned int N, class Iterator> template<unsigned int COLS>
    inline FixedMatrixPart<T,N,Iterator>& FixedMatrixPart<T,N,Iterator>::operator=(const FixedMatrix<T,COLS,N/COLS> &m){
      FixedMatrixBase::optimized_copy<const T*,Iterator,N>(m.begin(),m.end(),begin);
      //std::copy(m.begin(),m.end(),begin);
      return *this;
    }
    template<class T,unsigned int N, class Iterator> template<class T2, unsigned int COLS>
    inline FixedMatrixPart<T,N,Iterator>& FixedMatrixPart<T,N,Iterator>::operator=(const FixedMatrix<T2,COLS,N/COLS> &m){
      std::transform(m.begin(),m.end(),begin,utils::clipped_cast<T2,T>);
      return *this;
    }
  #ifdef HAVE_IPP
  #define OPTIMIZED_MATRIX_MULTIPLICATION(LEFT_COLS,LEFT_ROWS,RIGHT_COLS,TYPE,IPPSUFFIX) \
    template<> template<>                                                                \
    inline void                                                                          \
    FixedMatrix<TYPE,RIGHT_COLS,LEFT_ROWS>::mult                                         \
       (                                                                                 \
          const FixedMatrix<TYPE,RIGHT_COLS,LEFT_COLS> &m,                               \
          FixedMatrix<TYPE,RIGHT_COLS,LEFT_ROWS> &dst                                    \
       ) const {                                                                         \
      static const unsigned int ST=sizeof(TYPE);                                         \
      ippmMul_mm_##IPPSUFFIX(data(),LEFT_COLS*ST,ST,LEFT_COLS,LEFT_ROWS,                 \
                             m.data(),RIGHT_COLS*ST,ST,RIGHT_COLS,LEFT_COLS,             \
                             dst.data(),RIGHT_COLS*ST,ST);                               \
    }
  
    OPTIMIZED_MATRIX_MULTIPLICATION(2,2,2,float,32f);
    OPTIMIZED_MATRIX_MULTIPLICATION(3,3,3,float,32f);  
    OPTIMIZED_MATRIX_MULTIPLICATION(4,4,4,float,32f);
  
    OPTIMIZED_MATRIX_MULTIPLICATION(2,2,2,double,64f);
    OPTIMIZED_MATRIX_MULTIPLICATION(3,3,3,double,64f);  
    OPTIMIZED_MATRIX_MULTIPLICATION(4,4,4,double,64f);
  #undef OPTIMIZED_MATRIX_MULTIPLICATION
   
  
  #endif
  
  #define USE_OPTIMIZED_INV_AND_DET_FOR_2X2_3X3_AND_4X4_MATRICES
  
  #ifdef USE_OPTIMIZED_INV_AND_DET_FOR_2X2_3X3_AND_4X4_MATRICES
  
    // this functions are implemented in iclFixedMatrix.cpp. All templates are
    // instantiated for float and double
  
    template<class T> 
    void icl_util_get_fixed_4x4_matrix_inv(const T *src, T*dst);
    template<class T> 
    void icl_util_get_fixed_3x3_matrix_inv(const T *src, T*dst);
    template<class T> 
    void icl_util_get_fixed_2x2_matrix_inv(const T *src, T*dst);
  
    template<class T> 
    T icl_util_get_fixed_4x4_matrix_det(const T *src);
    template<class T> 
    T icl_util_get_fixed_3x3_matrix_det(const T *src);
    template<class T> 
    T icl_util_get_fixed_2x2_matrix_det(const T *src);
  
  #define SPECIALISED_MATRIX_INV_AND_DET(D,T) \
    template<>                                                            \
    inline FixedMatrix<T,D,D> FixedMatrix<T,D,D>::inv() const             \
    throw (InvalidMatrixDimensionException,SingularMatrixException){      \
      FixedMatrix<T,D,D> r;                                               \
      icl_util_get_fixed_##D##x##D##_matrix_inv<T>(begin(),r.begin());    \
      return r;                                                           \
    }                                                                     \
    template<>                                                            \
    inline T FixedMatrix<T,D,D>::det() const                              \
    throw(InvalidMatrixDimensionException){                               \
      return icl_util_get_fixed_##D##x##D##_matrix_det<T>(begin());       \
    }
  
  
    SPECIALISED_MATRIX_INV_AND_DET(2,float);
    SPECIALISED_MATRIX_INV_AND_DET(3,float);
    SPECIALISED_MATRIX_INV_AND_DET(4,float);
    SPECIALISED_MATRIX_INV_AND_DET(2,double);
    SPECIALISED_MATRIX_INV_AND_DET(3,double);
    SPECIALISED_MATRIX_INV_AND_DET(4,double);
  
  
  #undef SPECIALISED_MATRIX_INV_AND_DET
    
  #endif
  
  
  } // namespace math
}