/* 
  LU-decomposition according to Crout's algorithm with pivoting. 
	Description:
   int LU_decompos(double **a,int n,int *indx,int *d,double *vv);
	Parameters:
   a - source matrix (n x n) on input, destination on output;
   n - the matrix size;
   indx - integer array (size n) to remember permutations;
   d - on output, contains +1 or -1 for even or odd permutations number.
   vv - temporary array (size n).
	   Returns: 
   0 - the source matrix is singular (invalid for decomposition),
   1 - if OK.

  Back substitution, using LU decomposed matrix.
	Description:
  void LU_backsub(double **a,int n,int *indx,double *b);
	Parameters:
  a - the matrix decomposed by Crout;
  n - the matrix size;
  indx - permutation order obtained by decomposition algorithm;
  b - the vector (size n) to be substituted on input, the result
      of the substitution on output.
  Note: a and indx are not modified by this routine and could be 
  used in multiple calls.

  Invertation of matrix, using LU decomposed matrix.
	Description:
  void LU_invert(double **a,int n,int *indx,double **inv,double *col);
	Parameters:
  a - the matrix decomposed by Crout;
  n - the matrix size;
  indx - permutation order obtained by decomposition algorithm;
  inv - the destination matrix;
  col - temporary array (size n).
  Note: test for singularity has been already obtained on the 
  matrix decomposition, a and indx are not modified by this routine, 
  the routine uses multiple backsubstitutions (previous algorithm).

  Obtaining the matrix determinant, using LU-decomposed matrix
	Description:
  double LU_determ(double **a,int n,int *indx,int *d);
	Parameters:
  a - the matrix decomposed by Crout;
  n - the matrix size;
  indx - permutation order obtained by decomposition algorithm;
  d - the parity sign (+1 or -1) obtained at decomposition.
	Returns:
  the determinant value. Note: non-zero (the matrix cannot be 
  singular, if decomposed properly); a, indx and d are not modified 
  by this routine.

*/

/* for fabs(); inclusion could be removed if fabs() is implemented inline */ 
#include <math.h>
/* "small number" to avoid overflow in some cases */
#define TINY 1.e-30

int LU_decompos(double **a,int n,int *indx,int *d,double *vv) {
 register int i,imax,j,k;
 double big,sum,temp;
 *d=1;
 /* search for the largest element in each row; save the scaling in the 
    temporary array vv and return zero if the matrix is singular */
 for(i=0;i<n;i++) {
  big=0.;
  for(j=0;j<n;j++) if((temp=fabs(a[i][j]))>big) big=temp;
  if(big==0.) return(0);
  vv[i]=big;
 }
 /* the main loop for the Crout's algorithm */
 for(j=0;j<n;j++) {
  /* this is the part a) of the algorithm except for i==j */
  for(i=0;i<j;i++) {
   sum=a[i][j];
   for(k=0;k<i;k++) sum-=a[i][k]*a[k][j];
   a[i][j]=sum;
  }
  /* initialize for the search for the largest pivot element */
  big=0.;imax=j;
  /* this is the part a) for i==j and part b) for i>j + pivot search */
  for(i=j;i<n;i++) {
   sum=a[i][j];
   for(k=0;k&lt;j;k++) sum-=a[i][k]*a[k][j];
   a[i][j]=sum;
   /* is the figure of merit for the pivot better than the best so far? */
   if((temp=vv[i]*fabs(sum))>=big) {big=temp;imax=i;}
  }
  /* interchange rows, if needed, change parity and the scale factor */
  if(imax!=j) {
   for(k=0;k<n;k++) {temp=a[imax][k];a[imax][k]=a[j][k];a[j][k]=temp;}
   *d=-(*d);vv[imax]=vv[j];
  }
  /* store the index */
  indx[j]=imax;
  /* if the pivot element is zero, the matrix is singular but for some 
     applications a tiny number is desirable instead */
  if(a[j][j]==0.) a[j][j]=TINY;
  /* finally, divide by the pivot element */
  if(j<n-1) {
   temp=1./a[j][j];
   for(i=j+1;i<n;i++) a[i][j]*=temp;
  }
 }
 return(1);
}

void LU_backsub(double **a,int n,int *indx,double *b) {
 register int i,j,ip,ii=-1;
 double sum;
 /* First step of backsubstitution; the only wrinkle is to unscramble 
    the permutation order. Note: the algorithm is optimized for a 
    possibility of large amount of zeroes in b */
 for(i=0;i<n;i++) {
  ip=indx[i];
  sum=b[ip];b[ip]=b[i];
  if(ii>=0) for(j=ii;j<i;j++) sum-=a[i][j]*b[j];
  else if(sum) ii=i;	/* a nonzero element encounted */
  b[i]=sum;
 }
 /* the second step */
 for(i=n-1;i>=0;i--) {
  sum=b[i];
  for(j=i+1;j<n;j++) sum-=a[i][j]*b[j];
  b[i]=sum/a[i][i];
 }
}

void LU_invert(double **a,int n,int *indx,double **inv,double *col) {
 register int i,j;
 for(j=0;j<n;j++) {
  for(i=0;i<n;i++) col[i]=0.;
  col[j]=1.;
  LU_backsub(a,n,indx,col);
  for(i=0;i<n;i++) inv[i][j]=col[i];
 }
}

double LU_determ(double **a,int n,int *indx,int *d) {
 register int j;
 double res=(double)(*d);
 for(j=0;j<n;j++) res*=a[j][j];
 return(res);
}