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 /**
  18 A mathematical matrix with S rows and columns.  Some operations are provided
  19 here that are only possible for square matrices.
  20 */
  21 template<typename T, unsigned S>
  22 class SquareMatrix: public Matrix<T, S, S>
  23 {
  24 public:
  25         SquareMatrix() { }
  26         SquareMatrix(const T *d): Matrix<T, S, S>(d) { }
  27         template<typename U>
  28         SquareMatrix(const Matrix<U, S, S> &m): Matrix<T, S, S>(m) { }
  29
  30         static SquareMatrix identity();
  31
  32         SquareMatrix &operator*=(const SquareMatrix &);
  33
  34         SquareMatrix &invert();
  35 };
  36
  37 template<typename T, unsigned S>
  38 inline SquareMatrix<T, S> SquareMatrix<T, S>::identity()
  39 {
  40         SquareMatrix<T, S> m;
  41         for(unsigned i=0; i<S; ++i)
  42                 m(i, i) = T(1);
  43         return m;
  44 }
  45
  46 template<typename T, unsigned S>
  47 SquareMatrix<T, S> &SquareMatrix<T, S>::operator*=(const SquareMatrix<T, S> &m)
  48 {
  49         return *this = *this*m;
  50 }
  51
  52 template<typename T, unsigned S>
  53 SquareMatrix<T, S> &SquareMatrix<T, S>::invert()
  54 {
  55         SquareMatrix<T, S> r = identity();
  56         for(unsigned i=0; i<S; ++i)
  57         {
  58                 if(this->element(i, i)==T(0))
  59                 {
  60                         unsigned pivot = i;
  61                         for(unsigned j=i+1; j<S; ++j)
  62                                 if(abs(this->element(j, i))>abs(this->element(pivot, i)))
  63                                         pivot = j;
  64
  65                         if(pivot==i)
  66                                 throw not_invertible();
  67
  68                         this->exchange_rows(i, pivot);
  69                         r.exchange_rows(i, pivot);
  70                 }
  71
  72                 for(unsigned j=i+1; j<S; ++j)
  73                 {
  74                         T a = -this->element(j, i)/this->element(i, i);
  75                         this->add_row(i, j, a);
  76                         r.add_row(i, j, a);
  77                 }
  78
  79                 T a = T(1)/this->element(i, i);
  80                 this->multiply_row(i, a);
  81                 r.multiply_row(i, a);
  82         }
  83
  84         for(unsigned i=S; i-->0; )
  85                 for(unsigned j=i; j-->0; )
  86                         r.add_row(i, j, -this->element(j, i));
  87
  88         return *this = r;
  89 }
  90
  91 template<typename T, unsigned S>
  92 inline SquareMatrix<T, S> invert(const SquareMatrix<T, S> &m)
  93 {
  94         SquareMatrix<T, S> r = m;
  95         return r.invert();
  96 }
  97
  98 } // namespace LinAl
  99 } // namespace Msp
 100
 101 #endif