loom

1
import "math" m;
2

3
float Vc;
4
float Knx;
5
float Hz;
6
float Kdh;
7
float Kny;
8
float Knz;
9
float g;
10
float Zz;
11
float Xz;
12

13
float nxt;
14
float dH;
15
float Vyz;
16
float nyt;
17
float nzt;
18

19
float x;
20
float y;
21
float z;
22
float Vx;
23
float Vy;
24
float Vz;
25

26
float __dt_x;
27
float __dt_y;
28
float __dt_z;
29
float __dt_Vx;
30
float __dt_Vy;
31
float __dt_Vz;
32

33
func __EQU[*]()
34
{
35
    nxt = Knx * (Vc - m.sqrt(Vx^2 + Vy^2 + Vz^2));
36
    dH = Hz - y;
37
    Vyz = Kdh * dH;
38
    Vyz = (Vyz > 25.0) ? 25.0 : ((Vyz < -15.0) ? -15.0 : Vyz);
39
    nyt = 1 + Kny * (Vyz - Vy);
40
    nzt = Knz * (Vx * (Zz - z) - Vz * (Xz - x));
41
    nzt = (nzt > 5.0) ? 5.0 : ((nzt < -5.0) ? -5.0 : nzt);
42
}
43

44
func __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