jama_lu.h

#ifndef JAMA_LU_H
#define JAMA_LU_H

#include "tnt.h"
#include <algorithm>
//for min(), max() below

using namespace TNT;
using namespace std;

namespace JAMA
{

template <class Real>
class LU
{



   /* Array for internal storage of decomposition.  */
   Array2D<Real>  LU_;
   int m, n, pivsign;
   Array1D<int> piv;


   Array2D<Real> permute_copy(const Array2D<Real> &A,
            const Array1D<int> &piv, int j0, int j1)
    {
        int piv_length = piv.dim();

        Array2D<Real> X(piv_length, j1-j0+1);


         for (int i = 0; i < piv_length; i++)
            for (int j = j0; j <= j1; j++)
               X[i][j-j0] = A[piv[i]][j];

        return X;
    }

   Array1D<Real> permute_copy(const Array1D<Real> &A,
        const Array1D<int> &piv)
    {
        int piv_length = piv.dim();
        if (piv_length != A.dim())
            return Array1D<Real>();

        Array1D<Real> x(piv_length);


         for (int i = 0; i < piv_length; i++)
               x[i] = A[piv[i]];

        return x;
    }


    public :

    LU (const Array2D<Real> &A) : LU_(A.copy()), m(A.dim1()), n(A.dim2()),
        piv(A.dim1())
    
    {

   // Use a "left-looking", dot-product, Crout/Doolittle algorithm.


      for (int i = 0; i < m; i++) {
         piv[i] = i;
      }
      pivsign = 1;
      Real *LUrowi = 0;;
      Array1D<Real> LUcolj(m);

      // Outer loop.

      for (int j = 0; j < n; j++) {

         // Make a copy of the j-th column to localize references.

         for (int i = 0; i < m; i++) {
            LUcolj[i] = LU_[i][j];
         }

         // Apply previous transformations.

         for (int i = 0; i < m; i++) {
            LUrowi = LU_[i];

            // Most of the time is spent in the following dot product.

            int kmax = min(i,j);
            double s = 0.0;
            for (int 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 (int i = j+1; i < m; i++) {
            if (abs(LUcolj[i]) > abs(LUcolj[p])) {
               p = i;
            }
         }
         if (p != j) {
            int k=0;
            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 (int i = j+1; i < m; i++) {
               LU_[i][j] /= LU_[j][j];
            }
         }
      }
   }


   int isNonsingular () {
      for (int j = 0; j < n; j++) {
         if (LU_[j][j] == 0)
            return 0;
      }
      return 1;
   }

   Array2D<Real> getL () {
      Array2D<Real> L_(m,n);
      for (int i = 0; i < m; i++) {
         for (int j = 0; j < n; j++) {
            if (i > j) {
               L_[i][j] = LU_[i][j];
            } else if (i == j) {
               L_[i][j] = 1.0;
            } else {
               L_[i][j] = 0.0;
            }
         }
      }
      return L_;
   }

   Array2D<Real> getU () {
      Array2D<Real> U_(n,n);
      for (int i = 0; i < n; i++) {
         for (int j = 0; j < n; j++) {
            if (i <= j) {
               U_[i][j] = LU_[i][j];
            } else {
               U_[i][j] = 0.0;
            }
         }
      }
      return U_;
   }

   Array1D<int> getPivot () {
      return piv;
   }


   Real det () {
      if (m != n) {
         return Real(0);
      }
      Real d = Real(pivsign);
      for (int j = 0; j < n; j++) {
         d *= LU_[j][j];
      }
      return d;
   }

   Array2D<Real> solve (const Array2D<Real> &B)
   {

      /* Dimensions: A is mxn, X is nxk, B is mxk */

      if (B.dim1() != m) {
        return Array2D<Real>(0,0);
      }
      if (!isNonsingular()) {
        return Array2D<Real>(0,0);
      }

      // Copy right hand side with pivoting
      int nx = B.dim2();


      Array2D<Real> X = permute_copy(B, piv, 0, nx-1);

      // Solve L*Y = B(piv,:)
      for (int k = 0; k < n; k++) {
         for (int i = k+1; i < n; i++) {
            for (int j = 0; j < nx; j++) {
               X[i][j] -= X[k][j]*LU_[i][k];
            }
         }
      }
      // Solve U*X = Y;
      for (int k = n-1; k >= 0; k--) {
         for (int j = 0; j < nx; j++) {
            X[k][j] /= LU_[k][k];
         }
         for (int i = 0; i < k; i++) {
            for (int j = 0; j < nx; j++) {
               X[i][j] -= X[k][j]*LU_[i][k];
            }
         }
      }
      return X;
   }


   Array1D<Real> solve (const Array1D<Real> &b)
   {

      /* Dimensions: A is mxn, X is nxk, B is mxk */

      if (b.dim1() != m) {
        return Array1D<Real>();
      }
      if (!isNonsingular()) {
        return Array1D<Real>();
      }


      Array1D<Real> x = permute_copy(b, piv);

      // Solve L*Y = B(piv)
      for (int k = 0; k < n; k++) {
         for (int i = k+1; i < n; i++) {
               x[i] -= x[k]*LU_[i][k];
            }
         }

      // Solve U*X = Y;
      for (int k = 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;
   }

}; /* class LU */

} /* namespace JAMA */

#endif
/* JAMA_LU_H */