610 lines
16 KiB
HolyC
610 lines
16 KiB
HolyC
#help_index "Math/ODE"
|
|
#help_file "::/Doc/ODE"
|
|
|
|
//See $LK,"::/Doc/Credits.DD"$.
|
|
|
|
F64 LowPass1(F64 a,F64 y0,F64 y,F64 dt=1.0)
|
|
{//First order low pass filter
|
|
dt=Exp(-a*dt);
|
|
return y0*dt+y*(1.0-dt);
|
|
}
|
|
|
|
U0 ODERstPtrs(CMathODE *ode)
|
|
{
|
|
I64 s=ode->n_internal*sizeof(F64);
|
|
F64 *ptr=ode->array_base;
|
|
ode->state_internal=ptr; ptr(I64)+=s;
|
|
ode->state_scale=ptr; ptr(I64)+=s;
|
|
ode->DstateDt=ptr; ptr(I64)+=s;
|
|
ode->initial_state=ptr; ptr(I64)+=s;
|
|
ode->temp0=ptr; ptr(I64)+=s;
|
|
ode->temp1=ptr; ptr(I64)+=s;
|
|
ode->temp2=ptr; ptr(I64)+=s;
|
|
ode->temp3=ptr; ptr(I64)+=s;
|
|
ode->temp4=ptr; ptr(I64)+=s;
|
|
ode->temp5=ptr; ptr(I64)+=s;
|
|
ode->temp6=ptr; ptr(I64)+=s;
|
|
ode->temp7=ptr;
|
|
}
|
|
|
|
public CMathODE *ODENew(I64 n,F64 max_tolerance=1e-6,I64 flags=0)
|
|
{//Make differential equation ctrl struct. See $LK,"flags",A="MN:ODEF_HAS_MASSES"$.
|
|
|
|
//The tolerance is not precise.
|
|
//You can min_tolerance and it will
|
|
//dynamically adjust tolerance to utilize
|
|
//the CPU.
|
|
|
|
I64 s=n*sizeof(F64);
|
|
CMathODE *ode=MAlloc(sizeof(CMathODE));
|
|
|
|
ode->t=0;
|
|
ode->t_scale=1.0;
|
|
ode->base_t=0;
|
|
ode->flags=flags;
|
|
ode->n_internal=ode->n=n;
|
|
ode->h=1e-6;
|
|
ode->h_min=1e-64;
|
|
ode->h_max=1e32;
|
|
ode->max_tolerance=ode->min_tolerance=ode->tolerance_internal=max_tolerance;
|
|
ode->win_task=ode->mem_task=Fs;
|
|
ode->derivative=NULL;
|
|
QueInit(&ode->next_mass);
|
|
QueInit(&ode->next_spring);
|
|
ode->acceleration_limit=ode->drag_v=ode->drag_v2=ode->drag_v3=0;
|
|
|
|
ode->state=CAlloc(s);
|
|
ode->array_base=MAlloc(12*s);
|
|
ODERstPtrs(ode);
|
|
return ode;
|
|
}
|
|
|
|
public U0 ODEDel(CMathODE *ode)
|
|
{//Free ODE node, but not masses or springs.
|
|
if (!ode) return;
|
|
Free(ode->state);
|
|
Free(ode->array_base);
|
|
Free(ode);
|
|
}
|
|
|
|
public I64 ODESize(CMathODE *ode)
|
|
{//Mem size of ode ctrl, but not masses and springs.
|
|
if (!ode)
|
|
return 0;
|
|
else
|
|
return MSize2(ode->state)+MSize2(ode->array_base)+MSize2(ode);
|
|
}
|
|
|
|
U0 ODESetMassesPtrs(CMathODE *ode,F64 *state,F64 *DstateDt)
|
|
{
|
|
COrder2D3 *ptr1=state(F64 *)+ode->n,
|
|
*ptr2=DstateDt(F64 *)+ode->n;
|
|
CMass *tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
tempm->state=ptr1++;
|
|
tempm->DstateDt=ptr2++;
|
|
tempm=tempm->next;
|
|
}
|
|
}
|
|
|
|
U0 ODEState2Internal(CMathODE *ode)
|
|
{
|
|
CMass *tempm;
|
|
F64 *old_array_base;
|
|
I64 mass_cnt;
|
|
|
|
if (ode->flags&ODEF_HAS_MASSES) {
|
|
mass_cnt=0;
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
mass_cnt++;
|
|
tempm=tempm->next;
|
|
}
|
|
old_array_base=ode->array_base;
|
|
ode->n_internal=ode->n+6*mass_cnt;
|
|
ode->array_base=MAlloc(12*ode->n_internal*sizeof(F64),ode->mem_task);
|
|
Free(old_array_base);
|
|
ODERstPtrs(ode);
|
|
|
|
ODESetMassesPtrs(ode,ode->state_internal,ode->state_internal);
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
MemCpy(tempm->state,&tempm->saved_state,sizeof(COrder2D3));
|
|
tempm=tempm->next;
|
|
}
|
|
}
|
|
MemCpy(ode->state_internal,ode->state,ode->n*sizeof(F64));
|
|
}
|
|
|
|
U0 ODEInternal2State(CMathODE *ode)
|
|
{
|
|
CMass *tempm;
|
|
MemCpy(ode->state,ode->state_internal,ode->n*sizeof(F64));
|
|
if (ode->flags&ODEF_HAS_MASSES) {
|
|
ODESetMassesPtrs(ode,ode->state_internal,ode->state_internal);
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
MemCpy(&tempm->saved_state,tempm->state,sizeof(COrder2D3));
|
|
tempm=tempm->next;
|
|
}
|
|
}
|
|
}
|
|
|
|
public U0 ODERenum(CMathODE *ode)
|
|
{//Renumber masses and springs.
|
|
I64 i;
|
|
CSpring *temps;
|
|
CMass *tempm;
|
|
|
|
i=0;
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
tempm->num=i++;
|
|
tempm=tempm->next;
|
|
}
|
|
|
|
i=0;
|
|
temps=ode->next_spring;
|
|
while (temps!=&ode->next_spring) {
|
|
temps->num=i++;
|
|
temps->end1_num=temps->end1->num;
|
|
temps->end2_num=temps->end2->num;
|
|
temps=temps->next;
|
|
}
|
|
}
|
|
|
|
public CMass *MassFind(CMathODE *ode,F64 x,F64 y,F64 z=0)
|
|
{//Search for mass nearest to x,y,z.
|
|
CMass *tempm,*best_mass=NULL;
|
|
F64 dd,best_dd=MAX_F64;
|
|
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
dd=Sqr(tempm->x-x)+Sqr(tempm->y-y)+Sqr(tempm->z-z);
|
|
if (dd<best_dd) {
|
|
best_dd=dd;
|
|
best_mass=tempm;
|
|
}
|
|
tempm=tempm->next;
|
|
}
|
|
return best_mass;
|
|
}
|
|
|
|
public CSpring *SpringFind(CMathODE *ode,F64 x,F64 y,F64 z=0)
|
|
{//Find spring midpoint nearest x,y,z.
|
|
CSpring *temps,*best_spring=NULL;
|
|
F64 dd,best_dd=MAX_F64;
|
|
|
|
temps=ode->next_spring;
|
|
while (temps!=&ode->next_spring) {
|
|
dd=Sqr((temps->end1->x+temps->end2->x)/2-x)+
|
|
Sqr((temps->end1->y+temps->end2->y)/2-y)+
|
|
Sqr((temps->end1->z+temps->end2->z)/2-z);
|
|
if (dd<best_dd) {
|
|
best_dd=dd;
|
|
best_spring=temps;
|
|
}
|
|
temps=temps->next;
|
|
}
|
|
return best_spring;
|
|
}
|
|
|
|
public U0 MassOrSpringFind(
|
|
CMathODE *ode,CMass **res_mass,CSpring **res_spring,
|
|
F64 x,F64 y,F64 z=0)
|
|
{//Find spring or mass nearest x,y,z.
|
|
CMass *tempm,*best_mass=NULL;
|
|
CSpring *temps,*best_spring=NULL;
|
|
F64 dd,best_dd=MAX_F64;
|
|
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
dd=Sqr(tempm->x-x)+Sqr(tempm->y-y)+Sqr(tempm->z-z);
|
|
if (dd<best_dd) {
|
|
best_dd=dd;
|
|
best_mass=tempm;
|
|
}
|
|
tempm=tempm->next;
|
|
}
|
|
|
|
temps=ode->next_spring;
|
|
while (temps!=&ode->next_spring) {
|
|
dd=Sqr((temps->end1->x+temps->end2->x)/2-x)+
|
|
Sqr((temps->end1->y+temps->end2->y)/2-y)+
|
|
Sqr((temps->end1->z+temps->end2->z)/2-z);
|
|
if (dd<best_dd) {
|
|
best_dd=dd;
|
|
best_spring=temps;
|
|
best_mass=NULL;
|
|
}
|
|
temps=temps->next;
|
|
}
|
|
if (res_mass) *res_mass =best_mass;
|
|
if (res_spring) *res_spring=best_spring;
|
|
}
|
|
|
|
public CMass *MassFindNum(CMathODE *ode,I64 num)
|
|
{//Return mass number N.
|
|
CMass *tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
if (tempm->num==num)
|
|
return tempm;
|
|
tempm=tempm->next;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
public U0 ODERstInactive(CMathODE *ode)
|
|
{//Set all masses and springs to ACTIVE for new trial.
|
|
CMass *tempm;
|
|
CSpring *temps;
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
tempm->flags&=~MSF_INACTIVE;
|
|
tempm=tempm->next;
|
|
}
|
|
temps=ode->next_spring;
|
|
while (temps!=&ode->next_spring) {
|
|
temps->flags&=~SSF_INACTIVE;
|
|
temps=temps->next;
|
|
}
|
|
}
|
|
|
|
U0 ODECalcSprings(CMathODE *ode)
|
|
{
|
|
CSpring *temps=ode->next_spring;
|
|
CMass *e1,*e2;
|
|
F64 d;
|
|
CD3 p;
|
|
while (temps!=&ode->next_spring) {
|
|
if (temps->flags&SSF_INACTIVE) {
|
|
temps->displacement=0;
|
|
temps->f=0;
|
|
} else {
|
|
e1=temps->end1;
|
|
e2=temps->end2;
|
|
d=D3Norm(D3Sub(&p,&e2->state->x,&e1->state->x));
|
|
temps->displacement=d-temps->rest_len;
|
|
temps->f=temps->displacement*temps->const;
|
|
if (temps->f>0 && temps->flags&SSF_NO_TENSION)
|
|
temps->f=0;
|
|
else if (temps->f<0 && temps->flags&SSF_NO_COMPRESSION)
|
|
temps->f=0;
|
|
if (d>0) {
|
|
D3MulEqu(&p,temps->f/d);
|
|
D3AddEqu(&e1->DstateDt->DxDt,&p);
|
|
D3SubEqu(&e2->DstateDt->DxDt,&p);
|
|
}
|
|
}
|
|
temps=temps->next;
|
|
}
|
|
}
|
|
|
|
U0 ODECalcDrag(CMathODE *ode)
|
|
{
|
|
CMass *tempm;
|
|
F64 d,dd;
|
|
CD3 p;
|
|
if (ode->drag_v || ode->drag_v2 || ode->drag_v3) {
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
if (!(tempm->flags & MSF_INACTIVE) &&
|
|
tempm->drag_profile_factor &&
|
|
(dd=D3NormSqr(&tempm->state->DxDt))) {
|
|
d=ode->drag_v;
|
|
if (ode->drag_v2)
|
|
d+=ode->drag_v2*Sqrt(dd);
|
|
if (ode->drag_v3)
|
|
d+=dd*ode->drag_v3;
|
|
D3SubEqu(&tempm->DstateDt->DxDt,
|
|
D3Mul(&p,d*tempm->drag_profile_factor,&tempm->state->DxDt));
|
|
}
|
|
tempm=tempm->next;
|
|
}
|
|
}
|
|
}
|
|
|
|
U0 ODEApplyAccelerationLimit(CMathODE *ode)
|
|
{
|
|
CMass *tempm;
|
|
F64 d;
|
|
if (ode->acceleration_limit) {
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
if (!(tempm->flags & MSF_INACTIVE) &&
|
|
(d=D3Norm(&tempm->DstateDt->DxDt))>ode->acceleration_limit)
|
|
D3MulEqu(&tempm->DstateDt->DxDt,ode->acceleration_limit/d);
|
|
tempm=tempm->next;
|
|
}
|
|
}
|
|
}
|
|
|
|
U0 ODECallDerivative(CMathODE *ode,F64 t,F64 *state,F64 *DstateDt)
|
|
{
|
|
CMass *tempm;
|
|
if (ode->flags&ODEF_HAS_MASSES) {
|
|
ODESetMassesPtrs(ode,state,DstateDt);
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
if (!(tempm->flags&MSF_INACTIVE)) {
|
|
D3Zero(&tempm->DstateDt->DxDt);
|
|
D3Copy(&tempm->DstateDt->x,&tempm->state->DxDt);
|
|
}
|
|
tempm=tempm->next;
|
|
}
|
|
ODECalcSprings(ode);
|
|
ODECalcDrag(ode);
|
|
(*ode->derivative)(ode,t,state,DstateDt);
|
|
tempm=ode->next_mass;
|
|
while (tempm!=&ode->next_mass) {
|
|
if (!(tempm->flags&MSF_INACTIVE)) {
|
|
if (tempm->flags&MSF_FIXED) {
|
|
D3Zero(&tempm->DstateDt->DxDt);
|
|
D3Zero(&tempm->DstateDt->x);
|
|
} else if (tempm->mass)
|
|
D3DivEqu(&tempm->DstateDt->DxDt,tempm->mass);
|
|
}
|
|
tempm=tempm->next;
|
|
}
|
|
ODEApplyAccelerationLimit(ode);
|
|
} else
|
|
(*ode->derivative)(ode,t,state,DstateDt);
|
|
}
|
|
|
|
U0 ODEOneStep(CMathODE *ode)
|
|
{
|
|
I64 i;
|
|
ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
for (i=0;i<ode->n_internal;i++)
|
|
ode->state_internal[i]+=ode->h*ode->DstateDt[i];
|
|
ode->t+=ode->h;
|
|
}
|
|
|
|
U0 ODERK4OneStep(CMathODE *ode)
|
|
{
|
|
I64 i,n=ode->n_internal;
|
|
F64 xh,hh,h6,*dym,*dyt,*yt,*DstateDt;
|
|
|
|
dym =ode->temp0;
|
|
dyt =ode->temp1;
|
|
yt =ode->temp2;
|
|
DstateDt=ode->temp3;
|
|
hh =0.5*ode->h;
|
|
h6 =ode->h / 6.0;
|
|
xh =ode->t + hh;
|
|
|
|
ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
for (i=0;i<n;i++)
|
|
yt[i]=ode->state_internal[i]+hh*DstateDt[i];
|
|
ODECallDerivative(ode,xh,yt,dyt);
|
|
for (i=0;i<n;i++)
|
|
yt[i]=ode->state_internal[i]+hh*dyt[i];
|
|
ODECallDerivative(ode,xh,yt,dym);
|
|
for (i=0;i<n;i++) {
|
|
yt[i]=ode->state_internal[i]+ode->h*dym[i];
|
|
dym[i]+=dyt[i];
|
|
}
|
|
ode->t+=ode->h;
|
|
ODECallDerivative(ode,ode->t,yt,dyt);
|
|
for (i=0;i<n;i++)
|
|
ode->state_internal[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)
|
|
|
|
U0 ODECashKarp(CMathODE *ode)
|
|
{
|
|
I64 i,n=ode->n_internal;
|
|
F64 h=ode->h,*state=ode->state_internal,
|
|
*DstateDt=ode->DstateDt,*ak2,*ak3,*ak4,*ak5,*ak6,
|
|
*tempstate,*stateerr,*outstate;
|
|
|
|
ak2=ode->temp0;
|
|
ak3=ode->temp1;
|
|
ak4=ode->temp2;
|
|
ak5=ode->temp3;
|
|
ak6=ode->temp4;
|
|
tempstate=ode->temp5;
|
|
outstate=ode->temp6;
|
|
stateerr=ode->temp7;
|
|
|
|
for (i=0;i<n;i++)
|
|
tempstate[i]=state[i]+ODEb21*h*DstateDt[i];
|
|
ODECallDerivative(ode,ode->t+ODEa2*h,tempstate,ak2);
|
|
for (i=0;i<n;i++)
|
|
tempstate[i]=state[i]+h*(ODEb31*DstateDt[i]+ODEb32*ak2[i]);
|
|
ODECallDerivative(ode,ode->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]);
|
|
ODECallDerivative(ode,ode->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]);
|
|
ODECallDerivative(ode,ode->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]);
|
|
ODECallDerivative(ode,ode->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
|
|
|
|
U0 ODERK5OneStep(CMathODE *ode)
|
|
{
|
|
I64 i;
|
|
F64 errmax,temp,*tempstate=ode->temp6,*stateerr=ode->temp7;
|
|
while (TRUE) {
|
|
ode->h=Clamp(ode->h,ode->h_min,ode->h_max);
|
|
ODECashKarp(ode);
|
|
errmax=0.0;
|
|
for (i=0;i<ode->n_internal;i++) {
|
|
temp=Abs(stateerr[i]/ode->state_scale[i]);
|
|
if (temp>errmax)
|
|
errmax=temp;
|
|
}
|
|
errmax/=ode->tolerance_internal;
|
|
if (errmax<=1.0 || ode->h==ode->h_min) break;
|
|
temp=ode->h*SAFETY*errmax`PSHRNK;
|
|
if (temp<0.1*ode->h)
|
|
ode->h*=0.1;
|
|
else
|
|
ode->h=temp;
|
|
}
|
|
ode->t+=ode->h;
|
|
if (errmax>ERRCON)
|
|
ode->h*=SAFETY*errmax`PGROW;
|
|
else
|
|
ode->h*=5.0;
|
|
ode->h=Clamp(ode->h,ode->h_min,ode->h_max);
|
|
MemCpy(ode->state_internal,tempstate,sizeof(F64)*ode->n_internal);
|
|
}
|
|
|
|
F64 ode_alloced_factor=0.75;
|
|
|
|
U0 ODEsUpdate(CTask *task)
|
|
{/* This routine is called by the $LK,"window mgr",A="FF:::/Adam/Gr/GrScreen.HC,ODEsUpdate"$ on a continuous
|
|
basis to allow real-time simulation. It is intended
|
|
to provide ress good enough for games. It uses a runge-kutta
|
|
integrator which is a better algorithm than doing it with Euler.
|
|
|
|
It is adaptive step-sized, so it slows down when an important
|
|
event is taking place to improve accuracy, but in my implementation
|
|
it has a timeout.
|
|
*/
|
|
I64 i;
|
|
F64 d,start_time,timeout_time,t_desired,t_initial,interpolation;
|
|
CMathODE *ode;
|
|
|
|
if (task->next_ode==&task->next_ode)
|
|
task->last_ode_time=0;
|
|
else if (!Bt(&task->win_inhibit,WIf_SELF_ODE)) {
|
|
//See $LK,"GrUpdateTasks",A="MN:GrUpdateTasks"$() and $LK,"GrUpdateTaskODEs",A="MN:GrUpdateTaskODEs"$().
|
|
//We will not pick a time limit based on
|
|
//how busy the CPU is, what percent of the
|
|
//last refresh cycle was spent on ODE's
|
|
//and what the refresh cycle rate was.
|
|
start_time=tS;
|
|
d=1.0/winmgr.fps;
|
|
timeout_time=start_time+
|
|
(task->last_ode_time/d+0.1)/(winmgr.last_ode_time/d+0.1)*
|
|
ode_alloced_factor*d;
|
|
ode=task->next_ode;
|
|
while (ode!=&task->next_ode) {
|
|
t_initial=ode->t;
|
|
d=tS;
|
|
if (!(ode->flags&ODEF_STARTED)) {
|
|
ode->base_t=d;
|
|
ode->flags|=ODEF_STARTED;
|
|
}
|
|
d-=ode->base_t+t_initial;
|
|
t_desired=ode->t_scale*d+t_initial;
|
|
if (ode->flags&ODEF_PAUSED)
|
|
ode->base_t+=t_desired-ode->t; //Slip
|
|
else if (ode->derivative) {
|
|
ODEState2Internal(ode);
|
|
MemCpy(ode->initial_state,ode->state_internal,
|
|
ode->n_internal*sizeof(F64));
|
|
while (ode->t<t_desired) {
|
|
ode->h_max=t_desired-ode->t;
|
|
ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
for (i=0;i<ode->n_internal;i++)
|
|
ode->state_scale[i]=Abs(ode->state_internal[i])+
|
|
Abs(ode->DstateDt[i]*ode->h)+ode->tolerance_internal;
|
|
ODERK5OneStep(ode);
|
|
if (tS>timeout_time) {
|
|
ode->base_t+=t_desired-ode->t; //Slip
|
|
goto ode_done;
|
|
|
|
}
|
|
}
|
|
|
|
//Interpolate if end time was not exact.
|
|
if (ode->t!=t_desired) {
|
|
if (interpolation=ode->t-t_initial) {
|
|
interpolation=(t_desired-t_initial)/interpolation;
|
|
if (interpolation!=1.0)
|
|
for (i=0;i<ode->n_internal;i++)
|
|
ode->state_internal[i]=(ode->state_internal[i]-
|
|
ode->initial_state[i])*interpolation+
|
|
ode->initial_state[i];
|
|
}
|
|
ode->t=t_desired;
|
|
}
|
|
ode_done:
|
|
ODEInternal2State(ode);
|
|
|
|
//Convenience call to set vals
|
|
ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
}
|
|
ode->base_t+=(1.0-ode->t_scale)*d;
|
|
ode=ode->next;
|
|
}
|
|
|
|
//Now, we will dynamically adjust tolerances.
|
|
|
|
//We will regulate the tolerances
|
|
//to fill the time we decided was
|
|
//okay to devote to ODE's.
|
|
//Since we might have multiple ODE's
|
|
//active we scale them by the same factor.
|
|
|
|
//This algorithm is probably not stable or very good, but it's something.
|
|
|
|
//Target is 75% of alloced time.
|
|
d=(tS-start_time)/(timeout_time-start_time)-0.75;
|
|
|
|
ode=task->next_ode;
|
|
while (ode!=&task->next_ode) {
|
|
if (!(ode->flags&ODEF_PAUSED) && ode->derivative) {
|
|
if (ode->min_tolerance!=ode->max_tolerance) {
|
|
if (d>0)
|
|
ode->tolerance_internal*=10.0`d;
|
|
else
|
|
ode->tolerance_internal*=2.0`d;
|
|
}
|
|
ode->tolerance_internal=Clamp(ode->tolerance_internal,
|
|
ode->min_tolerance,ode->max_tolerance);
|
|
}
|
|
ode=ode->next;
|
|
}
|
|
winmgr.ode_time+=task->last_ode_time=tS-start_time;
|
|
}
|
|
}
|