z1.c

multipleshooting to find the roots of the non-linear functions

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "math_util.h"
/*
  
  find u(t) to minimimize 
  J = integral(from t=0 to t=5) { (x1^2 + x2^2 + u^2)/2 } dt
  
  subject to 
  
 x1_dot = x2
 x2_dot = (-x1+(1-x1^2)*x2)/tau + u
 -(x1+0.25) <= 0
 x1(0) = 1
 x2(0) = 0
  
*/
long double mu, tau;

int main()
{
	char file_name[12];
	FILE *output_file;
   int n, m, ms, i, j, ierr, itt;
   long double tend, **yout, **zout, *time, eps;

   void fun( long double *, long double *, long double *,
             long double * );
   void dfun( long double *, long double *, long double **,
              long double **, long double **, 
              long double ** );
   void bc( long double *, long double *, long double * );
   void dbc( long double *, long double *, long double **,
             long double ** );
   int mshoot_dae( long double **, long double **, 
                   long double *, int, int, int, 
                   long double, 
                   void(*)( long double *, long double *, 
                            long double *, long double * ), 
                   void(*)( long double *, long double *, 
                            long double **, long double **, 
                            long double **, long double ** ), 
                   void(*)( long double *, long double *, 
                            long double * ), 
                   void(*)( long double *, long double *,
                            long double **, long double ** ) );
   

   n = 4;
   m = 1;
   ms = 201;
   time = a1d_allo_dbl( ms );
   yout = a2d_allo_dbl( ms, n );
   zout = a2d_allo_dbl( ms, m );
   eps = 1.0e-4;
   tend = 5.0;
   
   if( time == NULL || yout == NULL || zout == NULL ){
      printf("Out of memory in main\n");
   }
   
   mat_null( yout, ms, n );
   
   for( i = 0; i < ms; i++ ) {
      time[i] = i*tend/(ms-1);
      zout[i][0] = 0.0;
      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]);
   }

   tau = 0.01;
   mu = 1.0;
   itt=0;
   while( itt < 30) {
	   sprintf(file_name,"z1_%d",itt++);
	   output_file = fopen(file_name,"w");
	   if( output_file == NULL ) {
		   printf("ERROR unable to open output file\n");
		   exit(1);
	   }
	   ierr = mshoot_dae( yout, zout, time, n, m, ms, eps, fun, dfun, bc, dbc );
	   if( ierr != 0 )
		   break;
	   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");
	   }
	   i=ms-1;
	   printf("%Le %Le %Le %Le %Le %Le\n",time[i],yout[i][0],yout[i][2],yout[i][4],zout[i][0],zout[i][1]);   
	   
	   fclose( output_file );
	   
	   mu *= 2.0;   
   }
   return(0);
}
void bc( y_o, y_f, r )
long double *y_o, *y_f, *r;
{
   r[0] = y_o[0]-1.0;
   r[1] = y_o[1];
   r[2] = y_f[2];
   r[3] = y_f[3];
}
void dbc( y_o, y_f, drdy_o, drdy_f )
long double *y_o, *y_f, **drdy_o, **drdy_f;
{

   mat_null( drdy_o, 4, 4 );
   mat_null( drdy_f, 4, 4 );

   drdy_o[0][0] = 1.0;
   drdy_o[1][1] = 1.0;
   drdy_f[2][2] = 1.0;
   drdy_f[3][3] = 1.0;
}
void fun( y, z, f, g )
long double *y, *z, *f, *g;
{
	long double mg, g_;
	
	g_ = -(y[1]+0.25);
	mg = g_<0?0:g_;
	
   f[0] = y[1];
   f[1] = (-y[0]+(1.0-y[0]*y[0])*y[1])/tau + z[0];
   f[2] = -(y[0]+y[3]*((-1.0-2.0*y[0]*y[1])/tau));
   f[3] = -(y[1]-2.0*mu*mg*mg*mg+y[2]+y[3]*(1.0-y[0]*y[0])/tau);
   g[0] = y[3]+z[0];
}
void dfun( y, z, fy, fz, gy, gz )
long double *y, *z, **fy, **fz, **gy, **gz;
{
   
	long double mg, g_;
	
	g_ = -(y[1]+0.25);
	mg = g_<0?0:g_;

	mat_null( fy, 4, 4 ); /* fy(n,n) */
   mat_null( fz, 4, 1 ); /* fz(n,m) */
   mat_null( gy, 1, 4 ); /* gy(m,n) */
   mat_null( gz, 1, 1 ); /* gz(m,m) */
   
   fy[0][1] = 1.0;
   
   fy[1][0] = (-1.0-2.0*y[0]*y[1])/tau;
   fy[1][1] = (1.0-y[0]*y[0])/tau;
   
   fy[2][0] = -(1.0+y[3]*((-2.0*y[1])/tau));
   fy[2][1] = -(y[3]*((-2.0*y[0])/tau));
   fy[2][3] = -((-1.0-2.0*y[0]*y[1])/tau);
   
   fy[3][0] = -(y[3]*(-2.0*y[0])/tau);
   fy[3][1] = -(1.0+6.0*mu*mg*mg);
   fy[3][2] = -1.0;
   fy[3][3] = -((1.0-y[0]*y[0])/tau);
   
   fz[1][0] = 1.0;
   
   gy[0][3] = 1.0;
   
   gz[0][0] = 1.0;
}