rose.c

multipleshooting to find the roots of the non-linear functions

         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 );
         if( m > 0 ) {
            a2d_free_dbl( lambda, 4 );
            a2d_free_dbl( fz, n );
            a1d_free_dbl( g );
            a1d_free_dbl( g0 );
            a2d_free_dbl( gz, m );
            a2d_free_dbl( gy, m );
            a1d_free_dbl( z0 );
            a1d_free_dbl( z1 );
         }

         if( h < MACH_EPS && t != t1 ) {
            printf("\n*** ERROR in ROSE -> Required step size too small\n");
            return( 5 );
         }
         if( itt <= (MAXIT-1) ) 
            return( 0 );
         else {
            printf("\n*** ERROR in ROSE -> Too many iterations required\n");
            return( 4 );
         }
      }
      h = hnew;
   }
}
/*





*/
int rose_mod_sen_a( t0, t1, y, z, ysy, ysz, 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). The funtion also computes the sensitivity of y and z
      with respect to the initial conditions. 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, **ysz, **ysy, 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 ** );
/*


*/
{
   void fm_lhs(long double **,long double **,long double **,long double **,long double **,long double **,long double **,int,int);
   void fm_lhs2(long double **,long double **,long double **,long double **,long double **,long double **,long double **,int,int);
   long double **big_jac1, **big_jac2, *rhs, *lhs, *y0, *z0, *y1, *z1;
   long double **ysy0, **zsy0, **zsy, ***kapa_sen, ***lambda_sen, **rhs_sen, **lhs_sen;
   long double **ysz0, **zsz0, **zsz, *g_fx, **gy_fx, **gz_fx;
   long double ***kapa_sen2, ***lambda_sen2, **rhs_sen2, **lhs_sen2;
   long double **fy, **fz, **gy, **gz, **fy0, **fz0, **gy0, **gz0, **kapa, **lambda, *f, *g;
   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 );
   gy_fx = a2d_allo_dbl( m, n );
   gz_fx = a2d_allo_dbl( m, m );
   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 );
   g_fx = a1d_allo_dbl( m );
   y1 = a1d_allo_dbl( n );
   z1 = a1d_allo_dbl( m );
   y0 = a1d_allo_dbl( n );
   z0 = a1d_allo_dbl( m );
   
   zsy = a2d_allo_dbl( m, n );
   zsy0 = a2d_allo_dbl( m, n );
   ysy0 = a2d_allo_dbl( n, n );
   kapa_sen = a3d_allo_dbl( 4, n, n );
   lambda_sen = a3d_allo_dbl( 4, m, n );
   rhs_sen = a2d_allo_dbl( p, n );
   lhs_sen = a2d_allo_dbl( p, n );
   
   zsz = a2d_allo_dbl( m, m );
   zsz0 = a2d_allo_dbl( m, m );
   ysz0 = a2d_allo_dbl( n, m );
   kapa_sen2 = a3d_allo_dbl( 4, n, m );
   lambda_sen2 = a3d_allo_dbl( 4, m, m );
   rhs_sen2 = a2d_allo_dbl( p, m );
   lhs_sen2 = a2d_allo_dbl( p, 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 ||
       ysy0 == NULL || kapa_sen == NULL ||  rhs_sen == NULL || lhs_sen == NULL ||
       ysz0 == NULL || kapa_sen2 == NULL ||  rhs_sen2 == NULL || lhs_sen2 == NULL ) {
      printf("*** ERROR in ROSE_SEN -> 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 );
      a2d_free_dbl( rhs_sen, p );
      a2d_free_dbl( lhs_sen, p );
      a2d_free_dbl( ysy0, n );
      a3d_free_dbl( kapa_sen, 4, n );
      
      a2d_free_dbl( ysz0, n );
      a2d_free_dbl( rhs_sen2, p );
      a2d_free_dbl( lhs_sen2, p );
      a3d_free_dbl( kapa_sen2, 4, n );
      return( 2 );
   }
     
   if( (m > 0) && (fz == NULL || gz == NULL ||
        gy == NULL ||z0 == NULL ||z1 == NULL ||g == NULL || lambda == NULL || zsy == NULL ||
        zsy0 == NULL || lambda_sen == NULL || zsz == NULL || zsz0 == NULL || lambda_sen2 == NULL ) ) {
      printf("*** ERROR in ROSE_SEN -> Out of memory\n");
      a2d_free_dbl( lambda, 4 );
      a2d_free_dbl( fz, n );
      a1d_free_dbl( g );
      a2d_free_dbl( gz, m );
      a2d_free_dbl( gy, m );
      a1d_free_dbl( z0 );
      a1d_free_dbl( z1 );
      a2d_free_dbl( zsy, m );
      a2d_free_dbl( zsy0, m );
      a3d_free_dbl( lambda_sen, 4, m );

      a2d_free_dbl( gz_fx, m );
      a2d_free_dbl( zsz, m );
      a2d_free_dbl( zsz0, m );
      a3d_free_dbl( lambda_sen2, 4, m );
      return( 2 );
   }
/*
   Get initial conditions for the algebraic sensitivites, zsy(0)
*/
   
   (*u_fun)( y, z, f, g );
   (*u_dfun)( y, z, fy, fz, gy, gz );
   vec_copy( g_fx, g, m );
   mat_copy( gy_fx, gy, m, n );
   mat_copy( gz_fx, gz, m, m );
   mat_null( zsy, m, n );
   mat_null( zsz, m, m );
   for( i = 0; i < m; i++ )
      zsz[i][i] = 1.0;
      
/* 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);
      mat_copy( ysy0, ysy, n, n );
      mat_copy( ysz0, ysz, n, m );
      mat_copy( zsy0, zsy, m, n );
      mat_copy( zsz0, zsz, m, 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_SEN -> Jacobian matrix singular\n");
         printf("                         DAE is not index 1\n");         
         return( 3 );
      }
      
      fm_lhs(lhs_sen,fy,fz,gy,gz,ysy0,zsy0,n,m);
      fm_lhs2(lhs_sen2,fy,fz,gy,gz,ysz0,zsz0,n,m);
/*

*/      
      for( i = 0; i < p; i++ ) {
         if( i < n )
            rhs[i] = DELTA1*h*f[i];
         else
            rhs[i] = -(g[i-n]-g_fx[i-n]);
         for( j = 0; j < n; j++ ) {
            if( i < n )
               rhs_sen[i][j] = DELTA1*h*lhs_sen[i][j];
            else
               rhs_sen[i][j] = -(lhs_sen[i][j]-gy_fx[i-n][j]);
         }
         for( j = 0; j < m; j++ ) {
            if( i < n )
               rhs_sen2[i][j] = DELTA1*h*lhs_sen2[i][j];
            else
               rhs_sen2[i][j] = -(lhs_sen2[i][j]-gz_fx[i-n][j]);
         }
      }

      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];
         }
      }
      for( i = 0; i < n; i++ ) {
         vec_column( rhs, rhs_sen, i, p );
         solve1(  big_jac1, pivot1, rhs, p, lhs );
         for( j = 0; j < p; j++ )
            lhs_sen[j][i] = lhs[j];
      } 
      for( j = 0; j < n; j++ ) 
         for( i = 0; i < p; i++ ) {
            if( i < n ) {
               kapa_sen[0][i][j] = lhs_sen[i][j];
               ysy0[i][j] += lhs_sen[i][j];            /* Update ysy0 and zsy0 for the next step */
            }
            else {
               lambda_sen[0][i-n][j] = lhs_sen[i][j];
               zsy0[i-n][j] += lhs_sen[i][j];
            }
         }
      for( i = 0; i < m; i++ ) {
         vec_column( rhs, rhs_sen2, i, p );
         solve1(  big_jac1, pivot1, rhs, p, lhs );
         for( j = 0; j < p; j++ )
            lhs_sen2[j][i] = lhs[j];
      } 
      for( j = 0; j < m; j++ ) 
         for( i = 0; i < p; i++ ) {
            if( i < n ) {
               kapa_sen2[0][i][j] = lhs_sen2[i][j];
               ysz0[i][j] += lhs_sen2[i][j];            /* Update ysz0 and zsz0 for the next step */
            }
            else {
               lambda_sen2[0][i-n][j] = lhs_sen2[i][j];
               zsz0[i-n][j] += lhs_sen2[i][j];
            }
         }
/*
   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_SEN -> Jacobian matrix singular\n");
         printf("                         DAE is not index 1\n");         
         return( 3 );
      }
      fm_lhs(lhs_sen,fy,fz,gy,gz,ysy0,zsy0,n,m);
      fm_lhs2(lhs_sen2,fy,fz,gy,gz,ysz0,zsz0,n,m);
/*

*/      
      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]-g_fx[i-n]);
/* */         
      for( j = 0; j < n; j++ ) {
         for( i = 0; i < n; i++ ) {
            sum = h*lhs_sen[i][j];
            for( k = 0; k < n; k++ ) {
                sum += fy[i][k]*kapa_sen[0][k][j]*h*gdeli10;
            /*for( k = 0; k < m; k++ )*/
               if( k < m ) sum += fz[i][k]*lambda_sen[0][k][j]*h*gdeli10;
            }
            rhs_sen[i][j] = DELTA2*(sum - deli10*kapa_sen[0][i][j]);
         }
         for( i = n; i < p; i++ )
            rhs_sen[i][j] = -(lhs_sen[i][j]-gy_fx[i-n][j]);
      }
/* */         
      for( j = 0; j < m; j++ ) {
         for( i = 0; i < n; i++ ) {
            sum = h*lhs_sen2[i][j];
            for( k = 0; k < n; k++ ) {
                sum += fy[i][k]*kapa_sen2[0][k][j]*h*gdeli10;
            /*for( k = 0; k < m; k++ )*/
               if( k < m ) sum += fz[i][k]*lambda_sen2[0][k][j]*h*gdeli10;
            }
            rhs_sen2[i][j] = DELTA2*(sum - deli10*kapa_sen2[0][i][j]);
         }
         for( i = n; i < p; i++ )
            rhs_sen2[i][j] = -(lhs_sen2[i][j]-gz_fx[i-n][j]);
      }

      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];
         }
      }
/**/      
      for( i = 0; i < n; i++ ) {
         vec_column( rhs, rhs_sen, i, p );
         solve1(  big_jac2, pivot2, rhs, p, lhs );
         for( j = 0; j < p; j++ )
            lhs_sen[j][i] = lhs[j];
      } 
      for( j = 0; j < n; j++ ) 
         for( i = 0; i < p; i++ ) {
            if( i < n ) {
               kapa_sen[1][i][j] = lhs_sen[i][j];
               ysy0[i][j] += lhs_sen[i][j];            /* Update ysy0 and zsy0 for the next step */
            }
            else {
               lambda_sen[1][i-n][j] = lhs_sen[i][j];
               zsy0[i-n][j] += lhs_sen[i][j];
            }
         }
/**/      
      for( i = 0; i < m; i++ ) {
         vec_column( rhs, rhs_sen2, i, p );
         solve1(  big_jac2, pivot2, rhs, p, lhs );
         for( j = 0; j < p; j++ )