source/linal/squarematrix.h

   1 #ifndef MSP_LINAL_SQUAREMATRIX_H_
   2 #define MSP_LINAL_SQUAREMATRIX_H_
   3
   4 #include <stdexcept>
   5 #include "matrix.h"
   6
   7 namespace Msp {
   8 namespace LinAl {
   9
  10 class not_invertible: public std::domain_error
  11 {
  12 public:
  13         not_invertible(): domain_error(std::string()) { }
  14         virtual ~not_invertible() throw() { }
  15 };
  16
  17 template<typename T, unsigned S>
  18 class SquareMatrix: public Matrix<T, S, S>
  19 {
  20 public:
  21         SquareMatrix() { }
  22         SquareMatrix(const T *d): Matrix<T, S, S>(d) { }
  23         template<typename U>
  24         SquareMatrix(const Matrix<U, S, S> &m): Matrix<T, S, S>(m) { }
  25
  26         static SquareMatrix identity();
  27
  28         SquareMatrix &operator*=(const SquareMatrix &);
  29
  30         SquareMatrix &invert();
  31 };
  32
  33 template<typename T, unsigned S>
  34 inline SquareMatrix<T, S> SquareMatrix<T, S>::identity()
  35 {
  36         SquareMatrix<T, S> m;
  37         for(unsigned i=0; i<S; ++i)
  38                 m(i, i) = T(1);
  39         return m;
  40 }
  41
  42 template<typename T, unsigned S>
  43 SquareMatrix<T, S> &SquareMatrix<T, S>::operator*=(const SquareMatrix<T, S> &m)
  44 {
  45         Matrix<T, S, S>::operator*=(m);
  46         return *this;
  47 }
  48
  49 template<typename T, unsigned S>
  50 SquareMatrix<T, S> &SquareMatrix<T, S>::invert()
  51 {
  52         SquareMatrix<T, S> r = identity();
  53         for(unsigned i=0; i<S; ++i)
  54         {
  55                 if(this->element(i, i)==T(0))
  56                 {
  57                         unsigned pivot = i;
  58                         for(unsigned j=i+1; j<S; ++j)
  59                                 if(abs(this->element(j, i))>abs(this->element(pivot, i)))
  60                                         pivot = j;
  61
  62                         if(pivot==i)
  63                                 throw not_invertible();
  64
  65                         this->exchange_rows(i, pivot);
  66                         r.exchange_rows(i, pivot);
  67                 }
  68
  69                 for(unsigned j=i+1; j<S; ++j)
  70                 {
  71                         T a = -this->element(j, i)/this->element(i, i);
  72                         this->add_row(i, j, a);
  73                         r.add_row(i, j, a);
  74                 }
  75
  76                 T a = T(1)/this->element(i, i);
  77                 this->multiply_row(i, a);
  78                 r.multiply_row(i, a);
  79         }
  80
  81         for(unsigned i=S; i-->0; )
  82                 for(unsigned j=i; j-->0; )
  83                         r.add_row(i, j, -this->element(j, i));
  84
  85         return *this = r;
  86 }
  87
  88 template<typename T, unsigned S>
  89 inline SquareMatrix<T, S> invert(const SquareMatrix<T, S> &m)
  90 {
  91         SquareMatrix<T, S> r = m;
  92         return r.invert();
  93 }
  94
  95 } // namespace LinAl
  96 } // namespace Msp
  97
  98 #endif