720 lines
67 KiB
HTML
Executable File
720 lines
67 KiB
HTML
Executable File
<!DOCTYPE html>
|
|
<html lang="en">
|
|
<head>
|
|
<meta charset="UTF-8">
|
|
<meta name="generator" content="TempleOS V5.03">
|
|
<meta name="viewport" content="width=device-width">
|
|
<link rel="stylesheet" href="/style/templeos.css">
|
|
<script src="/script/templeos.js"></script>
|
|
<style type="text/css">
|
|
.cF0{color:#000000;background-color:#ffffff;}
|
|
.cF1{color:#0000aa;background-color:#ffffff;}
|
|
.cF2{color:#00aa00;background-color:#ffffff;}
|
|
.cF3{color:#00aaaa;background-color:#ffffff;}
|
|
.cF4{color:#aa0000;background-color:#ffffff;}
|
|
.cF5{color:#aa00aa;background-color:#ffffff;}
|
|
.cF6{color:#aa5500;background-color:#ffffff;}
|
|
.cF7{color:#aaaaaa;background-color:#ffffff;}
|
|
.cF8{color:#555555;background-color:#ffffff;}
|
|
.cF9{color:#5555ff;background-color:#ffffff;}
|
|
.cFA{color:#55ff55;background-color:#ffffff;}
|
|
.cFB{color:#55ffff;background-color:#ffffff;}
|
|
.cFC{color:#ff5555;background-color:#ffffff;}
|
|
.cFD{color:#ff55ff;background-color:#ffffff;}
|
|
.cFE{color:#ffff55;background-color:#ffffff;}
|
|
.cFF{color:#ffffff;background-color:#ffffff;}
|
|
</style>
|
|
</head>
|
|
<body>
|
|
<pre id="content">
|
|
<a name="l1"></a><span class=cF0>#</span><span class=cF1>help_index</span><span class=cF0> </span><span class=cF6>"Math/ODE"</span><span class=cF0>
|
|
<a name="l2"></a>#</span><span class=cF1>help_file</span><span class=cF0> </span><span class=cF6>"::/Doc/ODE"</span><span class=cF0>
|
|
<a name="l3"></a>
|
|
<a name="l4"></a></span><span class=cF2>//See </span><a href="/Wb/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=1.0)
|
|
<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*(1.0-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>ODERstPtrs</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->n_internal*</span><span class=cF1>sizeof</span><span class=cF0>(</span><span class=cF1>F64</span><span class=cF0>);
|
|
<a name="l15"></a> </span><span class=cF1>F64</span><span class=cF0> *ptr=ode->array_base;
|
|
<a name="l16"></a> ode->state_internal=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l17"></a> ode->state_scale=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l18"></a> ode->DstateDt=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l19"></a> ode->initial_state=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l20"></a> ode->tmp0=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l21"></a> ode->tmp1=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l22"></a> ode->tmp2=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l23"></a> ode->tmp3=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l24"></a> ode->tmp4=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l25"></a> ode->tmp5=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l26"></a> ode->tmp6=ptr; ptr(</span><span class=cF9>I64</span><span class=cF0>)+=s;
|
|
<a name="l27"></a> ode->tmp7=ptr;
|
|
<a name="l28"></a>}
|
|
<a name="l29"></a>
|
|
<a name="l30"></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=1e-6,</span><span class=cF9>I64</span><span class=cF0> flags=0)
|
|
<a name="l31"></a>{</span><span class=cF2>//Make differential equation ctrl struct. See </span><a href="/Wb/Kernel/KernelA.HH.HTML#l241"><span class=cF4>flags</span></a><span class=cF2>.</span><span class=cF0>
|
|
<a name="l32"></a> </span><span class=cF2>//The tolerance is not precise.</span><span class=cF0>
|
|
<a name="l33"></a> </span><span class=cF2>//You can min_tolerance and it will</span><span class=cF0>
|
|
<a name="l34"></a> </span><span class=cF2>//dynamically adjust tolerance to utilize</span><span class=cF0>
|
|
<a name="l35"></a> </span><span class=cF2>//the CPU.</span><span class=cF0>
|
|
<a name="l36"></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="l37"></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="l38"></a> ode->t_scale=1.0;
|
|
<a name="l39"></a> ode->flags=flags;
|
|
<a name="l40"></a> ode->n_internal=ode->n=n;
|
|
<a name="l41"></a> ode->h=1e-6;
|
|
<a name="l42"></a> ode->h_min=1e-64;
|
|
<a name="l43"></a> ode->h_max=1e32;
|
|
<a name="l44"></a> ode->max_tolerance=ode->min_tolerance=ode->tolerance_internal=max_tolerance;
|
|
<a name="l45"></a> ode->win_task=ode->mem_task=</span><span class=cF5>Fs</span><span class=cF0>;
|
|
<a name="l46"></a> </span><span class=cF5>QueInit</span><span class=cF0>(&ode->next_mass);
|
|
<a name="l47"></a> </span><span class=cF5>QueInit</span><span class=cF0>(&ode->next_spring);
|
|
<a name="l48"></a> ode->state=</span><span class=cF5>CAlloc</span><span class=cF0>(s);
|
|
<a name="l49"></a> ode->array_base=</span><span class=cF5>MAlloc</span><span class=cF0>(12*s);
|
|
<a name="l50"></a> </span><span class=cF5>ODERstPtrs</span><span class=cF0>(ode);
|
|
<a name="l51"></a> </span><span class=cF1>return</span><span class=cF0> ode;
|
|
<a name="l52"></a>}
|
|
<a name="l53"></a>
|
|
<a name="l54"></a>
|
|
<a name="l55"></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="l56"></a>{</span><span class=cF2>//Pause ODE.</span><span class=cF0>
|
|
<a name="l57"></a> </span><span class=cF1>Bool</span><span class=cF0> res;
|
|
<a name="l58"></a> </span><span class=cF1>if</span><span class=cF0> (!ode) </span><span class=cF1>return</span><span class=cF0> </span><span class=cF3>OFF</span><span class=cF0>;
|
|
<a name="l59"></a> res=</span><span class=cF5>LBEqu</span><span class=cF0>(&ode->flags,</span><span class=cF3>ODEf_PAUSED</span><span class=cF0>,val);
|
|
<a name="l60"></a> </span><span class=cF1>if</span><span class=cF0> (val)
|
|
<a name="l61"></a> </span><span class=cF1>while</span><span class=cF0> (</span><span class=cF5>Bt</span><span class=cF7>(</span><span class=cF0>&ode->flags,</span><span class=cF3>ODEf_BUSY</span><span class=cF7>)</span><span class=cF0>)
|
|
<a name="l62"></a> </span><span class=cF5>Yield</span><span class=cF0>;
|
|
<a name="l63"></a> </span><span class=cF1>return</span><span class=cF0> res;
|
|
<a name="l64"></a>}
|
|
<a name="l65"></a>
|
|
<a name="l66"></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="l67"></a>{</span><span class=cF2>//Free ODE node, but not masses or springs.</span><span class=cF0>
|
|
<a name="l68"></a> </span><span class=cF9>I64</span><span class=cF0> i;
|
|
<a name="l69"></a> </span><span class=cF1>if</span><span class=cF0> (!ode) </span><span class=cF1>return</span><span class=cF0>;
|
|
<a name="l70"></a> </span><span class=cF5>ODEPause</span><span class=cF0>(ode);
|
|
<a name="l71"></a> </span><span class=cF5>Free</span><span class=cF0>(ode->state);
|
|
<a name="l72"></a> </span><span class=cF5>Free</span><span class=cF0>(ode->array_base);
|
|
<a name="l73"></a> </span><span class=cF1>if</span><span class=cF0> (ode->slave_tasks) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l74"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<</span><span class=cFB>mp_cnt</span><span class=cF0>;i++)
|
|
<a name="l75"></a> </span><span class=cF5>Kill</span><span class=cF0>(ode->slave_tasks[i]);
|
|
<a name="l76"></a> </span><span class=cF5>Free</span><span class=cF0>(ode->slave_tasks);
|
|
<a name="l77"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l78"></a> </span><span class=cF5>Free</span><span class=cF0>(ode);
|
|
<a name="l79"></a>}
|
|
<a name="l80"></a>
|
|
<a name="l81"></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="l82"></a>{</span><span class=cF2>//Mem size of ode ctrl, but not masses and springs.</span><span class=cF0>
|
|
<a name="l83"></a> </span><span class=cF1>if</span><span class=cF0> (!ode)
|
|
<a name="l84"></a> </span><span class=cF1>return</span><span class=cF0> 0;
|
|
<a name="l85"></a> </span><span class=cF1>else</span><span class=cF0>
|
|
<a name="l86"></a> </span><span class=cF1>return</span><span class=cF0> </span><span class=cF5>MSize2</span><span class=cF0>(ode->state)+</span><span class=cF5>MSize2</span><span class=cF0>(ode->array_base)+</span><span class=cF5>MSize2</span><span class=cF0>(ode);
|
|
<a name="l87"></a>}
|
|
<a name="l88"></a>
|
|
<a name="l89"></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="l90"></a>{
|
|
<a name="l91"></a> </span><span class=cF9>COrder2D3</span><span class=cF0> *ptr1=state(</span><span class=cF1>F64</span><span class=cF0> *)+ode->n,
|
|
<a name="l92"></a> *ptr2=DstateDt(</span><span class=cF1>F64</span><span class=cF0> *)+ode->n;
|
|
<a name="l93"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm=ode->next_mass;
|
|
<a name="l94"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l95"></a> tmpm->state=ptr1++;
|
|
<a name="l96"></a> tmpm->DstateDt=ptr2++;
|
|
<a name="l97"></a> tmpm=tmpm->next;
|
|
<a name="l98"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l99"></a>}
|
|
<a name="l100"></a>
|
|
<a name="l101"></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="l102"></a>{
|
|
<a name="l103"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
|
|
<a name="l104"></a> </span><span class=cF1>F64</span><span class=cF0> *old_array_base;
|
|
<a name="l105"></a> </span><span class=cF9>I64</span><span class=cF0> mass_cnt;
|
|
<a name="l106"></a>
|
|
<a name="l107"></a> </span><span class=cF1>if</span><span class=cF0> (ode->flags&</span><span class=cF3>ODEF_HAS_MASSES</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l108"></a> mass_cnt=0;
|
|
<a name="l109"></a> tmpm=ode->next_mass;
|
|
<a name="l110"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) {
|
|
<a name="l111"></a> mass_cnt++;
|
|
<a name="l112"></a> tmpm=tmpm->next;
|
|
<a name="l113"></a> }
|
|
<a name="l114"></a> old_array_base=ode->array_base;
|
|
<a name="l115"></a> ode->n_internal=ode->n+6*mass_cnt;
|
|
<a name="l116"></a> ode->array_base=</span><span class=cF5>MAlloc</span><span class=cF0>(12*ode->n_internal*</span><span class=cF1>sizeof</span><span class=cF7>(</span><span class=cF1>F64</span><span class=cF7>)</span><span class=cF0>,ode->mem_task);
|
|
<a name="l117"></a> </span><span class=cF5>Free</span><span class=cF0>(old_array_base);
|
|
<a name="l118"></a> </span><span class=cF5>ODERstPtrs</span><span class=cF0>(ode);
|
|
<a name="l119"></a>
|
|
<a name="l120"></a> </span><span class=cF5>ODESetMassesPtrs</span><span class=cF0>(ode,ode->state_internal,ode->state_internal);
|
|
<a name="l121"></a> tmpm=ode->next_mass;
|
|
<a name="l122"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) {
|
|
<a name="l123"></a> </span><span class=cF5>MemCpy</span><span class=cF0>(tmpm->state,&tmpm->saved_state,</span><span class=cF1>sizeof</span><span class=cF7>(</span><span class=cF9>COrder2D3</span><span class=cF7>)</span><span class=cF0>);
|
|
<a name="l124"></a> tmpm=tmpm->next;
|
|
<a name="l125"></a> }
|
|
<a name="l126"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l127"></a> </span><span class=cF5>MemCpy</span><span class=cF0>(ode->state_internal,ode->state,ode->n*</span><span class=cF1>sizeof</span><span class=cF7>(</span><span class=cF1>F64</span><span class=cF7>)</span><span class=cF0>);
|
|
<a name="l128"></a>}
|
|
<a name="l129"></a>
|
|
<a name="l130"></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="l131"></a>{
|
|
<a name="l132"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
|
|
<a name="l133"></a> </span><span class=cF5>MemCpy</span><span class=cF0>(ode->state,ode->state_internal,ode->n*</span><span class=cF1>sizeof</span><span class=cF7>(</span><span class=cF1>F64</span><span class=cF7>)</span><span class=cF0>);
|
|
<a name="l134"></a> </span><span class=cF1>if</span><span class=cF0> (ode->flags&</span><span class=cF3>ODEF_HAS_MASSES</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l135"></a> </span><span class=cF5>ODESetMassesPtrs</span><span class=cF0>(ode,ode->state_internal,ode->state_internal);
|
|
<a name="l136"></a> tmpm=ode->next_mass;
|
|
<a name="l137"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) {
|
|
<a name="l138"></a> </span><span class=cF5>MemCpy</span><span class=cF0>(&tmpm->saved_state,tmpm->state,</span><span class=cF1>sizeof</span><span class=cF7>(</span><span class=cF9>COrder2D3</span><span class=cF7>)</span><span class=cF0>);
|
|
<a name="l139"></a> tmpm=tmpm->next;
|
|
<a name="l140"></a> }
|
|
<a name="l141"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l142"></a>}
|
|
<a name="l143"></a>
|
|
<a name="l144"></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="l145"></a>{</span><span class=cF2>//Renumber masses and springs.</span><span class=cF0>
|
|
<a name="l146"></a> </span><span class=cF9>I64</span><span class=cF0> i;
|
|
<a name="l147"></a> </span><span class=cF9>CSpring</span><span class=cF0> *tmps;
|
|
<a name="l148"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
|
|
<a name="l149"></a>
|
|
<a name="l150"></a> i=0;
|
|
<a name="l151"></a> tmpm=ode->next_mass;
|
|
<a name="l152"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l153"></a> tmpm->num=i++;
|
|
<a name="l154"></a> tmpm=tmpm->next;
|
|
<a name="l155"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l156"></a>
|
|
<a name="l157"></a> i=0;
|
|
<a name="l158"></a> tmps=ode->next_spring;
|
|
<a name="l159"></a> </span><span class=cF1>while</span><span class=cF0> (tmps!=&ode->next_spring) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l160"></a> tmps->num=i++;
|
|
<a name="l161"></a> tmps->end1_num=tmps->end1->num;
|
|
<a name="l162"></a> tmps->end2_num=tmps->end2->num;
|
|
<a name="l163"></a> tmps=tmps->next;
|
|
<a name="l164"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l165"></a>}
|
|
<a name="l166"></a>
|
|
<a name="l167"></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=0)
|
|
<a name="l168"></a>{</span><span class=cF2>//Search for mass nearest to x,y,z.</span><span class=cF0>
|
|
<a name="l169"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm,*best_mass=</span><span class=cF3>NULL</span><span class=cF0>;
|
|
<a name="l170"></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="l171"></a>
|
|
<a name="l172"></a> tmpm=ode->next_mass;
|
|
<a name="l173"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l174"></a> dd=</span><span class=cF5>Sqr</span><span class=cF0>(tmpm->x-x)+</span><span class=cF5>Sqr</span><span class=cF0>(tmpm->y-y)+</span><span class=cF5>Sqr</span><span class=cF0>(tmpm->z-z);
|
|
<a name="l175"></a> </span><span class=cF1>if</span><span class=cF0> (dd<best_dd) {
|
|
<a name="l176"></a> best_dd=dd;
|
|
<a name="l177"></a> best_mass=tmpm;
|
|
<a name="l178"></a> }
|
|
<a name="l179"></a> tmpm=tmpm->next;
|
|
<a name="l180"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l181"></a> </span><span class=cF1>return</span><span class=cF0> best_mass;
|
|
<a name="l182"></a>}
|
|
<a name="l183"></a>
|
|
<a name="l184"></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=0)
|
|
<a name="l185"></a>{</span><span class=cF2>//Find spring midpoint nearest x,y,z.</span><span class=cF0>
|
|
<a name="l186"></a> </span><span class=cF9>CSpring</span><span class=cF0> *tmps,*best_spring=</span><span class=cF3>NULL</span><span class=cF0>;
|
|
<a name="l187"></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="l188"></a>
|
|
<a name="l189"></a> tmps=ode->next_spring;
|
|
<a name="l190"></a> </span><span class=cF1>while</span><span class=cF0> (tmps!=&ode->next_spring) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l191"></a> dd=</span><span class=cF5>Sqr</span><span class=cF0>(</span><span class=cF7>(</span><span class=cF0>tmps->end1->x+tmps->end2->x</span><span class=cF7>)</span><span class=cF0>/2-x)+
|
|
<a name="l192"></a> </span><span class=cF5>Sqr</span><span class=cF0>(</span><span class=cF7>(</span><span class=cF0>tmps->end1->y+tmps->end2->y</span><span class=cF7>)</span><span class=cF0>/2-y)+
|
|
<a name="l193"></a> </span><span class=cF5>Sqr</span><span class=cF0>(</span><span class=cF7>(</span><span class=cF0>tmps->end1->z+tmps->end2->z</span><span class=cF7>)</span><span class=cF0>/2-z);
|
|
<a name="l194"></a> </span><span class=cF1>if</span><span class=cF0> (dd<best_dd) {
|
|
<a name="l195"></a> best_dd=dd;
|
|
<a name="l196"></a> best_spring=tmps;
|
|
<a name="l197"></a> }
|
|
<a name="l198"></a> tmps=tmps->next;
|
|
<a name="l199"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l200"></a> </span><span class=cF1>return</span><span class=cF0> best_spring;
|
|
<a name="l201"></a>}
|
|
<a name="l202"></a>
|
|
<a name="l203"></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>(
|
|
<a name="l204"></a> </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,
|
|
<a name="l205"></a> </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=0)
|
|
<a name="l206"></a>{</span><span class=cF2>//Find spring or mass nearest x,y,z.</span><span class=cF0>
|
|
<a name="l207"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm,*best_mass=</span><span class=cF3>NULL</span><span class=cF0>;
|
|
<a name="l208"></a> </span><span class=cF9>CSpring</span><span class=cF0> *tmps,*best_spring=</span><span class=cF3>NULL</span><span class=cF0>;
|
|
<a name="l209"></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="l210"></a>
|
|
<a name="l211"></a> tmpm=ode->next_mass;
|
|
<a name="l212"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l213"></a> dd=</span><span class=cF5>Sqr</span><span class=cF0>(tmpm->x-x)+</span><span class=cF5>Sqr</span><span class=cF0>(tmpm->y-y)+</span><span class=cF5>Sqr</span><span class=cF0>(tmpm->z-z);
|
|
<a name="l214"></a> </span><span class=cF1>if</span><span class=cF0> (dd<best_dd) {
|
|
<a name="l215"></a> best_dd=dd;
|
|
<a name="l216"></a> best_mass=tmpm;
|
|
<a name="l217"></a> }
|
|
<a name="l218"></a> tmpm=tmpm->next;
|
|
<a name="l219"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l220"></a>
|
|
<a name="l221"></a> tmps=ode->next_spring;
|
|
<a name="l222"></a> </span><span class=cF1>while</span><span class=cF0> (tmps!=&ode->next_spring) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l223"></a> dd=</span><span class=cF5>Sqr</span><span class=cF0>(</span><span class=cF7>(</span><span class=cF0>tmps->end1->x+tmps->end2->x</span><span class=cF7>)</span><span class=cF0>/2-x)+
|
|
<a name="l224"></a> </span><span class=cF5>Sqr</span><span class=cF0>(</span><span class=cF7>(</span><span class=cF0>tmps->end1->y+tmps->end2->y</span><span class=cF7>)</span><span class=cF0>/2-y)+
|
|
<a name="l225"></a> </span><span class=cF5>Sqr</span><span class=cF0>(</span><span class=cF7>(</span><span class=cF0>tmps->end1->z+tmps->end2->z</span><span class=cF7>)</span><span class=cF0>/2-z);
|
|
<a name="l226"></a> </span><span class=cF1>if</span><span class=cF0> (dd<best_dd) {
|
|
<a name="l227"></a> best_dd=dd;
|
|
<a name="l228"></a> best_spring=tmps;
|
|
<a name="l229"></a> best_mass=</span><span class=cF3>NULL</span><span class=cF0>;
|
|
<a name="l230"></a> }
|
|
<a name="l231"></a> tmps=tmps->next;
|
|
<a name="l232"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l233"></a> </span><span class=cF1>if</span><span class=cF0> (res_mass) *res_mass =best_mass;
|
|
<a name="l234"></a> </span><span class=cF1>if</span><span class=cF0> (res_spring) *res_spring=best_spring;
|
|
<a name="l235"></a>}
|
|
<a name="l236"></a>
|
|
<a name="l237"></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="l238"></a>{</span><span class=cF2>//Return mass number N.</span><span class=cF0>
|
|
<a name="l239"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm=ode->next_mass;
|
|
<a name="l240"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l241"></a> </span><span class=cF1>if</span><span class=cF0> (tmpm->num==num)
|
|
<a name="l242"></a> </span><span class=cF1>return</span><span class=cF0> tmpm;
|
|
<a name="l243"></a> tmpm=tmpm->next;
|
|
<a name="l244"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l245"></a> </span><span class=cF1>return</span><span class=cF0> </span><span class=cF3>NULL</span><span class=cF0>;
|
|
<a name="l246"></a>}
|
|
<a name="l247"></a>
|
|
<a name="l248"></a></span><span class=cF1>public</span><span class=cF0> </span><span class=cF1>U0</span><span class=cF0> </span><span class=cF5>ODERstInactive</span><span class=cF0>(</span><span class=cF9>CMathODE</span><span class=cF0> *ode)
|
|
<a name="l249"></a>{</span><span class=cF2>//Set all masses and springs to ACTIVE for new trial.</span><span class=cF0>
|
|
<a name="l250"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
|
|
<a name="l251"></a> </span><span class=cF9>CSpring</span><span class=cF0> *tmps;
|
|
<a name="l252"></a> tmpm=ode->next_mass;
|
|
<a name="l253"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l254"></a> tmpm->flags&=~</span><span class=cF3>MSF_INACTIVE</span><span class=cF0>;
|
|
<a name="l255"></a> tmpm=tmpm->next;
|
|
<a name="l256"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l257"></a> tmps=ode->next_spring;
|
|
<a name="l258"></a> </span><span class=cF1>while</span><span class=cF0> (tmps!=&ode->next_spring) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l259"></a> tmps->flags&=~</span><span class=cF3>SSF_INACTIVE</span><span class=cF0>;
|
|
<a name="l260"></a> tmps=tmps->next;
|
|
<a name="l261"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l262"></a>}
|
|
<a name="l263"></a>
|
|
<a name="l264"></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="l265"></a>{
|
|
<a name="l266"></a> </span><span class=cF9>CSpring</span><span class=cF0> *tmps=ode->next_spring;
|
|
<a name="l267"></a> </span><span class=cF9>CMass</span><span class=cF0> *e1,*e2;
|
|
<a name="l268"></a> </span><span class=cF1>F64</span><span class=cF0> d;
|
|
<a name="l269"></a> </span><span class=cF9>CD3</span><span class=cF0> p;
|
|
<a name="l270"></a> </span><span class=cF1>while</span><span class=cF0> (tmps!=&ode->next_spring) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l271"></a> </span><span class=cF1>if</span><span class=cF0> (tmps->flags&</span><span class=cF3>SSF_INACTIVE</span><span class=cF0>) {
|
|
<a name="l272"></a> tmps->displacement=0;
|
|
<a name="l273"></a> tmps->f=0;
|
|
<a name="l274"></a> } </span><span class=cF1>else</span><span class=cF0> {
|
|
<a name="l275"></a> e1=tmps->end1;
|
|
<a name="l276"></a> e2=tmps->end2;
|
|
<a name="l277"></a> d=</span><span class=cF5>D3Norm</span><span class=cF0>(</span><span class=cF5>D3Sub</span><span class=cF7>(</span><span class=cF0>&p,&e2->state->x,&e1->state->x</span><span class=cF7>)</span><span class=cF0>);
|
|
<a name="l278"></a> tmps->displacement=d-tmps->rest_len;
|
|
<a name="l279"></a> tmps->f=tmps->displacement*tmps->const;
|
|
<a name="l280"></a> </span><span class=cF1>if</span><span class=cF0> (tmps->f>0 && tmps->flags&</span><span class=cF3>SSF_NO_TENSION</span><span class=cF0>)
|
|
<a name="l281"></a> tmps->f=0;
|
|
<a name="l282"></a> </span><span class=cF1>else</span><span class=cF0> </span><span class=cF1>if</span><span class=cF0> (tmps->f<0 && tmps->flags&</span><span class=cF3>SSF_NO_COMPRESSION</span><span class=cF0>)
|
|
<a name="l283"></a> tmps->f=0;
|
|
<a name="l284"></a> </span><span class=cF1>if</span><span class=cF0> (d>0) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l285"></a> </span><span class=cF5>D3MulEqu</span><span class=cF0>(&p,tmps->f/d);
|
|
<a name="l286"></a> </span><span class=cF5>D3AddEqu</span><span class=cF0>(&e1->DstateDt->DxDt,&p);
|
|
<a name="l287"></a> </span><span class=cF5>D3SubEqu</span><span class=cF0>(&e2->DstateDt->DxDt,&p);
|
|
<a name="l288"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l289"></a> }
|
|
<a name="l290"></a> tmps=tmps->next;
|
|
<a name="l291"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l292"></a>}
|
|
<a name="l293"></a>
|
|
<a name="l294"></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="l295"></a>{
|
|
<a name="l296"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
|
|
<a name="l297"></a> </span><span class=cF1>F64</span><span class=cF0> d,dd;
|
|
<a name="l298"></a> </span><span class=cF9>CD3</span><span class=cF0> p;
|
|
<a name="l299"></a> </span><span class=cF1>if</span><span class=cF0> (ode->drag_v || ode->drag_v2 || ode->drag_v3) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l300"></a> tmpm=ode->next_mass;
|
|
<a name="l301"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) {
|
|
<a name="l302"></a> </span><span class=cF1>if</span><span class=cF0> (!</span><span class=cF7>(</span><span class=cF0>tmpm->flags & </span><span class=cF3>MSF_INACTIVE</span><span class=cF7>)</span><span class=cF0> &&
|
|
<a name="l303"></a> tmpm->drag_profile_factor &&
|
|
<a name="l304"></a> </span><span class=cF7>(</span><span class=cF0>dd=</span><span class=cF5>D3NormSqr</span><span class=cF0>(&tmpm->state->DxDt)</span><span class=cF7>)</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l305"></a> d=ode->drag_v;
|
|
<a name="l306"></a> </span><span class=cF1>if</span><span class=cF0> (ode->drag_v2)
|
|
<a name="l307"></a> d+=ode->drag_v2*</span><span class=cF5>Sqrt</span><span class=cF0>(dd);
|
|
<a name="l308"></a> </span><span class=cF1>if</span><span class=cF0> (ode->drag_v3)
|
|
<a name="l309"></a> d+=dd*ode->drag_v3;
|
|
<a name="l310"></a> </span><span class=cF5>D3SubEqu</span><span class=cF0>(&tmpm->DstateDt->DxDt,
|
|
<a name="l311"></a> </span><span class=cF5>D3Mul</span><span class=cF7>(</span><span class=cF0>&p,d*tmpm->drag_profile_factor,&tmpm->state->DxDt</span><span class=cF7>)</span><span class=cF0>);
|
|
<a name="l312"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l313"></a> tmpm=tmpm->next;
|
|
<a name="l314"></a> }
|
|
<a name="l315"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l316"></a>}
|
|
<a name="l317"></a>
|
|
<a name="l318"></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="l319"></a>{
|
|
<a name="l320"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
|
|
<a name="l321"></a> </span><span class=cF1>F64</span><span class=cF0> d;
|
|
<a name="l322"></a> </span><span class=cF1>if</span><span class=cF0> (ode->acceleration_limit) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l323"></a> tmpm=ode->next_mass;
|
|
<a name="l324"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) {
|
|
<a name="l325"></a> </span><span class=cF1>if</span><span class=cF0> (!</span><span class=cF7>(</span><span class=cF0>tmpm->flags & </span><span class=cF3>MSF_INACTIVE</span><span class=cF7>)</span><span class=cF0> &&
|
|
<a name="l326"></a> </span><span class=cF7>(</span><span class=cF0>d=</span><span class=cF5>D3Norm</span><span class=cF0>(&tmpm->DstateDt->DxDt)</span><span class=cF7>)</span><span class=cF0>>ode->acceleration_limit)
|
|
<a name="l327"></a> </span><span class=cF5>D3MulEqu</span><span class=cF0>(&tmpm->DstateDt->DxDt,ode->acceleration_limit/d);
|
|
<a name="l328"></a> tmpm=tmpm->next;
|
|
<a name="l329"></a> }
|
|
<a name="l330"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l331"></a>}
|
|
<a name="l332"></a>
|
|
<a name="l333"></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="l334"></a>{
|
|
<a name="l335"></a> </span><span class=cF1>while</span><span class=cF0> (</span><span class=cF3>TRUE</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l336"></a> </span><span class=cF1>while</span><span class=cF0> (!</span><span class=cF5>Bt</span><span class=cF7>(</span><span class=cF0>&ode->mp_not_done_flags,</span><span class=cF5>Gs</span><span class=cF0>->num</span><span class=cF7>)</span><span class=cF0>)
|
|
<a name="l337"></a> </span><span class=cF5>Yield</span><span class=cF0>;
|
|
<a name="l338"></a> </span><span class=cF1>if</span><span class=cF0> (ode->mp_derive)
|
|
<a name="l339"></a> (*ode->mp_derive)(ode,ode->mp_t,
|
|
<a name="l340"></a> </span><span class=cF5>Gs</span><span class=cF0>->num,ode->mp_state,ode->mp_DstateDt);
|
|
<a name="l341"></a> </span><span class=cF5>LBtr</span><span class=cF0>(&ode->mp_not_done_flags,</span><span class=cF5>Gs</span><span class=cF0>->num);
|
|
<a name="l342"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l343"></a>}
|
|
<a name="l344"></a>
|
|
<a name="l345"></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="l346"></a>{
|
|
<a name="l347"></a> </span><span class=cF9>I64</span><span class=cF0> i;
|
|
<a name="l348"></a> </span><span class=cF1>if</span><span class=cF0> (!ode->slave_tasks) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l349"></a> ode->slave_tasks=</span><span class=cF5>CAlloc</span><span class=cF0>(</span><span class=cFB>mp_cnt</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="l350"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<</span><span class=cFB>mp_cnt</span><span class=cF0>;i++)
|
|
<a name="l351"></a> ode->slave_tasks[i]=</span><span class=cF5>Spawn</span><span class=cF0>(&</span><span class=cF5>ODEMPTask</span><span class=cF0>,ode,</span><span class=cF6>"ODE Slave"</span><span class=cF0>,i);
|
|
<a name="l352"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l353"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<</span><span class=cFB>mp_cnt</span><span class=cF0>;i++) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l354"></a> </span><span class=cF5>Suspend</span><span class=cF0>(ode->slave_tasks[i],</span><span class=cF3>FALSE</span><span class=cF0>);
|
|
<a name="l355"></a> </span><span class=cF5>MPInt</span><span class=cF0>(</span><span class=cF3>I_WAKE</span><span class=cF0>,i);
|
|
<a name="l356"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l357"></a>}
|
|
<a name="l358"></a>
|
|
<a name="l359"></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="l360"></a>{
|
|
<a name="l361"></a> </span><span class=cF9>I64</span><span class=cF0> i;
|
|
<a name="l362"></a> </span><span class=cF1>if</span><span class=cF0> (ode->slave_tasks) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l363"></a> </span><span class=cF1>while</span><span class=cF0> (ode->mp_not_done_flags)
|
|
<a name="l364"></a> </span><span class=cF5>Yield</span><span class=cF0>;
|
|
<a name="l365"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<</span><span class=cFB>mp_cnt</span><span class=cF0>;i++)
|
|
<a name="l366"></a> </span><span class=cF5>Suspend</span><span class=cF0>(ode->slave_tasks[i]);
|
|
<a name="l367"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l368"></a>}
|
|
<a name="l369"></a>
|
|
<a name="l370"></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="l371"></a>{
|
|
<a name="l372"></a> ode->mp_t=t;
|
|
<a name="l373"></a> ode->mp_state=state;
|
|
<a name="l374"></a> ode->mp_DstateDt=DstateDt;
|
|
<a name="l375"></a> ode->mp_not_done_flags=1<<</span><span class=cFB>mp_cnt</span><span class=cF0>-1;
|
|
<a name="l376"></a> </span><span class=cF1>do</span><span class=cF0> </span><span class=cF5>Yield</span><span class=cF0>;
|
|
<a name="l377"></a> </span><span class=cF1>while</span><span class=cF0> (ode->mp_not_done_flags);
|
|
<a name="l378"></a>}
|
|
<a name="l379"></a>
|
|
<a name="l380"></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="l381"></a>{
|
|
<a name="l382"></a> </span><span class=cF9>CMass</span><span class=cF0> *tmpm;
|
|
<a name="l383"></a> </span><span class=cF1>if</span><span class=cF0> (ode->flags&</span><span class=cF3>ODEF_HAS_MASSES</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l384"></a> </span><span class=cF5>ODESetMassesPtrs</span><span class=cF0>(ode,state,DstateDt);
|
|
<a name="l385"></a> tmpm=ode->next_mass;
|
|
<a name="l386"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) {
|
|
<a name="l387"></a> </span><span class=cF1>if</span><span class=cF0> (!</span><span class=cF7>(</span><span class=cF0>tmpm->flags&</span><span class=cF3>MSF_INACTIVE</span><span class=cF7>)</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l388"></a> </span><span class=cF5>D3Zero</span><span class=cF0>(&tmpm->DstateDt->DxDt);
|
|
<a name="l389"></a> </span><span class=cF5>D3Copy</span><span class=cF0>(&tmpm->DstateDt->x,&tmpm->state->DxDt);
|
|
<a name="l390"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l391"></a> tmpm=tmpm->next;
|
|
<a name="l392"></a> }
|
|
<a name="l393"></a> </span><span class=cF5>ODECalcSprings</span><span class=cF0>(ode);
|
|
<a name="l394"></a> </span><span class=cF5>ODECalcDrag</span><span class=cF0>(ode);
|
|
<a name="l395"></a> </span><span class=cF1>if</span><span class=cF0> (ode->mp_derive)
|
|
<a name="l396"></a> </span><span class=cF5>ODECallMPDerivative</span><span class=cF0>(ode,t,state,DstateDt);
|
|
<a name="l397"></a> </span><span class=cF1>if</span><span class=cF0> (ode->derive)
|
|
<a name="l398"></a> (*ode->derive)(ode,t,state,DstateDt);
|
|
<a name="l399"></a> tmpm=ode->next_mass;
|
|
<a name="l400"></a> </span><span class=cF1>while</span><span class=cF0> (tmpm!=&ode->next_mass) {
|
|
<a name="l401"></a> </span><span class=cF1>if</span><span class=cF0> (!</span><span class=cF7>(</span><span class=cF0>tmpm->flags&</span><span class=cF3>MSF_INACTIVE</span><span class=cF7>)</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l402"></a> </span><span class=cF1>if</span><span class=cF0> (tmpm->flags&</span><span class=cF3>MSF_FIXED</span><span class=cF0>) {
|
|
<a name="l403"></a> </span><span class=cF5>D3Zero</span><span class=cF0>(&tmpm->DstateDt->DxDt);
|
|
<a name="l404"></a> </span><span class=cF5>D3Zero</span><span class=cF0>(&tmpm->DstateDt->x);
|
|
<a name="l405"></a> } </span><span class=cF1>else</span><span class=cF0> </span><span class=cF1>if</span><span class=cF0> (tmpm->mass)
|
|
<a name="l406"></a> </span><span class=cF5>D3DivEqu</span><span class=cF0>(&tmpm->DstateDt->DxDt,tmpm->mass);
|
|
<a name="l407"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l408"></a> tmpm=tmpm->next;
|
|
<a name="l409"></a> }
|
|
<a name="l410"></a> </span><span class=cF5>ODEApplyAccelerationLimit</span><span class=cF0>(ode);
|
|
<a name="l411"></a> </span><span class=cF7>}</span><span class=cF0> </span><span class=cF1>else</span><span class=cF0> </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l412"></a> </span><span class=cF1>if</span><span class=cF0> (ode->mp_derive)
|
|
<a name="l413"></a> </span><span class=cF5>ODECallMPDerivative</span><span class=cF0>(ode,t,state,DstateDt);
|
|
<a name="l414"></a> </span><span class=cF1>if</span><span class=cF0> (ode->derive)
|
|
<a name="l415"></a> (*ode->derive)(ode,t,state,DstateDt);
|
|
<a name="l416"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l417"></a>}
|
|
<a name="l418"></a>
|
|
<a name="l419"></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="l420"></a>{
|
|
<a name="l421"></a> </span><span class=cF9>I64</span><span class=cF0> i;
|
|
<a name="l422"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
<a name="l423"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<ode->n_internal;i++)
|
|
<a name="l424"></a> ode->state_internal[i]+=ode->h*ode->DstateDt[i];
|
|
<a name="l425"></a> ode->t+=ode->h;
|
|
<a name="l426"></a>}
|
|
<a name="l427"></a>
|
|
<a name="l428"></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="l429"></a>{
|
|
<a name="l430"></a> </span><span class=cF9>I64</span><span class=cF0> i,n=ode->n_internal;
|
|
<a name="l431"></a> </span><span class=cF1>F64</span><span class=cF0> xh,hh,h6,*dym,*dyt,*yt,*DstateDt;
|
|
<a name="l432"></a>
|
|
<a name="l433"></a> dym =ode->tmp0;
|
|
<a name="l434"></a> dyt =ode->tmp1;
|
|
<a name="l435"></a> yt =ode->tmp2;
|
|
<a name="l436"></a> DstateDt=ode->tmp3;
|
|
<a name="l437"></a> hh =0.5*ode->h;
|
|
<a name="l438"></a> h6 =ode->h / 6.0;
|
|
<a name="l439"></a> xh =ode->t + hh;
|
|
<a name="l440"></a>
|
|
<a name="l441"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
<a name="l442"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l443"></a> yt[i]=ode->state_internal[i]+hh*DstateDt[i];
|
|
<a name="l444"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,xh,yt,dyt);
|
|
<a name="l445"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l446"></a> yt[i]=ode->state_internal[i]+hh*dyt[i];
|
|
<a name="l447"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,xh,yt,dym);
|
|
<a name="l448"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l449"></a> yt[i]=ode->state_internal[i]+ode->h*dym[i];
|
|
<a name="l450"></a> dym[i]+=dyt[i];
|
|
<a name="l451"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l452"></a> ode->t+=ode->h;
|
|
<a name="l453"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t,yt,dyt);
|
|
<a name="l454"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l455"></a> ode->state_internal[i]+=h6*(DstateDt[i]+dyt[i]+2.0*dym[i]);
|
|
<a name="l456"></a>}
|
|
<a name="l457"></a>
|
|
<a name="l458"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEa2</span><span class=cF0> 0.2
|
|
<a name="l459"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEa3</span><span class=cF0> 0.3
|
|
<a name="l460"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEa4</span><span class=cF0> 0.6
|
|
<a name="l461"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEa5</span><span class=cF0> 1.0
|
|
<a name="l462"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEa6</span><span class=cF0> 0.875
|
|
<a name="l463"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb21</span><span class=cF0> 0.2
|
|
<a name="l464"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb31</span><span class=cF0> (3.0/40.0)
|
|
<a name="l465"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb32</span><span class=cF0> (9.0/40.0)
|
|
<a name="l466"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb41</span><span class=cF0> 0.3
|
|
<a name="l467"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb42</span><span class=cF0> (-0.9)
|
|
<a name="l468"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb43</span><span class=cF0> 1.2
|
|
<a name="l469"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb51</span><span class=cF0> (-11.0/54.0)
|
|
<a name="l470"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb52</span><span class=cF0> 2.5
|
|
<a name="l471"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb53</span><span class=cF0> (-70.0/27.0)
|
|
<a name="l472"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb54</span><span class=cF0> (35.0/27.0)
|
|
<a name="l473"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb61</span><span class=cF0> (1631.0/55296.0)
|
|
<a name="l474"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb62</span><span class=cF0> (175.0/512.0)
|
|
<a name="l475"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb63</span><span class=cF0> (575.0/13824.0)
|
|
<a name="l476"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb64</span><span class=cF0> (44275.0/110592.0)
|
|
<a name="l477"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEb65</span><span class=cF0> (253.0/4096.0)
|
|
<a name="l478"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEc1</span><span class=cF0> (37.0/378.0)
|
|
<a name="l479"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEc3</span><span class=cF0> (250.0/621.0)
|
|
<a name="l480"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEc4</span><span class=cF0> (125.0/594.0)
|
|
<a name="l481"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEc6</span><span class=cF0> (512.0/1771.0)
|
|
<a name="l482"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEdc1</span><span class=cF0> (37.0/378.0-2825.0/27648.0)
|
|
<a name="l483"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEdc3</span><span class=cF0> (250.0/621.0-18575.0/48384.0)
|
|
<a name="l484"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEdc4</span><span class=cF0> (125.0/594.0-13525.0/55296.0)
|
|
<a name="l485"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEdc5</span><span class=cF0> (-277.0/14336.0)
|
|
<a name="l486"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ODEdc6</span><span class=cF0> (512.0/1771.0-0.25)
|
|
<a name="l487"></a>
|
|
<a name="l488"></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="l489"></a>{
|
|
<a name="l490"></a> </span><span class=cF9>I64</span><span class=cF0> i,n=ode->n_internal;
|
|
<a name="l491"></a> </span><span class=cF1>F64</span><span class=cF0> h=ode->h,*state=ode->state_internal,
|
|
<a name="l492"></a> *DstateDt=ode->DstateDt,*ak2,*ak3,*ak4,*ak5,*ak6,
|
|
<a name="l493"></a> *tmpstate,*stateerr,*outstate;
|
|
<a name="l494"></a>
|
|
<a name="l495"></a> ak2=ode->tmp0;
|
|
<a name="l496"></a> ak3=ode->tmp1;
|
|
<a name="l497"></a> ak4=ode->tmp2;
|
|
<a name="l498"></a> ak5=ode->tmp3;
|
|
<a name="l499"></a> ak6=ode->tmp4;
|
|
<a name="l500"></a> tmpstate=ode->tmp5;
|
|
<a name="l501"></a> outstate=ode->tmp6;
|
|
<a name="l502"></a> stateerr=ode->tmp7;
|
|
<a name="l503"></a>
|
|
<a name="l504"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l505"></a> tmpstate[i]=state[i]+</span><span class=cF3>ODEb21</span><span class=cF0>*h*DstateDt[i];
|
|
<a name="l506"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t+</span><span class=cF3>ODEa2</span><span class=cF0>*h,tmpstate,ak2);
|
|
<a name="l507"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l508"></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="l509"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t+</span><span class=cF3>ODEa3</span><span class=cF0>*h,tmpstate,ak3);
|
|
<a name="l510"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l511"></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="l512"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t+</span><span class=cF3>ODEa4</span><span class=cF0>*h,tmpstate,ak4);
|
|
<a name="l513"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l514"></a> tmpstate[i]=state[i]+h*(</span><span class=cF3>ODEb51</span><span class=cF0>*DstateDt[i]+
|
|
<a name="l515"></a> </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="l516"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t+</span><span class=cF3>ODEa5</span><span class=cF0>*h,tmpstate,ak5);
|
|
<a name="l517"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l518"></a> tmpstate[i]=state[i]+h*(</span><span class=cF3>ODEb61</span><span class=cF0>*DstateDt[i]+
|
|
<a name="l519"></a> </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="l520"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t+</span><span class=cF3>ODEa6</span><span class=cF0>*h,tmpstate,ak6);
|
|
<a name="l521"></a>
|
|
<a name="l522"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l523"></a> outstate[i]=state[i]+h*(</span><span class=cF3>ODEc1</span><span class=cF0>*DstateDt[i]+
|
|
<a name="l524"></a> </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="l525"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<n;i++)
|
|
<a name="l526"></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]+
|
|
<a name="l527"></a> </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="l528"></a>}
|
|
<a name="l529"></a>
|
|
<a name="l530"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>SAFETY</span><span class=cF0> 0.9
|
|
<a name="l531"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>PGROW</span><span class=cF0> (-0.2)
|
|
<a name="l532"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>PSHRNK</span><span class=cF0> (-0.25)
|
|
<a name="l533"></a>#</span><span class=cF1>define</span><span class=cF0> </span><span class=cF3>ERRCON</span><span class=cF0> 1.89e-4
|
|
<a name="l534"></a>
|
|
<a name="l535"></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="l536"></a>{
|
|
<a name="l537"></a> </span><span class=cF9>I64</span><span class=cF0> i;
|
|
<a name="l538"></a> </span><span class=cF1>F64</span><span class=cF0> errmax,tmp,*tmpstate=ode->tmp6,*stateerr=ode->tmp7;
|
|
<a name="l539"></a> </span><span class=cF1>while</span><span class=cF0> (</span><span class=cF3>TRUE</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l540"></a> ode->h=</span><span class=cF5>Clamp</span><span class=cF0>(ode->h,ode->h_min,ode->h_max);
|
|
<a name="l541"></a> </span><span class=cF5>ODECashKarp</span><span class=cF0>(ode);
|
|
<a name="l542"></a> errmax=0.0;
|
|
<a name="l543"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<ode->n_internal;i++) {
|
|
<a name="l544"></a> tmp=</span><span class=cF5>Abs</span><span class=cF0>(stateerr[i]/ode->state_scale[i]);
|
|
<a name="l545"></a> </span><span class=cF1>if</span><span class=cF0> (tmp>errmax)
|
|
<a name="l546"></a> errmax=tmp;
|
|
<a name="l547"></a> }
|
|
<a name="l548"></a> errmax/=ode->tolerance_internal;
|
|
<a name="l549"></a> </span><span class=cF1>if</span><span class=cF0> (errmax<=1.0 || ode->h==ode->h_min) </span><span class=cF1>break</span><span class=cF0>;
|
|
<a name="l550"></a> tmp=ode->h*</span><span class=cF3>SAFETY</span><span class=cF0>*errmax`</span><span class=cF3>PSHRNK</span><span class=cF0>;
|
|
<a name="l551"></a> </span><span class=cF1>if</span><span class=cF0> (tmp<0.1*ode->h)
|
|
<a name="l552"></a> ode->h*=0.1;
|
|
<a name="l553"></a> </span><span class=cF1>else</span><span class=cF0>
|
|
<a name="l554"></a> ode->h=tmp;
|
|
<a name="l555"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l556"></a> ode->t+=ode->h;
|
|
<a name="l557"></a> </span><span class=cF1>if</span><span class=cF0> (errmax></span><span class=cF3>ERRCON</span><span class=cF0>)
|
|
<a name="l558"></a> ode->h*=</span><span class=cF3>SAFETY</span><span class=cF0>*errmax`</span><span class=cF3>PGROW</span><span class=cF0>;
|
|
<a name="l559"></a> </span><span class=cF1>else</span><span class=cF0>
|
|
<a name="l560"></a> ode->h*=5.0;
|
|
<a name="l561"></a> ode->h=</span><span class=cF5>Clamp</span><span class=cF0>(ode->h,ode->h_min,ode->h_max);
|
|
<a name="l562"></a> </span><span class=cF5>MemCpy</span><span class=cF0>(ode->state_internal,tmpstate,</span><span class=cF1>sizeof</span><span class=cF7>(</span><span class=cF1>F64</span><span class=cF7>)</span><span class=cF0>*ode->n_internal);
|
|
<a name="l563"></a>}
|
|
<a name="l564"></a>
|
|
<a name="l565"></a></span><span class=cF1>F64</span><span class=cF0> </span><span class=cFB>ode_alloced_factor</span><span class=cF0>=0.75;
|
|
<a name="l566"></a>
|
|
<a name="l567"></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="l568"></a>{</span><span class=cF2>/* This routine is called by the </span><a href="/Wb/Adam/Gr/GrScrn.HC.HTML#l7"><span class=cF4>window mgr</span></a><span class=cF2>on a continuous</span><span class=cF0>
|
|
<a name="l569"></a></span><span class=cF2>basis to allow real-time simulation. It is intended</span><span class=cF0>
|
|
<a name="l570"></a></span><span class=cF2>to provide ress good enough for games. It uses a runge-kutta</span><span class=cF0>
|
|
<a name="l571"></a></span><span class=cF2>integrator which is a better algorithm than doing it with Euler.</span><span class=cF0>
|
|
<a name="l572"></a>
|
|
<a name="l573"></a></span><span class=cF2>It is adaptive step-sized, so it slows down when an important</span><span class=cF0>
|
|
<a name="l574"></a></span><span class=cF2>event is taking place to improve accuracy, but in my implementation</span><span class=cF0>
|
|
<a name="l575"></a></span><span class=cF2>it has a timeout.</span><span class=cF0>
|
|
<a name="l576"></a></span><span class=cF2>*/</span><span class=cF0>
|
|
<a name="l577"></a> </span><span class=cF9>I64</span><span class=cF0> i;
|
|
<a name="l578"></a> </span><span class=cF1>F64</span><span class=cF0> d,start_time,timeout_time,t_desired,t_initial,interpolation;
|
|
<a name="l579"></a> </span><span class=cF9>CMathODE</span><span class=cF0> *ode;
|
|
<a name="l580"></a>
|
|
<a name="l581"></a> </span><span class=cF1>if</span><span class=cF0> (task->next_ode==&task->next_ode)
|
|
<a name="l582"></a> task->last_ode_time=0;
|
|
<a name="l583"></a> </span><span class=cF1>else</span><span class=cF0> </span><span class=cF1>if</span><span class=cF0> (!</span><span class=cF5>Bt</span><span class=cF7>(</span><span class=cF0>&task->win_inhibit,</span><span class=cF3>WIf_SELF_ODE</span><span class=cF7>)</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l584"></a></span><span class=cF2>//See </span><a href="/Wb/Adam/Gr/GrScrn.HC.HTML#l58"><span class=cF4>GrUpdateTasks</span></a><span class=cF2>() and </span><a href="/Wb/Adam/Gr/GrScrn.HC.HTML#l3"><span class=cF4>GrUpdateTaskODEs</span></a><span class=cF2>().</span><span class=cF0>
|
|
<a name="l585"></a> </span><span class=cF2>//We will not pick a time limit based on</span><span class=cF0>
|
|
<a name="l586"></a> </span><span class=cF2>//how busy the CPU is, what percent of the</span><span class=cF0>
|
|
<a name="l587"></a> </span><span class=cF2>//last refresh cycle was spent on ODE's</span><span class=cF0>
|
|
<a name="l588"></a> </span><span class=cF2>//and what the refresh cycle rate was.</span><span class=cF0>
|
|
<a name="l589"></a> start_time=</span><span class=cF5>tS</span><span class=cF0>;
|
|
<a name="l590"></a> d=1.0/</span><span class=cFB>winmgr</span><span class=cF0>.fps;
|
|
<a name="l591"></a> timeout_time=start_time+
|
|
<a name="l592"></a> (task->last_ode_time/d+0.1)/(</span><span class=cFB>winmgr</span><span class=cF0>.last_ode_time/d+0.1)*
|
|
<a name="l593"></a> </span><span class=cFB>ode_alloced_factor</span><span class=cF0>*d;
|
|
<a name="l594"></a> ode=task->next_ode;
|
|
<a name="l595"></a> </span><span class=cF1>while</span><span class=cF0> (ode!=&task->next_ode) {
|
|
<a name="l596"></a> t_initial=ode->t;
|
|
<a name="l597"></a> d=</span><span class=cF5>tS</span><span class=cF0>;
|
|
<a name="l598"></a> </span><span class=cF1>if</span><span class=cF0> (!</span><span class=cF7>(</span><span class=cF0>ode->flags&</span><span class=cF3>ODEF_STARTED</span><span class=cF7>)</span><span class=cF0>) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l599"></a> ode->base_t=d;
|
|
<a name="l600"></a> ode->flags|=</span><span class=cF3>ODEF_STARTED</span><span class=cF0>;
|
|
<a name="l601"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l602"></a> d-=ode->base_t+t_initial;
|
|
<a name="l603"></a> t_desired=ode->t_scale*d+t_initial;
|
|
<a name="l604"></a> </span><span class=cF1>if</span><span class=cF0> (ode->flags&</span><span class=cF3>ODEF_PAUSED</span><span class=cF0>)
|
|
<a name="l605"></a> ode->base_t+=t_desired-ode->t; </span><span class=cF2>//Slip</span><span class=cF0>
|
|
<a name="l606"></a> </span><span class=cF1>else</span><span class=cF0> </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l607"></a> ode->flags|=</span><span class=cF3>ODEF_BUSY</span><span class=cF0>;
|
|
<a name="l608"></a> </span><span class=cF1>if</span><span class=cF0> (ode->flags&</span><span class=cF3>ODEF_PAUSED</span><span class=cF0>)
|
|
<a name="l609"></a> ode->base_t+=t_desired-ode->t; </span><span class=cF2>//Slip</span><span class=cF0>
|
|
<a name="l610"></a> </span><span class=cF1>else</span><span class=cF0> {
|
|
<a name="l611"></a> </span><span class=cF1>if</span><span class=cF0> (ode->derive || ode->mp_derive) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l612"></a> </span><span class=cF1>if</span><span class=cF0> (ode->mp_derive)
|
|
<a name="l613"></a> </span><span class=cF5>ODEMPWake</span><span class=cF0>(ode);
|
|
<a name="l614"></a> </span><span class=cF5>ODEState2Internal</span><span class=cF0>(ode);
|
|
<a name="l615"></a> </span><span class=cF5>MemCpy</span><span class=cF0>(ode->initial_state,ode->state_internal,
|
|
<a name="l616"></a> ode->n_internal*</span><span class=cF1>sizeof</span><span class=cF7>(</span><span class=cF1>F64</span><span class=cF7>)</span><span class=cF0>);
|
|
<a name="l617"></a> </span><span class=cF1>while</span><span class=cF0> (ode->t<t_desired) {
|
|
<a name="l618"></a> ode->h_max=t_desired-ode->t;
|
|
<a name="l619"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
<a name="l620"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<ode->n_internal;i++)
|
|
<a name="l621"></a> ode->state_scale[i]=</span><span class=cF5>Abs</span><span class=cF0>(ode->state_internal[i])+
|
|
<a name="l622"></a> </span><span class=cF5>Abs</span><span class=cF0>(ode->DstateDt[i]*ode->h)+ode->tolerance_internal;
|
|
<a name="l623"></a> </span><span class=cF5>ODERK5OneStep</span><span class=cF0>(ode);
|
|
<a name="l624"></a> </span><span class=cF1>if</span><span class=cF0> (</span><span class=cF5>tS</span><span class=cF0>>timeout_time) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l625"></a> ode->base_t+=t_desired-ode->t; </span><span class=cF2>//Slip</span><span class=cF0>
|
|
<a name="l626"></a> </span><span class=cF1>goto</span><span class=cF0> ode_done;
|
|
<a name="l627"></a>
|
|
<a name="l628"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l629"></a> }
|
|
<a name="l630"></a>
|
|
<a name="l631"></a> </span><span class=cF2>//Interpolate if end time was not exact.</span><span class=cF0>
|
|
<a name="l632"></a> </span><span class=cF1>if</span><span class=cF0> (ode->t!=t_desired) {
|
|
<a name="l633"></a> </span><span class=cF1>if</span><span class=cF0> (interpolation=ode->t-t_initial) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l634"></a> interpolation=(t_desired-t_initial)/interpolation;
|
|
<a name="l635"></a> </span><span class=cF1>if</span><span class=cF0> (interpolation!=1.0)
|
|
<a name="l636"></a> </span><span class=cF1>for</span><span class=cF0> (i=0;i<ode->n_internal;i++)
|
|
<a name="l637"></a> ode->state_internal[i]=(ode->state_internal[i]-
|
|
<a name="l638"></a> ode->initial_state[i])*interpolation+
|
|
<a name="l639"></a> ode->initial_state[i];
|
|
<a name="l640"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l641"></a> ode->t=t_desired;
|
|
<a name="l642"></a> }
|
|
<a name="l643"></a>ode_done:
|
|
<a name="l644"></a> </span><span class=cF5>ODEInternal2State</span><span class=cF0>(ode);
|
|
<a name="l645"></a>
|
|
<a name="l646"></a> </span><span class=cF2>//Convenience call to set vals</span><span class=cF0>
|
|
<a name="l647"></a> </span><span class=cF5>ODECallDerivative</span><span class=cF0>(ode,ode->t,ode->state_internal,ode->DstateDt);
|
|
<a name="l648"></a>
|
|
<a name="l649"></a> </span><span class=cF1>if</span><span class=cF0> (ode->mp_derive)
|
|
<a name="l650"></a> </span><span class=cF5>ODEMPSleep</span><span class=cF0>(ode);
|
|
<a name="l651"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l652"></a> }
|
|
<a name="l653"></a> ode->flags&=~</span><span class=cF3>ODEF_BUSY</span><span class=cF0>;
|
|
<a name="l654"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l655"></a> ode->base_t+=(1.0-ode->t_scale)*d;
|
|
<a name="l656"></a> ode=ode->next;
|
|
<a name="l657"></a> }
|
|
<a name="l658"></a>
|
|
<a name="l659"></a> </span><span class=cF2>//Now, we will dynamically adjust tolerances.</span><span class=cF0>
|
|
<a name="l660"></a>
|
|
<a name="l661"></a> </span><span class=cF2>//We will regulate the tolerances</span><span class=cF0>
|
|
<a name="l662"></a> </span><span class=cF2>//to fill the time we decided was</span><span class=cF0>
|
|
<a name="l663"></a> </span><span class=cF2>//okay to devote to ODE's.</span><span class=cF0>
|
|
<a name="l664"></a> </span><span class=cF2>//Since we might have multiple ODE's</span><span class=cF0>
|
|
<a name="l665"></a> </span><span class=cF2>//active we scale them by the same factor.</span><span class=cF0>
|
|
<a name="l666"></a>
|
|
<a name="l667"></a> </span><span class=cF2>//This algorithm is probably not stable or very good, but it's something.</span><span class=cF0>
|
|
<a name="l668"></a>
|
|
<a name="l669"></a> </span><span class=cF2>//Target is 75% of alloced time.</span><span class=cF0>
|
|
<a name="l670"></a> d=(</span><span class=cF5>tS</span><span class=cF0>-start_time)/(timeout_time-start_time)-0.75;
|
|
<a name="l671"></a>
|
|
<a name="l672"></a> ode=task->next_ode;
|
|
<a name="l673"></a> </span><span class=cF1>while</span><span class=cF0> (ode!=&task->next_ode) {
|
|
<a name="l674"></a> </span><span class=cF1>if</span><span class=cF0> (!</span><span class=cF7>(</span><span class=cF0>ode->flags&</span><span class=cF3>ODEF_PAUSED</span><span class=cF7>)</span><span class=cF0> && ode->derive) </span><span class=cF7>{</span><span class=cF0>
|
|
<a name="l675"></a> </span><span class=cF1>if</span><span class=cF0> (ode->min_tolerance!=ode->max_tolerance) {
|
|
<a name="l676"></a> </span><span class=cF1>if</span><span class=cF0> (d>0)
|
|
<a name="l677"></a> ode->tolerance_internal*=10.0`d;
|
|
<a name="l678"></a> </span><span class=cF1>else</span><span class=cF0>
|
|
<a name="l679"></a> ode->tolerance_internal*=2.0`d;
|
|
<a name="l680"></a> }
|
|
<a name="l681"></a> ode->tolerance_internal=</span><span class=cF5>Clamp</span><span class=cF0>(ode->tolerance_internal,
|
|
<a name="l682"></a> ode->min_tolerance,ode->max_tolerance);
|
|
<a name="l683"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l684"></a> ode=ode->next;
|
|
<a name="l685"></a> }
|
|
<a name="l686"></a> </span><span class=cFB>winmgr</span><span class=cF0>.ode_time+=task->last_ode_time=</span><span class=cF5>tS</span><span class=cF0>-start_time;
|
|
<a name="l687"></a> </span><span class=cF7>}</span><span class=cF0>
|
|
<a name="l688"></a>}
|
|
</span></pre></body>
|
|
</html>
|