EquationSolver.h

Go to the documentation of this file.

#ifndef _utl_EquationSolver_h_
#define _utl_EquationSolver_h_

#include <utl/AugerException.h>
#include <cmath>
#include <vector>


namespace utl {

  template<typename AMatrix>
  void
  InvertMatrix(const size_t n, AMatrix& a)
  {
    std::vector<size_t> indxc(n);
    std::vector<size_t> indxr(n);
    std::vector<size_t> ipiv(n);
    for (size_t i = 0; i < n; ++i) { // loop over columns
      // find pivot
      double big = 0;
      size_t irow = 0;
      size_t icol = 0;
      for (size_t j = 0; j < n; ++j)
        if (ipiv[j] != 1)
          for (size_t k = 0; k < n; ++k)
            if (ipiv[k] == 0 && std::fabs(a[j][k]) >= big) {
              big = std::fabs(a[j][k]);
              irow = j;
              icol = k;
            }
      ++ipiv[icol];

      // put pivot on the diagonal
      if (irow != icol)
        for (size_t l = 0; l < n; ++l)
          std::swap(a[irow][l], a[icol][l]);
      indxr[i] = irow;
      indxc[i] = icol;

      if (a[icol][icol] == 0)
        throw utl::DoesNotComputeException("InvertMatrix: matrix is singular");

      // eliminate rows
      const double t = 1. / a[icol][icol];
      a[icol][icol] = 1;
      for (size_t l = 0; l < n; ++l)
        a[icol][l] *= t;
      for (size_t ll = 0; ll < n; ++ll)
        if (ll != icol) {
          const double t = a[ll][icol];
          a[ll][icol] = 0;
          for (size_t l = 0; l < n; ++l)
            a[ll][l] -= a[icol][l] * t;
        }
    }

    // unscramble changed columns
    for (int l = n - 1; l >= 0; --l)
      if (indxr[l] != indxc[l])
        for (size_t k = 0; k < n; ++k)
          std::swap(a[k][indxr[l]], a[k][indxc[l]]);
  }

}


#endif