rose.c

multipleshooting to find the roots of the non-linear functions

            lhs_sen2[j][i] = lhs[j];
      } 
      for( j = 0; j < m; j++ ) 
         for( i = 0; i < p; i++ ) {
            if( i < n ) {
               kapa_sen2[1][i][j] = lhs_sen2[i][j];
               ysz0[i][j] += lhs_sen2[i][j];            /* Update ysz0 and zsz0 for the next step */
            }
            else {
               lambda_sen2[1][i-n][j] = lhs_sen2[i][j];
               zsz0[i-n][j] += lhs_sen2[i][j];
            }
         }
/*
   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_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 += 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]-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 += h*fy[i][k]*(gdeli20*kapa_sen[0][k][j] + gdeli21*kapa_sen[1][k][j]);
            /*for( k = 0; k < m; k++ )*/
               if( k < m ) sum += h*fz[i][k]*(gdeli20*lambda_sen[0][k][j] + gdeli21*lambda_sen[1][k][j]);
            }
            rhs_sen[i][j] = DELTA1*(sum - (deli20*kapa_sen[0][i][j] + deli21*kapa_sen[1][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 += h*fy[i][k]*(gdeli20*kapa_sen2[0][k][j] + gdeli21*kapa_sen2[1][k][j]);
            /*for( k = 0; k < m; k++ )*/
               if( k < m ) sum += h*fz[i][k]*(gdeli20*lambda_sen2[0][k][j] + gdeli21*lambda_sen2[1][k][j]);
            }
            rhs_sen2[i][j] = DELTA1*(sum - (deli20*kapa_sen2[0][i][j] + deli21*kapa_sen2[1][i][j]));
         }
         for( i = n; i < p; i++ )
            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[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];
         }
      }
/**/      
      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[2][i][j] = lhs_sen[i][j];
               ysy0[i][j] += lhs_sen[i][j];            /* Update ysy0 and zsy0 for the next step */
            }
            else {
               lambda_sen[2][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[2][i][j] = lhs_sen2[i][j];
               ysz0[i][j] += lhs_sen2[i][j];            /* Update ysy0 and zsy0 for the next step */
            }
            else {
               lambda_sen2[2][i-n][j] = lhs_sen2[i][j];
               zsz0[i-n][j] += lhs_sen2[i][j];
            }
         }
/*
   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_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 += 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]-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 += h*fy[i][k]*(gdeli30*kapa_sen[0][k][j] + gdeli31*kapa_sen[1][k][j] + gdeli32*kapa_sen[2][k][j]);
            /*for( k = 0; k < m; k++ )*/
               if( k < m ) sum += h*fz[i][k]*(gdeli30*lambda_sen[0][k][j] + gdeli31*lambda_sen[1][k][j] + gdeli32*lambda_sen[2][k][j]);
            }
            rhs_sen[i][j] = DELTA2*(sum - (deli30*kapa_sen[0][i][j] + deli31*kapa_sen[1][i][j] + deli32*kapa_sen[2][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 += h*fy[i][k]*(gdeli30*kapa_sen2[0][k][j] + gdeli31*kapa_sen2[1][k][j] + gdeli32*kapa_sen2[2][k][j]);
            /*for( k = 0; k < m; k++ )*/
               if( k < m ) sum += h*fz[i][k]*(gdeli30*lambda_sen2[0][k][j] + gdeli31*lambda_sen2[1][k][j] + gdeli32*lambda_sen2[2][k][j]);
            }
            rhs_sen2[i][j] = DELTA2*(sum - (deli30*kapa_sen2[0][i][j] + deli31*kapa_sen2[1][i][j] + deli32*kapa_sen2[2][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[3][i] = lhs[i];
            y0[i] += lhs[i];            /* Get the solution */        
         }
         else {
            lambda[3][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[3][i][j] = lhs_sen[i][j];
               ysy0[i][j] += lhs_sen[i][j];            /* Get the solution */
            }
            else {
               lambda_sen[3][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++ )
            lhs_sen2[j][i] = lhs[j];
      } 
      for( j = 0; j < m; j++ ) 
         for( i = 0; i < p; i++ ) {
            if( i < n ) {
               kapa_sen2[3][i][j] = lhs_sen2[i][j];
               ysz0[i][j] += lhs_sen2[i][j];            /* Get the solution */
            }
            else {
               lambda_sen2[3][i-n][j] = lhs_sen2[i][j];
               zsz0[i-n][j] += lhs_sen2[i][j];
            }
         }

/* 
   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 up the integration, 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);
         mat_copy( ysy, ysy0, n, n );
         mat_copy( zsy, zsy0, m, n );
         mat_copy( ysz, ysz0, n, m );
         mat_copy( zsz, zsz0, m, 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 ) {
         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( ysy0, n );
         a2d_free_dbl( rhs_sen, p );
         a2d_free_dbl( lhs_sen, p );
         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 );
         
         if( m > 0 ) {
            a2d_free_dbl( lambda, 4 );
            a2d_free_dbl( fz, n );
            a2d_free_dbl( gz, m );
            a2d_free_dbl( gy, m );
            a2d_free_dbl( gy_fx, m );
            a1d_free_dbl( g );
            a1d_free_dbl( g_fx );
            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 );
         }
         
         if( h < MACH_EPS && t != t1 ) {
            printf("\n*** ERROR in ROSE_SEN -> Required step size too small\n");
            return( 5 );
         }
         if( itt <= (MAXIT-1) ) 
            return( 0 );
         else {
            printf("\n*** ERROR in ROSE_SEN -> Too many iterations required\n");
            return( 4 );
         }
      }
      h = hnew;
   }
}
void fm_lhs(lhs_sen,fy,fz,gy,gz,ysy0,zsy0,n,m)
long double **lhs_sen, **fy, **fz, **gy, **gz, **ysy0, **zsy0;
int n, m;
{
   int p, i, j, k;
   p = n+m;

/*

                     |fy*ysy + fz*zsy |
   Compute lhs_sen = |                |   This is the function evaluation for the sensitivities
                     |gy*ysy + gz*zsy |
                      
*/
      for( i = 0; i < p; i++ )
         for( j = 0; j < n; j++ ) {
            lhs_sen[i][j] = 0.0;
            if( i < n ) {
               for( k = 0; k < n; k++ ) {
                  lhs_sen[i][j] += fy[i][k]*ysy0[k][j];
               /*for( k = 0; k < m; k++ )*/
                  if( k < m ) lhs_sen[i][j] += fz[i][k]*zsy0[k][j];
               }
            } else {
               for( k = 0; k < n; k++ ) {
                  lhs_sen[i][j] += gy[i-n][k]*ysy0[k][j];
               /*for( k = 0; k < m; k++ )*/
                  if( k < m ) lhs_sen[i][j] += gz[i-n][k]*zsy0[k][j];
               }
            }
         }
}
void fm_lhs2(lhs_sen2,fy,fz,gy,gz,ysz0,zsz0,n,m)
long double **lhs_sen2, **fy, **fz, **gy, **gz, **ysz0, **zsz0;
int n, m;
{
   int p, i, j, k;
   p = n+m;
/*

                      |fy*ysz + fz*zsz |
   Compute lhs_sen2 = |                |   This is the function evaluation for the sensitivities
                      |gy*ysz + gz*zsz |
                      
*/
      for( i = 0; i < p; i++ )
         for( j = 0; j < m; j++ ) {
            lhs_sen2[i][j] = 0.0;
            if( i < n ) {
               for( k = 0; k < n; k++ ) {
                  lhs_sen2[i][j] += fy[i][k]*ysz0[k][j];
               /*for( k = 0; k < m; k++ )*/
                  if( k < m ) lhs_sen2[i][j] += fz[i][k]*zsz0[k][j];
               }
            } else {
               for( k = 0; k < n; k++ ) {
                  lhs_sen2[i][j] += gy[i-n][k]*ysz0[k][j];
               /*for( k = 0; k < m; k++ )*/
                  if( k < m ) lhs_sen2[i][j] += gz[i-n][k]*zsz0[k][j];
               }
            }
         }
}