tristan/ZealOS
1
0
Fork 0
mirror of https://github.com/Zeal-Operating-System/ZealOS.git synced 2024-12-28 16:26:35 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions
ZealOS/docs/System/MathODE.CC.html
TomAwezome 293db76ddb Update documentation.
2021-10-27 03:28:16 -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 V1.04">
<style type="text/css">
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;}
</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