gaussian_elimination.h

00001 // -*- c++ -*-
00002 
00003 // Copyright (C) 2008,2009 Ethan Eade, Tom Drummond (twd20@cam.ac.uk)
00004 // and Ed Rosten (er258@cam.ac.uk)
00005 //
00006 // This file is part of the TooN Library.  This library is free
00007 // software; you can redistribute it and/or modify it under the
00008 // terms of the GNU General Public License as published by the
00009 // Free Software Foundation; either version 2, or (at your option)
00010 // any later version.
00011 
00012 // This library is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 
00017 // You should have received a copy of the GNU General Public License along
00018 // with this library; see the file COPYING.  If not, write to the Free
00019 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
00020 // USA.
00021 
00022 // As a special exception, you may use this file as part of a free software
00023 // library without restriction.  Specifically, if other files instantiate
00024 // templates or use macros or inline functions from this file, or you compile
00025 // this file and link it with other files to produce an executable, this
00026 // file does not by itself cause the resulting executable to be covered by
00027 // the GNU General Public License.  This exception does not however
00028 // invalidate any other reasons why the executable file might be covered by
00029 // the GNU General Public License.
00030 
00031 
00032 #ifndef GAUSSIAN_ELIMINATION_H
00033 #define GAUSSIAN_ELIMINATION_H
00034 
00035 #include <utility>
00036 #include <TooN/TooN.h>
00037 
00038 namespace TooN {
00040     template<int N, typename Precision>
00041     inline Vector<N, Precision> gaussian_elimination (Matrix<N,N,Precision> A, Vector<N, Precision> b) {
00042         using std::swap;
00043 
00044         int size=b.size();
00045 
00046         for (int i=0; i<size; ++i) {
00047             int argmax = i;
00048             Precision maxval = abs(A[i][i]);
00049             
00050             for (int ii=i+1; ii<size; ++ii) {
00051                 double v =  abs(A[ii][i]);
00052                 if (v > maxval) {
00053                     maxval = v;
00054                     argmax = ii;
00055                 }
00056             }
00057             Precision pivot = A[argmax][i];
00058             //assert(abs(pivot) > 1e-16);
00059             Precision inv_pivot = static_cast<Precision>(1)/pivot;
00060             if (argmax != i) {
00061                 for (int j=i; j<size; ++j)
00062                     swap(A[i][j], A[argmax][j]);
00063                 swap(b[i], b[argmax]);
00064             }
00065             //A[i][i] = 1;
00066             for (int j=i+1; j<size; ++j)
00067                 A[i][j] *= inv_pivot;
00068             b[i] *= inv_pivot;
00069             
00070             for (int u=i+1; u<size; ++u) {
00071                 double factor = A[u][i];
00072                 //A[u][i] = 0;
00073                 for (int j=i+1; j<size; ++j)
00074                     A[u][j] -= factor * A[i][j];
00075                 b[u] -= factor * b[i];
00076             }
00077         }
00078         
00079         Vector<N,Precision> x(size);
00080         for (int i=size-1; i>=0; --i) {
00081             x[i] = b[i];
00082             for (int j=i+1; j<size; ++j)
00083                 x[i] -= A[i][j] * x[j];
00084         }
00085         return x;
00086     }
00087     
00088     namespace Internal
00089     {
00090         template<int i, int j, int k> struct Size3
00091         {
00092             static const int s=(i!= -1)?i:(j!=-1?j:k);
00093         };
00094 
00095     };
00096 
00098     template<int R1, int C1, int R2, int C2, typename Precision>
00099     inline Matrix<Internal::Size3<R1, C1, R2>::s, C2, Precision> gaussian_elimination (Matrix<R1,C1,Precision> A, Matrix<R2, C2, Precision> b) {
00100         using std::swap;
00101         SizeMismatch<R1, C1>::test(A.num_rows(), A.num_cols());
00102         SizeMismatch<R1, R2>::test(A.num_rows(), b.num_rows());
00103 
00104         int size=A.num_rows();
00105 
00106         for (int i=0; i<size; ++i) {
00107             int argmax = i;
00108             Precision maxval = abs(A[i][i]);
00109             
00110             for (int ii=i+1; ii<size; ++ii) {
00111                 double v =  abs(A[ii][i]);
00112                 if (v > maxval) {
00113                     maxval = v;
00114                     argmax = ii;
00115                 }
00116             }
00117             Precision pivot = A[argmax][i];
00118             //assert(abs(pivot) > 1e-16);
00119             Precision inv_pivot = static_cast<Precision>(1)/pivot;
00120             if (argmax != i) {
00121                 for (int j=i; j<size; ++j)
00122                     swap(A[i][j], A[argmax][j]);
00123 
00124                 for(int j=0; j < b.num_cols(); j++)
00125                     swap(b[i][j], b[argmax][j]);
00126             }
00127             //A[i][i] = 1;
00128             for (int j=i+1; j<size; ++j)
00129                 A[i][j] *= inv_pivot;
00130             b[i] *= inv_pivot;
00131             
00132             for (int u=i+1; u<size; ++u) {
00133                 double factor = A[u][i];
00134                 //A[u][i] = 0;
00135                 for (int j=i+1; j<size; ++j)
00136                     A[u][j] -= factor * A[i][j];
00137                 b[u] -= factor * b[i];
00138             }
00139         }
00140         
00141         Matrix<Internal::Size3<R1, C1, R2>::s,C2,Precision> x(b.num_rows(), b.num_cols());
00142         for (int i=size-1; i>=0; --i) {
00143             for(int k=0; k <b.num_cols(); k++)
00144             {
00145                 x[i][k] = b[i][k];
00146                 for (int j=i+1; j<size; ++j)
00147                     x[i][k] -= A[i][j] * x[j][k];
00148             }
00149         }
00150         return x;
00151     }
00152 }
00153 #endif

Generated on Thu May 7 20:28:40 2009 for TooN by  doxygen 1.5.3