[Toon-members] TooN gaussian_elimination.h

[Ethan Eade]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Ethan Eade <ethaneade>  08/03/31 13:58:39

Added files:
        .              : gaussian_elimination.h 

Log message:
        Standard implementation of gaussian elimination, for solving one-off 
Ax=b problems, where A is a nonsingular square matrix and x and b are either 
matrices or vectors.
        
        This is faster than using LU when you are solving using A only once, 
and doesn't need to call LAPACK.

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/gaussian_elimination.h?cvsroot=toon&rev=1.1

Patches:
Index: gaussian_elimination.h
===================================================================
RCS file: gaussian_elimination.h
diff -N gaussian_elimination.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ gaussian_elimination.h      31 Mar 2008 13:58:39 -0000      1.1
@@ -0,0 +1,66 @@
+#ifndef GAUSSIAN_ELIMINATION_H
+#define GAUSSIAN_ELIMINATION_H
+
+#include <utility>
+#include <TooN/TooN.h>
+
+namespace TooN {
+    template <class R, int N, class B> inline
+    R gaussian_elimination(Matrix<N> A, B b)
+    {
+       using std::swap;
+
+       for (int i=0; i<N; ++i) {
+           int argmax = i;
+           double maxval = abs(A[i][i]);
+       
+           for (int ii=i+1; ii<N; ++ii) {
+               double v =  abs(A[ii][i]);
+               if (v > maxval) {
+                   maxval = v;
+                   argmax = ii;
+               }
+           }
+           double pivot = A[argmax][i];
+           //assert(abs(pivot) > 1e-16);
+           double inv_pivot = 1.0/pivot;
+           if (argmax != i) {
+               for (int j=i; j<N; ++j)
+                   swap(A[i][j], A[argmax][j]);
+               swap(b[i], b[argmax]);
+           }
+           //A[i][i] = 1;
+           for (int j=i+1; j<N; ++j)
+               A[i][j] *= inv_pivot;
+           b[i] *= inv_pivot;
+
+           for (int u=i+1; u<N; ++u) {
+               double factor = A[u][i];
+               //A[u][i] = 0;
+               for (int j=i+1; j<N; ++j)
+                   A[u][j] -= factor * A[i][j];
+               b[u] -= factor * b[i];
+           }
+       }
+    
+       R x;
+       for (int i=N-1; i>=0; --i) {
+           x[i] = b[i];
+           for (int j=i+1; j<N; ++j)
+               x[i] -= A[i][j] * x[j];
+       }
+       return x;
+    }
+
+    template <int N, class A1, class A2>
+    inline Vector<N> gaussian_elimination(const FixedMatrix<N,N,A1>& A, const 
FixedVector<N,A2>& b) {
+       return gaussian_elimination<Vector<N> >(Matrix<N>(A), Vector<N>(b));
+    }
+
+    template <int N, class A1, class A2, int M>
+    inline Matrix<N> gaussian_elimination(const FixedMatrix<N,N,A1>& A, const 
FixedMatrix<N,M,A2>& b) {
+       return gaussian_elimination<Matrix<N,M> >(Matrix<N>(A), Matrix<N,M>(b));
+    }
+
+}
+#endif