loom
1import "math" m;
2
3float Vc;
4float Knx;
5float Hz;
6float Kdh;
7float Kny;
8float Knz;
9float g;
10float Zz;
11float Xz;
12
13float nxt;
14float dH;
15float Vyz;
16float nyt;
17float nzt;
18
19float x;
20float y;
21float z;
22float Vx;
23float Vy;
24float Vz;
25
26float __dt_x;
27float __dt_y;
28float __dt_z;
29float __dt_Vx;
30float __dt_Vy;
31float __dt_Vz;
32
33func __EQU[*]()
34{
35nxt = Knx * (Vc - m.sqrt(Vx^2 + Vy^2 + Vz^2));
36dH = Hz - y;
37Vyz = Kdh * dH;
38Vyz = (Vyz > 25.0) ? 25.0 : ((Vyz < -15.0) ? -15.0 : Vyz);
39nyt = 1 + Kny * (Vyz - Vy);
40nzt = Knz * (Vx * (Zz - z) - Vz * (Xz - x));
41nzt = (nzt > 5.0) ? 5.0 : ((nzt < -5.0) ? -5.0 : nzt);
42}
43
44func __ODE[*]()
45{
46__dt_x = Vx;
47__dt_y = Vy;
48__dt_z = Vz;
49__dt_Vx = (g/m.sqrt(Vx^2+Vy^2+Vz^2))*Vx*(nxt-Vy*nyt/m.sqrt(Vx^2+Vz^2))-g*Vz*nzt/m.sqrt(Vx^2+Vz^2);
50__dt_Vy = (g/m.sqrt(Vx^2+Vy^2+Vz^2))*Vy*nxt+(g/m.sqrt(Vx^2+Vy^2+Vz^2))*nyt*m.sqrt(Vx^2+Vz^2)-g;
51__dt_Vz = (g/m.sqrt(Vx^2+Vy^2+Vz^2))*Vz*(nxt-Vy*nyt/m.sqrt(Vx^2+Vz^2))+g*Vx*nzt/m.sqrt(Vx^2+Vz^2);
52}
53
54