src/math/linsolve.h

   1 //
   2 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   3 // +                                                                      +
   4 // + This file is part of enGrid.                                         +
   5 // +                                                                      +
   6 // + Copyright 2008-2013 enGits GmbH                                      +
   7 // +                                                                      +
   8 // + enGrid is free software: you can redistribute it and/or modify       +
   9 // + it under the terms of the GNU General Public License as published by +
  10 // + the Free Software Foundation, either version 3 of the License, or    +
  11 // + (at your option) any later version.                                  +
  12 // +                                                                      +
  13 // + enGrid is distributed in the hope that it will be useful,            +
  14 // + but WITHOUT ANY WARRANTY; without even the implied warranty of       +
  15 // + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        +
  16 // + GNU General Public License for more details.                         +
  17 // +                                                                      +
  18 // + You should have received a copy of the GNU General Public License    +
  19 // + along with enGrid. If not, see <http://www.gnu.org/licenses/>.       +
  20 // +                                                                      +
  21 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  22 //
  23 #ifndef linsolve_H
  24 #define linsolve_H
  25
  26 struct LinSolveError {
  27   double det;
  28   LinSolveError(double d) { det = d; };
  29 };
  30
  31 // Rainers full matrix solver
  32 template <class M, class V>
  33 void linsolve(const M &Ain, const V &rsv, V &b)
  34 {
  35   // Solve linear system Ax = rsv
  36   // copy vec to protect it. b will be the return value
  37   b = rsv;
  38   // copy yourself to protect matrix entries
  39   M A = Ain;
  40
  41   int n = A.size();
  42   int k,i,j,p[n];
  43   double q,s,max,h,det;
  44   double ele_max = 0;
  45
  46   // Find maximum element to get a relative value
  47   for (int i = 0; i < A.size(); ++i) {
  48     for (int j = 0; j < A[i].size(); ++j) {
  49       ele_max = std::max(ele_max,A[i][j]);
  50     };
  51   };
  52
  53   // Get in matrix reduction
  54   det = 1;
  55   for (k = 0; k < n-1; k++) {
  56     max = 0.0;
  57     p[k] = 0;
  58     for(i = k; i < n; i++) {
  59       s=0.0;
  60       for(j = k; j < n; j++) {
  61         s = s + fabs(A[i][j]);
  62       }
  63       q = fabs(A[i][k])/s;
  64       if(q > max) {
  65         max=q;
  66         p[k]=i;
  67       }
  68     }
  69     if(!(p[k] == k)) {
  70       det = -det;
  71       for(j = 0; j < n; j++) {
  72         h = A[k][j];
  73         A[k][j] = A[p[k]][j];
  74         A[p[k]][j] = h;
  75       }
  76     }
  77     det = det*A[k][k];
  78     for(i = k+1; i < n; i++) {
  79       A[i][k] = A[i][k]/A[k][k];
  80       for(j = k+1; j < n; j++) {
  81         A[i][j] = A[i][j] - A[i][k]*A[k][j];
  82       };
  83     }
  84   }
  85   det = det*A[n-1][n-1];
  86
  87   // Proceed with rest of system reduction
  88   for(k = 0; k < n-1; k++) {
  89     if(!(p[k]==k)) {
  90       h=b[k];
  91       b[k]=b[p[k]];
  92       b[p[k]]=h;
  93     }
  94   };
  95   for(i = 0; i < n; i++) {
  96     for(j = 0; j < i; j++) {
  97       b[i] = b[i] - A[i][j]*b[j];
  98     };
  99   }
 100   for(i = n-1;i >= 0; i--) {
 101     s = b[i];
 102     for(k = i+1; k < n; k++)
 103       s = s - A[i][k]*b[k];
 104     b[i] = s/A[i][i];
 105   };
 106
 107   // Check Determinant and throw error, if needed
 108   if (fabs(det) < 1e-20) {
 109     throw LinSolveError(det);
 110   };
 111 };
 112
 113 #endif