二重振り子RK4シミュonHSP(修正版1) | Codetter（こーどったー）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
#packopt name "expt"
*mkwnds
    #define wsizex 800
    #define wsizey 600
    screen 0, wsizex,wsizey, 0
*init
    #define dt  0.01    ;[s] タイムステップ
    #const double hdt dt/2  ;[s] dt/2
    #define g   9.8 ;[m/s^2]
    #define σ   1.0 ;[kg/m^2] 錘の面密度
    #define l1  1.5 ;[m]
    #define l2  2.0 ;[m]
    #define r1  0.5 ;[m] 錘1の半径
    #define r2  0.3 ;[m] 〃2
    #const double m1    4.0*m_pi*σ*r1*r1
    #const double m2    4.0*m_pi*σ*r2*r2
        //#const double η       (m1+m2)/m2  ;コンパイルエラー
        //↓
        η = (m1+m2)/m2
    θ1 = deg2rad(180)
    θ2 = deg2rad(135)
    ω1 = 0.0
    ω2 = 0.0
    
    #define mtr2px  100.0   ;[px/meter] メートル→ピクセル 変換係数
    #const double wOx   wsizex/2    ;[px] 物理系原点のウィンドウ上でのx座標
    #const double wOy   200.0
    #const double l1w   l1*mtr2px   ;[px] l1のウィンドウ上での長さ
    #const double l2w   l2*mtr2px   ;[px]
    #const double r1w   r1*mtr2px   ;[px]
    #const double r2w   r2*mtr2px   ;[px]
    #define frmIntvl    16  ;[ms] フレーム間隔
    #define mlfreq      2   ;[ms] メインループ周期
    
*main
    etls = 0.0  ;[s] 前回ステップ計算からの経過時間
    etlf = 0.0  ;[ms] 前回画面更新からの経過時間
    repeat
        if etls >= dt : gosub *step : etls = -0.001*mlfreq
        if etlf >= frmIntvl : gosub *draw : etlf = -mlfreq
        await mlfreq
        etls += 0.001*mlfreq
        etlf += mlfreq
    loop
    
*step   //dt後の状態を計算
    ξn = θ1,θ2,ω1,ω2 : k1 = f1(ξn),f2(ξn),f3(ξn),f4(ξn)
    ξn2 = ξn+hdt*k1, ξn(1)+hdt*k1(1), ξn(2)+hdt*k1(2), ξn(3)+hdt*k1(3) : k2 = f1(ξn2),f2(ξn2),f3(ξn2),f4(ξn2)
    ξn3 = ξn+hdt*k2, ξn(1)+hdt*k2(1), ξn(2)+hdt*k2(2), ξn(3)+hdt*k2(3) : k3 = f1(ξn3),f2(ξn3),f3(ξn3),f4(ξn3)
    ξn4 = ξn+dt*k3, ξn(1)+dt*k3(1), ξn(2)+dt*k3(2), ξn(3)+dt*k3(3) : k4 = f1(ξn4),f2(ξn4),f3(ξn4),f4(ξn4)
    θ1 += dt/6*(k1+k2+k3+k4)
    θ2 += dt/6*(k1(1)+k2(1)+k3(1)+k4(1))
    ω1 += dt/6*(k1(2)+k2(2)+k3(2)+k4(2))
    ω2 += dt/6*(k1(3)+k2(3)+k3(3)+k4(3))
    return
#module func
    #define g   g@
    #define l1  l1@
    #define l2  l2@
    #define η   η@
    
    #defcfunc f1 array ξ
        return ξ(2)
    
    #defcfunc f2 array ξ
        return ξ(3)
    #defcfunc f3 array ξ
        θ1 = ξ : θ2 = ξ(1) : ω1 = ξ(2) : ω2 = ξ(3)
        φ = θ1-θ2 : cosφ = cos(φ)
        return (g*(cosφ*sin(θ2)-η*sin(θ1))-(l1*ω1*ω1*cosφ+l2*ω2*ω2)*sin(φ))/l1/(η-cosφ*cosφ)
    
    #defcfunc f4 array ξ
        θ1 = ξ : θ2 = ξ(1) : ω1 = ξ(2) : ω2 = ξ(3)
        φ = ξ-ξ(1) : cosφ = cos(φ)
        return (g*η*(cosφ*sin(θ1)-sin(θ2))+(η*l1*ω1*ω1+l2*ω2*ω2*cosφ)*sin(φ))/l2/(η-cosφ*cosφ)
#global
*draw   //描画
    redraw 0
        color : boxf
        color 100,100,100
        line -1,wOy, wsizex,wOy : line wOx,-1, wOx,wsizey
        color 255,255,255
        xtmp = wOx+l1w*sin(θ1) : ytmp = wOy+l1w*cos(θ1)
        line wOx,wOy, xtmp,ytmp : circle xtmp-r1w,ytmp-r1w, xtmp+r1w,ytmp+r1w, 0
        xtmp2 = xtmp + l2w*sin(θ2) : ytmp2 = ytmp + l2w*cos(θ2)
        line xtmp,ytmp,xtmp2,ytmp2 : circle xtmp2-r2w,ytmp2-r2w, xtmp2+r2w,ytmp2+r2w, 0
    redraw 1
    return
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX