2021-07-03 05:07:57 +01:00
<!DOCTYPE HTML>
< html >
< head >
< meta http-equiv = "Content-Type" content = "text/html;charset=US-ASCII" >
2021-12-11 11:10:58 +00:00
< meta name = "generator" content = "ZealOS V1.07" >
2021-07-03 05:07:57 +01:00
< style type = "text/css" >
2021-10-08 07:06:11 +01:00
body {background-color:#1f1f1f;}
.cF0{color:#e3e3e3;background-color:#1f1f1f;}
.cF1{color:#4f84a6;background-color:#1f1f1f;}
.cF2{color:#73a255;background-color:#1f1f1f;}
.cF3{color:#297582;background-color:#1f1f1f;}
.cF4{color:#b34f4b;background-color:#1f1f1f;}
.cF5{color:#8a52c3;background-color:#1f1f1f;}
.cF6{color:#b7822f;background-color:#1f1f1f;}
.cF7{color:#444444;background-color:#1f1f1f;}
.cF8{color:#6d6d6d;background-color:#1f1f1f;}
.cF9{color:#94bfde;background-color:#1f1f1f;}
.cFA{color:#a1ce97;background-color:#1f1f1f;}
.cFB{color:#6db4be;background-color:#1f1f1f;}
.cFC{color:#e88e88;background-color:#1f1f1f;}
.cFD{color:#ca94e8;background-color:#1f1f1f;}
.cFE{color:#d4b475;background-color:#1f1f1f;}
.cFF{color:#1f1f1f;background-color:#1f1f1f;}
2021-07-03 05:07:57 +01:00
< / style >
< / head >
< body >
2021-07-05 01:12:38 +01:00
< pre style = "font-family:monospace;font-size:12pt" >
2021-07-03 05:07:57 +01:00
< a name = "l1" > < / a > < span class = cF0 > #< / span > < span class = cF1 > help_index< / span > < span class = cF0 > < / span > < span class = cF6 > " Math/ODE" < / span > < span class = cF0 >
< a name = "l2" > < / a > #< / span > < span class = cF1 > help_file< / span > < span class = cF0 > < / span > < span class = cF6 > " ::/Doc/ODE" < / span > < span class = cF0 >
< a name = "l3" > < / a >
2021-07-26 20:29:49 +01:00
< a name = "l4" > < / a > < / span > < span class = cF2 > //See < / span > < a href = "https://zeal-operating-system.github.io/ZealOS/Doc/Credits.DD.html#l1" > < span class = cF4 > ::/Doc/Credits.DD< / span > < / a > < span class = cF2 > .< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l5" > < / a >
< a name = "l6" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > < / span > < span class = cF5 > LowPass1< / span > < span class = cF0 > (< / span > < span class = cF1 > F64< / span > < span class = cF0 > a, < / span > < span class = cF1 > F64< / span > < span class = cF0 > y0, < / span > < span class = cF1 > F64< / span > < span class = cF0 > y, < / span > < span class = cF1 > F64< / span > < span class = cF0 > dt=< / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l7" > < / a > {< / span > < span class = cF2 > //First order low pass filter< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l8" > < / a > dt = < / span > < span class = cF5 > Exp< / span > < span class = cF0 > (-a * dt);
< a name = "l9" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > y0 * dt + y * (< / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > - dt);
2021-07-03 05:07:57 +01:00
< a name = "l10" > < / a > }
< a name = "l11" > < / a >
< a name = "l12" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEResetPtrs< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l13" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l14" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > s = ode-> n_internal * < / span > < span class = cF1 > sizeof< / span > < span class = cF0 > (< / span > < span class = cF1 > F64< / span > < span class = cF0 > );
< a name = "l15" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > *ptr = ode-> array_base;
2021-07-03 05:07:57 +01:00
< a name = "l16" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l17" > < / a > ode-> state_internal = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l18" > < / a > ode-> state_scale = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l19" > < / a > ode-> DstateDt = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l20" > < / a > ode-> initial_state = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l21" > < / a > ode-> tmp0 = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l22" > < / a > ode-> tmp1 = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l23" > < / a > ode-> tmp2 = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l24" > < / a > ode-> tmp3 = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l25" > < / a > ode-> tmp4 = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l26" > < / a > ode-> tmp5 = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l27" > < / a > ode-> tmp6 = ptr; ptr(< / span > < span class = cF9 > I64< / span > < span class = cF0 > ) += s;
< a name = "l28" > < / a > ode-> tmp7 = ptr;
2021-07-03 05:07:57 +01:00
< a name = "l29" > < / a > }
< a name = "l30" > < / a >
< a name = "l31" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *< / span > < span class = cF5 > ODENew< / span > < span class = cF0 > (< / span > < span class = cF9 > I64< / span > < span class = cF0 > n, < / span > < span class = cF1 > F64< / span > < span class = cF0 > max_tolerance=< / span > < span class = cFE > 1< / span > < span class = cF0 > e-< / span > < span class = cFE > 6< / span > < span class = cF0 > , < / span > < span class = cF9 > I64< / span > < span class = cF0 > flags=< / span > < span class = cFE > 0< / span > < span class = cF0 > )
2021-07-26 20:29:49 +01:00
< a name = "l32" > < / a > {< / span > < span class = cF2 > //Make differential equation ctrl struct. See < / span > < a href = "https://zeal-operating-system.github.io/ZealOS/Kernel/KernelA.HH.html#l271" > < span class = cF4 > flags< / span > < / a > < span class = cF2 > .< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l33" > < / a > < / span > < span class = cF2 > //The tolerance is not precise.< / span > < span class = cF0 >
< a name = "l34" > < / a > < / span > < span class = cF2 > //You can min_tolerance and it will< / span > < span class = cF0 >
< a name = "l35" > < / a > < / span > < span class = cF2 > //dynamically adjust tolerance to utilize< / span > < span class = cF0 >
< a name = "l36" > < / a > < / span > < span class = cF2 > //the CPU.< / span > < span class = cF0 >
< a name = "l37" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > s = n * < / span > < span class = cF1 > sizeof< / span > < span class = cF0 > (< / span > < span class = cF1 > F64< / span > < span class = cF0 > );
< a name = "l38" > < / a > < / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode = < / span > < span class = cF5 > CAlloc< / span > < span class = cF0 > (< / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF7 > )< / span > < span class = cF0 > );
2021-07-03 05:07:57 +01:00
< a name = "l39" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l40" > < / a > ode-> t_scale = < / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l41" > < / a > ode-> flags = flags;
< a name = "l42" > < / a > ode-> n_internal = ode-> n = n;
< a name = "l43" > < / a > ode-> h = < / span > < span class = cFE > 1< / span > < span class = cF0 > e-< / span > < span class = cFE > 6< / span > < span class = cF0 > ;
< a name = "l44" > < / a > ode-> h_min = < / span > < span class = cFE > 1< / span > < span class = cF0 > e-< / span > < span class = cFE > 64< / span > < span class = cF0 > ;
< a name = "l45" > < / a > ode-> h_max = < / span > < span class = cFE > 1< / span > < span class = cF0 > e32;
< a name = "l46" > < / a > ode-> max_tolerance = ode-> min_tolerance = ode-> tolerance_internal = max_tolerance;
< a name = "l47" > < / a > ode-> win_task = ode-> mem_task = < / span > < span class = cF5 > Fs< / span > < span class = cF0 > ;
< a name = "l48" > < / a > < / span > < span class = cF5 > QueueInit< / span > < span class = cF0 > (& ode-> next_mass);
< a name = "l49" > < / a > < / span > < span class = cF5 > QueueInit< / span > < span class = cF0 > (& ode-> next_spring);
< a name = "l50" > < / a > ode-> state = < / span > < span class = cF5 > CAlloc< / span > < span class = cF0 > (s);
< a name = "l51" > < / a > ode-> array_base = < / span > < span class = cF5 > MAlloc< / span > < span class = cF0 > (< / span > < span class = cFE > 12< / span > < span class = cF0 > * s);
< a name = "l52" > < / a > < / span > < span class = cF5 > ODEResetPtrs< / span > < span class = cF0 > (ode);
2021-07-03 05:07:57 +01:00
< a name = "l53" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l54" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > ode;
2021-07-03 05:07:57 +01:00
< a name = "l55" > < / a > }
< a name = "l56" > < / a >
< a name = "l57" > < / a >
< a name = "l58" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF1 > Bool< / span > < span class = cF0 > < / span > < span class = cF5 > ODEPause< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF1 > Bool< / span > < span class = cF0 > val=< / span > < span class = cF3 > ON< / span > < span class = cF0 > )
< a name = "l59" > < / a > {< / span > < span class = cF2 > //Pause ODE.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l60" > < / a > < / span > < span class = cF1 > Bool< / span > < span class = cF0 > res;
2021-07-03 05:07:57 +01:00
< a name = "l61" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l62" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!ode)
< a name = "l63" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > < / span > < span class = cF3 > OFF< / span > < span class = cF0 > ;
< a name = "l64" > < / a > res = < / span > < span class = cF5 > LBEqual< / span > < span class = cF0 > (& ode-> flags, < / span > < span class = cF3 > ODEf_PAUSED< / span > < span class = cF0 > , val);
< a name = "l65" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (val)
< a name = "l66" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (< / span > < span class = cF5 > Bt< / span > < span class = cF7 > (< / span > < span class = cF0 > & ode-> flags, < / span > < span class = cF3 > ODEf_BUSY< / span > < span class = cF7 > )< / span > < span class = cF0 > )
< a name = "l67" > < / a > < / span > < span class = cF5 > Yield< / span > < span class = cF0 > ;
2021-07-03 05:07:57 +01:00
< a name = "l68" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l69" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > res;
2021-07-03 05:07:57 +01:00
< a name = "l70" > < / a > }
< a name = "l71" > < / a >
< a name = "l72" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEDel< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l73" > < / a > {< / span > < span class = cF2 > //Free ODE node, but not masses or springs.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l74" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i;
2021-07-03 05:07:57 +01:00
< a name = "l75" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l76" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!ode)
< a name = "l77" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > ;
< a name = "l78" > < / a > < / span > < span class = cF5 > ODEPause< / span > < span class = cF0 > (ode);
< a name = "l79" > < / a > < / span > < span class = cF5 > Free< / span > < span class = cF0 > (ode-> state);
< a name = "l80" > < / a > < / span > < span class = cF5 > Free< / span > < span class = cF0 > (ode-> array_base);
< a name = "l81" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> slave_tasks)
< a name = "l82" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l83" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < < / span > < span class = cFB > mp_count< / span > < span class = cF0 > ; i++)
< a name = "l84" > < / a > < / span > < span class = cF5 > Kill< / span > < span class = cF0 > (ode-> slave_tasks[i]);
< a name = "l85" > < / a > < / span > < span class = cF5 > Free< / span > < span class = cF0 > (ode-> slave_tasks);
< a name = "l86" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l87" > < / a > < / span > < span class = cF5 > Free< / span > < span class = cF0 > (ode);
2021-07-03 05:07:57 +01:00
< a name = "l88" > < / a > }
< a name = "l89" > < / a >
< a name = "l90" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF9 > I64< / span > < span class = cF0 > < / span > < span class = cF5 > ODESize< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l91" > < / a > {< / span > < span class = cF2 > //Mem size of ode ctrl, but not masses and springs.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l92" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!ode)
< a name = "l93" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l94" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l95" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > < / span > < span class = cF5 > MSize2< / span > < span class = cF0 > (ode-> state) + < / span > < span class = cF5 > MSize2< / span > < span class = cF0 > (ode-> array_base) + < / span > < span class = cF5 > MSize2< / span > < span class = cF0 > (ode);
2021-07-03 05:07:57 +01:00
< a name = "l96" > < / a > }
< a name = "l97" > < / a >
< a name = "l98" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODESetMassesPtrs< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF1 > F64< / span > < span class = cF0 > *state, < / span > < span class = cF1 > F64< / span > < span class = cF0 > *DstateDt)
< a name = "l99" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l100" > < / a > < / span > < span class = cF9 > COrder2D3< / span > < span class = cF0 > *ptr1 = state(< / span > < span class = cF1 > F64< / span > < span class = cF0 > *) + ode-> n, *ptr2 = DstateDt(< / span > < span class = cF1 > F64< / span > < span class = cF0 > *) + ode-> n;
< a name = "l101" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm = ode-> next_mass;
2021-07-03 05:07:57 +01:00
< a name = "l102" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l103" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l104" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l105" > < / a > tmpm-> state = ptr1++;
< a name = "l106" > < / a > tmpm-> DstateDt = ptr2++;
< a name = "l107" > < / a > tmpm = tmpm-> next;
< a name = "l108" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l109" > < / a > }
< a name = "l110" > < / a >
< a name = "l111" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEState2Internal< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l112" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l113" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm;
< a name = "l114" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > *old_array_base;
< a name = "l115" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > mass_count;
2021-07-03 05:07:57 +01:00
< a name = "l116" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l117" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> flags & < / span > < span class = cF3 > ODEF_HAS_MASSES< / span > < span class = cF0 > )
< a name = "l118" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l119" > < / a > mass_count = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l120" > < / a > tmpm = ode-> next_mass;
< a name = "l121" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l122" > < / a > {
< a name = "l123" > < / a > mass_count++;
< a name = "l124" > < / a > tmpm = tmpm-> next;
< a name = "l125" > < / a > }
< a name = "l126" > < / a > old_array_base = ode-> array_base;
< a name = "l127" > < / a > ode-> n_internal = ode-> n + < / span > < span class = cFE > 6< / span > < span class = cF0 > * mass_count;
< a name = "l128" > < / a > ode-> array_base = < / span > < span class = cF5 > MAlloc< / span > < span class = cF0 > (< / span > < span class = cFE > 12< / span > < span class = cF0 > * ode-> n_internal * < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF1 > F64< / span > < span class = cF7 > )< / span > < span class = cF0 > , ode-> mem_task);
< a name = "l129" > < / a > < / span > < span class = cF5 > Free< / span > < span class = cF0 > (old_array_base);
< a name = "l130" > < / a > < / span > < span class = cF5 > ODEResetPtrs< / span > < span class = cF0 > (ode);
2021-07-03 05:07:57 +01:00
< a name = "l131" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l132" > < / a > < / span > < span class = cF5 > ODESetMassesPtrs< / span > < span class = cF0 > (ode, ode-> state_internal, ode-> state_internal);
< a name = "l133" > < / a > tmpm = ode-> next_mass;
< a name = "l134" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l135" > < / a > {
< a name = "l136" > < / a > < / span > < span class = cF5 > MemCopy< / span > < span class = cF0 > (tmpm-> state, & tmpm-> saved_state, < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF9 > COrder2D3< / span > < span class = cF7 > )< / span > < span class = cF0 > );
< a name = "l137" > < / a > tmpm = tmpm-> next;
< a name = "l138" > < / a > }
< a name = "l139" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l140" > < / a > < / span > < span class = cF5 > MemCopy< / span > < span class = cF0 > (ode-> state_internal, ode-> state, ode-> n * < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF1 > F64< / span > < span class = cF7 > )< / span > < span class = cF0 > );
2021-07-03 05:07:57 +01:00
< a name = "l141" > < / a > }
< a name = "l142" > < / a >
< a name = "l143" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEInternal2State< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l144" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l145" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm;
2021-07-03 05:07:57 +01:00
< a name = "l146" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l147" > < / a > < / span > < span class = cF5 > MemCopy< / span > < span class = cF0 > (ode-> state, ode-> state_internal, ode-> n * < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF1 > F64< / span > < span class = cF7 > )< / span > < span class = cF0 > );
< a name = "l148" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> flags & < / span > < span class = cF3 > ODEF_HAS_MASSES< / span > < span class = cF0 > )
< a name = "l149" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l150" > < / a > < / span > < span class = cF5 > ODESetMassesPtrs< / span > < span class = cF0 > (ode, ode-> state_internal, ode-> state_internal);
< a name = "l151" > < / a > tmpm = ode-> next_mass;
< a name = "l152" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l153" > < / a > {
< a name = "l154" > < / a > < / span > < span class = cF5 > MemCopy< / span > < span class = cF0 > (& tmpm-> saved_state, tmpm-> state, < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF9 > COrder2D3< / span > < span class = cF7 > )< / span > < span class = cF0 > );
< a name = "l155" > < / a > tmpm = tmpm-> next;
< a name = "l156" > < / a > }
< a name = "l157" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l158" > < / a > }
< a name = "l159" > < / a >
< a name = "l160" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODERenum< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l161" > < / a > {< / span > < span class = cF2 > //Renumber masses and springs.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l162" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i;
< a name = "l163" > < / a > < / span > < span class = cF9 > CSpring< / span > < span class = cF0 > *tmps;
< a name = "l164" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm;
2021-07-03 05:07:57 +01:00
< a name = "l165" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l166" > < / a > i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l167" > < / a > tmpm = ode-> next_mass;
< a name = "l168" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l169" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l170" > < / a > tmpm-> num = i++;
< a name = "l171" > < / a > tmpm = tmpm-> next;
< a name = "l172" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l173" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l174" > < / a > i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l175" > < / a > tmps = ode-> next_spring;
< a name = "l176" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmps != & ode-> next_spring)
< a name = "l177" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l178" > < / a > tmps-> num = i++;
< a name = "l179" > < / a > tmps-> end1_num = tmps-> end1-> num;
< a name = "l180" > < / a > tmps-> end2_num = tmps-> end2-> num;
< a name = "l181" > < / a > tmps = tmps-> next;
< a name = "l182" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l183" > < / a > }
< a name = "l184" > < / a >
< a name = "l185" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *< / span > < span class = cF5 > MassFind< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF1 > F64< / span > < span class = cF0 > x, < / span > < span class = cF1 > F64< / span > < span class = cF0 > y, < / span > < span class = cF1 > F64< / span > < span class = cF0 > z=< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l186" > < / a > {< / span > < span class = cF2 > //Search for mass nearest to x,y,z.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l187" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm, *best_mass = < / span > < span class = cF3 > NULL< / span > < span class = cF0 > ;
< a name = "l188" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > dd, best_dd = < / span > < span class = cF3 > F64_MAX< / span > < span class = cF0 > ;
2021-07-03 05:07:57 +01:00
< a name = "l189" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l190" > < / a > tmpm = ode-> next_mass;
< a name = "l191" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l192" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l193" > < / a > dd = < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (tmpm-> x - x) + < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (tmpm-> y - y) + < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (tmpm-> z - z);
< a name = "l194" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (dd < best_dd)
< a name = "l195" > < / a > {
< a name = "l196" > < / a > best_dd = dd;
< a name = "l197" > < / a > best_mass = tmpm;
< a name = "l198" > < / a > }
< a name = "l199" > < / a > tmpm = tmpm-> next;
< a name = "l200" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l201" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l202" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > best_mass;
2021-07-03 05:07:57 +01:00
< a name = "l203" > < / a > }
< a name = "l204" > < / a >
< a name = "l205" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF9 > CSpring< / span > < span class = cF0 > *< / span > < span class = cF5 > SpringFind< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF1 > F64< / span > < span class = cF0 > x, < / span > < span class = cF1 > F64< / span > < span class = cF0 > y, < / span > < span class = cF1 > F64< / span > < span class = cF0 > z=< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l206" > < / a > {< / span > < span class = cF2 > //Find spring midpoint nearest x,y,z.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l207" > < / a > < / span > < span class = cF9 > CSpring< / span > < span class = cF0 > *tmps, *best_spring = < / span > < span class = cF3 > NULL< / span > < span class = cF0 > ;
< a name = "l208" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > dd, best_dd = < / span > < span class = cF3 > F64_MAX< / span > < span class = cF0 > ;
2021-07-03 05:07:57 +01:00
< a name = "l209" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l210" > < / a > tmps = ode-> next_spring;
< a name = "l211" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmps != & ode-> next_spring)
< a name = "l212" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l213" > < / a > dd = < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (< / span > < span class = cF7 > (< / span > < span class = cF0 > tmps-> end1-> x + tmps-> end2-> x< / span > < span class = cF7 > )< / span > < span class = cF0 > / < / span > < span class = cFE > 2< / span > < span class = cF0 > - x) +
< a name = "l214" > < / a > < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (< / span > < span class = cF7 > (< / span > < span class = cF0 > tmps-> end1-> y + tmps-> end2-> y< / span > < span class = cF7 > )< / span > < span class = cF0 > / < / span > < span class = cFE > 2< / span > < span class = cF0 > - y) +
< a name = "l215" > < / a > < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (< / span > < span class = cF7 > (< / span > < span class = cF0 > tmps-> end1-> z + tmps-> end2-> z< / span > < span class = cF7 > )< / span > < span class = cF0 > / < / span > < span class = cFE > 2< / span > < span class = cF0 > - z);
2021-07-03 05:07:57 +01:00
< a name = "l216" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l217" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (dd < best_dd)
< a name = "l218" > < / a > {
< a name = "l219" > < / a > best_dd = dd;
< a name = "l220" > < / a > best_spring = tmps;
< a name = "l221" > < / a > }
< a name = "l222" > < / a > tmps = tmps-> next;
< a name = "l223" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l224" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l225" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > best_spring;
2021-07-03 05:07:57 +01:00
< a name = "l226" > < / a > }
< a name = "l227" > < / a >
< a name = "l228" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > MassOrSpringFind< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF9 > CMass< / span > < span class = cF0 > **res_mass, < / span > < span class = cF9 > CSpring< / span > < span class = cF0 > **res_spring, < / span > < span class = cF1 > F64< / span > < span class = cF0 > x, < / span > < span class = cF1 > F64< / span > < span class = cF0 > y, < / span > < span class = cF1 > F64< / span > < span class = cF0 > z=< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l229" > < / a > {< / span > < span class = cF2 > //Find spring or mass nearest x,y,z.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l230" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm, *best_mass = < / span > < span class = cF3 > NULL< / span > < span class = cF0 > ;
< a name = "l231" > < / a > < / span > < span class = cF9 > CSpring< / span > < span class = cF0 > *tmps, *best_spring = < / span > < span class = cF3 > NULL< / span > < span class = cF0 > ;
< a name = "l232" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > dd, best_dd = < / span > < span class = cF3 > F64_MAX< / span > < span class = cF0 > ;
2021-07-03 05:07:57 +01:00
< a name = "l233" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l234" > < / a > tmpm = ode-> next_mass;
< a name = "l235" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l236" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l237" > < / a > dd = < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (tmpm-> x - x) + < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (tmpm-> y - y) + < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (tmpm-> z - z);
< a name = "l238" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (dd < best_dd)
< a name = "l239" > < / a > {
< a name = "l240" > < / a > best_dd = dd;
< a name = "l241" > < / a > best_mass = tmpm;
< a name = "l242" > < / a > }
< a name = "l243" > < / a > tmpm = tmpm-> next;
< a name = "l244" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l245" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l246" > < / a > tmps = ode-> next_spring;
< a name = "l247" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmps != & ode-> next_spring)
< a name = "l248" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l249" > < / a > dd = < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (< / span > < span class = cF7 > (< / span > < span class = cF0 > tmps-> end1-> x + tmps-> end2-> x< / span > < span class = cF7 > )< / span > < span class = cF0 > / < / span > < span class = cFE > 2< / span > < span class = cF0 > - x) +
< a name = "l250" > < / a > < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (< / span > < span class = cF7 > (< / span > < span class = cF0 > tmps-> end1-> y + tmps-> end2-> y< / span > < span class = cF7 > )< / span > < span class = cF0 > / < / span > < span class = cFE > 2< / span > < span class = cF0 > - y) +
< a name = "l251" > < / a > < / span > < span class = cF5 > Sqr< / span > < span class = cF0 > (< / span > < span class = cF7 > (< / span > < span class = cF0 > tmps-> end1-> z + tmps-> end2-> z< / span > < span class = cF7 > )< / span > < span class = cF0 > / < / span > < span class = cFE > 2< / span > < span class = cF0 > - z);
2021-07-03 05:07:57 +01:00
< a name = "l252" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l253" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (dd < best_dd)
< a name = "l254" > < / a > {
< a name = "l255" > < / a > best_dd = dd;
< a name = "l256" > < / a > best_spring = tmps;
< a name = "l257" > < / a > best_mass = < / span > < span class = cF3 > NULL< / span > < span class = cF0 > ;
< a name = "l258" > < / a > }
< a name = "l259" > < / a > tmps = tmps-> next;
< a name = "l260" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l261" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (res_mass)
< a name = "l262" > < / a > *res_mass = best_mass;
< a name = "l263" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (res_spring)
< a name = "l264" > < / a > *res_spring = best_spring;
2021-07-03 05:07:57 +01:00
< a name = "l265" > < / a > }
< a name = "l266" > < / a >
< a name = "l267" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *< / span > < span class = cF5 > MassFindNum< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF9 > I64< / span > < span class = cF0 > num)
< a name = "l268" > < / a > {< / span > < span class = cF2 > //Return mass number N.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l269" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm = ode-> next_mass;
2021-07-03 05:07:57 +01:00
< a name = "l270" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l271" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l272" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l273" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmpm-> num == num)
< a name = "l274" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > tmpm;
< a name = "l275" > < / a > tmpm = tmpm-> next;
< a name = "l276" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l277" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l278" > < / a > < / span > < span class = cF1 > return< / span > < span class = cF0 > < / span > < span class = cF3 > NULL< / span > < span class = cF0 > ;
2021-07-03 05:07:57 +01:00
< a name = "l279" > < / a > }
< a name = "l280" > < / a >
< a name = "l281" > < / a > < / span > < span class = cF1 > public< / span > < span class = cF0 > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEResetInactive< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l282" > < / a > {< / span > < span class = cF2 > //Set all masses and springs to ACTIVE for new trial.< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l283" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm;
< a name = "l284" > < / a > < / span > < span class = cF9 > CSpring< / span > < span class = cF0 > *tmps;
2021-07-03 05:07:57 +01:00
< a name = "l285" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l286" > < / a > tmpm = ode-> next_mass;
< a name = "l287" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l288" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l289" > < / a > tmpm-> flags & = ~< / span > < span class = cF3 > MSF_INACTIVE< / span > < span class = cF0 > ;
< a name = "l290" > < / a > tmpm = tmpm-> next;
< a name = "l291" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l292" > < / a > tmps = ode-> next_spring;
< a name = "l293" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmps != & ode-> next_spring)
< a name = "l294" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l295" > < / a > tmps-> flags & = ~< / span > < span class = cF3 > SSF_INACTIVE< / span > < span class = cF0 > ;
< a name = "l296" > < / a > tmps = tmps-> next;
< a name = "l297" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l298" > < / a > }
< a name = "l299" > < / a >
< a name = "l300" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODECalcSprings< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l301" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l302" > < / a > < / span > < span class = cF9 > CSpring< / span > < span class = cF0 > *tmps = ode-> next_spring;
< a name = "l303" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *e1, *e2;
< a name = "l304" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > d;
< a name = "l305" > < / a > < / span > < span class = cF9 > CD3< / span > < span class = cF0 > p;
2021-07-03 05:07:57 +01:00
< a name = "l306" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l307" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmps != & ode-> next_spring)
< a name = "l308" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l309" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmps-> flags & < / span > < span class = cF3 > SSF_INACTIVE< / span > < span class = cF0 > )
< a name = "l310" > < / a > {
< a name = "l311" > < / a > tmps-> displacement = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l312" > < / a > tmps-> f = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l313" > < / a > }
< a name = "l314" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l315" > < / a > {
< a name = "l316" > < / a > e1 = tmps-> end1;
< a name = "l317" > < / a > e2 = tmps-> end2;
< a name = "l318" > < / a > d = < / span > < span class = cF5 > D3Norm< / span > < span class = cF0 > (< / span > < span class = cF5 > D3Sub< / span > < span class = cF7 > (< / span > < span class = cF0 > & p, & e2-> state-> x, & e1-> state-> x< / span > < span class = cF7 > )< / span > < span class = cF0 > );
< a name = "l319" > < / a > tmps-> displacement = d - tmps-> rest_len;
< a name = "l320" > < / a > tmps-> f = tmps-> displacement * tmps-> const;
< a name = "l321" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmps-> f > < / span > < span class = cFE > 0< / span > < span class = cF0 > & & tmps-> flags & < / span > < span class = cF3 > SSF_NO_TENSION< / span > < span class = cF0 > )
< a name = "l322" > < / a > tmps-> f = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l323" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmps-> f < < / span > < span class = cFE > 0< / span > < span class = cF0 > & & tmps-> flags & < / span > < span class = cF3 > SSF_NO_COMPRESSION< / span > < span class = cF0 > )
< a name = "l324" > < / a > tmps-> f = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l325" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (d > < / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l326" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l327" > < / a > < / span > < span class = cF5 > D3MulEqu< / span > < span class = cF0 > (& p, tmps-> f / d);
< a name = "l328" > < / a > < / span > < span class = cF5 > D3AddEqu< / span > < span class = cF0 > (& e1-> DstateDt-> DxDt, & p);
< a name = "l329" > < / a > < / span > < span class = cF5 > D3SubEqu< / span > < span class = cF0 > (& e2-> DstateDt-> DxDt, & p);
< a name = "l330" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l331" > < / a > }
< a name = "l332" > < / a > tmps = tmps-> next;
< a name = "l333" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l334" > < / a > }
< a name = "l335" > < / a >
< a name = "l336" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODECalcDrag< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l337" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l338" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm;
< a name = "l339" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > d, dd;
< a name = "l340" > < / a > < / span > < span class = cF9 > CD3< / span > < span class = cF0 > p;
2021-07-03 05:07:57 +01:00
< a name = "l341" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l342" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> drag_v || ode-> drag_v2 || ode-> drag_v3)
< a name = "l343" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l344" > < / a > tmpm = ode-> next_mass;
< a name = "l345" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l346" > < / a > {
< a name = "l347" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!< / span > < span class = cF7 > (< / span > < span class = cF0 > tmpm-> flags & < / span > < span class = cF3 > MSF_INACTIVE< / span > < span class = cF7 > )< / span > < span class = cF0 > & & tmpm-> drag_profile_factor & & < / span > < span class = cF7 > (< / span > < span class = cF0 > dd = < / span > < span class = cF5 > D3NormSqr< / span > < span class = cF0 > (& tmpm-> state-> DxDt)< / span > < span class = cF7 > )< / span > < span class = cF0 > )
< a name = "l348" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l349" > < / a > d = ode-> drag_v;
< a name = "l350" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> drag_v2)
< a name = "l351" > < / a > d += ode-> drag_v2 * < / span > < span class = cF5 > Sqrt< / span > < span class = cF0 > (dd);
< a name = "l352" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> drag_v3)
< a name = "l353" > < / a > d += dd * ode-> drag_v3;
< a name = "l354" > < / a > < / span > < span class = cF5 > D3SubEqu< / span > < span class = cF0 > (& tmpm-> DstateDt-> DxDt, < / span > < span class = cF5 > D3Mul< / span > < span class = cF7 > (< / span > < span class = cF0 > & p, d * tmpm-> drag_profile_factor, & tmpm-> state-> DxDt< / span > < span class = cF7 > )< / span > < span class = cF0 > );
< a name = "l355" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l356" > < / a > tmpm = tmpm-> next;
< a name = "l357" > < / a > }
< a name = "l358" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l359" > < / a > }
< a name = "l360" > < / a >
< a name = "l361" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEApplyAccelerationLimit< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l362" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l363" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm;
< a name = "l364" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > d;
2021-07-03 05:07:57 +01:00
< a name = "l365" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l366" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> acceleration_limit)
< a name = "l367" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l368" > < / a > tmpm = ode-> next_mass;
< a name = "l369" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l370" > < / a > {
< a name = "l371" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!< / span > < span class = cF7 > (< / span > < span class = cF0 > tmpm-> flags & < / span > < span class = cF3 > MSF_INACTIVE< / span > < span class = cF7 > )< / span > < span class = cF0 > & & < / span > < span class = cF7 > (< / span > < span class = cF0 > d = < / span > < span class = cF5 > D3Norm< / span > < span class = cF0 > (& tmpm-> DstateDt-> DxDt)< / span > < span class = cF7 > )< / span > < span class = cF0 > > ode-> acceleration_limit)
< a name = "l372" > < / a > < / span > < span class = cF5 > D3MulEqu< / span > < span class = cF0 > (& tmpm-> DstateDt-> DxDt, ode-> acceleration_limit / d);
< a name = "l373" > < / a > tmpm = tmpm-> next;
< a name = "l374" > < / a > }
< a name = "l375" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l376" > < / a > }
< a name = "l377" > < / a >
< a name = "l378" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEMPTask< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l379" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l380" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (< / span > < span class = cF3 > TRUE< / span > < span class = cF0 > )
< a name = "l381" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l382" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (!< / span > < span class = cF5 > Bt< / span > < span class = cF7 > (< / span > < span class = cF0 > & ode-> mp_not_done_flags, < / span > < span class = cF5 > Gs< / span > < span class = cF0 > -> num< / span > < span class = cF7 > )< / span > < span class = cF0 > )
< a name = "l383" > < / a > < / span > < span class = cF5 > Yield< / span > < span class = cF0 > ;
< a name = "l384" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> mp_derive)
< a name = "l385" > < / a > (*ode-> mp_derive)(ode, ode-> mp_t, < / span > < span class = cF5 > Gs< / span > < span class = cF0 > -> num, ode-> mp_state, ode-> mp_DstateDt);
< a name = "l386" > < / a > < / span > < span class = cF5 > LBtr< / span > < span class = cF0 > (& ode-> mp_not_done_flags, < / span > < span class = cF5 > Gs< / span > < span class = cF0 > -> num);
< a name = "l387" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l388" > < / a > }
< a name = "l389" > < / a >
< a name = "l390" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEMPWake< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l391" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l392" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i;
2021-07-03 05:07:57 +01:00
< a name = "l393" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l394" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!ode-> slave_tasks)
< a name = "l395" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l396" > < / a > ode-> slave_tasks = < / span > < span class = cF5 > CAlloc< / span > < span class = cF0 > (< / span > < span class = cFB > mp_count< / span > < span class = cF0 > * < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF9 > CTask< / span > < span class = cF0 > *< / span > < span class = cF7 > )< / span > < span class = cF0 > );
< a name = "l397" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < < / span > < span class = cFB > mp_count< / span > < span class = cF0 > ; i++)
< a name = "l398" > < / a > ode-> slave_tasks[i] = < / span > < span class = cF5 > Spawn< / span > < span class = cF0 > (& < / span > < span class = cF5 > ODEMPTask< / span > < span class = cF0 > , ode, < / span > < span class = cF6 > " ODE Slave" < / span > < span class = cF0 > , i);
< a name = "l399" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l400" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < < / span > < span class = cFB > mp_count< / span > < span class = cF0 > ; i++)
< a name = "l401" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l402" > < / a > < / span > < span class = cF5 > Suspend< / span > < span class = cF0 > (ode-> slave_tasks[i], < / span > < span class = cF3 > FALSE< / span > < span class = cF0 > );
< a name = "l403" > < / a > < / span > < span class = cF5 > MPInt< / span > < span class = cF0 > (< / span > < span class = cF3 > I_WAKE< / span > < span class = cF0 > , i);
< a name = "l404" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l405" > < / a > }
< a name = "l406" > < / a >
< a name = "l407" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEMPSleep< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l408" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l409" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i;
2021-07-03 05:07:57 +01:00
< a name = "l410" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l411" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> slave_tasks)
< a name = "l412" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l413" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (ode-> mp_not_done_flags)
< a name = "l414" > < / a > < / span > < span class = cF5 > Yield< / span > < span class = cF0 > ;
< a name = "l415" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < < / span > < span class = cFB > mp_count< / span > < span class = cF0 > ; i++)
< a name = "l416" > < / a > < / span > < span class = cF5 > Suspend< / span > < span class = cF0 > (ode-> slave_tasks[i]);
< a name = "l417" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l418" > < / a > }
< a name = "l419" > < / a >
< a name = "l420" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODECallMPDerivative< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF1 > F64< / span > < span class = cF0 > t, < / span > < span class = cF1 > F64< / span > < span class = cF0 > *state, < / span > < span class = cF1 > F64< / span > < span class = cF0 > *DstateDt)
< a name = "l421" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l422" > < / a > ode-> mp_t = t;
< a name = "l423" > < / a > ode-> mp_state = state;
< a name = "l424" > < / a > ode-> mp_DstateDt = DstateDt;
< a name = "l425" > < / a > ode-> mp_not_done_flags = < / span > < span class = cFE > 1< / span > < span class = cF0 > < < < / span > < span class = cFB > mp_count< / span > < span class = cF0 > - < / span > < span class = cFE > 1< / span > < span class = cF0 > ;
< a name = "l426" > < / a > < / span > < span class = cF1 > do< / span > < span class = cF0 >
< a name = "l427" > < / a > < / span > < span class = cF5 > Yield< / span > < span class = cF0 > ;
< a name = "l428" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (ode-> mp_not_done_flags);
2021-07-03 05:07:57 +01:00
< a name = "l429" > < / a > }
< a name = "l430" > < / a >
< a name = "l431" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode, < / span > < span class = cF1 > F64< / span > < span class = cF0 > t, < / span > < span class = cF1 > F64< / span > < span class = cF0 > *state, < / span > < span class = cF1 > F64< / span > < span class = cF0 > *DstateDt)
< a name = "l432" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l433" > < / a > < / span > < span class = cF9 > CMass< / span > < span class = cF0 > *tmpm;
2021-07-03 05:07:57 +01:00
< a name = "l434" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l435" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> flags & < / span > < span class = cF3 > ODEF_HAS_MASSES< / span > < span class = cF0 > )
< a name = "l436" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l437" > < / a > < / span > < span class = cF5 > ODESetMassesPtrs< / span > < span class = cF0 > (ode, state, DstateDt);
< a name = "l438" > < / a > tmpm = ode-> next_mass;
< a name = "l439" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l440" > < / a > {
< a name = "l441" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!< / span > < span class = cF7 > (< / span > < span class = cF0 > tmpm-> flags & < / span > < span class = cF3 > MSF_INACTIVE< / span > < span class = cF7 > )< / span > < span class = cF0 > )
< a name = "l442" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l443" > < / a > < / span > < span class = cF5 > D3Zero< / span > < span class = cF0 > (& tmpm-> DstateDt-> DxDt);
< a name = "l444" > < / a > < / span > < span class = cF5 > D3Copy< / span > < span class = cF0 > (& tmpm-> DstateDt-> x, & tmpm-> state-> DxDt);
< a name = "l445" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l446" > < / a > tmpm = tmpm-> next;
< a name = "l447" > < / a > }
< a name = "l448" > < / a > < / span > < span class = cF5 > ODECalcSprings< / span > < span class = cF0 > (ode);
< a name = "l449" > < / a > < / span > < span class = cF5 > ODECalcDrag< / span > < span class = cF0 > (ode);
< a name = "l450" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> mp_derive)
< a name = "l451" > < / a > < / span > < span class = cF5 > ODECallMPDerivative< / span > < span class = cF0 > (ode, t, state, DstateDt);
< a name = "l452" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> derive)
< a name = "l453" > < / a > (*ode-> derive)(ode, t, state, DstateDt);
< a name = "l454" > < / a > tmpm = ode-> next_mass;
< a name = "l455" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (tmpm != & ode-> next_mass)
< a name = "l456" > < / a > {
< a name = "l457" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!< / span > < span class = cF7 > (< / span > < span class = cF0 > tmpm-> flags & < / span > < span class = cF3 > MSF_INACTIVE< / span > < span class = cF7 > )< / span > < span class = cF0 > )
< a name = "l458" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l459" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmpm-> flags & < / span > < span class = cF3 > MSF_FIXED< / span > < span class = cF0 > )
< a name = "l460" > < / a > {
< a name = "l461" > < / a > < / span > < span class = cF5 > D3Zero< / span > < span class = cF0 > (& tmpm-> DstateDt-> DxDt);
< a name = "l462" > < / a > < / span > < span class = cF5 > D3Zero< / span > < span class = cF0 > (& tmpm-> DstateDt-> x);
< a name = "l463" > < / a > }
< a name = "l464" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmpm-> mass)
< a name = "l465" > < / a > < / span > < span class = cF5 > D3DivEqu< / span > < span class = cF0 > (& tmpm-> DstateDt-> DxDt, tmpm-> mass);
< a name = "l466" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l467" > < / a > tmpm = tmpm-> next;
< a name = "l468" > < / a > }
< a name = "l469" > < / a > < / span > < span class = cF5 > ODEApplyAccelerationLimit< / span > < span class = cF0 > (ode);
< a name = "l470" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l471" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l472" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l473" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> mp_derive)
< a name = "l474" > < / a > < / span > < span class = cF5 > ODECallMPDerivative< / span > < span class = cF0 > (ode, t, state, DstateDt);
< a name = "l475" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> derive)
< a name = "l476" > < / a > (*ode-> derive)(ode, t, state, DstateDt);
< a name = "l477" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l478" > < / a > }
< a name = "l479" > < / a >
< a name = "l480" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEOneStep< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l481" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l482" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i;
2021-07-03 05:07:57 +01:00
< a name = "l483" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l484" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t, ode-> state_internal, ode-> DstateDt);
< a name = "l485" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < ode-> n_internal; i++)
< a name = "l486" > < / a > ode-> state_internal[i] += ode-> h * ode-> DstateDt[i];
< a name = "l487" > < / a > ode-> t += ode-> h;
2021-07-03 05:07:57 +01:00
< a name = "l488" > < / a > }
< a name = "l489" > < / a >
< a name = "l490" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODERK4OneStep< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l491" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l492" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i, n = ode-> n_internal;
< a name = "l493" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > xh, hh, h6, *dym, *dyt, *yt, *DstateDt;
2021-07-03 05:07:57 +01:00
< a name = "l494" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l495" > < / a > dym = ode-> tmp0;
< a name = "l496" > < / a > dyt = ode-> tmp1;
< a name = "l497" > < / a > yt = ode-> tmp2;
< a name = "l498" > < / a > DstateDt = ode-> tmp3;
< a name = "l499" > < / a > hh = < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 5< / span > < span class = cF0 > * ode-> h;
< a name = "l500" > < / a > h6 = ode-> h / < / span > < span class = cFE > 6< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l501" > < / a > xh = ode-> t + hh;
2021-07-03 05:07:57 +01:00
< a name = "l502" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l503" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t, ode-> state_internal, ode-> DstateDt);
< a name = "l504" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l505" > < / a > yt[i] = ode-> state_internal[i] + hh * DstateDt[i];
< a name = "l506" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, xh, yt, dyt);
< a name = "l507" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l508" > < / a > yt[i] = ode-> state_internal[i] + hh * dyt[i];
< a name = "l509" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, xh, yt, dym);
< a name = "l510" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l511" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l512" > < / a > yt[i] = ode-> state_internal[i] + ode-> h * dym[i];
< a name = "l513" > < / a > dym[i] += dyt[i];
< a name = "l514" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l515" > < / a > ode-> t += ode-> h;
< a name = "l516" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t, yt, dyt);
< a name = "l517" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l518" > < / a > ode-> state_internal[i] += h6 * (DstateDt[i] + dyt[i] + < / span > < span class = cFE > 2< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > * dym[i]);
2021-07-03 05:07:57 +01:00
< a name = "l519" > < / a > }
< a name = "l520" > < / a >
< a name = "l521" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEa2< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 2< / span > < span class = cF0 >
< a name = "l522" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEa3< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 3< / span > < span class = cF0 >
< a name = "l523" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEa4< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 6< / span > < span class = cF0 >
< a name = "l524" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEa5< / span > < span class = cF0 > < / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 >
< a name = "l525" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEa6< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 875< / span > < span class = cF0 >
< a name = "l526" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb21< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 2< / span > < span class = cF0 >
< a name = "l527" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb31< / span > < span class = cF0 > (< / span > < span class = cFE > 3< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 40< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l528" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb32< / span > < span class = cF0 > (< / span > < span class = cFE > 9< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 40< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l529" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb41< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 3< / span > < span class = cF0 >
< a name = "l530" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb42< / span > < span class = cF0 > (-< / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 9< / span > < span class = cF0 > )
< a name = "l531" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb43< / span > < span class = cF0 > < / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 2< / span > < span class = cF0 >
< a name = "l532" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb51< / span > < span class = cF0 > (-< / span > < span class = cFE > 11< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 54< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l533" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb52< / span > < span class = cF0 > < / span > < span class = cFE > 2< / span > < span class = cF0 > .< / span > < span class = cFE > 5< / span > < span class = cF0 >
< a name = "l534" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb53< / span > < span class = cF0 > (-< / span > < span class = cFE > 70< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 27< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l535" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb54< / span > < span class = cF0 > (< / span > < span class = cFE > 35< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 27< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l536" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb61< / span > < span class = cF0 > (< / span > < span class = cFE > 1631< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 55296< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l537" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb62< / span > < span class = cF0 > (< / span > < span class = cFE > 175< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 512< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l538" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb63< / span > < span class = cF0 > (< / span > < span class = cFE > 575< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 13824< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l539" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb64< / span > < span class = cF0 > (< / span > < span class = cFE > 44275< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 110592< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l540" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEb65< / span > < span class = cF0 > (< / span > < span class = cFE > 253< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 4096< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l541" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEc1< / span > < span class = cF0 > (< / span > < span class = cFE > 37< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 378< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l542" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEc3< / span > < span class = cF0 > (< / span > < span class = cFE > 250< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 621< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l543" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEc4< / span > < span class = cF0 > (< / span > < span class = cFE > 125< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 594< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l544" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEc6< / span > < span class = cF0 > (< / span > < span class = cFE > 512< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 1771< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l545" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEdc1< / span > < span class = cF0 > (< / span > < span class = cFE > 37< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 378< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > - < / span > < span class = cFE > 2825< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 27648< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l546" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEdc3< / span > < span class = cF0 > (< / span > < span class = cFE > 250< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 621< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > - < / span > < span class = cFE > 18575< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 48384< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l547" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEdc4< / span > < span class = cF0 > (< / span > < span class = cFE > 125< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 594< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > - < / span > < span class = cFE > 13525< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 55296< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l548" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEdc5< / span > < span class = cF0 > (-< / span > < span class = cFE > 277< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 14336< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l549" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ODEdc6< / span > < span class = cF0 > (< / span > < span class = cFE > 512< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFE > 1771< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > - < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 25< / span > < span class = cF0 > )
< a name = "l550" > < / a >
< a name = "l551" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODECashKarp< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l552" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l553" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i, n = ode-> n_internal;
< a name = "l554" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > h = ode-> h, *state = ode-> state_internal, *DstateDt = ode-> DstateDt, *ak2, *ak3, *ak4, *ak5, *ak6, *tmpstate, *stateerr, *outstate;
2021-07-03 05:07:57 +01:00
< a name = "l555" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l556" > < / a > ak2 = ode-> tmp0;
< a name = "l557" > < / a > ak3 = ode-> tmp1;
< a name = "l558" > < / a > ak4 = ode-> tmp2;
< a name = "l559" > < / a > ak5 = ode-> tmp3;
< a name = "l560" > < / a > ak6 = ode-> tmp4;
< a name = "l561" > < / a > tmpstate = ode-> tmp5;
< a name = "l562" > < / a > outstate = ode-> tmp6;
< a name = "l563" > < / a > stateerr = ode-> tmp7;
2021-07-03 05:07:57 +01:00
< a name = "l564" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l565" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l566" > < / a > tmpstate[i] = state[i] + < / span > < span class = cF3 > ODEb21< / span > < span class = cF0 > * h * DstateDt[i];
< a name = "l567" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t + < / span > < span class = cF3 > ODEa2< / span > < span class = cF0 > * h, tmpstate, ak2);
< a name = "l568" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l569" > < / a > tmpstate[i] =state[i] + h * (< / span > < span class = cF3 > ODEb31< / span > < span class = cF0 > * DstateDt[i] + < / span > < span class = cF3 > ODEb32< / span > < span class = cF0 > * ak2[i]);
< a name = "l570" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t + < / span > < span class = cF3 > ODEa3< / span > < span class = cF0 > * h, tmpstate, ak3);
< a name = "l571" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l572" > < / a > tmpstate[i] = state[i] + h * (< / span > < span class = cF3 > ODEb41< / span > < span class = cF0 > * DstateDt[i] + < / span > < span class = cF3 > ODEb42< / span > < span class = cF0 > * ak2[i] + < / span > < span class = cF3 > ODEb43< / span > < span class = cF0 > * ak3[i]);
< a name = "l573" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t + < / span > < span class = cF3 > ODEa4< / span > < span class = cF0 > * h, tmpstate, ak4);
< a name = "l574" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l575" > < / a > tmpstate[i] = state[i] + h * (< / span > < span class = cF3 > ODEb51< / span > < span class = cF0 > * DstateDt[i] + < / span > < span class = cF3 > ODEb52< / span > < span class = cF0 > * ak2[i] + < / span > < span class = cF3 > ODEb53< / span > < span class = cF0 > * ak3[i] + < / span > < span class = cF3 > ODEb54< / span > < span class = cF0 > *ak4[i]);
< a name = "l576" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t + < / span > < span class = cF3 > ODEa5< / span > < span class = cF0 > * h, tmpstate,ak5);
< a name = "l577" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l578" > < / a > tmpstate[i] = state[i] + h * (< / span > < span class = cF3 > ODEb61< / span > < span class = cF0 > * DstateDt[i]+ < / span > < span class = cF3 > ODEb62< / span > < span class = cF0 > * ak2[i] + < / span > < span class = cF3 > ODEb63< / span > < span class = cF0 > * ak3[i] + < / span > < span class = cF3 > ODEb64< / span > < span class = cF0 > * ak4[i] + < / span > < span class = cF3 > ODEb65< / span > < span class = cF0 > * ak5[i]);
< a name = "l579" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t + < / span > < span class = cF3 > ODEa6< / span > < span class = cF0 > * h, tmpstate, ak6);
2021-07-03 05:07:57 +01:00
< a name = "l580" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l581" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l582" > < / a > outstate[i] = state[i] + h * (< / span > < span class = cF3 > ODEc1< / span > < span class = cF0 > * DstateDt[i] + < / span > < span class = cF3 > ODEc3< / span > < span class = cF0 > * ak3[i] + < / span > < span class = cF3 > ODEc4< / span > < span class = cF0 > * ak4[i] + < / span > < span class = cF3 > ODEc6< / span > < span class = cF0 > * ak6[i]);
< a name = "l583" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < n; i++)
< a name = "l584" > < / a > stateerr[i] = h * (< / span > < span class = cF3 > ODEdc1< / span > < span class = cF0 > * DstateDt[i] + < / span > < span class = cF3 > ODEdc3< / span > < span class = cF0 > * ak3[i] + < / span > < span class = cF3 > ODEdc4< / span > < span class = cF0 > * ak4[i] + < / span > < span class = cF3 > ODEdc5< / span > < span class = cF0 > * ak5[i] + < / span > < span class = cF3 > ODEdc6< / span > < span class = cF0 > * ak6[i]);
2021-07-03 05:07:57 +01:00
< a name = "l585" > < / a > }
< a name = "l586" > < / a >
< a name = "l587" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > SAFETY< / span > < span class = cF0 > < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 9< / span > < span class = cF0 >
< a name = "l588" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > PGROW< / span > < span class = cF0 > (-< / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 2< / span > < span class = cF0 > )
< a name = "l589" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > PSHRNK< / span > < span class = cF0 > (-< / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 25< / span > < span class = cF0 > )
< a name = "l590" > < / a > #< / span > < span class = cF1 > define< / span > < span class = cF0 > < / span > < span class = cF3 > ERRCON< / span > < span class = cF0 > < / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 89< / span > < span class = cF0 > e-< / span > < span class = cFE > 4< / span > < span class = cF0 >
< a name = "l591" > < / a >
< a name = "l592" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODERK5OneStep< / span > < span class = cF0 > (< / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode)
< a name = "l593" > < / a > {
2021-07-04 23:11:34 +01:00
< a name = "l594" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i;
< a name = "l595" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > errmax, tmp, *tmpstate = ode-> tmp6, *stateerr = ode-> tmp7;
2021-07-03 05:07:57 +01:00
< a name = "l596" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l597" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (< / span > < span class = cF3 > TRUE< / span > < span class = cF0 > )
< a name = "l598" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l599" > < / a > ode-> h = < / span > < span class = cF5 > Clamp< / span > < span class = cF0 > (ode-> h, ode-> h_min, ode-> h_max);
< a name = "l600" > < / a > < / span > < span class = cF5 > ODECashKarp< / span > < span class = cF0 > (ode);
< a name = "l601" > < / a > errmax = < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l602" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < ode-> n_internal; i++)
< a name = "l603" > < / a > {
< a name = "l604" > < / a > tmp = < / span > < span class = cF5 > Abs< / span > < span class = cF0 > (stateerr[i] / ode-> state_scale[i]);
< a name = "l605" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmp > errmax)
< a name = "l606" > < / a > errmax = tmp;
< a name = "l607" > < / a > }
< a name = "l608" > < / a > errmax /= ode-> tolerance_internal;
< a name = "l609" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (errmax < = < / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > || ode-> h == ode-> h_min)
< a name = "l610" > < / a > < / span > < span class = cF1 > break< / span > < span class = cF0 > ;
< a name = "l611" > < / a > tmp = ode-> h * < / span > < span class = cF3 > SAFETY< / span > < span class = cF0 > * errmax ` < / span > < span class = cF3 > PSHRNK< / span > < span class = cF0 > ;
< a name = "l612" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (tmp < < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 1< / span > < span class = cF0 > * ode-> h)
< a name = "l613" > < / a > ode-> h *= < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 1< / span > < span class = cF0 > ;
< a name = "l614" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l615" > < / a > ode-> h = tmp;
< a name = "l616" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l617" > < / a > ode-> t += ode-> h;
< a name = "l618" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (errmax > < / span > < span class = cF3 > ERRCON< / span > < span class = cF0 > )
< a name = "l619" > < / a > ode-> h *= < / span > < span class = cF3 > SAFETY< / span > < span class = cF0 > * errmax ` < / span > < span class = cF3 > PGROW< / span > < span class = cF0 > ;
< a name = "l620" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l621" > < / a > ode-> h *= < / span > < span class = cFE > 5< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l622" > < / a > ode-> h = < / span > < span class = cF5 > Clamp< / span > < span class = cF0 > (ode-> h, ode-> h_min, ode-> h_max);
< a name = "l623" > < / a > < / span > < span class = cF5 > MemCopy< / span > < span class = cF0 > (ode-> state_internal, tmpstate, < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF1 > F64< / span > < span class = cF7 > )< / span > < span class = cF0 > * ode-> n_internal);
2021-07-03 05:07:57 +01:00
< a name = "l624" > < / a > }
< a name = "l625" > < / a >
< a name = "l626" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > < / span > < span class = cFB > ode_alloced_factor< / span > < span class = cF0 > = < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 75< / span > < span class = cF0 > ;
< a name = "l627" > < / a >
< a name = "l628" > < / a > < / span > < span class = cF1 > U0< / span > < span class = cF0 > < / span > < span class = cF5 > ODEsUpdate< / span > < span class = cF0 > (< / span > < span class = cF9 > CTask< / span > < span class = cF0 > *task)
2021-12-11 11:10:58 +00:00
< a name = "l629" > < / a > {< / span > < span class = cF2 > /* This routine is called by the < / span > < a href = "https://zeal-operating-system.github.io/ZealOS/System/Gr/GrScreen.ZC.html#l7" > < span class = cF4 > window mgr< / span > < / a > < span class = cF2 > on a continuous< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l630" > < / a > < / span > < span class = cF2 > basis to allow real-time simulation. It is intended< / span > < span class = cF0 >
< a name = "l631" > < / a > < / span > < span class = cF2 > to provide ress good enough for games. It uses a runge-kutta< / span > < span class = cF0 >
< a name = "l632" > < / a > < / span > < span class = cF2 > integrator which is a better algorithm than doing it with Euler.< / span > < span class = cF0 >
< a name = "l633" > < / a >
< a name = "l634" > < / a > < / span > < span class = cF2 > It is adaptive step-sized, so it slows down when an important< / span > < span class = cF0 >
< a name = "l635" > < / a > < / span > < span class = cF2 > event is taking place to improve accuracy, but in this implementation< / span > < span class = cF0 >
< a name = "l636" > < / a > < / span > < span class = cF2 > it has a timeout.< / span > < span class = cF0 >
< a name = "l637" > < / a > < / span > < span class = cF2 > */< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l638" > < / a > < / span > < span class = cF9 > I64< / span > < span class = cF0 > i;
< a name = "l639" > < / a > < / span > < span class = cF1 > F64< / span > < span class = cF0 > d, start_time, timeout_time, t_desired, t_initial, interpolation;
< a name = "l640" > < / a > < / span > < span class = cF9 > CMathODE< / span > < span class = cF0 > *ode;
2021-07-03 05:07:57 +01:00
< a name = "l641" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l642" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (task-> next_ode == & task-> next_ode)
< a name = "l643" > < / a > task-> last_ode_time = < / span > < span class = cFE > 0< / span > < span class = cF0 > ;
< a name = "l644" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!< / span > < span class = cF5 > Bt< / span > < span class = cF7 > (< / span > < span class = cF0 > & task-> win_inhibit, < / span > < span class = cF3 > WIf_SELF_ODE< / span > < span class = cF7 > )< / span > < span class = cF0 > )
2021-12-11 11:10:58 +00:00
< a name = "l645" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF2 > //See < / span > < a href = "https://zeal-operating-system.github.io/ZealOS/System/Gr/GrScreen.ZC.html#l69" > < span class = cF4 > GrUpdateTasks< / span > < / a > < span class = cF2 > () and < / span > < a href = "https://zeal-operating-system.github.io/ZealOS/System/Gr/GrScreen.ZC.html#l3" > < span class = cF4 > GrUpdateTaskODEs< / span > < / a > < span class = cF2 > ().< / span > < span class = cF0 >
2021-07-04 23:11:34 +01:00
< a name = "l646" > < / a > < / span > < span class = cF2 > //We will not pick a time limit based on< / span > < span class = cF0 >
< a name = "l647" > < / a > < / span > < span class = cF2 > //how busy the CPU is, what percent of the< / span > < span class = cF0 >
< a name = "l648" > < / a > < / span > < span class = cF2 > //last refresh cycle was spent on ODE's< / span > < span class = cF0 >
< a name = "l649" > < / a > < / span > < span class = cF2 > //and what the refresh cycle rate was.< / span > < span class = cF0 >
< a name = "l650" > < / a > start_time = < / span > < span class = cF5 > tS< / span > < span class = cF0 > ;
< a name = "l651" > < / a > d = < / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > / < / span > < span class = cFB > winmgr< / span > < span class = cF0 > .fps;
< a name = "l652" > < / a > timeout_time = start_time + (task-> last_ode_time / d + < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 1< / span > < span class = cF0 > ) / (< / span > < span class = cFB > winmgr< / span > < span class = cF0 > .last_ode_time / d + < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 1< / span > < span class = cF0 > ) * < / span > < span class = cFB > ode_alloced_factor< / span > < span class = cF0 > * d;
< a name = "l653" > < / a > ode = task-> next_ode;
< a name = "l654" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (ode != & task-> next_ode)
< a name = "l655" > < / a > {
< a name = "l656" > < / a > t_initial = ode-> t;
< a name = "l657" > < / a > d = < / span > < span class = cF5 > tS< / span > < span class = cF0 > ;
< a name = "l658" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!< / span > < span class = cF7 > (< / span > < span class = cF0 > ode-> flags & < / span > < span class = cF3 > ODEF_STARTED< / span > < span class = cF7 > )< / span > < span class = cF0 > )
< a name = "l659" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l660" > < / a > ode-> base_t = d;
< a name = "l661" > < / a > ode-> flags |= < / span > < span class = cF3 > ODEF_STARTED< / span > < span class = cF0 > ;
< a name = "l662" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l663" > < / a > d -= ode-> base_t + t_initial;
< a name = "l664" > < / a > t_desired = ode-> t_scale * d + t_initial;
< a name = "l665" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> flags & < / span > < span class = cF3 > ODEF_PAUSED< / span > < span class = cF0 > )
< a name = "l666" > < / a > ode-> base_t += t_desired - ode-> t; < / span > < span class = cF2 > //Slip< / span > < span class = cF0 >
< a name = "l667" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l668" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l669" > < / a > ode-> flags |= < / span > < span class = cF3 > ODEF_BUSY< / span > < span class = cF0 > ;
< a name = "l670" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> flags & < / span > < span class = cF3 > ODEF_PAUSED< / span > < span class = cF0 > )
< a name = "l671" > < / a > ode-> base_t += t_desired-ode-> t; < / span > < span class = cF2 > //Slip< / span > < span class = cF0 >
< a name = "l672" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l673" > < / a > {
< a name = "l674" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> derive || ode-> mp_derive)
< a name = "l675" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l676" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> mp_derive)
< a name = "l677" > < / a > < / span > < span class = cF5 > ODEMPWake< / span > < span class = cF0 > (ode);
< a name = "l678" > < / a > < / span > < span class = cF5 > ODEState2Internal< / span > < span class = cF0 > (ode);
< a name = "l679" > < / a > < / span > < span class = cF5 > MemCopy< / span > < span class = cF0 > (ode-> initial_state, ode-> state_internal, ode-> n_internal * < / span > < span class = cF1 > sizeof< / span > < span class = cF7 > (< / span > < span class = cF1 > F64< / span > < span class = cF7 > )< / span > < span class = cF0 > );
< a name = "l680" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (ode-> t < t_desired)
< a name = "l681" > < / a > {
< a name = "l682" > < / a > ode-> h_max = t_desired - ode-> t;
< a name = "l683" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t, ode-> state_internal, ode-> DstateDt);
< a name = "l684" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < ode-> n_internal; i++)
< a name = "l685" > < / a > ode-> state_scale[i] = < / span > < span class = cF5 > Abs< / span > < span class = cF0 > (ode-> state_internal[i]) +
< a name = "l686" > < / a > < / span > < span class = cF5 > Abs< / span > < span class = cF0 > (ode-> DstateDt[i] * ode-> h) +
< a name = "l687" > < / a > ode-> tolerance_internal;
< a name = "l688" > < / a > < / span > < span class = cF5 > ODERK5OneStep< / span > < span class = cF0 > (ode);
< a name = "l689" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (< / span > < span class = cF5 > tS< / span > < span class = cF0 > > timeout_time)
< a name = "l690" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l691" > < / a > ode-> base_t += t_desired - ode-> t; < / span > < span class = cF2 > //Slip< / span > < span class = cF0 >
< a name = "l692" > < / a > < / span > < span class = cF1 > goto< / span > < span class = cF0 > ode_done;
2021-07-03 05:07:57 +01:00
< a name = "l693" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l694" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l695" > < / a > }
2021-07-03 05:07:57 +01:00
< a name = "l696" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l697" > < / a > < / span > < span class = cF2 > //Interpolate if end time was not exact.< / span > < span class = cF0 >
< a name = "l698" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> t != t_desired)
< a name = "l699" > < / a > {
< a name = "l700" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (interpolation = ode-> t - t_initial)
< a name = "l701" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l702" > < / a > interpolation = (t_desired - t_initial) / interpolation;
< a name = "l703" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (interpolation != < / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l704" > < / a > < / span > < span class = cF1 > for< / span > < span class = cF0 > (i = < / span > < span class = cFE > 0< / span > < span class = cF0 > ; i < ode-> n_internal; i++)
< a name = "l705" > < / a > ode-> state_internal[i] = (ode-> state_internal[i] -
< a name = "l706" > < / a > ode-> initial_state[i]) * interpolation +
< a name = "l707" > < / a > ode-> initial_state[i];
< a name = "l708" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l709" > < / a > ode-> t = t_desired;
< a name = "l710" > < / a > }
2021-07-03 05:07:57 +01:00
< a name = "l711" > < / a > ode_done:
2021-07-04 23:11:34 +01:00
< a name = "l712" > < / a > < / span > < span class = cF5 > ODEInternal2State< / span > < span class = cF0 > (ode);
2021-07-03 05:07:57 +01:00
< a name = "l713" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l714" > < / a > < / span > < span class = cF2 > //Convenience call to set vals< / span > < span class = cF0 >
< a name = "l715" > < / a > < / span > < span class = cF5 > ODECallDerivative< / span > < span class = cF0 > (ode, ode-> t, ode-> state_internal, ode-> DstateDt);
2021-07-03 05:07:57 +01:00
< a name = "l716" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l717" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> mp_derive)
< a name = "l718" > < / a > < / span > < span class = cF5 > ODEMPSleep< / span > < span class = cF0 > (ode);
< a name = "l719" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l720" > < / a > }
< a name = "l721" > < / a > ode-> flags & = ~< / span > < span class = cF3 > ODEF_BUSY< / span > < span class = cF0 > ;
< a name = "l722" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l723" > < / a > ode-> base_t += (< / span > < span class = cFE > 1< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > - ode-> t_scale) * d;
< a name = "l724" > < / a > ode = ode-> next;
< a name = "l725" > < / a > }
2021-07-03 05:07:57 +01:00
< a name = "l726" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l727" > < / a > < / span > < span class = cF2 > //Now, we will dynamically adjust tolerances.< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l728" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l729" > < / a > < / span > < span class = cF2 > //We will regulate the tolerances< / span > < span class = cF0 >
< a name = "l730" > < / a > < / span > < span class = cF2 > //to fill the time we decided was< / span > < span class = cF0 >
< a name = "l731" > < / a > < / span > < span class = cF2 > //okay to devote to ODE's.< / span > < span class = cF0 >
< a name = "l732" > < / a > < / span > < span class = cF2 > //Since we might have multiple ODE's< / span > < span class = cF0 >
< a name = "l733" > < / a > < / span > < span class = cF2 > //active we scale them by the same factor.< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l734" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l735" > < / a > < / span > < span class = cF2 > //This algorithm is probably not stable or very good, but it's something.< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l736" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l737" > < / a > < / span > < span class = cF2 > //Target is 75% of alloced time.< / span > < span class = cF0 >
< a name = "l738" > < / a > d = (< / span > < span class = cF5 > tS< / span > < span class = cF0 > - start_time) / (timeout_time - start_time) - < / span > < span class = cFE > 0< / span > < span class = cF0 > .< / span > < span class = cFE > 75< / span > < span class = cF0 > ;
2021-07-03 05:07:57 +01:00
< a name = "l739" > < / a >
2021-07-04 23:11:34 +01:00
< a name = "l740" > < / a > ode = task-> next_ode;
< a name = "l741" > < / a > < / span > < span class = cF1 > while< / span > < span class = cF0 > (ode != & task-> next_ode)
< a name = "l742" > < / a > {
< a name = "l743" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (!< / span > < span class = cF7 > (< / span > < span class = cF0 > ode-> flags & < / span > < span class = cF3 > ODEF_PAUSED< / span > < span class = cF7 > )< / span > < span class = cF0 > & & ode-> derive)
< a name = "l744" > < / a > < / span > < span class = cF7 > {< / span > < span class = cF0 >
< a name = "l745" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (ode-> min_tolerance != ode-> max_tolerance)
< a name = "l746" > < / a > {
< a name = "l747" > < / a > < / span > < span class = cF1 > if< / span > < span class = cF0 > (d > < / span > < span class = cFE > 0< / span > < span class = cF0 > )
< a name = "l748" > < / a > ode-> tolerance_internal *= < / span > < span class = cFE > 10< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > ` d;
< a name = "l749" > < / a > < / span > < span class = cF1 > else< / span > < span class = cF0 >
< a name = "l750" > < / a > ode-> tolerance_internal *= < / span > < span class = cFE > 2< / span > < span class = cF0 > .< / span > < span class = cFE > 0< / span > < span class = cF0 > ` d;
< a name = "l751" > < / a > }
< a name = "l752" > < / a > ode-> tolerance_internal = < / span > < span class = cF5 > Clamp< / span > < span class = cF0 > (ode-> tolerance_internal, ode-> min_tolerance, ode-> max_tolerance);
< a name = "l753" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
< a name = "l754" > < / a > ode = ode-> next;
< a name = "l755" > < / a > }
< a name = "l756" > < / a > < / span > < span class = cFB > winmgr< / span > < span class = cF0 > .ode_time += task-> last_ode_time = < / span > < span class = cF5 > tS< / span > < span class = cF0 > - start_time;
< a name = "l757" > < / a > < / span > < span class = cF7 > }< / span > < span class = cF0 >
2021-07-03 05:07:57 +01:00
< a name = "l758" > < / a > }
< / span > < / pre > < / body >
< / html >
