src/math/linsolve.h

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