mshoot_dae.c

multipleshooting to find the roots of the non-linear functions

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "math_util.h"
#define L1 long double *
#define L2 long double **
#define L3 long double ***
#define MAXIT 1000
#define JMAX 10
int mshoot_dae( yout, zout, time, p, m, ms, u_eps, u_fun, u_dfun, u_bc, u_dbc )
int p;               /* number of differential equations */
int m;               /* number of algebraic equations */
int ms;              /* number of shooting time steps */
long double **yout;  /* output vector of differential variables at every time step */ 
long double **zout;  /* output vector of algebraic variables at every time step */ 
long double *time;   /* vector containing the shooting times*/
long double u_eps;   /* convergence tolerance */
void (*u_fun)( long double *, long double *, long double *, long double * );
   /* user defined function returning the lhs of the differential and algebraic equations */
void (*u_dfun)( long double *, long double *, long double **, long double **, long double **, long double ** );
   /* user defined function returning the jacobian of the differential and algebraic equations  */
void (*u_bc)( long double *, long double *, long double * );
   /* user defined function returning the boundary conditions */
void (*u_dbc)( long double *, long double *, long double **, long double ** );
   /* user defined function returning the derivative of the boundary conditions */
   {   
      int fun_dfun( long double *, long double *, long double *, long double *, long double *, long double **,
                 long double, long double, int, int, void(*)(), void(*)(), long double );
      int m_fun( long double *, long double *, long double *, long double *, long double *,
                 long double, long double, int, int, void(*)(), void(*)(), long double );
      
      void free_all(L3,L2,L3,L3,L2,L2,L2,L2,L2,L2,L2,L1,L1,L1,L2,L2,L2,L2,L2,L2,int,int,int,int);

      long double ***e, **f, ***g, ***r, ***h, **ad, **bd, **a, **b, **ds, **ynew, **znew;
      long double *ebc, *fbc, *gbc, **fa, **fb, **fy, **fz, **gy, **gz;      
      long double fnorm1, fnorm2, fnorm3, hnorm1, hnorm2, fac, rhs;
      int n, i, j, k, ierr, iblock, last;
      int itt = 0;
      FILE *output_file;

      printf("\n************************************\n");
      printf("***                              ***\n");
      printf("***         MSHOOT-DAE           ***\n");
      printf("***     Version c (6/21/94)      ***\n");
      printf("***    Rosenbrock integration    ***\n");
      printf("***      QR Factorization        ***\n");
      printf("***                              ***\n");
      printf("************************************\n\n");
/*
   Some simple error checks
*/
      if( p <= 0 || m <= 0 || m > p ) { /* rose_mod_a and rose_mod_sen_a requires m <= p */
         printf("\n*** ERROR in MSHOOT -> Invalid number of differential/algebraic equations\n");
         return( 1 );
      }
      if( ms < 2 ) {
         printf("\n*** ERROR in MSHOOT -> Invalid number of shooting points\n");
         return( 2 );
      }      
      for( i = 0; i < (ms-1); i++ )
         if( time[i] >= time[i+1] ) {
            printf("\n*** ERROR in MSHOOT -> Invalid sequence of shooting times\n");
            return( 3 );
         }
/*
   Allocate storage
*/
      n = p+m;
      e  = a3d_allo_dbl( (ms-1), n, n );
      f  = a2d_allo_dbl( ms, n );
      g  = a3d_allo_dbl( ms, n, n );
      r = g;
      h  = a3d_allo_dbl( (ms-1), n, n );
      ds = a2d_allo_dbl( ms, n );
      ynew = a2d_allo_dbl( ms, p );
      znew = a2d_allo_dbl( ms, m );
      a  = a2d_allo_dbl( n, n );
      b  = a2d_allo_dbl( n, n );
      ad = a2d_allo_dbl( (2*n), n );
      bd = a2d_allo_dbl( (2*n), (2*n+1) );
      ebc = a1d_allo_dbl( p );
      fbc = a1d_allo_dbl( p );
      gbc = a1d_allo_dbl( m );
      fa  = a2d_allo_dbl( p, p );
      fb  = a2d_allo_dbl( p, p );
      fy  = a2d_allo_dbl( p, p );
      fz  = a2d_allo_dbl( p, m );
      gy  = a2d_allo_dbl( m, p );
      gz  = a2d_allo_dbl( m, m );
      if( e == NULL || f == NULL || g == NULL || h == NULL || ds == NULL || ynew == NULL || znew == NULL || a == NULL ||
          b == NULL || ad == NULL || bd == NULL || ebc == NULL || fbc == NULL || gbc == NULL || fa == NULL 
          || fb == NULL || fy == NULL || fz == NULL || gy == NULL || gz == NULL ) {
         printf("\n*** Error in MSHOOT_DAE -> Out of memory\n");          
         free_all(e,f,g,h,ds,ynew,znew,a,b,ad,bd,ebc,fbc,gbc,fa,fb,fy,fz,gy,gz,ms,n,p,m);
         return( -1 );
      }
/*
   Newton iteration
*/
      do {
/*
   Evaluate the function at the initial guess
*/
         for( i = 0; i < (ms-1); i++ ) {
            ierr = fun_dfun( yout[i], zout[i], yout[i+1], zout[i+1], f[i], g[i], time[i], time[i+1], p, m, (*u_fun), (*u_dfun), u_eps );
            if( ierr != 0 ) {
               printf("*** ERROR in MSHOOT_DAE -> FUN_DFUN -> %d\n",ierr);
               free_all(e,f,g,h,ds,ynew,znew,a,b,ad,bd,ebc,fbc,gbc,fa,fb,fy,fz,gy,gz,ms,n,p,m);
               return( ierr );
            }
         }     
        (*u_bc)( yout[0], yout[ms-1], ebc );
        (*u_fun)( yout[ms-1], zout[ms-1], fbc, gbc );
        for( i = 0; i < n; i++ )
           if( i < m )
              f[ms-1][i] = gbc[i];
           else
              f[ms-1][i] = ebc[i-m];
/*
   Compute the maximum residual error for first iteration
*/
         if( itt == 0 ) {
            fnorm1 = 0.0;
            for( i = 0; i < ms; i++ ) {
               fnorm2 = 0.0;
               for( j = 0; j < n; j++ )
                  fnorm2 += f[i][j]*f[i][j];
               if( fnorm2 > fnorm1 )
                  fnorm1 = fnorm2;
            }
            fnorm1 = sqrt( (double) fnorm1 );
         }
      
         printf("itt = %d, |F(s^%d)| = %8.3Le, ",itt,itt,fnorm1);

         if( fnorm1 <= u_eps ) {
            ierr = 0;
            break;
         }
/*
   Evaluate Jacobian of boundary condition
*/         
         (*u_dbc)( yout[0], yout[ms-1], fa, fb );
         (*u_dfun)( yout[ms-1], zout[ms-1], fy, fz, gy, gz );
         mat_null( a, n, n );
         mat_null( b, n, n );
         for( i = 0; i < n; i++ )
            for( j = 0; j < n; j++ ) {
               if( (i >= m) && (j >= m) ) {
                  a[i][j] = fa[i-m][j-m];
                  b[i][j] = fb[i-m][j-m];
               }
               if( (i < m) && (j < m) )
                  b[i][j] = gz[i][j];
               if( (i < m) && (j >= m) )
                  b[i][j] = gy[i][j-m];
            }
/*
   Block QR factorization to compute ds
*/
         for( i = 0; i < n; i++ )   /* E0 =[0,0;0,-I], H0 = G0 */
            for( j = 0; j < n; j++ ) {
               h[0][i][j] = g[0][i][j];
               e[0][i][j] = 0.0;
               if( (i >= m) && (i == j) )
                  e[0][i][j] = -1.0;
            }
            
         for( i = 0; i < (ms-2); i++ ) {
            mat_null( h[i+1], n, n ); /*  Hi = 0, i = 1, 2,..., ms-2 */
/* Form Ad */
            for( j = 0; j < 2*n; j++ )
               for( k = 0; k < n; k++ )
                  if( j < n ) 
                     ad[j][k] = e[i][j][k];
                  else 
                     ad[j][k] = g[i+1][j-n][k];

            ierr = qr_fac( ad, 2*n, n );
            
            if( ierr != 0 ) {
               printf("\n*** ERROR in MSHOOT-DAE -> Unable to perform QR factorization, out of memory\n");
               free_all(e,f,g,h,ds,ynew,znew,a,b,ad,bd,ebc,fbc,gbc,fa,fb,fy,fz,gy,gz,ms,n,p,m);
               return( 5 );
            }

            for( j = 0; j < n; j++ )
               for( k = j; k < n; k++ )
                  r[i][j][k] = ad[j][k];
/* Form Bd */
            for( j = 0; j < 2*n; j++ ) {
               for( k = 0; k < n; k++ ) {
                  if( j < n )
                     bd[j][k] = 0.0;
                  else {
                     bd[j][k] = 0.0;
                     if( ((j-n) >= m) && ((j-n) == k ) ) 
                        bd[j][k] = -1.0;
                  }
               }
               for( k = n; k < 2*n; k++ ) {
                  if( j < n )
                     bd[j][k] = h[i][j][k-n];
                  else
                     bd[j][k] = h[i+1][j-n][k-n];
               }
               if( j < n )
                  bd[j][2*n] = f[i][j];
               else
                  bd[j][2*n] = f[i+1][j-n];
            }
/* Compute Q(transpose)*Bd */ 

            qr_qtc( ad, bd, 2*n, n, (2*n+1) );

            for( j = 0; j < 2*n; j++ ) {
               for( k = 0; k < n; k++ ) {
                  if( j < n )
                     e[i][j][k] = bd[j][k];
                  else
                     e[i+1][j-n][k] = bd[j][k];
               }
               for( k = n; k < 2*n; k++ ) {
                  if( j < n )
                     h[i][j][k-n] = bd[j][k];
                  else
                     h[i+1][j-n][k-n] = bd[j][k];
               }
               if( j < n )
                  f[i][j] = bd[j][2*n];
               else
                  f[i+1][j-n] = bd[j][2*n];
            }
         }
/* Factorize the last 2n by 2n block */         
         for( i = 0; i < 2*n; i++ ) {
            for( j = 0; j < n; j++ ) {
               if( i < n )
                  bd[i][j] = e[ms-2][i][j];
               else
                  bd[i][j] = b[i-n][j];
            }
            for( j = n; j < 2*n; j++ ) {
               if( i < n )
                  bd[i][j] = h[ms-2][i][j-n];
               else
                  bd[i][j] = a[i-n][j-n];
            }
         }
         ierr = qr_fac( bd, 2*n, 2*n );
         if( ierr != 0 ) {
            printf("\n*** ERROR in MSHOOT-DAE -> Unable to perform QR factorization, out of memory\n");
            free_all(e,f,g,h,ds,ynew,znew,a,b,ad,bd,ebc,fbc,gbc,fa,fb,fy,fz,gy,gz,ms,n,p,m);
            return( 6 );
         }
         
         for( i = 0; i < n; i++ )
            ad[i][0] = f[ms-2][i];
         for( i = 0; i < n; i++ )
            ad[i+n][0] = f[ms-1][i];
         
         qr_qtc( bd, ad, 2*n, 2*n, 1 );
            
         for( i = 0; i < n; i++ )
            f[ms-2][i] = ad[i][0];
         for( i = 0; i < n; i++ )
            f[ms-1][i] = ad[i+n][0];
            
         for( i = 0; i < 2*n; i++ ) {
            for( j = 0; j < n; j++ )
               if( i < n )
                  r[ms-2][i][j] = bd[i][j];
            for( j = n; j < 2*n; j++ ) {
               if( i < n )
                  h[ms-2][i][j-n] = bd[i][j];
               else
                  r[ms-1][i-n][j-n] = bd[i][j];
            }
         }
/*
   Backsubstitution to compute ds[i]
*/
         for( iblock = ms-1; iblock >= 0; iblock-- ) {
            for( i = 0; i < n; i++ ) {
              if( fabs((double)r[iblock][i][i]) < MACH_EPS ) {
                 printf("\n*** WARNING in MSHOOT-DAE -> Shooting matrix singular, in block -> %d\n",iblock);
              }
           }
         }
         for( iblock = ms-1; iblock >= 0; iblock-- ) {
            if( iblock == (ms-1) ) {
               if( fabs((double)r[ms-1][n-1][n-1]) < MACH_EPS )
                  ds[0][n-1] = 0.0;
               else
                  ds[0][n-1] = f[ms-1][n-1]/r[ms-1][n-1][n-1];
               for( j = n-2; j >= 0; j-- ) {
                  rhs = f[ms-1][j];
                  for( k = j+1; k < n; k++ )
                     rhs -= r[ms-1][j][k]*ds[0][k];
                     if( fabs((double)r[ms-1][j][j]) < MACH_EPS )
                        ds[0][j] = 0.0;
                     else
                        ds[0][j] = rhs/r[ms-1][j][j];
               }
            } else if( iblock == (ms-2) ) {
               for( j = n-1; j >= 0; j-- ) {
                  rhs = f[ms-2][j];
                  for( k = 0; k < n; k++ )