ludec.cc

Go to the documentation of this file.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "DataTypes.h"
#include "ludec.h"
 
#define TINY 1.0e-20;
 
bool ludcmp(Real **a, size_t n, int *indx, Real *d)
{
  bool decomposed = true;
  size_t i,j;
  size_t imax = 0,k;
  Real big,dum,temp;
  Real sum;
  Real *dumpoint, *vv;
 
  vv=(Real *)malloc((n+1)*sizeof(Real));
  *d=1.0;
  i = 1;
  do {
    big=0.0;
    for (j=1;j<=n;j++)
      if ((temp=fabs(a[i][j])) > big) big=temp;
    if (big == 0.0) {
      //printf("Singular matrix in routine LUDCMP");
      //exit(1);
      decomposed = false;
      return decomposed;
    }
    vv[i]=1.0/big;
  } while ( ++i <= n );
 
  j = 1;
  do {
    if ( j > 1 ) {
      i = 1;
      do {
        sum=a[i][j];
        if ( i != 1 ) {
          k = 1;
          do {
            sum -= a[i][k]*a[k][j];
          } while ( ++k < i );
        }
        a[i][j]=sum;
      } while ( ++i < j );
    }
    big=0.0;
    i = j;
    do {
      sum=a[i][j];
      if ( j > 1 ) {
        k =1 ;
        do {
          sum -= a[i][k]*a[k][j];
        } while ( ++k < j );
      }
      a[i][j]=sum;
      if ( (dum=vv[i]*fabs(sum)) >= big) {
        big=dum;
        imax=i;
      }
    } while ( ++i <= n );
    if (j != imax) {    /* exchange rows */
      dumpoint=a[imax];
      a[imax]=a[j];
      a[j]=dumpoint;
      *d = -(*d);
      vv[imax]=vv[j];
    }
    indx[j]=imax;
    if (a[j][j] == 0.0) a[j][j]=TINY;
    if (j != n) {
      dum=1.0/(a[j][j]);
      for (i=j+1;i<=n;i++) a[i][j] *= dum;
    }
  } while ( ++j <= n );
  free(vv);
 
  return decomposed;
}
 
#undef TINY
 
 
 
void lubksb(Real **a, size_t n, int *indx, Real *b)
{
  size_t i, ii=0, ip;
  size_t j;
  Real sum;
 
  for (i=1;i<=n;i++) {
    ip=indx[i];
    sum=b[ip];
    b[ip]=b[i];
    if (ii)
      for (j=ii;j<=i-1;j++)
        sum -= a[i][j]*b[j];
    else if (sum)
      ii=i;
    b[i]=sum;
  }
  for (i=n;i>=1;i--) {
    sum=b[i];
    for (j=i+1;j<=n;j++)
      sum -= a[i][j]*b[j];
    b[i]=sum/a[i][i];
  }
}
 
 
Real *
lumult( int n, Real **a, Real *b, Real *c, int *indx )
/* routine to multiply LUdecomposed matrix by vector: ab=c */
{
  int i,j, dum, *order;
  Real *product;
 
  /* allocate memory */
  order   = ( int * )calloc( n, sizeof(int ) );
  product = ( Real * )calloc( n, sizeof(Real) );
 
  /* unscramble row wise permutations and store them
   so that multiplication may be performed          */
  for (i=0; i<n; i++)
    order[i]=i+1;
  for (i=n-1; i>=0;i--) {
    dum = order[i];
    order[i] = order[indx[i]-1];
    order[indx[i]-1] = dum;
  }
 
  /* perform multiplication by upper triangular matrix */
  for (i=0; i<n; i++) {
    product[i] = 0.0;
    for (j=i; j<n; j++ )
      product[i] += a[i][j]*b[j];
  }
 
  /* perform multiplication by lower triangiular matrix */
  for (i=0; i<n; i++) {
    c[i] = 0.0;
    for (j=0; j<order[i]-1; j++ )
      c[i] += a[order[i]-1][j]*product[j];
    c[i] += product[order[i]-1];
  }
 
  return( c );
}
 
 
Real **
mat_mult_ludcmp( int n, Real **a, int *indx, Real **b, Real **c )
/* routine to multiply LUdecomposed matrix by matrix: ab=c */
{
  int i,j, k, dum, *order;
  Real *product;
 
  /* allocate memory */
  order   = ( int * )calloc( n, sizeof(int ) );
  product = ( Real * )calloc( n, sizeof(Real) );
 
  /* unscramble row wise permutations and store them
   so that multiplication may be performed          */
  for (i=0; i<n; i++)
    order[i]=i+1;
  for (i=n-1; i>=0;i--) {
    dum = order[i];
    order[i] = order[indx[i]-1];
    order[indx[i]-1] = dum;
  }
 
  for ( k=0; k<n; k++ ) {
    /* perform multiplication by upper triangular matrix */
    for (i=0; i<n; i++) {
      product[i] = 0.0;
      for (j=i; j<n; j++ )
        product[i] += a[i][j]*b[j][k];
    }
 
    /* perform multiplication by lower triangiular matrix */
    for (i=0; i<n; i++) {
      c[i][k] = 0.0;
      for (j=0; j<order[i]-1; j++ )
        c[i][k] += a[order[i]-1][j]*product[j];
      c[i][k] += product[order[i]-1];
    }
  }
  return( c );
}
 
void
tridiagDecomp( int n, Real *a, Real *b, Real *c )
{
  int i;
 
  for ( i=1; i<n; i++ ) {
    b[i] /= a[i-1];
    a[i] -= b[i]*c[i-1];
  }
}
 
 
void tridiagbksb( int n, Real *a, Real *b, Real *c, Real *f )
/*
 * Given a decomposed tridiagonal matrix with rows a, b, and c
 * back substitute to find the solution. f is altered to contain the answer
 *
 */
{
  int i;
 
  for ( i=1; i<n; i++ )
    f[i] -= b[i]*f[i-1];
  f[n-1] /= a[n-1];
  for ( i=n-2; i>=0; i-- )
    f[i] = (f[i]-c[i]*f[i+1])/a[i];
}
 
void choldc(Real **a, int n, Real p[])
{
  int i,j,k;
  Real sum;
 
  for (i=1;i<=n;i++) {
    for (j=i;j<=n;j++) {
      for (sum=a[i][j],k=i-1;k>=1;k--) sum -= a[i][k]*a[j][k];
      if (i == j) {
        if (sum <= 0.0) {
          fprintf( stderr, "choldc failed\n");
          exit(1);
        }
        p[i]=sqrt(sum);
      } else a[j][i]=sum/p[i];
    }
  }
}
 
void cholsl(Real **a, int n, Real p[], Real b[], Real x[])
{
  int i,k;
  Real sum;
 
  for (i=1;i<=n;i++) {
    for (sum=b[i],k=i-1;k>=1;k--) sum -= a[i][k]*x[k];
    x[i]=sum/p[i];
  }
  for (i=n;i>=1;i--) {
    for (sum=x[i],k=i+1;k<=n;k++) sum -= a[k][i]*x[k];
    x[i]=sum/p[i];
  }
}