source/linal/matrixops.h

   1 #ifndef MSP_LINAL_MATRIXOPS_H_
   2 #define MSP_LINAL_MATRIXOPS_H_
   3
   4 #include <cmath>
   5 #include <stdexcept>
   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 template<typename T>
  19 inline T &invert_matrix(T &m, T &r)
  20 {
  21         typedef typename T::ElementType V;
  22         using std::abs;
  23
  24         for(unsigned i=0; i<m.rows(); ++i)
  25         {
  26                 unsigned pivot = i;
  27                 for(unsigned j=i+1; j<m.rows(); ++j)
  28                         if(abs(m.element(j, i))>abs(m.element(pivot, i)))
  29                                 pivot = j;
  30
  31                 if(m.element(pivot, i)==V(0))
  32                         throw not_invertible();
  33
  34                 if(pivot!=i)
  35                 {
  36                         m.exchange_rows(i, pivot);
  37                         r.exchange_rows(i, pivot);
  38                 }
  39
  40                 for(unsigned j=i+1; j<m.rows(); ++j)
  41                 {
  42                         V a = -m.element(j, i)/m.element(i, i);
  43                         m.add_row(i, j, a);
  44                         r.add_row(i, j, a);
  45                 }
  46
  47                 V a = V(1)/m.element(i, i);
  48                 m.multiply_row(i, a);
  49                 r.multiply_row(i, a);
  50         }
  51
  52         for(unsigned i=m.rows(); i-->0; )
  53                 for(unsigned j=i; j-->0; )
  54                         r.add_row(i, j, -m.element(j, i));
  55
  56         return m = r;
  57 }
  58
  59 } // namespace LinAl
  60 } // namespace Msp
  61
  62 #endif