172 lines
4.5 KiB
Plaintext
172 lines
4.5 KiB
Plaintext
//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->base_t=ReadSysTimer*1.0/SYS_TIMER_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));
|
|
return o;
|
|
}
|
|
|
|
void OdeDel(Ode *o)
|
|
{
|
|
Free(o->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->state,o->DstateDt);
|
|
for (i=0;i<o->n;i++)
|
|
o->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->state,o->DstateDt);
|
|
for (i=0;i<n;i++)
|
|
yt[i]=o->state[i]+hh*DstateDt[i];
|
|
o->derivative(o,xh,yt,dyt);
|
|
for (i=0;i<n;i++)
|
|
yt[i]=o->state[i]+hh*dyt[i];
|
|
o->derivative(o,xh,yt,dym);
|
|
for (i=0;i<n;i++) {
|
|
yt[i]=o->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->state[i]+=h6*(DstateDt[i]+dyt[i]+2.0*dym[i]);
|
|
}
|
|
|
|
|
|
double ODEa2=0.2, ODEa3=0.3, ODEa4=0.6, ODEa5=1.0, ODEa6=0.875, ODEb21=0.2,
|
|
ODEb31=3.0/40.0, ODEb32=9.0/40.0, ODEb41=0.3, ODEb42=-0.9, ODEb43=1.2,
|
|
ODEb51=-11.0/54.0, ODEb52=2.5, ODEb53=-70.0/27.0, ODEb54=35.0/27.0,
|
|
ODEb61=1631.0/55296.0, ODEb62=175.0/512.0, ODEb63=575.0/13824.0,
|
|
ODEb64=44275.0/110592.0, ODEb65=253.0/4096.0, ODEc1=37.0/378.0,
|
|
ODEc3=250.0/621.0, ODEc4=125.0/594.0, ODEc6=512.0/1771.0,
|
|
ODEdc1=37.0/378.0 -2825.0/27648.0,
|
|
ODEdc3=250.0/621.0-18575.0/48384.0,
|
|
ODEdc4=125.0/594.0-13525.0/55296.0,
|
|
ODEdc5=-277.0/14336.0,
|
|
ODEdc6=512.0/1771.0-0.25;
|
|
|
|
|
|
|
|
void OdeCashKarp(Ode *o)
|
|
{
|
|
I8 i,n=o->n;
|
|
double h=o->h,*state=o->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->state,tempstate,sizeof(double)*o->n);
|
|
break;
|
|
}
|
|
}
|
|
}
|