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
3.16.25 made this copy (rk3.c) for the THREEBODY project,
     changing mallocate to malloc and elimininating term and deval
3.28.25 corrected mass[k] to mass[k-1]  in Newton
*/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "rk3.h"

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

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

double * vector(int nl,int nh)
{
   double *v;
   v = (double *) malloc((unsigned) (nh-nl+1)*sizeof(double));
   if(!v)
      { printf("malloc failed");
        exit(1);
      }
   return v-nl;
}
/*_____________________________________________________________________*/
#include <assert.h> 

static int Newton(int n, double *y, int i, double *mass, double *ansX, double *ansY)
/* n is the number of bodies, y the vector of their positions
with indices 1 to n, 2 consecutive variables for each body.
The third argument i is one of those indices, so y[2i-1] is
the x-coordinate of the i-th body and y[2i] the y-coordinate.
Here mass is a vector of n positive masses.
Compute the acceleration of the i-th body by Newton's law
and put it in *ansX and *ansY (it's a 2-vector)
Return 0 for success.   TODO:  catch numerical errors.
*/
{ int k;
  double accelerationX = 0.0;
  double accelerationY = 0.0;
  double acceleration;
  assert( 1 <= i); // y is 1-based
  assert( i <= n); // and reference a valid body
  assert( 1 < n);
  double rik;  // r_{ik}, distance from body i to body k
  //for(k=1;k<=8;k++) printf("%d %lf\n",k,y[k]);
  for(k=1;k<=n;k+=1)
     {  if(k==i)
          continue; // no self-gravitation
        // calculate the attraction between body i and body k
        // the next lines only work in two-space
        // i-th bodies position is in y[2*i - 1] and y[2*i] since i starts from 1.
        double deltax = y[2*k-1]- y[2*i-1];
        double deltay = y[2*k]- y[2*i];
        rik = hypot(deltax, deltay);   // sqrt(deltax^2 + deltay^2)
        double rsquared = deltax*deltax + deltay * deltay;  // distance^2 between bodies i and k
        if (rsquared == 0.0)
          {
             printf("WARNING: bodies %d and %d are at same position!\n", i, k);
             return 1;  // collision
          }
        //  double force = mass[i]*mass[k]/rsquared;
        //  acceleration  = force/ mass[i]
        //  but shorter:
        acceleration = mass[k-1]/rsquared; // independent of mass[i];  this is Newton's law of gravitation
                        // mass has 0-based indexing, but k here starts from 1
        // Check for numerical issues
        if (!isfinite(acceleration))
           {
             printf("COLLISION OR NUMERICAL ERROR: body %d attracted by %d (rsquared = %.15lf)\n", i, k, rsquared);
             return 1;  // Treat as collision or error
           }
        // break acceleration into two components; (deltax, deltay)/rik is a unit vector
        accelerationX += acceleration  * (deltax / rik);
        accelerationY +=  acceleration * (deltay / rik);
     }
 *ansX = accelerationX;
 *ansY = accelerationY;
 //printf("Newton calculated %lf,%lf with i = %d\n",*ansX,*ansY,i);
 return 0;
}
/*_____________________________________________________________________*/

int derivs(
            int n, double x, double *y, double *dy,
            double *xptr,      /* points to current time */
            double *mass       /* vector of n masses */
           )
/*  n is the number of bodies.
    So the number of variables is 4n, indexed starting at 1.
    y is a vector of dependent variables, 2 position variables
    and 2 velocity variables for each of n bodies.
    Vector means indices start at 1.
    We list all the position variables first, then all the velocity variables,
    so the first 2n variables are positions, the last 2n are velocities
    so the velocity variable corresponding to a position y[k] is y[k+2*n]
   
    Put (what should be) the derivative of y[i] into dy+i.
    so for the position variables that's the corresponding velocity variable,
    and for two consecutive  velocity variables (y[i],y[i+1]) with i odd,
    it's the sum over all the other bodies k of
    1/ m_i * m_j * (r_ij)^2 times the unit vector in direction (y[k]-y[i],y[k+1]-y[i]).
*/
/* compute dy[1]...dy[4n] by  dy[i] = derivative of y at x,y */
/* Note that the value of variables x and y[i] are changed */
/* Return 0 for success, 1 if evaluation fails. */
{  int i,err;
   *xptr=x;
   for(i=1;i<=2*n;i++)
      { // for the position variables that's the corresponding velocity variable,
        dy[i] = y[i+2*n];  // right side is the velocity of y[i]
      }
   for(i=1; i<= n; i++)   // each call to Newton computes two values to enter in dy
      { err = Newton(n, y,i,mass,dy+ 2*n+ 2*i-1, dy + 2*n+ 2*i);
        if(err)
           return 1;  // that will be considered "collision"
      }
   return 0;
}

/*_____________________________________________________________________*/

static int rk4(double *y,double *dydx,int n,double x,double h,double *yout,
         double *xptr, double *mass)
/* return 0 for success, 1 for numerical error */
/*  Here x is one value of the independent variable (time for THREEBODY)
    n is the number of bodies, times 4, which is the number of variables.
    y is a vector of dependent variables, 4 of them for each body,
    the first half for positions, the next half for velocities
*/
{ 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];
  assert(n % 4 == 0);
  err = derivs(n/4,xh,yt,dyt,xptr,mass);
  if(err)
     goto out;
  for (i=1;i<=n/2;i++)   // update the n/2 position variables
     yt[i]=y[i]+hh*dyt[i];
  err = derivs(n/4,xh,yt,dym,xptr,mass);
  if(err)
     goto out;
  for (i=1;i<=n/2;i++)
     { yt[i]=y[i]+h*dym[i];
       dym[i] += dyt[i];
     }
  err = derivs(n/4,x+h,yt,dyt,xptr,mass);
  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,
         double *xptr,
         double xtol,
         double ytol,
         double *mass
        )
{ 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,xptr,mass);
       if(err)
          { free_vector(ytemp,1);
            free_vector(dysav,1);
            free_vector(ysav,1);
            return 5; /* deval failed */
          }
       *x=xsav+hh;
       derivs(n/4,*x,ytemp,dydx,xptr,mass);  // derivs wants nbodies, which is n/4
       err = rk4(ytemp,dydx,n,*x,hh,y,xptr,mass);
       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,xptr,mass);
       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