source/linal/matrixops.h

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