rose.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 MAXIT 200
#define NEW_ITT_1 20
#define NEW_ITT_2 20
#define MACH_EPS 1.0e-19
#define GAMMA2 (7.0/24.0)
#define DELTA1 (1.0)
#define DELTA2 (7.0/24.0)
#define gdeli00 (0.0)
#define gdeli10 (19.0/7.0)
#define gdeli11 (1.0)
#define gdeli20 (53.0/14.0)
#define gdeli21 (0.0)
#define gdeli22 (0.0)
#define gdeli30 (3.0/98.0)
#define gdeli31 (0.0)
#define gdeli32 (1.0/7.0)
#define gdeli33 (1.0)
#define deli00 (1.0)
#define deli10 (55.0/7.0)
#define deli11 (24.0/7.0)
#define deli20 (71.0/7.0)
#define deli21 (39.0/7.0)
#define deli22 (1.0)
#define deli30 (-15.0/49.0)
#define deli31 (129.0/49.0)
#define deli32 (23.0/7.0)
#define deli33 (24.0/7.0)
#define mudeli0 (71.0/39.0)
#define mudeli1 (1.0)
#define mudeli2 (23.0/39.0)
#define mudeli3 (24.0/39.0)
#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))
/*



*/
int rose_mod_a( t0, t1, y, z, u_fun, u_dfun, n, m, tol )
/*
      This function uses the 4th order Runge-Kutta (ROW) method to integrate a system
      of (n,m) differential-algebraic equations. The integration goes from t0 to t1.
      The result is put in y, z. On input, y = y(t0), z = z(t0).
      Method adapted from Schneider, C., Rosenbrock-type Methods Adapted to Differential-
      Algebraic Systems, Mathematics of Computation, Vol. 56, pp. 201-213, 1989.
  
      Return code -> 0 : Normal completion.
      Return code -> 1 : Error, invalid number of differential and algebraic equations.
      Return code -> 2 : Error, out of memory.
      Return code -> 4 : Error, too many iterations required.
      Return code -> 5 : Error, required stepsize too small.

*/
int n, m;
long double t0, t1, *y, *z, tol;    
void (*u_fun)( long double *, long double *, long double *, long double * );
void (*u_dfun)( long double *, long double *, long double **, long double **, long double **, long double ** );
/*


*/
{
   long double **big_jac1, **big_jac2, *rhs, *lhs, *y0, *z0, *y1, *z1;
   long double **fy, **fz, **gy, **gz, **kapa, **lambda, *f, *g, *g0;
   long double h, hnew, sum, t, error, err_2, fac;
   int i, j, k, p, err, itt;
   int *pivot1, *pivot2;  
/*
   Some simple checks
*/
   if( (n < 1) || (m < 0) ) {
      printf("*** ERROR in ROSE -> Invalid number of differential and algebraic equations\n");
      return( 1 );
   }   
/*
   Allocate storage
*/
   p = m+n;
   
   big_jac1 = a2d_allo_dbl( p, p );
   big_jac2 = a2d_allo_dbl( p, p );
   fy = a2d_allo_dbl( n, n );
   fz = a2d_allo_dbl( n, m );
   gz = a2d_allo_dbl( m, m );
   gy = a2d_allo_dbl( m, n );
   kapa = a2d_allo_dbl( 4, n );
   lambda = a2d_allo_dbl( 4, m );
   pivot1 = a1d_allo_int( p );
   pivot2 = a1d_allo_int( p );
   rhs = a1d_allo_dbl( p );
   lhs = a1d_allo_dbl( p );
   f = a1d_allo_dbl( n );
   g = a1d_allo_dbl( m );
   g0 = a1d_allo_dbl( m );
   y1 = a1d_allo_dbl( n );
   z1 = a1d_allo_dbl( m );
   y0 = a1d_allo_dbl( n );
   z0 = a1d_allo_dbl( m );
   if ( big_jac1 == NULL || big_jac2 == NULL || fy == NULL || pivot1 == NULL || pivot2 == NULL || 
        rhs == NULL || lhs == NULL || y0 == NULL || y1 == NULL || f == NULL || kapa == NULL  ) {
      printf("*** ERROR in ROSE -> Out of memory\n");
      a2d_free_dbl( big_jac1, p );
      a2d_free_dbl( big_jac2, p );
      a2d_free_dbl( kapa, 4 );
      a2d_free_dbl( fy, n );
      a1d_free_int( pivot1 );
      a1d_free_int( pivot2 );
      a1d_free_dbl( rhs );
      a1d_free_dbl( lhs );
      a1d_free_dbl( f );
      a1d_free_dbl( y0 );
      a1d_free_dbl( y1 );
      return( 2 );
   }
   if( (m > 0) && ( fz == NULL || gz == NULL || gy == NULL || z0 == NULL || z1 == NULL || g == NULL ||
        g0 == NULL || lambda == NULL) ){
      printf("*** ERROR in ROSE -> Out of memory\n");
      a2d_free_dbl( fz, n );
      a2d_free_dbl( gz, m );
      a2d_free_dbl( gy, m );
      a1d_free_dbl( g );
      a1d_free_dbl( g0 );
      a1d_free_dbl( z0 );
      a1d_free_dbl( z1 );
      a2d_free_dbl( lambda, 4 );
      return( 2 );
   }
/*
   Make initial conditions consistent  
 */
     
   (*u_fun)( y, z, f, g );
   vec_copy( g0, g, m );
   
/* Initial step size */

   h = pow(tol,0.25);
   if( h > fabs((double)(t1-t0)) )
       h = dmax(t1,t0)-dmin(t1,t0);
   if( (t0+h) > t1 )
      h = t1-t0;
      
   t = t0;
      
   for( itt = 0; itt < MAXIT; itt++ ) {
   
      t += h;
   
      vec_copy( y0, y, n );
      vec_copy( z0, z, m );
   /*
   Step 1
*/            

/*
Form big_jac_1
*/            
      mat_null( big_jac1, p, p );
   
      (*u_fun)( y0, z0, f, g );
      (*u_dfun)( y0, z0, fy, fz, gy, gz );
   
      for( i = 0; i < p; i++ )
         for( j = 0; j < p; j++ ) {
            if( i < n ) {
               if( i == j )
                  big_jac1[i][j] = 1.0;
            }
            if( i >= n ) {
               if( j < n )
                  big_jac1[i][j] = gy[i-n][j];
               else
                  big_jac1[i][j] = gz[i-n][j-n];
            }
         }
      err = factor1( big_jac1, p, pivot1 );
      if( err == 0 || err == 2 ) {
         printf("*** ERROR in ROSE -> Jacobian matrix singular\n");
         printf("                     DAE is not index 1\n");         
         return( 3 );
      }
      vec_null( rhs, p );
   
      for( i = 0; i < p; i++ ) {
         if( i < n )
            rhs[i] = DELTA1*h*f[i];
         else
            rhs[i] = -(g[i-n]-g0[i-n]);
      }

      solve1( big_jac1, pivot1, rhs, p, lhs );
   
      for( i = 0; i < p; i++ ) {
         if( i < n ) {
            kapa[0][i] = lhs[i];
            y0[i] += lhs[i];            /* Update y0 and z0 for the next step */
         }
         else {
            lambda[0][i-n] = lhs[i];
            z0[i-n] += lhs[i];
         }
      }
/*
   Step 2
*/            
/*
Form big_jac_2
*/            
      mat_null( big_jac2, p, p );
   
      (*u_fun)( y0, z0, f, g );   
      (*u_dfun)( y0, z0, fy, fz, gy, gz );
   
      for( i = 0; i < p; i++ )
         for( j = 0; j < p; j++ ) {
            if( i < n ) {
               if( i == j )
                  big_jac2[i][j] = 1.0;
               if( j < n )
                  big_jac2[i][j] += -h*GAMMA2*fy[i][j];
               if( j >= n ) 
                  big_jac2[i][j] = -h*GAMMA2*fz[i][j-n];
            }
            if( i >= n ) {
               if( j < n )
                  big_jac2[i][j] = gy[i-n][j];
               else
                  big_jac2[i][j] = gz[i-n][j-n];
            }
         }
   
      err = factor1( big_jac2, p, pivot2 );
      if( err == 0 || err == 2 ) {
         printf("*** ERROR in ROSE -> Jacobian matrix singular\n");
         printf("                     DAE is not index 1\n");         
         return( 3 );
      }
   
      for( i = 0; i < n; i++ ) {
         sum = h*f[i];
         for( k = 0; k < n; k++ ) {
            sum += fy[i][k]*kapa[0][k]*h*gdeli10;
         /*for( k = 0; k < m; k++ )*/
            if( k < m ) sum += fz[i][k]*lambda[0][k]*h*gdeli10;
         }
         rhs[i] = DELTA2*(sum - deli10*kapa[0][i]);
      }
      for( i = n; i < p; i++ )
         rhs[i] = -(g[i-n]-g0[i-n]);

      solve1( big_jac2, pivot2, rhs, p, lhs );
   
      for( i = 0; i < p; i++ ) {
         if( i < n ) {
            kapa[1][i] = lhs[i];
            y0[i] += lhs[i];            /* Update y0 and z0 for the next step */
         }
         else {
            lambda[1][i-n] = lhs[i];
            z0[i-n] += lhs[i];
         }
      }

/*
   Step 3
*/            
/*
Form big_jac_1
*/            
      mat_null( big_jac1, p, p );
   
      (*u_fun)( y0, z0, f, g );
      (*u_dfun)( y0, z0, fy, fz, gy, gz );
   
      for( i = 0; i < p; i++ )
         for( j = 0; j < p; j++ ) {
            if( i < n ) {
               if( i == j )
                  big_jac1[i][j] = 1.0;
            }
            if( i >= n ) {
               if( j < n )
                  big_jac1[i][j] = gy[i-n][j];
               else
                  big_jac1[i][j] = gz[i-n][j-n];
            }
         }
      err = factor1( big_jac1, p, pivot1 );
      if( err == 0 || err == 2 ) {
         printf("*** ERROR in ROSE -> Jacobian matrix singular\n");
         printf("                     DAE is not index 1\n");         
         return( 3 );
      }
   
      for( i = 0; i < n; i++ ) {
         sum = h*f[i];
         for( k = 0; k < n; k++ ) {
            sum += h*fy[i][k]*(gdeli20*kapa[0][k] + gdeli21*kapa[1][k]);
         /*for( k = 0; k < m; k++ )*/
            if( k < m ) sum += h*fz[i][k]*(gdeli20*lambda[0][k] + gdeli21*lambda[1][k]);
         }
         rhs[i] = DELTA1*(sum - (deli20*kapa[0][i] + deli21*kapa[1][i]));
      }
      for( i = n; i < p; i++ )
         rhs[i] = -(g[i-n]-g0[i-n]);

      solve1( big_jac1, pivot1, rhs, p, lhs );
   
      for( i = 0; i < p; i++ ) {
         if( i < n ) {
            kapa[2][i] = lhs[i];
            y0[i] += lhs[i];            /* Update y0 and z0 for the next step */
         }
         else {
            lambda[2][i-n] = lhs[i];
            z0[i-n] += lhs[i];
         }
      }

/*
   Step 4
*/            
/*
Form big_jac_2
*/            
      mat_null( big_jac2, p, p );
   
      (*u_fun)( y0, z0, f, g );   
      (*u_dfun)( y0, z0, fy, fz, gy, gz );
   
      for( i = 0; i < p; i++ )
         for( j = 0; j < p; j++ ) {
            if( i < n ) {
               if( i == j )
                  big_jac2[i][j] = 1.0;
               if( j < n )
                  big_jac2[i][j] += -h*GAMMA2*fy[i][j];
               if( j >= n ) 
                  big_jac2[i][j] = -h*GAMMA2*fz[i][j-n];
            }
            if( i >= n ) {
               if( j < n )
                  big_jac2[i][j] = gy[i-n][j];
               else
                  big_jac2[i][j] = gz[i-n][j-n];
            }
         }
   
      err = factor1( big_jac2, p, pivot2 );
      if( err == 0 || err == 2 ) {
         printf("*** ERROR in ROSE -> Jacobian matrix singular\n");
         printf("                         DAE is not index 1\n");         
         return( 3 );
      }
   
      for( i = 0; i < n; i++ ) {
         sum = h*f[i];
         for( k = 0; k < n; k++ ) {
            sum += h*fy[i][k]*(gdeli30*kapa[0][k] + gdeli31*kapa[1][k] + gdeli32*kapa[2][k]);
         /*for( k = 0; k < m; k++ )*/
            if( k < m ) sum += h*fz[i][k]*(gdeli30*lambda[0][k] + gdeli31*lambda[1][k] + gdeli32*lambda[2][k]);
         }
         rhs[i] = DELTA2*(sum - (deli30*kapa[0][i] + deli31*kapa[1][i] + deli32*kapa[2][i]));
      }
      for( i = n; i < p; i++ )
         rhs[i] = -(g[i-n]-g0[i-n]);

      solve1( big_jac2, pivot2, rhs, p, lhs );
   
      for( i = 0; i < p; i++ ) {
         if( i < n ) {
            kapa[3][i] = lhs[i];
            y0[i] += lhs[i];            /* Get the solution */        
         }
         else {
            lambda[3][i-n] = lhs[i];
            z0[i-n] += lhs[i];
         }
      }

/* 
   Compute the error estimate 
*/      
      error = 0.0;
      for( i = 0; i < n; i++ ) {        
         y1[i] = y[i] + mudeli0*kapa[0][i] + mudeli1*kapa[1][i]+ mudeli2*kapa[2][i]+ mudeli3*kapa[3][i];
         if(i==0) error = (y0[i]-y1[i])*(y0[i]-y1[i]);
         else error = dmax(error,(y0[i]-y1[i])*(y0[i]-y1[i]));
      }
/*
 
     To speed things up comment out the next four lines 
     
*/
/*     
      for( i = 0; i < m; i++ ) {
         z1[i] = z[i] + mudeli0*lambda[0][i] + mudeli1*lambda[1][i] + mudeli2*lambda[2][i] + mudeli3*lambda[3][i];
         error += (z0[i]-z1[i])*(z0[i]-z1[i]);
      }
*/

      error = sqrt( (double) error);   

/*
   Compute a new step size
*/
      if( error == 0.0 )
         error = tol/100.0;
      
      hnew = h*dmin(6.0,dmax(0.2,0.9*(sqrt(sqrt(tol/error)))));
      
      if( error <= tol ) { 
         vec_copy( y, y0, n);
         vec_copy( z, z0, m);
      }
      else   /* repeat this iteration with a new step size */
         t = t - h;

      if( (t+hnew) > t1 )
         hnew = t1-t;

      if( t == t1 || itt >= (MAXIT-1) || h < MACH_EPS ) {