TempleOS-Rijn/LT/Adam/ODE.APZ

#help_index "Math"

//This is taken from the book, $UL,1$Numeric Recipies in C$UL,0$

Ode *OdeNew(U8 n)
{
  Ode *o=MAllocZ(sizeof(Ode));
  o->n=n;
  o->state=MAllocZ(n*sizeof(double));
  o->internal_state=MAllocZ(n*sizeof(double));
  o->base_t=GetTimeStamp*1.0/time_stamp_freq;
  o->h=1.0e-6;
  o->DstateDt=MAllocZ(n*sizeof(double));
  o->state_scale=MAllocZ(n*sizeof(double));
  o->initial_state=MAllocZ(n*sizeof(double));
  o->tolerance_start=1.0e-6;
  o->temp0=MAllocZ(n*sizeof(double));
  o->temp1=MAllocZ(n*sizeof(double));
  o->temp2=MAllocZ(n*sizeof(double));
  o->temp3=MAllocZ(n*sizeof(double));
  o->temp4=MAllocZ(n*sizeof(double));
  o->temp5=MAllocZ(n*sizeof(double));
  o->temp6=MAllocZ(n*sizeof(double));
  o->temp7=MAllocZ(n*sizeof(double));
  o->win_tss=Fs;
  o->mem_tss=Fs;
  return o;
}

void OdeDel(Ode *o)
{
  Free(o->state);
  Free(o->internal_state);
  Free(o->DstateDt);
  Free(o->state_scale);
  Free(o->initial_state);
  Free(o->temp0);
  Free(o->temp1);
  Free(o->temp2);
  Free(o->temp3);
  Free(o->temp4);
  Free(o->temp5);
  Free(o->temp6);
  Free(o->temp7);
  Free(o);
}

void OdeOneStep(Ode *o)
{
  I8 i;
  o->derivative(o,o->t+o->base_t,o->internal_state,o->DstateDt);
  for (i=0;i<o->n;i++)
    o->internal_state[i]+=o->h*o->DstateDt[i];
  o->t+=o->h;
}

void OdeRK4OneStep(Ode *o)
{
  I8 i,n=o->n;
  double xh,hh,h6;
  double *dym,*dyt,*yt,*DstateDt;

  dym =o->temp0;
  dyt =o->temp1;
  yt  =o->temp2;
  DstateDt=o->temp3;
  hh  =0.5*o->h;
  h6  =o->h / 6.0;
  xh  =o->t +o->base_t+ hh;

  o->derivative(o,o->t+o->base_t,o->internal_state,o->DstateDt);
  for (i=0;i<n;i++)
    yt[i]=o->internal_state[i]+hh*DstateDt[i];
  o->derivative(o,xh,yt,dyt);
  for (i=0;i<n;i++)
    yt[i]=o->internal_state[i]+hh*dyt[i];
  o->derivative(o,xh,yt,dym);
  for (i=0;i<n;i++) {
    yt[i]=o->internal_state[i]+o->h*dym[i];
    dym[i]+=dyt[i];
  }
  o->t+=o->h;
  o->derivative(o,o->t+o->base_t,yt,dyt);
  for (i=0;i<n;i++)
    o->internal_state[i]+=h6*(DstateDt[i]+dyt[i]+2.0*dym[i]);
}


#define ODEa2 0.2
#define ODEa3 0.3
#define ODEa4 0.6
#define ODEa5 1.0
#define ODEa6 0.875
#define ODEb21 0.2
#define ODEb31 (3.0/40.0)
#define ODEb32 (9.0/40.0)
#define ODEb41 0.3
#define ODEb42 (-0.9)
#define ODEb43 1.2
#define ODEb51 (-11.0/54.0)
#define ODEb52 2.5
#define ODEb53 (-70.0/27.0)
#define ODEb54 (35.0/27.0)
#define ODEb61 (1631.0/55296.0)
#define ODEb62 (175.0/512.0)
#define ODEb63 (575.0/13824.0)
#define ODEb64 (44275.0/110592.0)
#define ODEb65 (253.0/4096.0)
#define ODEc1  (37.0/378.0)
#define ODEc3  (250.0/621.0)
#define ODEc4  (125.0/594.0)
#define ODEc6  (512.0/1771.0)
#define ODEdc1 (37.0/378.0-2825.0/27648.0)
#define ODEdc3 (250.0/621.0-18575.0/48384.0)
#define ODEdc4 (125.0/594.0-13525.0/55296.0)
#define ODEdc5 (-277.0/14336.0)
#define ODEdc6 (512.0/1771.0-0.25)

void OdeCashKarp(Ode *o)
{
  I8 i,n=o->n;
  double h=o->h,*state=o->internal_state,*DstateDt=o->DstateDt,*ak2,*ak3,*ak4,*ak5,*ak6,*tempstate,*stateerr,*outstate;

  ak2=o->temp0;
  ak3=o->temp1;
  ak4=o->temp2;
  ak5=o->temp3;
  ak6=o->temp4;
  tempstate=o->temp5;
  outstate=o->temp6;
  stateerr=o->temp7;

  for (i=0;i<n;i++)
    tempstate[i]=state[i]+ODEb21*h*DstateDt[i];
  o->derivative(o,o->t+o->base_t+ODEa2*h,tempstate,ak2);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb31*DstateDt[i]+ODEb32*ak2[i]);
  o->derivative(o,o->t+o->base_t+ODEa3*h,tempstate,ak3);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb41*DstateDt[i]+ODEb42*ak2[i]+ODEb43*ak3[i]);
  o->derivative(o,o->t+o->base_t+ODEa4*h,tempstate,ak4);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb51*DstateDt[i]+ODEb52*ak2[i]+ODEb53*ak3[i]+ODEb54*ak4[i]);
  o->derivative(o,o->t+o->base_t+ODEa5*h,tempstate,ak5);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb61*DstateDt[i]+ODEb62*ak2[i]+ODEb63*ak3[i]+ODEb64*ak4[i]+ODEb65*ak5[i]);
  o->derivative(o,o->t+o->base_t+ODEa6*h,tempstate,ak6);
  for (i=0;i<n;i++)
    outstate[i]=state[i]+h*(ODEc1*DstateDt[i]+ODEc3*ak3[i]+ODEc4*ak4[i]+ODEc6*ak6[i]);
  for (i=0;i<n;i++)
    stateerr[i]=h*(ODEdc1*DstateDt[i]+ODEdc3*ak3[i]+ODEdc4*ak4[i]+ODEdc5*ak5[i]+ODEdc6*ak6[i]);
}


#define SAFETY (0.9)
#define PGROW  (-0.2)
#define PSHRNK (-0.25)
#define ERRCON (1.89e-4)


void OdeRK5OneStep(Ode *o)
{
  I8 i;
  double errmax,temp;
  double *tempstate=o->temp6,*stateerr=o->temp7;
  while (TRUE) {
    OdeCashKarp(o);
    errmax=0.0;
    for (i=0;i<o->n;i++) {
      temp=Abs(stateerr[i]/o->state_scale[i]);
      if (temp>errmax)
	errmax=temp;
    }
    errmax/=o->tolerance;
    if (errmax>1.0 && o->h>1.0e-64) {
      temp=o->h*SAFETY*errmax`PSHRNK;
      if (temp<0.1*o->h)
	o->h*=0.1;
      else
	o->h=temp;
    } else {
      o->t+=o->h;
      if (errmax>ERRCON)
	o->h*=SAFETY*errmax`PGROW;
      else
	o->h*=5.0;
      if (o->h>1.0e32)
	o->h=1.0e32;
      MemCpy(o->internal_state,tempstate,sizeof(double)*o->n);
      break;
    }
  }
}
#help_index ""