hschumann2/TempleOS-Source-Code
0847
1 2#help_index "Math/ODE"3#help_file "::/Doc/ODE"4 5//See ::/Doc/Credits.DD.6 7F64 LowPass1(F64 a,F64 y0,F64 y,F64 dt=1.0)8{//First order low pass filter9 dt=Exp(-a*dt);10 return y0*dt+y*(1.0-dt);11}12 13U0 ODERstPtrs(CMathODE *ode)14{15 I64 s=ode->n_internal*sizeof(F64);16 F64 *ptr=ode->array_base;17 ode->state_internal=ptr; ptr(I64)+=s;18 ode->state_scale=ptr; ptr(I64)+=s;19 ode->DstateDt=ptr; ptr(I64)+=s;20 ode->initial_state=ptr; ptr(I64)+=s;21 ode->tmp0=ptr; ptr(I64)+=s;22 ode->tmp1=ptr; ptr(I64)+=s;23 ode->tmp2=ptr; ptr(I64)+=s;24 ode->tmp3=ptr; ptr(I64)+=s;25 ode->tmp4=ptr; ptr(I64)+=s;26 ode->tmp5=ptr; ptr(I64)+=s;27 ode->tmp6=ptr; ptr(I64)+=s;28 ode->tmp7=ptr;29}30 31public CMathODE *ODENew(I64 n,F64 max_tolerance=1e-6,I64 flags=0)32{//Make differential equation ctrl struct. See flags.33 //The tolerance is not precise.34 //You can min_tolerance and it will35 //dynamically adjust tolerance to utilize36 //the CPU.37 I64 s=n*sizeof(F64);38 CMathODE *ode=CAlloc(sizeof(CMathODE));39 ode->t_scale=1.0;40 ode->flags=flags;41 ode->n_internal=ode->n=n;42 ode->h=1e-6;43 ode->h_min=1e-64;44 ode->h_max=1e32;45 ode->max_tolerance=ode->min_tolerance=ode->tolerance_internal=max_tolerance;46 ode->win_task=ode->mem_task=Fs;47 QueInit(&ode->next_mass);48 QueInit(&ode->next_spring);49 ode->state=CAlloc(s);50 ode->array_base=MAlloc(12*s);51 ODERstPtrs(ode);52 return ode;53}54 55 56public Bool ODEPause(CMathODE *ode,Bool val=ON)57{//Pause ODE.58 Bool res;59 if (!ode) return OFF;60 res=LBEqu(&ode->flags,ODEf_PAUSED,val);61 if (val)62 while (Bt(&ode->flags,ODEf_BUSY))63 Yield;64 return res;65}66 67public U0 ODEDel(CMathODE *ode)68{//Free ODE node, but not masses or springs.69 I64 i;70 if (!ode) return;71 ODEPause(ode);72 Free(ode->state);73 Free(ode->array_base);74 if (ode->slave_tasks) {75 for (i=0;i<mp_cnt;i++)76 Kill(ode->slave_tasks[i]);77 Free(ode->slave_tasks);78 }79 Free(ode);80}81 82public I64 ODESize(CMathODE *ode)83{//Mem size of ode ctrl, but not masses and springs.84 if (!ode)85 return 0;86 else87 return MSize2(ode->state)+MSize2(ode->array_base)+MSize2(ode);88}89 90U0 ODESetMassesPtrs(CMathODE *ode,F64 *state,F64 *DstateDt)91{92 COrder2D3 *ptr1=state(F64 *)+ode->n,93 *ptr2=DstateDt(F64 *)+ode->n;94 CMass *tmpm=ode->next_mass;95 while (tmpm!=&ode->next_mass) {96 tmpm->state=ptr1++;97 tmpm->DstateDt=ptr2++;98 tmpm=tmpm->next;99 }100}101 102U0 ODEState2Internal(CMathODE *ode)103{104 CMass *tmpm;105 F64 *old_array_base;106 I64 mass_cnt;107 108 if (ode->flags&ODEF_HAS_MASSES) {109 mass_cnt=0;110 tmpm=ode->next_mass;111 while (tmpm!=&ode->next_mass) {112 mass_cnt++;113 tmpm=tmpm->next;114 }115 old_array_base=ode->array_base;116 ode->n_internal=ode->n+6*mass_cnt;117 ode->array_base=MAlloc(12*ode->n_internal*sizeof(F64),ode->mem_task);118 Free(old_array_base);119 ODERstPtrs(ode);120 121 ODESetMassesPtrs(ode,ode->state_internal,ode->state_internal);122 tmpm=ode->next_mass;123 while (tmpm!=&ode->next_mass) {124 MemCpy(tmpm->state,&tmpm->saved_state,sizeof(COrder2D3));125 tmpm=tmpm->next;126 }127 }128 MemCpy(ode->state_internal,ode->state,ode->n*sizeof(F64));129}130 131U0 ODEInternal2State(CMathODE *ode)132{133 CMass *tmpm;134 MemCpy(ode->state,ode->state_internal,ode->n*sizeof(F64));135 if (ode->flags&ODEF_HAS_MASSES) {136 ODESetMassesPtrs(ode,ode->state_internal,ode->state_internal);137 tmpm=ode->next_mass;138 while (tmpm!=&ode->next_mass) {139 MemCpy(&tmpm->saved_state,tmpm->state,sizeof(COrder2D3));140 tmpm=tmpm->next;141 }142 }143}144 145public U0 ODERenum(CMathODE *ode)146{//Renumber masses and springs.147 I64 i;148 CSpring *tmps;149 CMass *tmpm;150 151 i=0;152 tmpm=ode->next_mass;153 while (tmpm!=&ode->next_mass) {154 tmpm->num=i++;155 tmpm=tmpm->next;156 }157 158 i=0;159 tmps=ode->next_spring;160 while (tmps!=&ode->next_spring) {161 tmps->num=i++;162 tmps->end1_num=tmps->end1->num;163 tmps->end2_num=tmps->end2->num;164 tmps=tmps->next;165 }166}167 168public CMass *MassFind(CMathODE *ode,F64 x,F64 y,F64 z=0)169{//Search for mass nearest to x,y,z.170 CMass *tmpm,*best_mass=NULL;171 F64 dd,best_dd=F64_MAX;172 173 tmpm=ode->next_mass;174 while (tmpm!=&ode->next_mass) {175 dd=Sqr(tmpm->x-x)+Sqr(tmpm->y-y)+Sqr(tmpm->z-z);176 if (dd<best_dd) {177 best_dd=dd;178 best_mass=tmpm;179 }180 tmpm=tmpm->next;181 }182 return best_mass;183}184 185public CSpring *SpringFind(CMathODE *ode,F64 x,F64 y,F64 z=0)186{//Find spring midpoint nearest x,y,z.187 CSpring *tmps,*best_spring=NULL;188 F64 dd,best_dd=F64_MAX;189 190 tmps=ode->next_spring;191 while (tmps!=&ode->next_spring) {192 dd=Sqr((tmps->end1->x+tmps->end2->x)/2-x)+193 Sqr((tmps->end1->y+tmps->end2->y)/2-y)+194 Sqr((tmps->end1->z+tmps->end2->z)/2-z);195 if (dd<best_dd) {196 best_dd=dd;197 best_spring=tmps;198 }199 tmps=tmps->next;200 }201 return best_spring;202}203 204public U0 MassOrSpringFind(205 CMathODE *ode,CMass **res_mass,CSpring **res_spring,206 F64 x,F64 y,F64 z=0)207{//Find spring or mass nearest x,y,z.208 CMass *tmpm,*best_mass=NULL;209 CSpring *tmps,*best_spring=NULL;210 F64 dd,best_dd=F64_MAX;211 212 tmpm=ode->next_mass;213 while (tmpm!=&ode->next_mass) {214 dd=Sqr(tmpm->x-x)+Sqr(tmpm->y-y)+Sqr(tmpm->z-z);215 if (dd<best_dd) {216 best_dd=dd;217 best_mass=tmpm;218 }219 tmpm=tmpm->next;220 }221 222 tmps=ode->next_spring;223 while (tmps!=&ode->next_spring) {224 dd=Sqr((tmps->end1->x+tmps->end2->x)/2-x)+225 Sqr((tmps->end1->y+tmps->end2->y)/2-y)+226 Sqr((tmps->end1->z+tmps->end2->z)/2-z);227 if (dd<best_dd) {228 best_dd=dd;229 best_spring=tmps;230 best_mass=NULL;231 }232 tmps=tmps->next;233 }234 if (res_mass) *res_mass =best_mass;235 if (res_spring) *res_spring=best_spring;236}237 238public CMass *MassFindNum(CMathODE *ode,I64 num)239{//Return mass number N.240 CMass *tmpm=ode->next_mass;241 while (tmpm!=&ode->next_mass) {242 if (tmpm->num==num)243 return tmpm;244 tmpm=tmpm->next;245 }246 return NULL;247}248 249public U0 ODERstInactive(CMathODE *ode)250{//Set all masses and springs to ACTIVE for new trial.251 CMass *tmpm;252 CSpring *tmps;253 tmpm=ode->next_mass;254 while (tmpm!=&ode->next_mass) {255 tmpm->flags&=~MSF_INACTIVE;256 tmpm=tmpm->next;257 }258 tmps=ode->next_spring;259 while (tmps!=&ode->next_spring) {260 tmps->flags&=~SSF_INACTIVE;261 tmps=tmps->next;262 }263}264 265U0 ODECalcSprings(CMathODE *ode)266{267 CSpring *tmps=ode->next_spring;268 CMass *e1,*e2;269 F64 d;270 CD3 p;271 while (tmps!=&ode->next_spring) {272 if (tmps->flags&SSF_INACTIVE) {273 tmps->displacement=0;274 tmps->f=0;275 } else {276 e1=tmps->end1;277 e2=tmps->end2;278 d=D3Norm(D3Sub(&p,&e2->state->x,&e1->state->x));279 tmps->displacement=d-tmps->rest_len;280 tmps->f=tmps->displacement*tmps->const;281 if (tmps->f>0 && tmps->flags&SSF_NO_TENSION)282 tmps->f=0;283 else if (tmps->f<0 && tmps->flags&SSF_NO_COMPRESSION)284 tmps->f=0;285 if (d>0) {286 D3MulEqu(&p,tmps->f/d);287 D3AddEqu(&e1->DstateDt->DxDt,&p);288 D3SubEqu(&e2->DstateDt->DxDt,&p);289 }290 }291 tmps=tmps->next;292 }293}294 295U0 ODECalcDrag(CMathODE *ode)296{297 CMass *tmpm;298 F64 d,dd;299 CD3 p;300 if (ode->drag_v || ode->drag_v2 || ode->drag_v3) {301 tmpm=ode->next_mass;302 while (tmpm!=&ode->next_mass) {303 if (!(tmpm->flags & MSF_INACTIVE) &&304 tmpm->drag_profile_factor &&305 (dd=D3NormSqr(&tmpm->state->DxDt))) {306 d=ode->drag_v;307 if (ode->drag_v2)308 d+=ode->drag_v2*Sqrt(dd);309 if (ode->drag_v3)310 d+=dd*ode->drag_v3;311 D3SubEqu(&tmpm->DstateDt->DxDt,312 D3Mul(&p,d*tmpm->drag_profile_factor,&tmpm->state->DxDt));313 }314 tmpm=tmpm->next;315 }316 }317}318 319U0 ODEApplyAccelerationLimit(CMathODE *ode)320{321 CMass *tmpm;322 F64 d;323 if (ode->acceleration_limit) {324 tmpm=ode->next_mass;325 while (tmpm!=&ode->next_mass) {326 if (!(tmpm->flags & MSF_INACTIVE) &&327 (d=D3Norm(&tmpm->DstateDt->DxDt))>ode->acceleration_limit)328 D3MulEqu(&tmpm->DstateDt->DxDt,ode->acceleration_limit/d);329 tmpm=tmpm->next;330 }331 }332}333 334U0 ODEMPTask(CMathODE *ode)335{336 while (TRUE) {337 while (!Bt(&ode->mp_not_done_flags,Gs->num))338 Yield;339 if (ode->mp_derive)340 (*ode->mp_derive)(ode,ode->mp_t,341 Gs->num,ode->mp_state,ode->mp_DstateDt);342 LBtr(&ode->mp_not_done_flags,Gs->num);343 }344}345 346U0 ODEMPWake(CMathODE *ode)347{348 I64 i;349 if (!ode->slave_tasks) {350 ode->slave_tasks=CAlloc(mp_cnt*sizeof(CTask *));351 for (i=0;i<mp_cnt;i++)352 ode->slave_tasks[i]=Spawn(&ODEMPTask,ode,"ODE Slave",i);353 }354 for (i=0;i<mp_cnt;i++) {355 Suspend(ode->slave_tasks[i],FALSE);356 MPInt(I_WAKE,i);357 }358}359 360U0 ODEMPSleep(CMathODE *ode)361{362 I64 i;363 if (ode->slave_tasks) {364 while (ode->mp_not_done_flags)365 Yield;366 for (i=0;i<mp_cnt;i++)367 Suspend(ode->slave_tasks[i]);368 }369}370 371U0 ODECallMPDerivative(CMathODE *ode,F64 t,F64 *state,F64 *DstateDt)372{373 ode->mp_t=t;374 ode->mp_state=state;375 ode->mp_DstateDt=DstateDt;376 ode->mp_not_done_flags=1<<mp_cnt-1;377 do Yield;378 while (ode->mp_not_done_flags);379}380 381U0 ODECallDerivative(CMathODE *ode,F64 t,F64 *state,F64 *DstateDt)382{383 CMass *tmpm;384 if (ode->flags&ODEF_HAS_MASSES) {385 ODESetMassesPtrs(ode,state,DstateDt);386 tmpm=ode->next_mass;387 while (tmpm!=&ode->next_mass) {388 if (!(tmpm->flags&MSF_INACTIVE)) {389 D3Zero(&tmpm->DstateDt->DxDt);390 D3Copy(&tmpm->DstateDt->x,&tmpm->state->DxDt);391 }392 tmpm=tmpm->next;393 }394 ODECalcSprings(ode);395 ODECalcDrag(ode);396 if (ode->mp_derive)397 ODECallMPDerivative(ode,t,state,DstateDt);398 if (ode->derive)399 (*ode->derive)(ode,t,state,DstateDt);400 tmpm=ode->next_mass;401 while (tmpm!=&ode->next_mass) {402 if (!(tmpm->flags&MSF_INACTIVE)) {403 if (tmpm->flags&MSF_FIXED) {404 D3Zero(&tmpm->DstateDt->DxDt);405 D3Zero(&tmpm->DstateDt->x);406 } else if (tmpm->mass)407 D3DivEqu(&tmpm->DstateDt->DxDt,tmpm->mass);408 }409 tmpm=tmpm->next;410 }411 ODEApplyAccelerationLimit(ode);412 } else {413 if (ode->mp_derive)414 ODECallMPDerivative(ode,t,state,DstateDt);415 if (ode->derive)416 (*ode->derive)(ode,t,state,DstateDt);417 }418}419 420U0 ODEOneStep(CMathODE *ode)421{422 I64 i;423 ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);424 for (i=0;i<ode->n_internal;i++)425 ode->state_internal[i]+=ode->h*ode->DstateDt[i];426 ode->t+=ode->h;427}428 429U0 ODERK4OneStep(CMathODE *ode)430{431 I64 i,n=ode->n_internal;432 F64 xh,hh,h6,*dym,*dyt,*yt,*DstateDt;433 434 dym =ode->tmp0;435 dyt =ode->tmp1;436 yt =ode->tmp2;437 DstateDt=ode->tmp3;438 hh =0.5*ode->h;439 h6 =ode->h / 6.0;440 xh =ode->t + hh;441 442 ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);443 for (i=0;i<n;i++)444 yt[i]=ode->state_internal[i]+hh*DstateDt[i];445 ODECallDerivative(ode,xh,yt,dyt);446 for (i=0;i<n;i++)447 yt[i]=ode->state_internal[i]+hh*dyt[i];448 ODECallDerivative(ode,xh,yt,dym);449 for (i=0;i<n;i++) {450 yt[i]=ode->state_internal[i]+ode->h*dym[i];451 dym[i]+=dyt[i];452 }453 ode->t+=ode->h;454 ODECallDerivative(ode,ode->t,yt,dyt);455 for (i=0;i<n;i++)456 ode->state_internal[i]+=h6*(DstateDt[i]+dyt[i]+2.0*dym[i]);457}458 459#define ODEa2 0.2460#define ODEa3 0.3461#define ODEa4 0.6462#define ODEa5 1.0463#define ODEa6 0.875464#define ODEb21 0.2465#define ODEb31 (3.0/40.0)466#define ODEb32 (9.0/40.0)467#define ODEb41 0.3468#define ODEb42 (-0.9)469#define ODEb43 1.2470#define ODEb51 (-11.0/54.0)471#define ODEb52 2.5472#define ODEb53 (-70.0/27.0)473#define ODEb54 (35.0/27.0)474#define ODEb61 (1631.0/55296.0)475#define ODEb62 (175.0/512.0)476#define ODEb63 (575.0/13824.0)477#define ODEb64 (44275.0/110592.0)478#define ODEb65 (253.0/4096.0)479#define ODEc1 (37.0/378.0)480#define ODEc3 (250.0/621.0)481#define ODEc4 (125.0/594.0)482#define ODEc6 (512.0/1771.0)483#define ODEdc1 (37.0/378.0-2825.0/27648.0)484#define ODEdc3 (250.0/621.0-18575.0/48384.0)485#define ODEdc4 (125.0/594.0-13525.0/55296.0)486#define ODEdc5 (-277.0/14336.0)487#define ODEdc6 (512.0/1771.0-0.25)488 489U0 ODECashKarp(CMathODE *ode)490{491 I64 i,n=ode->n_internal;492 F64 h=ode->h,*state=ode->state_internal,493 *DstateDt=ode->DstateDt,*ak2,*ak3,*ak4,*ak5,*ak6,494 *tmpstate,*stateerr,*outstate;495 496 ak2=ode->tmp0;497 ak3=ode->tmp1;498 ak4=ode->tmp2;499 ak5=ode->tmp3;500 ak6=ode->tmp4;501 tmpstate=ode->tmp5;502 outstate=ode->tmp6;503 stateerr=ode->tmp7;504 505 for (i=0;i<n;i++)506 tmpstate[i]=state[i]+ODEb21*h*DstateDt[i];507 ODECallDerivative(ode,ode->t+ODEa2*h,tmpstate,ak2);508 for (i=0;i<n;i++)509 tmpstate[i]=state[i]+h*(ODEb31*DstateDt[i]+ODEb32*ak2[i]);510 ODECallDerivative(ode,ode->t+ODEa3*h,tmpstate,ak3);511 for (i=0;i<n;i++)512 tmpstate[i]=state[i]+h*(ODEb41*DstateDt[i]+ODEb42*ak2[i]+ODEb43*ak3[i]);513 ODECallDerivative(ode,ode->t+ODEa4*h,tmpstate,ak4);514 for (i=0;i<n;i++)515 tmpstate[i]=state[i]+h*(ODEb51*DstateDt[i]+516 ODEb52*ak2[i]+ODEb53*ak3[i]+ODEb54*ak4[i]);517 ODECallDerivative(ode,ode->t+ODEa5*h,tmpstate,ak5);518 for (i=0;i<n;i++)519 tmpstate[i]=state[i]+h*(ODEb61*DstateDt[i]+520 ODEb62*ak2[i]+ODEb63*ak3[i]+ODEb64*ak4[i]+ODEb65*ak5[i]);521 ODECallDerivative(ode,ode->t+ODEa6*h,tmpstate,ak6);522 523 for (i=0;i<n;i++)524 outstate[i]=state[i]+h*(ODEc1*DstateDt[i]+525 ODEc3*ak3[i]+ODEc4*ak4[i]+ODEc6*ak6[i]);526 for (i=0;i<n;i++)527 stateerr[i]=h*(ODEdc1*DstateDt[i]+ODEdc3*ak3[i]+528 ODEdc4*ak4[i]+ODEdc5*ak5[i]+ODEdc6*ak6[i]);529}530 531#define SAFETY 0.9532#define PGROW (-0.2)533#define PSHRNK (-0.25)534#define ERRCON 1.89e-4535 536U0 ODERK5OneStep(CMathODE *ode)537{538 I64 i;539 F64 errmax,tmp,*tmpstate=ode->tmp6,*stateerr=ode->tmp7;540 while (TRUE) {541 ode->h=Clamp(ode->h,ode->h_min,ode->h_max);542 ODECashKarp(ode);543 errmax=0.0;544 for (i=0;i<ode->n_internal;i++) {545 tmp=Abs(stateerr[i]/ode->state_scale[i]);546 if (tmp>errmax)547 errmax=tmp;548 }549 errmax/=ode->tolerance_internal;550 if (errmax<=1.0 || ode->h==ode->h_min) break;551 tmp=ode->h*SAFETY*errmax`PSHRNK;552 if (tmp<0.1*ode->h)553 ode->h*=0.1;554 else555 ode->h=tmp;556 }557 ode->t+=ode->h;558 if (errmax>ERRCON)559 ode->h*=SAFETY*errmax`PGROW;560 else561 ode->h*=5.0;562 ode->h=Clamp(ode->h,ode->h_min,ode->h_max);563 MemCpy(ode->state_internal,tmpstate,sizeof(F64)*ode->n_internal);564}565 566F64 ode_alloced_factor=0.75;567 568U0 ODEsUpdate(CTask *task)569{/* This routine is called by the window mgron a continuous570basis to allow real-time simulation. It is intended571to provide ress good enough for games. It uses a runge-kutta572integrator which is a better algorithm than doing it with Euler.573 574It is adaptive step-sized, so it slows down when an important575event is taking place to improve accuracy, but in my implementation576it has a timeout.577*/578 I64 i;579 F64 d,start_time,timeout_time,t_desired,t_initial,interpolation;580 CMathODE *ode;581 582 if (task->next_ode==&task->next_ode)583 task->last_ode_time=0;584 else if (!Bt(&task->win_inhibit,WIf_SELF_ODE)) {585//See GrUpdateTasks() and GrUpdateTaskODEs().586 //We will not pick a time limit based on587 //how busy the CPU is, what percent of the588 //last refresh cycle was spent on ODE's589 //and what the refresh cycle rate was.590 start_time=tS;591 d=1.0/winmgr.fps;592 timeout_time=start_time+593 (task->last_ode_time/d+0.1)/(winmgr.last_ode_time/d+0.1)*594 ode_alloced_factor*d;595 ode=task->next_ode;596 while (ode!=&task->next_ode) {597 t_initial=ode->t;598 d=tS;599 if (!(ode->flags&ODEF_STARTED)) {600 ode->base_t=d;601 ode->flags|=ODEF_STARTED;602 }603 d-=ode->base_t+t_initial;604 t_desired=ode->t_scale*d+t_initial;605 if (ode->flags&ODEF_PAUSED)606 ode->base_t+=t_desired-ode->t; //Slip607 else {608 ode->flags|=ODEF_BUSY;609 if (ode->flags&ODEF_PAUSED)610 ode->base_t+=t_desired-ode->t; //Slip611 else {612 if (ode->derive || ode->mp_derive) {613 if (ode->mp_derive)614 ODEMPWake(ode);615 ODEState2Internal(ode);616 MemCpy(ode->initial_state,ode->state_internal,617 ode->n_internal*sizeof(F64));618 while (ode->t<t_desired) {619 ode->h_max=t_desired-ode->t;620 ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);621 for (i=0;i<ode->n_internal;i++)622 ode->state_scale[i]=Abs(ode->state_internal[i])+623 Abs(ode->DstateDt[i]*ode->h)+ode->tolerance_internal;624 ODERK5OneStep(ode);625 if (tS>timeout_time) {626 ode->base_t+=t_desired-ode->t; //Slip627 goto ode_done;628 629 }630 }631 632 //Interpolate if end time was not exact.633 if (ode->t!=t_desired) {634 if (interpolation=ode->t-t_initial) {635 interpolation=(t_desired-t_initial)/interpolation;636 if (interpolation!=1.0)637 for (i=0;i<ode->n_internal;i++)638 ode->state_internal[i]=(ode->state_internal[i]-639 ode->initial_state[i])*interpolation+640 ode->initial_state[i];641 }642 ode->t=t_desired;643 }644ode_done:645 ODEInternal2State(ode);646 647 //Convenience call to set vals648 ODECallDerivative(ode,ode->t,ode->state_internal,ode->DstateDt);649 650 if (ode->mp_derive)651 ODEMPSleep(ode);652 }653 }654 ode->flags&=~ODEF_BUSY;655 }656 ode->base_t+=(1.0-ode->t_scale)*d;657 ode=ode->next;658 }659 660 //Now, we will dynamically adjust tolerances.661 662 //We will regulate the tolerances663 //to fill the time we decided was664 //okay to devote to ODE's.665 //Since we might have multiple ODE's666 //active we scale them by the same factor.667 668 //This algorithm is probably not stable or very good, but it's something.669 670 //Target is 75% of alloced time.671 d=(tS-start_time)/(timeout_time-start_time)-0.75;672 673 ode=task->next_ode;674 while (ode!=&task->next_ode) {675 if (!(ode->flags&ODEF_PAUSED) && ode->derive) {676 if (ode->min_tolerance!=ode->max_tolerance) {677 if (d>0)678 ode->tolerance_internal*=10.0`d;679 else680 ode->tolerance_internal*=2.0`d;681 }682 ode->tolerance_internal=Clamp(ode->tolerance_internal,683 ode->min_tolerance,ode->max_tolerance);684 }685 ode=ode->next;686 }687 winmgr.ode_time+=task->last_ode_time=tS-start_time;688 }689}690