Sindbad~EG File Manager

/* M. Beeson, utilities for ode solving, used also in numint.c
8.27.89 original date, following Numerical Recipes in C
        files odeint.c, rk4.c, rkqc.c, nrutil.c
1.13.96 last modified before release of Mathpert 1.06
1.28.98 last modified
5.5.13  made rk4 static
*/

#include <math.h>
#include "globals.h"
#include "rk.h"
#include "deval.h"

static int rk4(double *y,double *dydx,int n,double x,double h,double *yout,
         term *f, double *xptr, double **yptr);

/*________________________________________________________________*/
/* returns a 'vector' with legal indices running from nl to nh */

double * vector(int nl,int nh)
{
   double *v;
   v = (double *) mallocate((unsigned) (nh-nl+1)*sizeof(double));
   if(!v)
      nospace();
   return v-nl;
}



/*_____________________________________________________________________*/
int derivs(term *f,           /* y' = f(x,y), f is a vector */
            int n, double x, double *y, double *dy,
            double *xptr,      /* pointer to value of x */
            double **yptr     /* array of pointers to value of y[i] */
           )
/* compute dy[1]...dy[n] by  dy[i] = result of evaluating f[i] (x,y) */
/* Note that the value of variables x and y[i] are changed */
/* Return 0 for success, 1 if deval fails. */
{  int i;
   *xptr=x;
   for(i=1;i<=n;i++)
      *yptr[i] = y[i];
   for(i=1;i<=n;i++)
      { deval(f[i],dy+i);
        if(dy[i] == BADVAL)
           return 1;
      }
   return 0;
}

/*_____________________________________________________________________*/

static int rk4(double *y,double *dydx,int n,double x,double h,double *yout,
         term *f, double *xptr, double **yptr)
/* return 0 for success, 1 for failure of deval */
{ int i,err=0;
  double xh,hh,h6,*dym,*dyt,*yt;
  dym=vector(1,n);
  dyt=vector(1,n);
  yt=vector(1,n);
  hh=h*0.5;
  h6=h/6.0;
  xh=x+hh;
  for (i=1;i<=n;i++)
     yt[i]=y[i]+hh*dydx[i];
  err = derivs(f,n,xh,yt,dyt,xptr,yptr);
  if(err)
     goto out;
  for (i=1;i<=n;i++)
     yt[i]=y[i]+hh*dyt[i];
  err = derivs(f,n,xh,yt,dym,xptr,yptr);
  if(err)
     goto out;
  for (i=1;i<=n;i++)
     { yt[i]=y[i]+h*dym[i];
       dym[i] += dyt[i];
     }
  err = derivs(f,n,x+h,yt,dyt,xptr,yptr);
  if(err)
     goto out;
  for (i=1;i<=n;i++)
     yout[i]=y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);
  out:
  free_vector(yt,1);
  free_vector(dyt,1);
  free_vector(dym,1);
  return err;
}
/*____________________________________________________________________*/

#define PGROW -0.20
#define PSHRNK -0.25
#define FCOR 0.06666666         /* 1.0/15.0 */
#define SAFETY 0.9
#define ERRCON 6.0e-4

/* zero return value is success; nonzero is an error */
/* because used in numerical integration too  */
int rkqc(double *y,
         double *dydx,
         int n,
         double *x,
         double htry,
         double eps,
         double *yscal,
         double *hdid,
         double *hnext,
         term *f,
         double *xptr,
         double **yptr,
         double xtol,
         double ytol
        )
{ int i,err;
  double xsav,hh,h,temp,errmax;
  double *dysav,*ysav,*ytemp;
  dysav=vector(1,n);
  ysav=vector(1,n);
  ytemp=vector(1,n);
  xsav=(*x);
  for (i=1;i<=n;i++)
     { ysav[i]=y[i];
       dysav[i]=dydx[i];
     }
  h=htry;
  while(1)
     { hh=0.5*h;
       err = rk4(ysav,dysav,n,xsav,hh,ytemp,f,xptr,yptr);
       if(err)
          { free_vector(ytemp,1);
            free_vector(dysav,1);
            free_vector(ysav,1);
            return 5; /* deval failed */
          }
       *x=xsav+hh;
       derivs(f,n,*x,ytemp,dydx,xptr,yptr);
       err = rk4(ytemp,dydx,n,*x,hh,y,f,xptr,yptr);
       if(err)
          { free_vector(ytemp,1);
            free_vector(dysav,1);
            free_vector(ysav,1);
            return 5; /* deval failed */
          }
       *x=xsav+h;
       if (*x == xsav)
          { free_vector(ytemp,1);
            free_vector(dysav,1);
            free_vector(ysav,1);
            return 1; /* Step size too small */
          }
       err = rk4(ysav,dysav,n,xsav,h,ytemp,f,xptr,yptr);
       if(err)
          { free_vector(ytemp,1);
            free_vector(dysav,1);
            free_vector(ysav,1);
            return 5; /* deval failed */
          }
       errmax=0.0;
       for (i=1;i<=n;i++)
          { ytemp[i]=y[i]-ytemp[i];
            temp=fabs(ytemp[i]/yscal[i]);
            if (errmax < temp)
               errmax=temp;
          }
       errmax /= eps;

            /* check if y[1] or y[2] has changed too much */
            /* Numerical Recipes omits this check which is necessary
               on simple equations where Runge-Kutta is exact
               such as x'=1, y'= t */
       if( errmax <= 1.0
           &&
           (
            (fabs(y[1] - ysav[1]) <= xtol && (n <=2 || fabs(y[2] - ysav[2]) <= ytol))
            ||  fabs(y[1] - ysav[1]) <= 0.3 * xtol
            ||  (n >= 2 && fabs(y[2] - ysav[2]) <= 0.3 * ytol)
           )
         )  /* step succeeded. Compute size of next step */
          { *hdid=h;
            if( errmax > ERRCON)
               *hnext = SAFETY*h*exp(PGROW*log(errmax));
            else if (fabs(y[1] - ysav[1]) >= 0.45 * xtol
                     || (n >=2 && fabs(y[2] - ysav[2]) >= 0.45 * ytol)
                    )
               *hnext = h;   /* this is just fine */
            else if (fabs(y[1] -  ysav[1]) >= 0.1 * xtol
                     || (n >= 2 && fabs(y[2] - ysav[2]) >= 0.1 * ytol)
                    )
               *hnext = 2.0 *h;
            else
               *hnext = 4.0 * h;  /* go for it! */
            break;
          }
               /* so step didn't succeed */
       if( errmax > 1.0)
          h=SAFETY*h*exp(PSHRNK*log(errmax));
       else /* line is too many pixels long */
          h /= 2.0;  /* crude but hopefully effective */

    }
  for (i=1;i<=n;i++)
     y[i] += ytemp[i]*FCOR;
  free_vector(ytemp,1);
  free_vector(dysav,1);
  free_vector(ysav,1);
  return 0;
}

#undef PGROW
#undef PSHRNK
#undef FCOR
#undef SAFETY
#undef ERRCON