math_util.c

multipleshooting to find the roots of the non-linear functions

#include  <stdio.h>
#include  <math.h>
#include  <stdlib.h>
#define NOT(x) ((x != 0)?0:1)
#define max_num(x,y) ((x)>(y)?(x):(y))
#define sgn_num(x) (((x)<0.0)?-1.0:1.0)
#define dmax(x,y) (((x)>(y))?(x):(y))
#define dmin(x,y) (((x)>(y))?(y):(x))
#define YES 1
#define NO 0
#define DEBUG 0
#define MACHEPS 1.0e-19
/* ----------------------------------------------------------------------------
  
  SEC
  
      This function returns the secant of an angle.
  
-----------------------------------------------------------------------------*/   
long double sec( a  )
long double a;
   {
      long double x;
      x = cos( a );
      if( x == 0.0 )
         return(1.0/MACHEPS);
      else
         return(1.0/x);
   }
/* ----------------------------------------------------------------------------
  
  FACTOR
  
  
      Return code -> 1 : Normal completion.
      Return code -> 0 : Error.  Singular matrix a.
  
      This is algorithm 3.2.4 from Golub, G. H, and van Loan, C. F., Matrix Computations
      page 99.  LU Factorization *without* pivoting.
  
  -----------------------------------------------------------------------------*/
 int factor( a, n, ipivot )
     long double **a;         /* pointer to an n by n matrix */
     int *ipivot;        /* row elimination vector, dimension n */
     int n;              /* the number of equations in the system */
     {
        int i, j, k;
        
        for(j = 0; j < n; j++) {
           if( fabs((double)a[j][j]) < MACHEPS )
              return( 0 );
           ipivot[j]=j;
           for( k = 0; k < j; k++ )
              for( i = k+1; i < j; i++)
                 a[i][j]=a[i][j]-a[i][k]*a[k][j];
        
           for( k = 0; k < j; k++ )
              for( i = j; i < n; i++ )
                 a[i][j]=a[i][j]-a[i][k]*a[k][j];
                 
           for( k = j+1; k < n; k++ )
              a[k][j]=a[k][j]/a[j][j];
        }
        return( 1 );
     }
  /* ----------------------------------------------------------------------------
  
  SOLVE
  
      This function uses the factorized matrix [a] to solve the system of
      equations
                               [a]{x} = {b}
  
      This function uses the LU factorization from the function "factor". Hence
      the pivot vector "ipivot" is not used.
  
  -----------------------------------------------------------------------------*/
  void solve( a, ipivot, b, n, x )
  long double **a;
  long double *b;
  long double *x;
  int *ipivot;
  int n;
     {
        int i;
        int j;
        long double sum;
  
        if( n <= 0 ) return;
        if( n <= 1 ) {
           x[0] = b[0] / a[0][0];
           return;
        }
        x[0] = b[0];
        for( i = 1; i < n; i++ ) {
           sum = 0.0;
           for( j = 0; j < i; j++ )
              sum += a[i][j] * x[j];
           x[i] = b[i] - sum;
        }
        x[n-1] /= a[n-1][n-1];
        for( i = n-2; i >= 0; i-- ) {
           sum = 0.0;
           for( j = i+1; j < n; j++ )
              sum += a[i][j] * x[j];
           x[i] = (x[i] - sum) / a[i][i];
        }
        return;
     }
/* ----------------------------------------------------------------------------
  
  FACTOR1
  
      This function performs LU factorization of a matrix using partial pivoting
  
      Return code -> -1 or +1 : Normal completion.
      Return code -> 2        : Error.  Unable to allocate memory for working vectors.
      Return code -> 0        : Error.  Singular matrix a.
  
      This is the c version of the fortran code given in, "Elementary Numerical
      Analysis," S. D. Conte and C. de Boor, McGraw-Hill Co, pp. 165-166
  
  -----------------------------------------------------------------------------*/
  int factor1( a, n, ipivot )
     long double **a;         /* pointer to an n by n matrix */
     int *ipivot;        /* row elimination vector, dimension n */
     int n;              /* the number of equations in the system */
     {
        long double *a1d_allo_dbl( int );
        void a1d_free_dbl( long double * );
        int i;
        int j;
        int k;
        int istar;
        int iflag;
  
        long double *d;
        long double colmax;
        long double aaikod;
        long double temp;
        long double rowmax;
  
        iflag = 1;
  /*
     Allocate storage for the matrix diagonal
  */
        d = a1d_allo_dbl( n );
  
        if( d == NULL ) {
           printf("\n *** Error unable to allocate memory for factorization\n");
           return( 2 );
        }
  
        else {
  
           for( i = 0; i < n; i++ ) {
              ipivot[i] = i;
              rowmax = 0.0;
              for( j = 0; j < n; j++ )
                 rowmax = dmax( rowmax, (long double)fabs((double)a[i][j]) );
              if( rowmax == 0.0 ) {
                 iflag = 0;
                 rowmax = 1.0;
              }
              d[i] = rowmax;
           }
  
           if( n <= 1 ) {
              free( d );
              return( iflag );
           }
  
           for( k = 0; k < n-1; k++ ) {
              colmax = fabs((double)a[k][k])/d[k];
              istar = k;
              for( i = k+1; i < n; i++ ) {
                 aaikod = fabs((double)a[i][k])/d[i];
                 if( aaikod > colmax ) {
                    colmax = aaikod;
                    istar = i;
                 }
              }
              if( colmax == 0.0 )
                 iflag = 0;
              else {
                 if( istar > k ) {
                    iflag = -iflag;
                    i = ipivot[istar];
                    ipivot[istar] = ipivot[k];
                    ipivot[k] = i;
                    temp = d[istar];
                    d[istar] = d[k];
                    d[k] = temp;
                    for( j = 0; j < n; j++ ) {
                       temp = a[istar][j];
                       a[istar][j] = a[k][j];
                       a[k][j] = temp;
                    }
                 }
                 for( i = k+1; i < n; i++ ) {
                    a[i][k] /= a[k][k];
                    temp = a[i][k];
                    for ( j = k+1; j < n; j++ )
                       a[i][j] -= temp * a[k][j];
                 }
              }
           }
  /*
     Check to see if the matrix is singular
  */
           if( a[n-1][n-1] == 0.0 )
              iflag = 0;
           a1d_free_dbl( d );
           return( iflag );
  /*
     Check the ratio of the min and max pivots
  */
  /*
           rowmax = fabs((double)a[0][0]);
           colmax = rowmax;
           for( i = 1; i < n; i++ ) {
              temp = fabs((double)a[i][i]);
              if( temp < rowmax )
                 rowmax = temp;
              if( temp > colmax )
                 colmax = temp;
           }
           if( rowmax < colmax * MACHEPS )
              iflag = 0;
           a1d_free_dbl( d );
           return( iflag );
  */
        }
     }
  /* ----------------------------------------------------------------------------
  
  SOLVE1
  
      This function uses the factorized matrix [a] to solve the system of
      equations
                               [a]{x} = {b}
  
      This is the c version of the fortran code given in, "Elementary Numerical
      Analysis," S. D. Conte and C. de Boor, McGraw-Hill Co, pp. 164
  
  -----------------------------------------------------------------------------*/
  void solve1( a, ipivot, b, n, x )
  long double **a;
  long double *b;
  long double *x;
  int *ipivot;
  int n;
     {
        int i;
        int j;
        int ip;
        long double sum;
  
        if( n <= 0 ) return;
        if( n <= 1 ) {
           x[0] = b[0] / a[0][0];
           return;
        }
        ip = ipivot[0];
        x[0] = b[ip];
        for( i = 1; i < n; i++ ) {
           sum = 0.0;
           for( j = 0; j < i; j++ )
              sum += a[i][j] * x[j];
           ip = ipivot[i];
           x[i] = b[ip] - sum;
        }
        x[n-1] /= a[n-1][n-1];
        for( i = n-2; i >= 0; i-- ) {
           sum = 0.0;
           for( j = i+1; j < n; j++ )
              sum += a[i][j] * x[j];
           x[i] = (x[i] - sum) / a[i][i];
        }
        return;
     }
  /* ----------------------------------------------------------------------------
  
  A3D_ALLO_DBL
  
      This function allocates storage for a three dimensional long double precision
      array. The array deminsions are (dim1,dim2,dim3). The function returns an
      array of pointers to long doubles.
  
  -----------------------------------------------------------------------------*/
  long double ***a3d_allo_dbl( dim1, dim2, dim3 )
     int dim1;
     int dim2;
     int dim3;
     {
        long double ***array;
        long double **a2d_allo_dbl( int, int );
        void a2d_free_dbl( long double **, int );
        int i;
        int j;
  
        if( dim1 < 1 || dim2 < 1 || dim3 < 1 ) {
           array = NULL;
           return( array );
        }

        array = (long double ***)malloc(dim1 * sizeof(long double **));
  
        if( array == NULL )
           return( array );
  
        for( i = 0; i < dim1; i++ ) {
           array[i] = a2d_allo_dbl( dim2, dim3 );
           if( array[i] == NULL ) {
              for( j = i-1; j >= 0; j-- )
                 a2d_free_dbl( array[j], dim2 );
              free( array );
              array = NULL;
              return( array );
           }
        }
        return( array );
     }
  /* ----------------------------------------------------------------------------
  
  A3D_FREE_DBL
  
      This function frees the memory allocated by the function a3d_allo.
  
  -----------------------------------------------------------------------------*/
  void a3d_free_dbl( array, dim1, dim2 )
     long double ***array;
     int dim1;
     int dim2;
     {
        void a2d_free_dbl( long double **, int );
        int i;
  
        if( array == NULL )
           return;
           
        for( i = dim1-1; i >= 0; i-- )
           if( array[i] != NULL )
              a2d_free_dbl( array[i], dim2 );
  
        if( array != NULL)
           free( array );
     }
  /* ----------------------------------------------------------------------------
  
  A2D_ALLO_DBL
  
      This function allocates storage for a two dimensional long double precision
      array. The array deminsions are (dim1,dim2). The function returns an
      array of pointers to long doubles.
  
  -----------------------------------------------------------------------------*/
  long double **a2d_allo_dbl( dim1, dim2 )
     int dim1;
     int dim2;
     {
        long double **array;
        int i;
        int j;
  
        if( dim1 < 1 || dim2 < 1 ) {
           array = NULL;
           return( array );
        }
    
        array = (long double **)malloc(dim1 * sizeof(long double *));

        if( array == NULL )
           return( array );

        for( i = 0; i < dim1; i++ ) {
           array[i] = (long double *)malloc(dim2 * sizeof(long double));
           if( array[i] == NULL ) {
              for( j = i-1; j >= 0; j-- )
                 free( array[j] );
              free( array );
              array = NULL;
              return( array );
           }
        }
  
        return( array );
     }
  /* ----------------------------------------------------------------------------
  
  A2D_FREE_DBL
  
      This function frees the memory allocated by the function a2d_allo.
  
  -----------------------------------------------------------------------------*/
  void a2d_free_dbl( array, dim1 )
     long double **array;
     int dim1;
     {
        int i;
        
        if( array == NULL )
           return;
           
        for( i = dim1-1; i >= 0; i-- )
           if( array[i] != NULL )
              free( array[i] );
  
        if( array != NULL)
           free( array );
     }
  /* ----------------------------------------------------------------------------
  
  A1D_ALLO_DBL
  
     This function allocates storage for a one dimensional long double precision
     array of length dim1. The function returns a pointer to an array of long doubles.
  
  -----------------------------------------------------------------------------*/
  long double *a1d_allo_dbl( dim1 )
     int dim1;
     {
        long double *array;

        if( dim1 < 1 )
           array = NULL;
        else  
           array = (long double *)malloc(dim1 * sizeof(long double));
  
        return( array );
  
     }
  /* ----------------------------------------------------------------------------
  
  A1D_FREE_DBL
  
      This function frees the memory allocated by the function a1d_allo.
  
  -----------------------------------------------------------------------------*/
  void a1d_free_dbl( array )
     long double *array;
     {
  
        if( array == NULL )
           return;
        free( array );
     }
  /* ----------------------------------------------------------------------------
  
  A1D_ALLO_INT
  
     This function allocates storage for a one dimensional int
     array of length dim1. The function returns a pointer to an array of ints.
  
  -----------------------------------------------------------------------------*/
  int *a1d_allo_int( dim1 )