Tools/Math/LA_Lu_decomp.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
#include <math.h>
#include <assert.h>
#include "LA_Lu_decomp.h"
#include "LA_Vector.h"
#include "LA_Exception.h"
//------------------------------------------------------------------------------
namespace LA
{
//------------------------------------------------------------------------------
LU::LU(const Matrix& M)
: LU_(M), m(M.rows()), n(M.columns()), piv(M.rows())
{
  // Use a "left-looking", dot-product, Crout/Doolittle algorithm.
  int i = 0;
  int j = 0;
  int k = 0;

  for (i = 0; i < m; i++)
  {
     piv[i] = i;
  }
  pivsign = 1;
  Vector LUcolj(m);

  // Outer loop.
  for (j = 0; j < n; j++)
  {
    // Make a copy of the j-th column to localize references.
    for (i = 0; i < m; i++)
    {
      LUcolj[i] = LU_[i][j];
    }

    // Apply previous transformations.
    for (i = 0; i < m; i++)
    {
      Vector& LUrowi = LU_[i];

      // Most of the time is spent in the following dot product.
      int kmax = (i < j)? i: j;
      double s = 0.0;
      for (k = 0; k < kmax; k++)
      {
        s += LUrowi[k]*LUcolj[k];
      }

      LUrowi[j] = LUcolj[i] -= s;
    }

    // Find pivot and exchange if necessary.
    int p = j;
    for (i = j+1; i < m; i++)
    {
      if (fabs(LUcolj[i]) > fabs(LUcolj[p]))
      {
        p = i;
      }
    }
    if (p != j)
    {
      for (k = 0; k < n; k++)
      {
        double t = LU_[p][k]; 
        LU_[p][k] = LU_[j][k]; 
        LU_[j][k] = t;
      }
      k = piv[p]; 
      piv[p] = piv[j]; 
      piv[j] = k;
      pivsign = -pivsign;
    }

    // Compute multipliers.
    if ((j < m) && (LU_[j][j] != 0.0))
    {
      for (i = j+1; i < m; i++)
      {
        LU_[i][j] /= LU_[j][j];
      }
    }
  }
}
//------------------------------------------------------------------------------
bool LU::singular() const
{
  for (int j = 0; j < n; j++)
  {
    if (LU_[j][j] == 0.0)
      return true;
  }
  return false;
}
//------------------------------------------------------------------------------
double LU::det() const
{
  assert(m == n);
  double d = (double)pivsign;
  for (int j = 0; j < n; j++)
  {
    d *= LU_[j][j];
  }
  return d;
}
//------------------------------------------------------------------------------
Vector LU::solve(const Vector& b) const
{
  assert(b.dim() == m);
  if (singular())
    throw Exception(Exception::SingularMatrix);

  Vector x = permute_copy(b, piv);
  int i, k;

  for (k = 0; k < (int)n; k++)
  {
    for (i = k+1; i < (int)n; i++)
    {
      x[i] -= x[k]*LU_[i][k];
    }
  }
      
  for (k = (int)n-1; k >= 0; k--)
  {
    x[k] /= LU_[k][k];
    for (int i = 0; i < k; i++) 
      x[i] -= x[k]*LU_[i][k];
  }
  return x;
}
//------------------------------------------------------------------------------
Matrix LU::inverse() const
{
  assert(m == n);
  
  Matrix M(n, n);
  for (int i = 0; i < n; ++i)
  {
    Vector b(n);
    b[i] = 1.0;
    M.set_column(i + 1, solve(b));
  }
  return M;
}
//------------------------------------------------------------------------------
Vector LU::permute_copy(const Vector& A, const std::vector<int>& piv) const
{
  int piv_length = (int)piv.size();
  assert(piv_length == A.dim());
  
  Vector x(piv_length);
  for (int i = 0; i < piv_length; i++) 
    x[i] = A[piv[i]];

  return x;
}
//------------------------------------------------------------------------------
}
//------------------------------------------------------------------------------

---