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         return *this = *this*m;
  46 }
  47
  48 template<typename T, unsigned S>
  49 SquareMatrix<T, S> &SquareMatrix<T, S>::invert()
  50 {
  51         SquareMatrix<T, S> r = identity();
  52         for(unsigned i=0; i<S; ++i)
  53         {
  54                 if(this->element(i, i)==T(0))
  55                 {
  56                         unsigned pivot = i;
  57                         for(unsigned j=i+1; j<S; ++j)
  58                                 if(abs(this->element(j, i))>abs(this->element(pivot, i)))
  59                                         pivot = j;
  60
  61                         if(pivot==i)
  62                                 throw not_invertible();
  63
  64                         this->exchange_rows(i, pivot);
  65                         r.exchange_rows(i, pivot);
  66                 }
  67
  68                 for(unsigned j=i+1; j<S; ++j)
  69                 {
  70                         T a = -this->element(j, i)/this->element(i, i);
  71                         this->add_row(i, j, a);
  72                         r.add_row(i, j, a);
  73                 }
  74
  75                 T a = T(1)/this->element(i, i);
  76                 this->multiply_row(i, a);
  77                 r.multiply_row(i, a);
  78         }
  79
  80         for(unsigned i=S; i-->0; )
  81                 for(unsigned j=i; j-->0; )
  82                         r.add_row(i, j, -this->element(j, i));
  83
  84         return *this = r;
  85 }
  86
  87 template<typename T, unsigned S>
  88 inline SquareMatrix<T, S> invert(const SquareMatrix<T, S> &m)
  89 {
  90         SquareMatrix<T, S> r = m;
  91         return r.invert();
  92 }
  93
  94 } // namespace LinAl
  95 } // namespace Msp
  96
  97 #endif