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

#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