ex2con.c

multipleshooting to find the roots of the non-linear functions

#include <stdio.h>
#include <math.h>
#include "math_util.h"
#include <stdlib.h>
long double rho, sigma, umax;
main()
{
   FILE *output_file, *input_file;
   extern long double rho, sigma, umax;
   int n, m, ms, i, j, k, ierr;
   long double tend, y0, y1, y2, y3, z0, z1, z2, **yout, **zout, *time, tol, cost, dt;

   void dyn( long double *, long double *, long double *, long double * );
   void dynjac( long double *, long double *, long double **, long double **, long double **, long double ** );
   void dynbc( long double *, long double *, long double * );
   void dyndbc( long double *, long double *, long double **, long double ** );
   int mshoot_dae( long double **, long double **, long double *, int, int, int, long double, 
                   void(*)(), void(*)(), void(*)(), void(*)() );
   
   n = 4;
   m = 1;
   ms = 79;
   time = a1d_allo_dbl( ms );
   yout = a2d_allo_dbl( ms, n );
   zout = a2d_allo_dbl( ms, m );
   tol = 1.0e-4;
   tend = 0.78;
   dt = tend/(ms-1);
   umax = 1.0;
   
   output_file = fopen("ex2con.dat","w");
   if( output_file == NULL ) {
      printf("ERROR unable to open output file\n");
      exit(1);
   }

   cost = 0.0;
   
   input_file = fopen("ex2.dat","r");
   if( input_file == NULL ) {
      printf("Can not open input file\n");
      exit(1);
   }
   for( i = 0; i < ms; i++ ) {
      fscanf(input_file,"%Le\t",&time[i]);
      time[i] = i*tend/(ms-1);
      for( j = 0; j < n; j++ )
         fscanf(input_file,"%Le\t",&yout[i][j]);
      for( j = 0; j < m; j++ ) 
         fscanf(input_file,"%Le\n",&zout[i][j]);

      printf("%Le %Le %Le %Le %Le %Le\n",time[i],yout[i][0],yout[i][1],yout[i][2],yout[i][3],zout[i][0]);   

      if( i > 0 )
         cost += 0.5*dt*((yout[i][0]*yout[i][0]+yout[i][1]*yout[i][1])+(yout[i-1][0]*yout[i-1][0]+yout[i-1][1]*yout[i-1][1]));
   }
   printf("Cost = %Le\n",cost);
   fclose( input_file );
   
   for( k = 0; k < 30; k++ ) {
      rho = 1.0;
      sigma = 0.15*sqrt(rho);
      ierr = mshoot_dae( yout, zout, time, n, m, ms, tol, dyn, dynjac, dynbc, dyndbc );
      if( ierr != 0 )
         break;
      if( rho < FD_TOL )
         break;
      rho *= 0.1;
   }

   cost = 0.0;
   for( i = 0; i < ms; i++ ) {
      fprintf(output_file,"%Le\t",time[i]);
      for( j = 0; j < n; j++ )
         fprintf(output_file,"%Le\t",yout[i][j]);
      for( j = 0; j < m; j++ )
         fprintf(output_file,"%Le\t",zout[i][j]);
      fprintf(output_file,"\n");
      if( i > 0 )
         cost += 0.5*dt*((yout[i][0]*yout[i][0]+yout[i][1]*yout[i][1])+(yout[i-1][0]*yout[i-1][0]+yout[i-1][1]*yout[i-1][1]));
   }
   printf("Cost = %Le\n",cost);
   pause();
    
   fclose( output_file );    
}
void dynbc( sy0, syms, r )
long double *sy0, *syms, *r;
{
   extern long double rho;
   r[0] = sy0[0]-0.05;
   r[1] = sy0[1];
   r[2] = syms[0];
   r[3] = syms[1];
}
void dyndbc( sy0, syms, a, b )
long double *sy0, *syms, **a, **b;
{
   extern long double rho;
   mat_null( a, 4, 4 );
   mat_null( b, 4, 4 );
   a[0][0] = 1.0;
   a[1][1] = 1.0;

   b[2][0] = 1.0;
   b[3][1] = 1.0;
}
void dyn( y, z, f, g )
long double *y, *z, *f, *g;
{
   extern long double rho, sigma, umax;
   long double x1, x2, p1, p2, d, u,r,L,c, cu;
   x1 = y[0];
   x2 = y[1];
   p1 = y[2];
   p2 = y[3];
   u = z[0];
   d = x1+2.0;
   r=0.0;
   L = exp(25.0*x1/d);
   c = (umax-z[0])*(z[0]+umax);
   cu = -2.0*z[0];
   f[0] = -2.0*(x1+0.25)+(x2+0.5)*L-(x1+0.25)*u;
   f[1] = 0.5-x2-(x2+0.5)*L;
   f[2] = -2.0*x1+2.0*p1-p1*(x2+0.5)*(50.0/(d*d))*L+p1*u+p2*(x2+0.5)*(50.0/(d*d))*L;
   f[3] = -2.0*x2-p1*L+p2*(1.0+L);

   if( c >= sigma )
      g[0] = 2.0*r*u-p1*(x1+0.25)+rho*(-cu/(c*c));
   else
      g[0] = 2.0*r*u-p1*(x1+0.25)+rho*((2.0*c*cu)/(sigma*sigma)-(3.0*cu)/sigma)/sigma;

}
void dynjac( y, z, fy, fz, gy, gz )
long double *y, *z, **fy, **fz, **gy, **gz;
{
   extern long double rho, sigma, umax;
   int n = 4, m = 1;
   void mat_null( long double **, int, int );
   long double x1, x2, p1, p2, d, u, r, L, dL, c, cu, cuu;
   x1 = y[0];
   x2 = y[1];
   p1 = y[2];
   p2 = y[3];
   u = z[0];
   d = x1+2.0;
   r = 0.0;
   L = exp(25.0*x1/d);
   dL = L*(50.0/(d*d));
   
   c = (umax-z[0])*(z[0]+umax);
   cu = -2.0*z[0];
   cuu = -2.0;
   
   mat_null( fy, n, n );
   mat_null( fz, n, m );
   mat_null( gy, m, n );
   mat_null( gz, m, m );
   
   fy[0][0] = -2.0+(x2+0.5)*dL-u;
   fy[0][1] = L;
   fy[1][0] = -(x2+0.5)*dL;
   fy[1][1] = -1.0-L;
   fy[2][0] = - 2.0 - p1*(x2+0.5)*(50.0/(d*d))*dL + p2*(x2+0.5)*(50.0/(d*d))*dL
              + 2.0*p1*(x2+0.5)*(50.0/(d*d*d))*L - 2.0*p2*(x2+0.5)*(50.0/(d*d*d))*L;
   fy[2][1] = -p1*(50.0/(d*d))*L+p2*(50.0/(d*d))*L;
   fy[2][2] = 2.0-(x2+0.5)*(50.0/(d*d))*L+u;
   fy[2][3] = (x2+0.5)*(50.0/(d*d))*L;
   
   fy[3][0] = (-p1+p2)*dL;
   fy[3][1] = -2.0;
   fy[3][2] = -L;
   fy[3][3] = (1.0+L);
   
   fz[0][0] = -(x1+0.25);
   fz[2][0] = p1;
   
   gy[0][0] = -p1;
   gy[0][2] = -(x1+0.25);
   
   if( c >= sigma )
      gz[0][0] = 2.0*r+rho*(-cuu/(c*c)+(2.0*cu*cu)/(c*c*c));
   else 
      gz[0][0] = 2.0*r+rho*((2.0*cu*cu)/(sigma*sigma)+(2.0*c*cuu)/(sigma*sigma)-(3.0*cuu)/sigma)/sigma;

}