tristan/ZealOS
1
0
Fork 0
mirror of https://github.com/Zeal-Operating-System/ZealOS.git synced 2024-12-29 00:36:32 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions
ZealOS/docs/System/MathODE.CC.html
TomAwezome 1b75d91002 Fix Mount AHCI Port selection. 
Add arg to SATARep to specify drive types to show.
Add checks in AHCIPortInit to verify port signatures, add helper method to get signatures from port.
2021-08-02 16:40:05 -04:00

787 lines
82 KiB
HTML
Executable file
Raw Blame History

<!DOCTYPE HTML>
<html>
<head>
<meta http-equiv="Content-Type" content="text/html;charset=US-ASCII">
<meta name="generator" content="ZealOS V0.13">
<style type="text/css">
body {background-color:#fef1f0;}
.cF0{color:#000000;background-color:#fef1f0;}
.cF1{color:#0148a4;background-color:#fef1f0;}
.cF2{color:#3b7901;background-color:#fef1f0;}
.cF3{color:#057c7e;background-color:#fef1f0;}
.cF4{color:#bb2020;background-color:#fef1f0;}
.cF5{color:#9e42ae;background-color:#fef1f0;}
.cF6{color:#b57901;background-color:#fef1f0;}
.cF7{color:#b2b6af;background-color:#fef1f0;}
.cF8{color:#555753;background-color:#fef1f0;}
.cF9{color:#678fbb;background-color:#fef1f0;}
.cFA{color:#82bc49;background-color:#fef1f0;}
.cFB{color:#0097a2;background-color:#fef1f0;}
.cFC{color:#e26a6a;background-color:#fef1f0;}
.cFD{color:#c671bc;background-color:#fef1f0;}
.cFE{color:#c7ab00;background-color:#fef1f0;}
.cFF{color:#fef1f0;background-color:#fef1f0;}
</style>
</head>
<body>
<pre style="font-family:monospace;font-size:12pt">
<a name="l1"></a><span class=cF0>#</span><span class=cF1>help_index</span><span class=cF0> </span><span class=cF6>&quot;Math/ODE&quot;</span><span class=cF0>
<a name="l2"></a>#</span><span class=cF1>help_file</span><span class=cF0> </span><span class=cF6>&quot;::/Doc/ODE&quot;</span><span class=cF0>
<a name="l3"></a>
<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>
<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>
<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);
<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>{
<a name="l14"></a> </span><span class=cF9>I64</span><span class=cF0> s = ode-&gt;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-&gt;array_base;
<a name="l16"></a>
<a name="l17"></a> ode-&gt;state_internal = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l18"></a> ode-&gt;state_scale = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l19"></a> ode-&gt;DstateDt = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l20"></a> ode-&gt;initial_state = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l21"></a> ode-&gt;tmp0 = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l22"></a> ode-&gt;tmp1 = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l23"></a> ode-&gt;tmp2 = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l24"></a> ode-&gt;tmp3 = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l25"></a> ode-&gt;tmp4 = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l26"></a> ode-&gt;tmp5 = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l27"></a> ode-&gt;tmp6 = ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>) += s;
<a name="l28"></a> ode-&gt;tmp7 = ptr;
<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>)
<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>
<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>);
<a name="l39"></a>
<a name="l40"></a> ode-&gt;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-&gt;flags = flags;
<a name="l42"></a> ode-&gt;n_internal = ode-&gt;n = n;
<a name="l43"></a> ode-&gt;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-&gt;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-&gt;h_max = </span><span class=cFE>1</span><span class=cF0>e32;
<a name="l46"></a> ode-&gt;max_tolerance = ode-&gt;min_tolerance = ode-&gt;tolerance_internal = max_tolerance;
<a name="l47"></a> ode-&gt;win_task = ode-&gt;mem_task = </span><span class=cF5>Fs</span><span class=cF0>;
<a name="l48"></a> </span><span class=cF5>QueueInit</span><span class=cF0>(&amp;ode-&gt;next_mass);
<a name="l49"></a> </span><span class=cF5>QueueInit</span><span class=cF0>(&amp;ode-&gt;next_spring);
<a name="l50"></a> ode-&gt;state = </span><span class=cF5>CAlloc</span><span class=cF0>(s);
<a name="l51"></a> ode-&gt;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);
<a name="l53"></a>
<a name="l54"></a> </span><span class=cF1>return</span><span class=cF0> ode;
<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>
<a name="l60"></a> </span><span class=cF1>Bool</span><span class=cF0> res;
<a name="l61"></a>
<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>(&amp;ode-&gt;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>&amp;ode-&gt;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>;
<a name="l68"></a>
<a name="l69"></a> </span><span class=cF1>return</span><span class=cF0> res;
<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>
<a name="l74"></a> </span><span class=cF9>I64</span><span class=cF0> i;
<a name="l75"></a>
<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-&gt;state);
<a name="l80"></a> </span><span class=cF5>Free</span><span class=cF0>(ode-&gt;array_base);
<a name="l81"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;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 &lt; </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-&gt;slave_tasks[i]);
<a name="l85"></a> </span><span class=cF5>Free</span><span class=cF0>(ode-&gt;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);
<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>
<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-&gt;state) + </span><span class=cF5>MSize2</span><span class=cF0>(ode-&gt;array_base) + </span><span class=cF5>MSize2</span><span class=cF0>(ode);
<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>{
<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-&gt;n, *ptr2 = DstateDt(</span><span class=cF1>F64</span><span class=cF0> *) + ode-&gt;n;
<a name="l101"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm = ode-&gt;next_mass;
<a name="l102"></a>
<a name="l103"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;next_mass)
<a name="l104"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l105"></a> tmpm-&gt;state = ptr1++;
<a name="l106"></a> tmpm-&gt;DstateDt = ptr2++;
<a name="l107"></a> tmpm = tmpm-&gt;next;
<a name="l108"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<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;
<a name="l116"></a>
<a name="l117"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;flags &amp; </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-&gt;next_mass;
<a name="l121"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;next_mass)
<a name="l122"></a> {
<a name="l123"></a> mass_count++;
<a name="l124"></a> tmpm = tmpm-&gt;next;
<a name="l125"></a> }
<a name="l126"></a> old_array_base = ode-&gt;array_base;
<a name="l127"></a> ode-&gt;n_internal = ode-&gt;n + </span><span class=cFE>6</span><span class=cF0> * mass_count;
<a name="l128"></a> ode-&gt;array_base = </span><span class=cF5>MAlloc</span><span class=cF0>(</span><span class=cFE>12</span><span class=cF0> * ode-&gt;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-&gt;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);
<a name="l131"></a>
<a name="l132"></a> </span><span class=cF5>ODESetMassesPtrs</span><span class=cF0>(ode, ode-&gt;state_internal, ode-&gt;state_internal);
<a name="l133"></a> tmpm = ode-&gt;next_mass;
<a name="l134"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;next_mass)
<a name="l135"></a> {
<a name="l136"></a> </span><span class=cF5>MemCopy</span><span class=cF0>(tmpm-&gt;state, &amp;tmpm-&gt;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-&gt;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-&gt;state_internal, ode-&gt;state, ode-&gt;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="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>{
<a name="l145"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
<a name="l146"></a>
<a name="l147"></a> </span><span class=cF5>MemCopy</span><span class=cF0>(ode-&gt;state, ode-&gt;state_internal, ode-&gt;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-&gt;flags &amp; </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-&gt;state_internal, ode-&gt;state_internal);
<a name="l151"></a> tmpm = ode-&gt;next_mass;
<a name="l152"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;next_mass)
<a name="l153"></a> {
<a name="l154"></a> </span><span class=cF5>MemCopy</span><span class=cF0>(&amp;tmpm-&gt;saved_state, tmpm-&gt;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-&gt;next;
<a name="l156"></a> }
<a name="l157"></a> </span><span class=cF7>}</span><span class=cF0>
<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>
<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;
<a name="l165"></a>
<a name="l166"></a> i = </span><span class=cFE>0</span><span class=cF0>;
<a name="l167"></a> tmpm = ode-&gt;next_mass;
<a name="l168"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;next_mass)
<a name="l169"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l170"></a> tmpm-&gt;num = i++;
<a name="l171"></a> tmpm = tmpm-&gt;next;
<a name="l172"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l173"></a>
<a name="l174"></a> i = </span><span class=cFE>0</span><span class=cF0>;
<a name="l175"></a> tmps = ode-&gt;next_spring;
<a name="l176"></a> </span><span class=cF1>while</span><span class=cF0> (tmps != &amp;ode-&gt;next_spring)
<a name="l177"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l178"></a> tmps-&gt;num = i++;
<a name="l179"></a> tmps-&gt;end1_num = tmps-&gt;end1-&gt;num;
<a name="l180"></a> tmps-&gt;end2_num = tmps-&gt;end2-&gt;num;
<a name="l181"></a> tmps = tmps-&gt;next;
<a name="l182"></a> </span><span class=cF7>}</span><span class=cF0>
<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>
<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>;
<a name="l189"></a>
<a name="l190"></a> tmpm = ode-&gt;next_mass;
<a name="l191"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;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-&gt;x - x) + </span><span class=cF5>Sqr</span><span class=cF0>(tmpm-&gt;y - y) + </span><span class=cF5>Sqr</span><span class=cF0>(tmpm-&gt;z - z);
<a name="l194"></a> </span><span class=cF1>if</span><span class=cF0> (dd &lt; 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-&gt;next;
<a name="l200"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l201"></a>
<a name="l202"></a> </span><span class=cF1>return</span><span class=cF0> best_mass;
<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>
<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>;
<a name="l209"></a>
<a name="l210"></a> tmps = ode-&gt;next_spring;
<a name="l211"></a> </span><span class=cF1>while</span><span class=cF0> (tmps != &amp;ode-&gt;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-&gt;end1-&gt;x + tmps-&gt;end2-&gt;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-&gt;end1-&gt;y + tmps-&gt;end2-&gt;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-&gt;end1-&gt;z + tmps-&gt;end2-&gt;z</span><span class=cF7>)</span><span class=cF0> / </span><span class=cFE>2</span><span class=cF0> - z);
<a name="l216"></a>
<a name="l217"></a> </span><span class=cF1>if</span><span class=cF0> (dd &lt; 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-&gt;next;
<a name="l223"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l224"></a>
<a name="l225"></a> </span><span class=cF1>return</span><span class=cF0> best_spring;
<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>
<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>;
<a name="l233"></a>
<a name="l234"></a> tmpm = ode-&gt;next_mass;
<a name="l235"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;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-&gt;x - x) + </span><span class=cF5>Sqr</span><span class=cF0>(tmpm-&gt;y - y) + </span><span class=cF5>Sqr</span><span class=cF0>(tmpm-&gt;z - z);
<a name="l238"></a> </span><span class=cF1>if</span><span class=cF0> (dd &lt; 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-&gt;next;
<a name="l244"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l245"></a>
<a name="l246"></a> tmps = ode-&gt;next_spring;
<a name="l247"></a> </span><span class=cF1>while</span><span class=cF0> (tmps != &amp;ode-&gt;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-&gt;end1-&gt;x + tmps-&gt;end2-&gt;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-&gt;end1-&gt;y + tmps-&gt;end2-&gt;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-&gt;end1-&gt;z + tmps-&gt;end2-&gt;z</span><span class=cF7>)</span><span class=cF0> / </span><span class=cFE>2</span><span class=cF0> - z);
<a name="l252"></a>
<a name="l253"></a> </span><span class=cF1>if</span><span class=cF0> (dd &lt; 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-&gt;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;
<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>
<a name="l269"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm = ode-&gt;next_mass;
<a name="l270"></a>
<a name="l271"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;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-&gt;num == num)
<a name="l274"></a> </span><span class=cF1>return</span><span class=cF0> tmpm;
<a name="l275"></a> tmpm = tmpm-&gt;next;
<a name="l276"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l277"></a>
<a name="l278"></a> </span><span class=cF1>return</span><span class=cF0> </span><span class=cF3>NULL</span><span class=cF0>;
<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>
<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;
<a name="l285"></a>
<a name="l286"></a> tmpm = ode-&gt;next_mass;
<a name="l287"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;next_mass)
<a name="l288"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l289"></a> tmpm-&gt;flags &amp;= ~</span><span class=cF3>MSF_INACTIVE</span><span class=cF0>;
<a name="l290"></a> tmpm = tmpm-&gt;next;
<a name="l291"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l292"></a> tmps = ode-&gt;next_spring;
<a name="l293"></a> </span><span class=cF1>while</span><span class=cF0> (tmps != &amp;ode-&gt;next_spring)
<a name="l294"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l295"></a> tmps-&gt;flags &amp;= ~</span><span class=cF3>SSF_INACTIVE</span><span class=cF0>;
<a name="l296"></a> tmps = tmps-&gt;next;
<a name="l297"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<a name="l302"></a> </span><span class=cF9>CSpring</span><span class=cF0> *tmps = ode-&gt;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;
<a name="l306"></a>
<a name="l307"></a> </span><span class=cF1>while</span><span class=cF0> (tmps != &amp;ode-&gt;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-&gt;flags &amp; </span><span class=cF3>SSF_INACTIVE</span><span class=cF0>)
<a name="l310"></a> {
<a name="l311"></a> tmps-&gt;displacement = </span><span class=cFE>0</span><span class=cF0>;
<a name="l312"></a> tmps-&gt;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-&gt;end1;
<a name="l317"></a> e2 = tmps-&gt;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>&amp;p, &amp;e2-&gt;state-&gt;x, &amp;e1-&gt;state-&gt;x</span><span class=cF7>)</span><span class=cF0>);
<a name="l319"></a> tmps-&gt;displacement = d - tmps-&gt;rest_len;
<a name="l320"></a> tmps-&gt;f = tmps-&gt;displacement * tmps-&gt;const;
<a name="l321"></a> </span><span class=cF1>if</span><span class=cF0> (tmps-&gt;f &gt; </span><span class=cFE>0</span><span class=cF0> &amp;&amp; tmps-&gt;flags &amp; </span><span class=cF3>SSF_NO_TENSION</span><span class=cF0>)
<a name="l322"></a> tmps-&gt;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-&gt;f &lt; </span><span class=cFE>0</span><span class=cF0> &amp;&amp; tmps-&gt;flags &amp; </span><span class=cF3>SSF_NO_COMPRESSION</span><span class=cF0>)
<a name="l324"></a> tmps-&gt;f = </span><span class=cFE>0</span><span class=cF0>;
<a name="l325"></a> </span><span class=cF1>if</span><span class=cF0> (d &gt; </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>(&amp;p, tmps-&gt;f / d);
<a name="l328"></a> </span><span class=cF5>D3AddEqu</span><span class=cF0>(&amp;e1-&gt;DstateDt-&gt;DxDt, &amp;p);
<a name="l329"></a> </span><span class=cF5>D3SubEqu</span><span class=cF0>(&amp;e2-&gt;DstateDt-&gt;DxDt, &amp;p);
<a name="l330"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l331"></a> }
<a name="l332"></a> tmps = tmps-&gt;next;
<a name="l333"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<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;
<a name="l341"></a>
<a name="l342"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;drag_v || ode-&gt;drag_v2 || ode-&gt;drag_v3)
<a name="l343"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l344"></a> tmpm = ode-&gt;next_mass;
<a name="l345"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;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-&gt;flags &amp; </span><span class=cF3>MSF_INACTIVE</span><span class=cF7>)</span><span class=cF0> &amp;&amp; tmpm-&gt;drag_profile_factor &amp;&amp; </span><span class=cF7>(</span><span class=cF0>dd = </span><span class=cF5>D3NormSqr</span><span class=cF0>(&amp;tmpm-&gt;state-&gt;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-&gt;drag_v;
<a name="l350"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;drag_v2)
<a name="l351"></a> d += ode-&gt;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-&gt;drag_v3)
<a name="l353"></a> d += dd * ode-&gt;drag_v3;
<a name="l354"></a> </span><span class=cF5>D3SubEqu</span><span class=cF0>(&amp;tmpm-&gt;DstateDt-&gt;DxDt, </span><span class=cF5>D3Mul</span><span class=cF7>(</span><span class=cF0>&amp;p, d * tmpm-&gt;drag_profile_factor, &amp;tmpm-&gt;state-&gt;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-&gt;next;
<a name="l357"></a> }
<a name="l358"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<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;
<a name="l365"></a>
<a name="l366"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;acceleration_limit)
<a name="l367"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l368"></a> tmpm = ode-&gt;next_mass;
<a name="l369"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;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-&gt;flags &amp; </span><span class=cF3>MSF_INACTIVE</span><span class=cF7>)</span><span class=cF0> &amp;&amp; </span><span class=cF7>(</span><span class=cF0>d = </span><span class=cF5>D3Norm</span><span class=cF0>(&amp;tmpm-&gt;DstateDt-&gt;DxDt)</span><span class=cF7>)</span><span class=cF0> &gt; ode-&gt;acceleration_limit)
<a name="l372"></a> </span><span class=cF5>D3MulEqu</span><span class=cF0>(&amp;tmpm-&gt;DstateDt-&gt;DxDt, ode-&gt;acceleration_limit / d);
<a name="l373"></a> tmpm = tmpm-&gt;next;
<a name="l374"></a> }
<a name="l375"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<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>&amp;ode-&gt;mp_not_done_flags, </span><span class=cF5>Gs</span><span class=cF0>-&gt;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-&gt;mp_derive)
<a name="l385"></a> (*ode-&gt;mp_derive)(ode, ode-&gt;mp_t, </span><span class=cF5>Gs</span><span class=cF0>-&gt;num, ode-&gt;mp_state, ode-&gt;mp_DstateDt);
<a name="l386"></a> </span><span class=cF5>LBtr</span><span class=cF0>(&amp;ode-&gt;mp_not_done_flags, </span><span class=cF5>Gs</span><span class=cF0>-&gt;num);
<a name="l387"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<a name="l392"></a> </span><span class=cF9>I64</span><span class=cF0> i;
<a name="l393"></a>
<a name="l394"></a> </span><span class=cF1>if</span><span class=cF0> (!ode-&gt;slave_tasks)
<a name="l395"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l396"></a> ode-&gt;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 &lt; </span><span class=cFB>mp_count</span><span class=cF0>; i++)
<a name="l398"></a> ode-&gt;slave_tasks[i] = </span><span class=cF5>Spawn</span><span class=cF0>(&amp;</span><span class=cF5>ODEMPTask</span><span class=cF0>, ode, </span><span class=cF6>&quot;ODE Slave&quot;</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 &lt; </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-&gt;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>
<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>{
<a name="l409"></a> </span><span class=cF9>I64</span><span class=cF0> i;
<a name="l410"></a>
<a name="l411"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;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-&gt;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 &lt; </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-&gt;slave_tasks[i]);
<a name="l417"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<a name="l422"></a> ode-&gt;mp_t = t;
<a name="l423"></a> ode-&gt;mp_state = state;
<a name="l424"></a> ode-&gt;mp_DstateDt = DstateDt;
<a name="l425"></a> ode-&gt;mp_not_done_flags = </span><span class=cFE>1</span><span class=cF0> &lt;&lt; </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-&gt;mp_not_done_flags);
<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>{
<a name="l433"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
<a name="l434"></a>
<a name="l435"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;flags &amp; </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-&gt;next_mass;
<a name="l439"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;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-&gt;flags &amp; </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>(&amp;tmpm-&gt;DstateDt-&gt;DxDt);
<a name="l444"></a> </span><span class=cF5>D3Copy</span><span class=cF0>(&amp;tmpm-&gt;DstateDt-&gt;x, &amp;tmpm-&gt;state-&gt;DxDt);
<a name="l445"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l446"></a> tmpm = tmpm-&gt;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-&gt;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-&gt;derive)
<a name="l453"></a> (*ode-&gt;derive)(ode, t, state, DstateDt);
<a name="l454"></a> tmpm = ode-&gt;next_mass;
<a name="l455"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm != &amp;ode-&gt;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-&gt;flags &amp; </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-&gt;flags &amp; </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>(&amp;tmpm-&gt;DstateDt-&gt;DxDt);
<a name="l462"></a> </span><span class=cF5>D3Zero</span><span class=cF0>(&amp;tmpm-&gt;DstateDt-&gt;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-&gt;mass)
<a name="l465"></a> </span><span class=cF5>D3DivEqu</span><span class=cF0>(&amp;tmpm-&gt;DstateDt-&gt;DxDt, tmpm-&gt;mass);
<a name="l466"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l467"></a> tmpm = tmpm-&gt;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-&gt;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-&gt;derive)
<a name="l476"></a> (*ode-&gt;derive)(ode, t, state, DstateDt);
<a name="l477"></a> </span><span class=cF7>}</span><span class=cF0>
<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>{
<a name="l482"></a> </span><span class=cF9>I64</span><span class=cF0> i;
<a name="l483"></a>
<a name="l484"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode, ode-&gt;t, ode-&gt;state_internal, ode-&gt;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 &lt; ode-&gt;n_internal; i++)
<a name="l486"></a> ode-&gt;state_internal[i] += ode-&gt;h * ode-&gt;DstateDt[i];
<a name="l487"></a> ode-&gt;t += ode-&gt;h;
<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>{
<a name="l492"></a> </span><span class=cF9>I64</span><span class=cF0> i, n = ode-&gt;n_internal;
<a name="l493"></a> </span><span class=cF1>F64</span><span class=cF0> xh, hh, h6, *dym, *dyt, *yt, *DstateDt;
<a name="l494"></a>
<a name="l495"></a> dym = ode-&gt;tmp0;
<a name="l496"></a> dyt = ode-&gt;tmp1;
<a name="l497"></a> yt = ode-&gt;tmp2;
<a name="l498"></a> DstateDt = ode-&gt;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-&gt;h;
<a name="l500"></a> h6 = ode-&gt;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-&gt;t + hh;
<a name="l502"></a>
<a name="l503"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode, ode-&gt;t, ode-&gt;state_internal, ode-&gt;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 &lt; n; i++)
<a name="l505"></a> yt[i] = ode-&gt;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 &lt; n; i++)
<a name="l508"></a> yt[i] = ode-&gt;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 &lt; n; i++)
<a name="l511"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l512"></a> yt[i] = ode-&gt;state_internal[i] + ode-&gt;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-&gt;t += ode-&gt;h;
<a name="l516"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode, ode-&gt;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 &lt; n; i++)
<a name="l518"></a> ode-&gt;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]);
<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>{
<a name="l553"></a> </span><span class=cF9>I64</span><span class=cF0> i, n = ode-&gt;n_internal;
<a name="l554"></a> </span><span class=cF1>F64</span><span class=cF0> h = ode-&gt;h, *state = ode-&gt;state_internal, *DstateDt = ode-&gt;DstateDt, *ak2, *ak3, *ak4, *ak5, *ak6, *tmpstate, *stateerr, *outstate;
<a name="l555"></a>
<a name="l556"></a> ak2 = ode-&gt;tmp0;
<a name="l557"></a> ak3 = ode-&gt;tmp1;
<a name="l558"></a> ak4 = ode-&gt;tmp2;
<a name="l559"></a> ak5 = ode-&gt;tmp3;
<a name="l560"></a> ak6 = ode-&gt;tmp4;
<a name="l561"></a> tmpstate = ode-&gt;tmp5;
<a name="l562"></a> outstate = ode-&gt;tmp6;
<a name="l563"></a> stateerr = ode-&gt;tmp7;
<a name="l564"></a>
<a name="l565"></a> </span><span class=cF1>for</span><span class=cF0> (i = </span><span class=cFE>0</span><span class=cF0>; i &lt; 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-&gt;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 &lt; 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-&gt;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 &lt; 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-&gt;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 &lt; 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-&gt;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 &lt; 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-&gt;t + </span><span class=cF3>ODEa6</span><span class=cF0> * h, tmpstate, ak6);
<a name="l580"></a>
<a name="l581"></a> </span><span class=cF1>for</span><span class=cF0> (i = </span><span class=cFE>0</span><span class=cF0>; i &lt; 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 &lt; 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]);
<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>{
<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-&gt;tmp6, *stateerr = ode-&gt;tmp7;
<a name="l596"></a>
<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-&gt;h = </span><span class=cF5>Clamp</span><span class=cF0>(ode-&gt;h, ode-&gt;h_min, ode-&gt;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 &lt; ode-&gt;n_internal; i++)
<a name="l603"></a> {
<a name="l604"></a> tmp = </span><span class=cF5>Abs</span><span class=cF0>(stateerr[i] / ode-&gt;state_scale[i]);
<a name="l605"></a> </span><span class=cF1>if</span><span class=cF0> (tmp &gt; errmax)
<a name="l606"></a> errmax = tmp;
<a name="l607"></a> }
<a name="l608"></a> errmax /= ode-&gt;tolerance_internal;
<a name="l609"></a> </span><span class=cF1>if</span><span class=cF0> (errmax &lt;= </span><span class=cFE>1</span><span class=cF0>.</span><span class=cFE>0</span><span class=cF0> || ode-&gt;h == ode-&gt;h_min)
<a name="l610"></a> </span><span class=cF1>break</span><span class=cF0>;
<a name="l611"></a> tmp = ode-&gt;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 &lt; </span><span class=cFE>0</span><span class=cF0>.</span><span class=cFE>1</span><span class=cF0> * ode-&gt;h)
<a name="l613"></a> ode-&gt;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-&gt;h = tmp;
<a name="l616"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l617"></a> ode-&gt;t += ode-&gt;h;
<a name="l618"></a> </span><span class=cF1>if</span><span class=cF0> (errmax &gt; </span><span class=cF3>ERRCON</span><span class=cF0>)
<a name="l619"></a> ode-&gt;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-&gt;h *= </span><span class=cFE>5</span><span class=cF0>.</span><span class=cFE>0</span><span class=cF0>;
<a name="l622"></a> ode-&gt;h = </span><span class=cF5>Clamp</span><span class=cF0>(ode-&gt;h, ode-&gt;h_min, ode-&gt;h_max);
<a name="l623"></a> </span><span class=cF5>MemCopy</span><span class=cF0>(ode-&gt;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-&gt;n_internal);
<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)
<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.CC.html#l7"><span class=cF4>window mgr</span></a><span class=cF2>on a continuous</span><span class=cF0>
<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>
<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;
<a name="l641"></a>
<a name="l642"></a> </span><span class=cF1>if</span><span class=cF0> (task-&gt;next_ode == &amp;task-&gt;next_ode)
<a name="l643"></a> task-&gt;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>&amp;task-&gt;win_inhibit, </span><span class=cF3>WIf_SELF_ODE</span><span class=cF7>)</span><span class=cF0>)
<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.CC.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.CC.html#l3"><span class=cF4>GrUpdateTaskODEs</span></a><span class=cF2>().</span><span class=cF0>
<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-&gt;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-&gt;next_ode;
<a name="l654"></a> </span><span class=cF1>while</span><span class=cF0> (ode != &amp;task-&gt;next_ode)
<a name="l655"></a> {
<a name="l656"></a> t_initial = ode-&gt;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-&gt;flags &amp; </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-&gt;base_t = d;
<a name="l661"></a> ode-&gt;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-&gt;base_t + t_initial;
<a name="l664"></a> t_desired = ode-&gt;t_scale * d + t_initial;
<a name="l665"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;flags &amp; </span><span class=cF3>ODEF_PAUSED</span><span class=cF0>)
<a name="l666"></a> ode-&gt;base_t += t_desired - ode-&gt;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-&gt;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-&gt;flags &amp; </span><span class=cF3>ODEF_PAUSED</span><span class=cF0>)
<a name="l671"></a> ode-&gt;base_t += t_desired-ode-&gt;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-&gt;derive || ode-&gt;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-&gt;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-&gt;initial_state, ode-&gt;state_internal, ode-&gt;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-&gt;t &lt; t_desired)
<a name="l681"></a> {
<a name="l682"></a> ode-&gt;h_max = t_desired - ode-&gt;t;
<a name="l683"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode, ode-&gt;t, ode-&gt;state_internal, ode-&gt;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 &lt; ode-&gt;n_internal; i++)
<a name="l685"></a> ode-&gt;state_scale[i] = </span><span class=cF5>Abs</span><span class=cF0>(ode-&gt;state_internal[i]) +
<a name="l686"></a> </span><span class=cF5>Abs</span><span class=cF0>(ode-&gt;DstateDt[i] * ode-&gt;h) +
<a name="l687"></a> ode-&gt;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> &gt; timeout_time)
<a name="l690"></a> </span><span class=cF7>{</span><span class=cF0>
<a name="l691"></a> ode-&gt;base_t += t_desired - ode-&gt;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;
<a name="l693"></a>
<a name="l694"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l695"></a> }
<a name="l696"></a>
<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-&gt;t != t_desired)
<a name="l699"></a> {
<a name="l700"></a> </span><span class=cF1>if</span><span class=cF0> (interpolation = ode-&gt;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 &lt; ode-&gt;n_internal; i++)
<a name="l705"></a> ode-&gt;state_internal[i] = (ode-&gt;state_internal[i] -
<a name="l706"></a> ode-&gt;initial_state[i]) * interpolation +
<a name="l707"></a> ode-&gt;initial_state[i];
<a name="l708"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l709"></a> ode-&gt;t = t_desired;
<a name="l710"></a> }
<a name="l711"></a>ode_done:
<a name="l712"></a> </span><span class=cF5>ODEInternal2State</span><span class=cF0>(ode);
<a name="l713"></a>
<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-&gt;t, ode-&gt;state_internal, ode-&gt;DstateDt);
<a name="l716"></a>
<a name="l717"></a> </span><span class=cF1>if</span><span class=cF0> (ode-&gt;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-&gt;flags &amp;= ~</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-&gt;base_t += (</span><span class=cFE>1</span><span class=cF0>.</span><span class=cFE>0</span><span class=cF0> - ode-&gt;t_scale) * d;
<a name="l724"></a> ode = ode-&gt;next;
<a name="l725"></a> }
<a name="l726"></a>
<a name="l727"></a> </span><span class=cF2>//Now, we will dynamically adjust tolerances.</span><span class=cF0>
<a name="l728"></a>
<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>
<a name="l734"></a>
<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>
<a name="l736"></a>
<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>;
<a name="l739"></a>
<a name="l740"></a> ode = task-&gt;next_ode;
<a name="l741"></a> </span><span class=cF1>while</span><span class=cF0> (ode != &amp;task-&gt;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-&gt;flags &amp; </span><span class=cF3>ODEF_PAUSED</span><span class=cF7>)</span><span class=cF0> &amp;&amp; ode-&gt;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-&gt;min_tolerance != ode-&gt;max_tolerance)
<a name="l746"></a> {
<a name="l747"></a> </span><span class=cF1>if</span><span class=cF0> (d &gt; </span><span class=cFE>0</span><span class=cF0>)
<a name="l748"></a> ode-&gt;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-&gt;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-&gt;tolerance_internal = </span><span class=cF5>Clamp</span><span class=cF0>(ode-&gt;tolerance_internal, ode-&gt;min_tolerance, ode-&gt;max_tolerance);
<a name="l753"></a> </span><span class=cF7>}</span><span class=cF0>
<a name="l754"></a> ode = ode-&gt;next;
<a name="l755"></a> }
<a name="l756"></a> </span><span class=cFB>winmgr</span><span class=cF0>.ode_time += task-&gt;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>
<a name="l758"></a>}
</span></pre></body>
</html>
Reference in a new issue View git blame Copy permalink