lu.c

大师写的二代小波经典之作

/*
 *  -*- Mode: ANSI C -*-
 *  $Id: lu.c,v 1.6 1996/11/20 02:38:51 fernande Exp $
 *  $Source: /sgi.acct/sweldens/cvs/liftpack/Lifting/lu.c,v $
 *  Author: Gabriel Fernandez
 *
 *  Definition of the functions used to perform the LU decomposition
 *  of matrix a. Function LUdcmp() performs the LU operations on a[][]
 *  and function LUbksb() performs the backsubstitution giving the
 *  solution in b[];
 */
/* do not edit anything above this line */

/* System header files */
#include <math.h>
/* FLWT header files */
#include <flwterr.h>
#include <flwtdef.h>
#include <lu.h>
#include <mem.h>
#include <util.h>

#define TINY 1.0e-20;    /* A small number */

/* code */

/*
 * LUdcmp function: Given a matrix a [1..n][1..n], this routine
 *                  replaces it by the LU decomposition of a
 *                  rowwise permutation of itself. a and n are
 *                  input. a is output, arranged with L and U in
 *                  the same matrix; indx[1..n] is an output vector
 *                  that records the row permutation affected by
 *                  the partial pivoting; d is output as +/-1
 *                  depending on whether the number of row
 *                  interchanges was even or odd, respectively.
 *                  This routine is used in combination with LUbksb()
 *                  to solve linear equations or invert a matrix.
*/
void LUdcmp ( Matrix a, int n, int *indx, Flt *d )
{
    int i, imax, j, k;
    Flt big, dum, sum, temp;
    Vector vv;   /* vv stores the implicit scaling of each row */

    vv=vector( 1, (long)n );
    *d=(Flt)1;               /* No row interchanges yet. */
    for (i=1;i<=n;i++) {     /* Loop over rows to get the implicit scaling */
        big=(Flt)0;          /* information. */
        for (j=1;j<=n;j++)
            if ((temp=(Flt)fabs((Flt)a[i][j])) > big)
                big=temp;
        if (big == (Flt)0.0)
            Error ("LUdmcp", SINGULAR_MATRIX, ABORT);
        /* Nonzero largest element. */
        vv[i]=(Flt)1/big;    /* Save the scaling. */
    }
    for (j=1;j<=n;j++) {     /* This is the loop over columns of Crout's method. */
        for (i=1;i<j;i++) {  /* Sum form of a triangular matrix except for i=j. */
            sum=a[i][j];     
            for (k=1;k<i;k++) sum -= a[i][k]*a[k][j];
            a[i][j]=sum;
        }
        big=(Flt)0;      /* Initialize for the search for largest pivot element. */
        imax = 0;        /* Set default value for imax */
        for (i=j;i<=n;i++) {  /* This is i=j of previous sum and i=j+1...N */
            sum=a[i][j];      /* of the rest of the sum. */
            for (k=1;k<j;k++)
                sum -= a[i][k]*a[k][j];
            a[i][j]=sum;
            if ( (dum=vv[i]*(Flt)fabs((Flt)sum)) >= big) {
            /* Is the figure of merit for the pivot better than the best so far? */
                big=dum;
                imax=i;
            }
        }
        if (j != imax) {          /* Do we need to interchange rows? */
            for (k=1;k<=n;k++) {  /* Yes, do so... */
                dum=a[imax][k];
                a[imax][k]=a[j][k];
                a[j][k]=dum;
            }
            *d = -(*d);           /* ...and change the parity of d. */
            vv[imax]=vv[j];       /* Also interchange the scale factor. */
        }
        indx[j]=imax;
        if (a[j][j] == (Flt)0.0)
            a[j][j]=(Flt)TINY;
        /* If the pivot element is zero the matrix is singular (at least */
        /* to the precision of the algorithm). For some applications on */
        /* singular matrices, it is desiderable to substitute TINY for zero. */
        if (j != n) {           /* Now, finally divide by pivot element. */
            dum=(Flt)1/(a[j][j]);
            for ( i=j+1 ; i<=n ; i++ )
                a[i][j] *= dum;
        }
    }                           /* Go back for the next column in the reduction. */
    free_vector( vv, 1, (long)n);
}
    
#undef TINY
    
/*
 *  LUbksb function: Solves the set of n linear equations A.X = B.
 *                   Here a[1..n][1..n] is input, not as the matrix A
 *                   but rather as its LU decomposition, determined
 *                   by the routine LUdcmp(). indx[1..n] is input as
 *                   the permutation vector returned by LUdcmp().
 *                   b[1..n] is input as the right hand side vector B,
 *                   and returns with the solution vector X. a, n, and
 *                   indx are not modified by this routine and can be
 *                   left in place for successive calls with different
 *                   right-hand sides b. This routine takes into account
 *                   the possibility that b will begin with many zero
 *                   elements, so it it efficient for use in matrix
 *                   inversion.
 */
void LUbksb ( Matrix a, int n, int *indx, Vector b )
{
    int i,ii=0,ip,j;
    Flt sum;

    for (i=1;i<=n;i++) {   /* When ii is set to a positive value, it will */
        ip=indx[i];        /* become the index of the first nonvanishing */
        sum=b[ip];         /* element of b. We now do the forward substitution. */
        b[ip]=b[i];        /* The only new wrinkle is to unscramble the */
        if (ii)            /* permutation as we go. */
            for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
        else if (sum)      /* A nonzero element was encountered, so from now on */
            ii=i;          /* we will have to do the sums in the loop above */
        b[i]=sum;
    }
    for (i=n;i>=1;i--) {   /* Now we do the backsubstitution. */
        sum=b[i];
        for (j=i+1;j<=n;j++) sum -= a[i][j]*b[j];
        b[i]=sum/a[i][i];  /* Store a component of the solution vector X. */
        if (zero(b[i])) b[i] = (Flt)0;   /* Verify small numbers. */
    }                      /* All done! */
}