Add dynamically allocated versions of matrix and vector

[libs/math.git] / source / linal / matrixops.h

diff --git a/source/linal/matrixops.h b/source/linal/matrixops.h
new file mode 100644 (file)
index 0000000..790dfbf
--- /dev/null
+++ b/source/linal/matrixops.h
@@ -0,0 +1,62 @@
+#ifndef MSP_LINAL_MATRIXOPS_H_
+#define MSP_LINAL_MATRIXOPS_H_
+
+#include <cmath>
+#include <stdexcept>
+
+namespace Msp {
+namespace LinAl {
+
+class not_invertible: public std::domain_error
+{
+public:
+       not_invertible(): domain_error(std::string()) { }
+       virtual ~not_invertible() throw() { }
+};
+
+
+template<typename T>
+inline T &invert_matrix(T &m, T &r)
+{
+       typedef typename T::ElementType V;
+       using std::abs;
+
+       for(unsigned i=0; i<m.rows(); ++i)
+       {
+               unsigned pivot = i;
+               for(unsigned j=i+1; j<m.rows(); ++j)
+                       if(abs(m.element(j, i))>abs(m.element(pivot, i)))
+                               pivot = j;
+
+               if(m.element(pivot, i)==V(0))
+                       throw not_invertible();
+
+               if(pivot!=i)
+               {
+                       m.exchange_rows(i, pivot);
+                       r.exchange_rows(i, pivot);
+               }
+
+               for(unsigned j=i+1; j<m.rows(); ++j)
+               {
+                       V a = -m.element(j, i)/m.element(i, i);
+                       m.add_row(i, j, a);
+                       r.add_row(i, j, a);
+               }
+
+               V a = V(1)/m.element(i, i);
+               m.multiply_row(i, a);
+               r.multiply_row(i, a);
+       }
+
+       for(unsigned i=m.rows(); i-->0; )
+               for(unsigned j=i; j-->0; )
+                       r.add_row(i, j, -m.element(j, i));
+
+       return m = r;
+}
+
+} // namespace LinAl
+} // namespace Msp
+
+#endif