mshoot_dae.c

multipleshooting to find the roots of the non-linear functions

                     rhs -= h[ms-2][j][k]*ds[0][k];
                  for( k = j+1; k < n; k++ )
                     rhs -= r[ms-2][j][k]*ds[ms-1][k];
                  ds[ms-1][j] = rhs/r[ms-2][j][j];
               }
            } else {
               for( j = n-1; j >= 0; j-- ) {
                  rhs = f[iblock][j];
                  for( k = 0; k < n; k++ )
                     rhs -= h[iblock][j][k]*ds[0][k];
                  for( k = 0; k < n; k++ )
                     rhs -= e[iblock][j][k]*ds[iblock+2][k];
                  for( k = j+1; k < n; k++ )
                     rhs -= r[iblock][j][k]*ds[iblock+1][k];
                  ds[iblock+1][j] = rhs/r[iblock][j][j];
               }
            }
         }                
/*
   Compute maximum norm of the increment, ds
*/
         hnorm1 = 0.0;
         for( i = 0; i < ms; i++ ) {
            hnorm2 = 0.0;
            for( j = 0; j < n; j++ )
               hnorm2 += ds[i][j]*ds[i][j];
            if( hnorm2 > hnorm1 )
               hnorm1 = hnorm2;
         }
         hnorm1 = sqrt( (double) hnorm1 );
         
         printf("|ds| = %8.3Le, ", hnorm1);

/**/
         if( hnorm1 <= u_eps )
            printf("\n*** WARNING -> |ds| = %Le <= eps\n\n",hnorm1);
/*
   Damped correction
*/
         j = 0;fac=3.0;
         do {

            fac /= 3.0;

            fnorm2 = 0.0;

            for( i = 0; i < ms; i++ )
               for( k = 0; k < n; k++ ) {
                  if( k < m )
                     znew[i][k] = zout[i][k] - fac * ds[i][k];
                  else
                     ynew[i][k-m] = yout[i][k-m] - fac * ds[i][k];
               }
            for( i = 0; i < ms; i++ ) {
            
              /* printf(" Compute f for sub_arc -> %d\n",i);*/

               if( i < (ms-1) ) {
                  if( i <= (ms-2) )
                  ierr = m_fun( ynew[i], znew[i], ynew[i+1], znew[i+1], f[i], time[i], time[i+1], p, m, (*u_fun), (*u_dfun), u_eps );
                  if( ierr != 0 ) {
                     printf("\n*** ERROR in MSHOOT-DAE -> FUN = %d\n",ierr);
                     mat_copy( yout, ynew, ms, p );
                     mat_copy( zout, znew, ms, m );
                     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);
                  }
               } else if( i == (ms-1) ) {
                  (*u_bc)( ynew[0], ynew[ms-1], ebc );
                  (*u_fun)( ynew[ms-1], znew[ms-1], fbc, gbc );
                  for( k = 0; k < n; k++ )
                     if( k < m )
                       f[ms-1][k] = gbc[k];
                     else
                       f[ms-1][k] = ebc[k-m];
               }

               fnorm3 = 0.0;
               for( k = 0; k < n; k++ )
                  fnorm3 += f[i][k]*f[i][k];
               if( fnorm3 > fnorm2 )
                  fnorm2 = fnorm3;
            }
            fnorm2 = sqrt( (double) fnorm2 );
            
            
            if( fnorm2 < fnorm1 )
               break;
               
            j++;
            
         } while( j < JMAX );

         printf("|F(s^%d)| = %8.3Le\n",itt+1,fnorm2);

         if( j >= JMAX ) {
            ierr = 5;
            printf("\n*** ERROR in MSHOOT-DAE -> Unable to obtain an improved solution during damped Newton correction\n");
            mat_copy( yout, ynew, ms, p );
            mat_copy( zout, znew, ms, m );
            break;
         }
         

         mat_copy( yout, ynew, ms, p );
         mat_copy( zout, znew, ms, m );

         fnorm1 = fnorm2;

         if( fnorm1 <= u_eps ) {
            ierr = 0;
            break;
         }
         itt++;

      } while( itt < MAXIT );

      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);
      
      if( itt >= MAXIT ) {
         printf("\n*** ERROR in MSHOOT -> Too many Newton's iterations performed\n");
         return( 6 );
      }
      else
         return( ierr );
   }
int m_fun( p0, z0, p1, z1, f, tstart, tend, nequ, mequ, u_fun, u_dfun, u_eps )
long double *f;
long double *p0, *z0, *z1, *p1;
long double tstart, tend;
int nequ, mequ;
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 u_eps;
   {
      long double *a1d_allo_dbl( int );
      void a1d_free_dbl( long double * );
      int rose_mod_sen_a( long double, long double, long double *, long double *, long double **, long double **, void (*)(),  void (*)(), int, int, long double );
      int rose_mod_a( long double, long double, long double *, long double *, void (*)(),  void (*)(), int, int, long double );

      int i;
      
      long double *y, *z, *fi, *gi, eps;

      y = a1d_allo_dbl( nequ );
      z = a1d_allo_dbl( mequ );
      fi = a1d_allo_dbl( nequ );
      gi = a1d_allo_dbl( mequ );

      if( y == NULL || z == NULL || fi == NULL || gi == NULL ) {
         printf("\n*** ERROR in FUN -> Out of memory (a fatal error)\n");
         return(-1);
      }
/*
   Set initial conditions, start time, end time and step size
*/
      for( i = 0; i < nequ; i++ )
         y[i] = p0[i];
      for( i = 0; i < mequ; i++ )
         z[i] = z0[i];
         
      (*u_fun)( y, z, fi, gi );
/*
   Integrate the DAE using the Rosenbrock method
*/
      eps = u_eps;
      i = rose_mod_a( tstart, tend, y, z, (*u_fun), (*u_dfun), nequ, mequ, eps );
      
      if( i != 0 ) {
         printf("\n*** ERROR in FUN -> ROSE = %d (a fatal error)\n",i);
         return(i);
      }

      for( i = 0; i < nequ+mequ; i++ )
         if( i < mequ )
            f[i] = gi[i];
         else
            f[i] = y[i-mequ] - p1[i-mequ];
/*
   Free storage allocated
*/
      a1d_free_dbl( y );
      a1d_free_dbl( z );
      a1d_free_dbl( fi );
      a1d_free_dbl( gi );
      return( 0 );
   }
int fun_dfun( p0, z0, p1, z1, f, df, tstart, tend, nequ, mequ, u_fun, u_dfun, u_eps )
long double **df;
long double *p0, *p1, *z0, *z1, *f;
long double tstart, tend;
int nequ, mequ;
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 u_eps;
   {
      void mat_null( long double **, int, int );

      long double *a1d_allo_dbl( int );
      void a1d_free_dbl( long double * );

      long double **a2d_allo_dbl( int, int );
      void a2d_free_dbl( long double **, int );

      int rose_mod_sen_a( long double, long double, long double *, long double *, long double **, long double **, void (*)(),  void (*)(), int, int, long double );
      int rose_mod_a( long double, long double, long double *, long double *, void (*)(),  void (*)(), int, int, long double );
                   
      int i, j;
      long double *y, *z, **ysy, **ysz, **fy, **fz, **gy, **gz, *fi, *gi, eps;

      y = a1d_allo_dbl( nequ );
      z = a1d_allo_dbl( mequ );
      fi = a1d_allo_dbl( nequ );
      gi = a1d_allo_dbl( mequ );
      ysy = a2d_allo_dbl( nequ, nequ );
      ysz = a2d_allo_dbl( nequ, mequ );
      fy = a2d_allo_dbl( nequ, nequ );
      fz = a2d_allo_dbl( nequ, mequ );
      gy = a2d_allo_dbl( mequ, nequ );
      gz = a2d_allo_dbl( mequ, mequ );

      if( y == NULL || z == NULL || ysy == NULL || ysz == NULL || fy == NULL || fz == NULL || gy == NULL || gz == NULL
         || fi == NULL || gi == NULL ) {
         printf("\n*** ERROR in FUN_DFUN -> Out of memory (a fatal error)\n");
         return( 1 );
      }

      for( i = 0; i < nequ; i++ )
         y[i] = p0[i];
      for( i = 0; i < mequ; i++ )
         z[i] = z0[i];

      (*u_fun)( y, z, fi, gi );
      (*u_dfun)( y, z, fy, fz, gy, gz );
      
      mat_null( ysy, nequ, nequ );
      mat_null( ysz, nequ, mequ );

      for( i = 0; i < nequ; i++ )
         ysy[i][i] = 1.0;
/*
   Integrate the DAE and the sensitivity DAEs using the Rosenbrock method
*/
      eps = u_eps;
      i = rose_mod_sen_a( tstart, tend, y, z, ysy, ysz, (*u_fun), (*u_dfun), nequ, mequ, eps );
      
      if( i != 0 ) {
         printf("\n*** ERROR in FUN_DFUN -> ROSE_SEN = %d (a fatal error)\n",i);\
         return( i );
      }
      for( i = 0; i < nequ+mequ; i++ ) 
         if( i < mequ )
            f[i] = gi[i];
         else
            f[i] = y[i-mequ] - p1[i-mequ];
         
      for( i = 0; i < nequ+mequ; i++ )
         for( j = 0; j < nequ+mequ; j++ ) {
            if( (i >= mequ) && (j >= mequ) )
               df[i][j] = ysy[i-mequ][j-mequ];
            if( (i < mequ) && (j < mequ) )
               df[i][j] = gz[i][j];
            if( (i < mequ) && (j >= mequ) )
               df[i][j] = gy[i][j-mequ];
            if( (i >= mequ) && (j < mequ) )
               df[i][j] = ysz[i-mequ][j];
         }
      
      a1d_free_dbl( y );
      a1d_free_dbl( z );
      a1d_free_dbl( fi );
      a1d_free_dbl( gi );
      a2d_free_dbl( ysy, nequ );
      a2d_free_dbl( ysz, nequ );
      a2d_free_dbl( fy, nequ );
      a2d_free_dbl( fz, nequ );
      a2d_free_dbl( gy, mequ );
      a2d_free_dbl( gz, mequ );
      return( 0 );
   }
void 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)
int ms, n, p, m;
long double ***e, ***h, **f, ***g, **ds, **ynew, **znew, **a, **b, **ad, **bd, *ebc, *fbc, *gbc,
   **fa, **fb, **fy, **fz, **gy, **gz;
{
      a3d_free_dbl( e, (ms-1), n );
      a3d_free_dbl( h, (ms-1), n );
      a2d_free_dbl( f, ms );
      a3d_free_dbl( g, ms, n );
      a2d_free_dbl( ds, ms );
      a2d_free_dbl( ynew, ms );
      a2d_free_dbl( znew, ms );
      a2d_free_dbl( a, n );
      a2d_free_dbl( b, n );
      a2d_free_dbl( ad, 2*n );
      a2d_free_dbl( bd, 2*n );
      a1d_free_dbl( ebc );
      a1d_free_dbl( fbc );
      a1d_free_dbl( gbc );
      a2d_free_dbl( fa, p );
      a2d_free_dbl( fb, p );
      a2d_free_dbl( fy, p );
      a2d_free_dbl( fz, p );
      a2d_free_dbl( gy, m );
      a2d_free_dbl( gz, m );
}