math_util.c

multipleshooting to find the roots of the non-linear functions

         
      for( i = 0; i < m; i++ ) {
         vec_column( bvec, b, i, n );
         solve( a, ipivot, bvec, n, xvec );
         for( j = 0; j < n; j++ )
            x[j][i] = xvec[j];
      }
         
      a1d_free_int( ipivot );
      a1d_free_dbl( bvec );
      a1d_free_dbl( xvec );
      return(0);
      
   }
/* ----------------------------------------------------------------------------
  
  MAT_EXP
  
      This function computes the matrix exponential, via Pade approximation, of
      an n by n matrix a.  The result is put in the n by n matrix exp_a.
  
                               [exp_a] = exp([a])
 
    RETURN CODES:
   	       0 - normal completion.
           1 - Unable to allocate storage.
           2 - Unable to invert approximant.

-----------------------------------------------------------------------------*/

int mat_exp( exp_a, a, n )
long double **exp_a, **a;
int n;
   {
      int err, k, p, q, qp1, q2p1;
      long double c, s, s2;
      long double **X, **cX, **d, **eye, **temp;
      long double mat_norm_c( long double **, int );
      int mat_solve( long double **, long double **, long double **, int, int );
      void mat_muls( long double **, long double **, long double, int, int );
      void mat_mul( long double **, long double **, long double **, int, int, int );
      void mat_add( long double **, long double **, long double **, int, int );
      void mat_sub( long double **, long double **, long double **, int, int );
      void mat_copy( long double **, long double **, int, int );
    
      X = a2d_allo_dbl( n, n );      
      cX = a2d_allo_dbl( n, n );      
      eye = a2d_allo_dbl( n, n );      
      d = a2d_allo_dbl( n, n );
      temp = a2d_allo_dbl( n , n );      
      
      if( X == NULL || cX == NULL || eye == NULL || d == NULL || temp == NULL ) {
         a2d_free_dbl( X , n );
         a2d_free_dbl( cX , n );
         a2d_free_dbl( eye , n );
         a2d_free_dbl( d , n );
         a2d_free_dbl( temp , n );
         return(1);
      }
      
      mat_null( d, n, n );
      mat_null( eye, n, n );
/*

   Scale a by a power of 2 so that its norm is < 1/2.

*/   
      s = mat_norm_c( a, n );
            
      if( s > 0 )
         s = (int)max_num(0,(log(s)/log(2))+2);
                  
      s2 = 1.0/pow( 2, s );
     
      mat_muls( a, a, s2, n, n );
/*

   Pade approximation for exp( a );

*/      
      
      for( k = 0; k < n; k++ )
         eye[k][k] = 1.0;
         
      mat_copy( X, a, n, n );
      
      c = 0.5;
      q = 6;
      p = 1;
      qp1 = q + 1;
      q2p1 = 2*q + 1;
      
      mat_muls( temp, a, c, n, n );
     
      mat_add( exp_a, eye, temp, n, n );
      mat_sub( d, eye, temp, n, n );
      for( k = 2; k <= q; k++ ) {
         c = c * (qp1-k) / (k*(q2p1-k));
         mat_mul( temp, a, X, n, n, n);
         mat_copy( X, temp, n, n );
         mat_muls( cX, X, c, n, n );
         mat_add( exp_a, exp_a, cX, n, n );
         if( p )
            mat_add( d, d, cX, n, n );
         else
            mat_sub( d, d, cX, n, n );

         p = NOT(p);
      }
      
      err =  mat_solve( temp, d, exp_a, n, n );
      if( err != 0 ) {
         printf("Error = %d, from funtion mat_solve, in exp_a\n",err);
         a2d_free_dbl( X , n );
         a2d_free_dbl( cX , n );
         a2d_free_dbl( eye , n );
         a2d_free_dbl( d , n );
         a2d_free_dbl( temp , n );         
         return(2);
      }
/*
   Undo scaling by repeated squaring
*/      
      for( k = 1; k <= s; k++ ) {
         mat_mul( exp_a, temp, temp, n, n, n );
         mat_copy( temp, exp_a, n, n );
      }
      a2d_free_dbl( X , n );
      a2d_free_dbl( cX , n );
      a2d_free_dbl( eye , n );
      a2d_free_dbl( d , n );
      a2d_free_dbl( temp , n );      
      return(0);
   }
/* ----------------------------------------------------------------------------
   VEC_COLUMN
   
   This function copies the j-th column from matrix b into vector a, i.e.,
   
   a[i] = b[i][j].
   
 ----------------------------------------------------------------------------*/ 
void vec_column( a, b, j, n )
long double *a, **b;
int j, n;
   {
      int i;
      for( i = 0; i < n; i++ )
         a[i] = b[i][j];
    }
/* ----------------------------------------------------------------------------
   VEC_MUL_S
   
   This function multiplies a vector by a scalar.
   
   {a} = {b}*c.
   
----------------------------------------------------------------------------*/ 
void vec_mul_s( a, b, c, n )
long double *a, *b, c;
int n;
   {
      int i;
      for( i = 0; i < n; i++ )
         a[i] = b[i]*c;
    }
/* ----------------------------------------------------------------------------

PRINT_VEC_INT

    This function prints an vector of integers to the screen.

-----------------------------------------------------------------------------*/
void print_vec_int( a, m )
int *a;
int m;
   {
      int i;
      int loop;
      int j;
      int istart;
      int iend;

      if( m <= 0 ) return;
      loop = m/5 + 1;
      istart = 0;
      iend = 5;
      if( iend > m )
         iend = m;
      for( j = 0; j < loop; j++ ) {
         for( i = istart; i < iend; i++ )
            printf(" %d ",a[i]);
         printf("\n");
         istart = iend;
         iend += 5;
         if( iend > m )
            iend = m;
      }
   }
/* ----------------------------------------------------------------------------

PRINT_VEC_DBL

    This function prints an vector of long doubles to the screen.

-----------------------------------------------------------------------------*/
void print_vec_dbl( a, m )
long double *a;
int m;
   {
      int i;
      int loop;
      int j;
      int istart;
      int iend;

      if( m <= 0 ) return;
      loop = m/5 + 1;
      istart = 0;
      iend = 5;
      if( iend > m )
         iend = m;
      for( j = 0; j < loop; j++ ) {
         for( i = istart; i < iend; i++ )
            printf(" %-Le ",a[i]);
         printf("\n");
         istart = iend;
         iend += 5;
         if( iend > m )
            iend = m;
      }
   }
/* ----------------------------------------------------------------------------

PRINT_MAT

    This function prints a matrix of long doubles to the screen.

-----------------------------------------------------------------------------*/
void print_mat( a, m, n )
long double **a; /* pointer to an m by n matrix */
int m;
int n;
   {
      int i;
      int j;

      if( m <= 0 || n <= 0) return;
      for( i = 0; i < m; i++ ) {
         for( j = 0; j < n; j++ )
            printf(" %6.3Le ",a[i][j]);
/*            printf(" %-20.15Le ",a[i][j]);*/
         printf("\n");
      }
   }
/* ----------------------------------------------------------------------------

PAUSE - A crude pause function. (Halts program until a return is entered)

-----------------------------------------------------------------------------*/
void pause()
  {
     char c;
     printf("\nPause ...");
     c = getchar();
     c = c;
  }
/* ----------------------------------------------------------------------------

FPRINT_MAT

    This function prints a matrix of long doubles to the screen.

-----------------------------------------------------------------------------*/
void fprint_mat( file_name, a, m, n )
FILE *file_name;
long double **a; /* pointer to an m by n matrix */
int m;
int n;
   {
      int i;
      int j;

      for( i = 0; i < m; i++ ) {
         for( j = 0; j < n; j++ ) {
            fprintf(file_name," %-Le ",a[i][j]);
         }            
         fprintf(file_name,"\n");
         
      }
   }
/* ----------------------------------------------------------------------------

FILE_PRINT_VEC_DBL

    This function prints a vector of long doubles to a file.

-----------------------------------------------------------------------------*/
void file_print_vec_dbl( output_file, a, m )
FILE *output_file;
long double *a;
int m;
   {
      int i;
      int loop;
      int j;
      int istart;
      int iend;

      loop = m/5 + 1;
      istart = 0;
      iend = 5;
      if( iend > m )
         iend = m;
      for( j = 0; j < loop; j++ ) {
         for( i = istart; i < iend; i++ )
            fprintf(output_file," %-Le\t ",a[i]);
         fprintf(output_file,"\n");
         istart = iend;
         iend += 5;
         if( iend > m )
            iend = m;
      }
   }
/* ----------------------------------------------------------------------------

FILE_READ_VEC_DBL

    This function reads a vector of long doubles from a file.

-----------------------------------------------------------------------------*/
void file_read_vec_dbl( input_file, a, m )
FILE *input_file;
long double *a;
int m;
   {
      int i;
      for( i = 0; i < m; i++ )
         fscanf( input_file,"%Le",&a[i]);
   }
/* ----------------------------------------------------------------------------

    QR Factorization Routines

-----------------------------------------------------------------------------*/
void qr_qtc( a, c, m, n, p )
long double **a, **c;
int m, n, p;
/*
a = a(m,n) - contains the QR decomposition of a; with the essential part
    of Q in the lower triangle of a;
c = c(m,p)
c = Q(transpose)*c on output
*/
{
/*#include "proto.c"*/
   int i, j, k;
   long double **a1, *v;
   void row_house( long double **, long double *, int, int);
   
   a1 = a2d_allo_dbl( m, p );
   v = a1d_allo_dbl( m );
   
   if( a1 == NULL || v == NULL ) {
      printf("\nERROR -> Out of memory in QR_QTC\n");
      exit(1);
   }   
   
   for( j = 0; j < n; j++ ) {

      vec_null( v, m );
      mat_null( a1, m, p );
      
      v[0] = 1.0;
      for( k = j+1; k < m; k++ )
         v[k-j] = a[k][j];
      for( i = j; i < m; i++ )
         for( k = 0; k < p; k++ )
            a1[i-j][k] = c[i][k];
      row_house( a1, v, (m-j), p );
      for( i = j; i < m; i++ )
         for( k = 0; k < p; k++ )
            c[i][k] = a1[i-j][k];
   }
   a1d_free_dbl( v );
   a2d_free_dbl( a1, m );
}
void house( v, x, n )
long double *v, *x;
int n;
{
/*#include "proto.c"*/
   int i;
   long double mu, beta;
   
   mu = 0.0;
   
   for( i = 0; i < n; i++ ) {
      mu = mu + x[i]*x[i];
      v[i] = x[i];
   }
   
   mu = sqrt((double) mu);
   
   if( mu == 0.0 ) {
      v[0] = 1.0;
      return;
   } else {
      beta = x[0] + sgn_num(x[0])*mu;
      for( i = 1; i < n; i++)
         v[i] /= beta;
      v[0] = 1.0;
   }
   return;
}
void row_house( a, v, m, n )
long double **a, *v;
int m, n;
{
   int i, j;
   long double *w, beta;
   long double *a1d_allo_dbl( int );
   void a1d_free_dbl( long double * );
      
   w = a1d_allo_dbl( n );
   
   if( w == NULL ) {
      printf("\nERROR -> Out of memory in ROW_HOUSE\n");
      exit(1);
   }
   beta = 0;
   for( i = 0; i < m; i++ )
      beta += v[i]*v[i];
   beta = -2.0/beta;
   for( i = 0; i < n; i++ ) {
      w[i] = 0;
      for( j = 0; j < m; j++ )
         w[i] += a[j][i]*v[j];
      w[i] *= beta;
   }
   
   for( i = 0; i < m; i++ )
      for( j = 0; j < n; j++ )
         a[i][j] += v[i]*w[j];     
   
   a1d_free_dbl( w );
}  
int qr_fac( a, m, n )
long double **a;
int m, n;
/*
   This function performs the Householder QR factorization of an m by n
   matrix a, where m