物理のタグがつけられたコード一覧

HSP 二重振り子RK4シミュonHSP(修正版2)

#include "hsp3dish.as"
#packopt name "DPRK4"

/* モジュール認識 */
modDetect@mod_simuCfg
modDetect@mod_simu

/* 定数宣言 */
//#define hsproom
#define wsx	640 //("windowSizex") ウィンドウxサイズ
#define wsy	640
#packopt xsize wsx
#packopt ysize wsy
#define frmIntvl	16	//[ms] ("frameInterval") フレーム間隔
#define slfreq		2	//[ms] ("simulationLoopFrequency") シミュレーションループ周期
#define mtr2px	100.0	//[px/meter] ("meterToPixel") メートル→ピクセル 変換係数
#const Oxw	wsx/2	//[px] ("OriginxOnWindow") 物理系原点のウィンドウ上でのx座標
#const Oyw	wsy/2	//[px]
#define dt	0.01	//[s] タイムステップ
#const double hdt dt/2	//[s] ("half dt") dt/2
#define g	9.8	//[m/s^2]
#define σ	1.0	//[kg/m^2] 錘の面密度
#define mmin	0.1	//[kg] 質量の下限
#define mmax	25.0	//[kg] 〃上限

/* 変数初期化 */
	/* エイリアス */
	#define m1	m(0)
	#define m2	m(1)
	#define r1	r(0)
	#define r2	r(1)
	#define l1	l(0)
	#define l2	l(1)
	#define θ1	θ(0)
	#define θ2	θ(1)
	#define r1w	rw(0)
	#define r2w	rw(1)
	#define l1w	lw(0)
	#define l2w	lw(1)
	#define x1	x
	#define x2	x(1)
	#define y1	y
	#define y2	y(1)
	/* 物理系 */
	m = 1.5,1.0	//[kg] m1,m2
	r = sqrt(m1/σ/4/m_pi),sqrt(m2/σ/4/m_pi) //[m] おもり1,2の半径
	l = 1.5,1.0	//[m] l1,l2
	θ = deg2rad(45),deg2rad(90)	//[rad] θ1,θ2
	x = l1*sin(θ1),x+l2*sin(θ2)
	y = l1*cos(θ1),y+l2*cos(θ2)
	/* システム */
	ddim PRESET,5,6

/* 起動 */
*boot
	title "二重振り子シミュレーター"
	exist "save/preset.dat" : if strsize = -1 : initPreset	//プリセットデータが無ければ初期化する
	goto *boot@mod_simuCfg

#module mod_simuCfg	/* シミュレーション設定 */
	#define wsx			wsx@
	#define wsy			wsy@
	#define frmIntvl	frmIntvl@*3
	#define mtr2px		mtr2px@
	#define Oxw			Oxw@
	#define Oyw			Oyw@
	//#define mmin	mmin@
	//#define mmax	mmax@
	#define m		m@
	#define m1		m@
	#define m2		m@(1)
	#define l1		l@
	#define l2		l@(1)
	#define θ1		θ@
	#define θ2		θ@(1)
	#define x1		x@
	#define x2		x@(1)
	#define y1		y@
	#define y2		y@(1)
	#define r		r@
	#define r1		r@
	#define r2		r@(1)
	#define rw		rw@
	#define r1w		rw@
	#define r2w		rw@(1)
	#define l1w		lw@
	#define l2w		lw@(1)
	#define x1w		xw@
	#define x2w		xw@(1)
	#define y1w		yw@
	#define y2w		yw@(1)

	#define fontsize	16
	#define sxMS	300	//[px] ("sizexOfMassSlider") 質量調節スライダ(MS)の長さ
	#define syMS	4	//[px] 〃太さ
	#define sxMSK	8	//[px] ("sizexOfMassSliderKnob") MSつまみ(MSK)の横幅
	#define syMSK	14	//[ps] 〃高さ
	#deffunc local modDetect
		return

	*boot
		LBTNDWN = 0	//汎用変数初期化
		initUI
		usrRqstStat = 0	//("userRequestStatus") ユーザーからの要求の状態。(0,1,2,3) = (通常,プリセットロード要求,プリセット保存要求,シミュレーション開始要求)

	*main	//メインループ
		repeat
			handleMassSlds	//MS管理
			handlePndlmDrg	//おもりドラッグ管理
			reflesh	//画面更新
			if usrRqstStat : break	//ユーザーから新たなアクションの要求があるなら
			await frmIntvl
		loop
		switch usrRqstStat
			case 1
				loadPreset
				initUI
				goto *boot
			case 2
				savePreset
				initUI
				goto *boot
				swbreak
			case 3
				goto *boot@mod_simu
				swbreak
			default
				fatalError
		swend
		stop

	*btnIntrpt	/* ボタン割り込みへの対応 */
		usrRqstStat = 1+stat
		return

	#deffunc local initUI	/* UI初期化 */
		cls
		font msgothic,fontsize
		/* おもり */
			r1w = r1*mtr2px : r2w = r2*mtr2px	//[px] r1,r2の、ウィンドウ上での長さ
			l1w = l1*mtr2px : l2w = l2*mtr2px	//[px]
			x1w = Oxw+x1*mtr2px : x2w = Oxw+x2*mtr2px	//x1,x2の、ウィンドウ上での位置
			y1w = Oyw+y1*mtr2px : y2w = Oyw+y2*mtr2px
		/* MS */
			MSV = m2MSV(m1),m2MSV(m2)	;MSの値。0.0~1.0。要素0,1はm1,m2のスライダに対応。
			pxMSK = 140-sxMSK/2,pxMSK	;[px] MSつまみの左上x座標。要素0,1はm1,m2のスライダに対応。
			pyMSK = wsy-5-fontsize*2+8+syMS/2-syMSK/2,pyMSK+fontsize	;〃y
		/* 各種ボタン */
			#define sxBtn	180	//("sizexOfButton") ボタンの横幅
			#define syBtn	20
			objsize sxBtn,syBtn
			pos wsx-5-sxBtn,wsy-5-syBtn*3-2*2 : button gosub "プリセット読込",*btnIntrpt
			pos wsx-5-sxBtn,wsy-5-syBtn*2-2 : button gosub "プリセット保存",*btnIntrpt
			pos wsx-5-sxBtn,wsy-5-syBtn : button gosub "シミュレーション開始",*btnIntrpt
		return

	#deffunc local handleMassSlds	/* MS管理ルーチン */
		getkey LBTNDWN,1 : if LBTNDWN = 0 : return
		repeat 2	/* MS1,2 */
			if (pxMSK(cnt) <= mousex)&(mousex <= pxMSK(cnt)+sxMSK)&(pyMSK(cnt) <= mousey)&(mousey <= pyMSK(cnt)+syMSK) {	//カーソルがつまみに重なっているなら
				xofst = pxMSK(cnt)-mousex : yofst = pyMSK(cnt)-mousey	//マウスカーソルに対するつまみの相対位置
				lastmx = mousex	//以前のマウスカーソルのx座標
				cnt0 = cnt
				repeat
					getkey LBTNDWN,1 : if LBTNDWN = 0 : break
					if mousex != lastmx {	//マウスカーソルが横に動いたら
						lastmx = mousex
						pxMSK(cnt0) = limit(mousex+xofst, msv2pxMSK(0),msv2pxMSK(1))	//つまみ位置決定
						MSV(cnt0) = pxMSK2msv(pxMSK(cnt0))	//MS値計算
						m(cnt0) = MSV2m(MSV(cnt0))	//質量計算
						r(cnt0) = m2r(m(cnt0)) : rw(cnt0) = r(cnt0)*mtr2px	//半径計算
						reflesh
					}
					await frmIntvl
				loop
			}
		loop
		return

	#deffunc local handlePndlmDrg	/* おもりドラッグ管理 */
		getkey LBTNDWN,1 : if LBTNDWN = 0 : return
		/* おもり2 */
			if powf(x2w-mousex,2)+powf(y2w-mousey,2) <= powf(r2w,2) {
				xmofst = x2w-mousex : ymofst = y2w-mousey
				lastmx = mousex : lastmy = mousey
				repeat
					getkey LBTNDWN,1 : if LBTNDWN = 0 : break
					if (mousex != lastmx)|(mousey != lastmy) {
						lastmx = mousex : lastmy = mousey
						x2w = mousex+xmofst : y2w = mousey+ymofst
						calcPndlmPosFromUI
						reflesh	//画面更新
					}
					await frmIntvl
				loop
			}
		/* おもり1 */
			if powf(x1w-mousex,2)+powf(y1w-mousey,2) <= powf(r1w,2) {	//マウスカーソルがおもりに重なっているなら
				xmofst = x1w-mousex : ymofst = y1w-mousey	//マウスカーソルに対するおもりの中心の相対位置
				xRelPw = x2w-x1w : yRelPw = y2w-y1w	//[px] おもり1に対する2の相対位置
				lastmx = mousex : lastmy = mousey	//以前のマウスカーソルの位置
				repeat
					getkey LBTNDWN,1 : if LBTNDWN = 0 : break
					if (mousex != lastmx)|(mousey != lastmy) {	//マウスカーソルが動いたら
						lastmx = mousex : lastmy = mousey
						x1w = mousex+xmofst : y1w = mousey+ymofst
						x2w = x1w+xRelPw : y2w = y1w+yRelPw
						calcPndlmPosFromUI
						reflesh	//画面更新
					}
					await frmIntvl
				loop
			}
		return

	#deffunc local calcPndlmPosFromUI	/* ウィンドウ上でのおもりの位置からその他の位置情報をを計算 */
		x1 = double(x1w-Oxw)/mtr2px : y1 = double(y1w-Oyw)/mtr2px
		l1 = sqrt(x1*x1+y1*y1) : l1w = l1*mtr2px
		θ1 = atan(x1,y1)
		x2 = x1+double(x2w-x1w)/mtr2px : y2 = y1+double(y2w-y1w)/mtr2px
		l2 = sqrt(powf(x2-x1,2)+powf(y2-y1,2)) : l2w = l2*mtr2px
		θ2 = atan(x2-x1,y2-y1)
		return

	#deffunc local reflesh /* 再描画 */
		redraw 0
			color : boxf
			drawCdntAx	//座標軸
			drawPndlms	//振り子
			drawInfos	//各種情報
			drawMassSlds	//MS
		redraw 1
		return

	#deffunc local drawMassSlds /* MS描画*/
		repeat 2
			ytmp = wsy-5-fontsize*(2-cnt)+8
			color 100,100,100
			boxf 140,ytmp, 140+sxMS,ytmp+syMS
			pxMSK(cnt) = msv2pxMSK(MSV(cnt))
			color 200,150,100
			boxf pxMSK(cnt),pyMSK(cnt), pxMSK(cnt)+sxMSK,pyMSK(cnt)+syMSK
		loop
		return
#global

#module mod_simuCfg_2
	//#define mtr2px	mtr2px@
	//#define Oxw		Oxw@
	//#define Oyw		Oyw@
	#define mmin	mmin@
	#define mmax	mmax@
	//#define θ1		θ@
	//#define θ2		θ@(1)
	//#define l1w		lw@
	//#define l2w		lw@(1)

	#define sxMS	sxMS@mod_simuCfg	//[px] ("sizexOfMassSlider") 質量調節スライダ(MS)の長さ
	#define syMS	syMS@mod_simuCfg	//[px] 〃太さ
	#define sxMSK	sxMSK@mod_simuCfg	//[px] ("sizexOfMassSliderKnob") MSつまみ(MSK)の横幅
	#define syMSK	syMSK@mod_simuCfg	//[ps] 〃高さ

	#defcfunc m2MSV double m	//質量→MS値
		return (m-mmin)/(mmax-mmin)

	#defcfunc MSV2m double msv	//MS値→質量
		return mmin+msv*(mmax-mmin)

	#defcfunc msv2pxMSK double msv	//MS値→つまみの左上x座標
		return 140+msv*sxMS-sxMSK/2

	#defcfunc pxMSK2msv	double pxMSK//MSつまみの左上座標→MS値
		return double(pxMSK+sxMSK/2-140)/sxMS
#global

#module mod_commonCalc
	#define σ	σ@

	#defcfunc m2r double m	//質量→半径
		return sqrt(m/σ/4/m_pi)
#global

#module mod_simu
	#define wsx			wsx@
	#define wsy			wsy@
	#define slfreq		slfreq@
	#define frmIntvl	frmIntvl@
	#define mtr2px		mtr2px@
	#define Oxw			Oxw@
	#define Oyw			Oyw@
	#define dt		dt@
	#define hdt		hdt@
	#define g		g@
	#define m1		m@
	#define m2		m@(1)
	#define l1		l@
	#define l2		l@(1)
	#define θ1		θ@
	#define θ2		θ@(1)
	#define x1		x@
	#define x2		x@(1)
	#define y1		y@
	#define y2		y@(1)

	#define fontsize	16
	#define numPtTrack		100		//("numberOfPointsOfTrack") おもり2軌跡点の記録数
	#define TrkSmplngIntvl	dt*2	//[s] ("TrackSamplingInterval") 軌跡サンプリング間隔

	#deffunc local modDetect
		return

	*boot
		initUI
		usrRqstStat = 0	//("userRequestStatus") ユーザーからの要求の状態。(0,1) = (通常,シミュレーション終了要求)
		simuStat = 1	//("simulationStatus") シミュレーションの状態。(0,1) = (停止,実行)
		η = (m1+m2)/m2
		ω1 = 0.0
		ω2 = 0.0
		x1 = l1*sin(θ1) : y1 = l1*cos(θ1)
		x2 = x1+l2*sin(θ2) : y2 = y1+l2*cos(θ2)

		/* おもり2の軌跡点 */
		/*
			おもり2の軌跡点は直近 numPtTRACK 個分を TRACK [numPtTRACK,2] 配列記録する。
			フォーマットはリングバッファと同じ要領。frntTRACK に記録される位置を先頭とし、そこから numPtTRACK 個分描画する。
			古い順に詰まっている。要素 (*,0),(*,1) はそれぞれ然るべき番号の軌跡点のx,y座標。
		*/
		ddim TRACK,numPtTrack,2
		repeat numPtTrack : TRACK(cnt,0) = x2 : TRACK(cnt,1) = y2 : loop
		frntTRACK = 0

	*main
		etls = 0.0	//[s] ("elapsedTimeFromLastStep") 前回ステップ計算からの経過時間
		etlts = 0.0 //[s] ("elapsedTimeFromLastTrackSampling") 前回軌跡サンプリングからの経過時間
		etlf = 0.0	//[ms] ("elapsedTimeFromLastFrame") 前回画面更新からの経過時間
		repeat
			if etls >= dt : step : etls = -0.001*slfreq							//ステップ計算
			if etlts >= TrkSmplngIntvl : handleTrack : etlts = -0.001*slfreq	//軌跡サンプリング
			if etlf >= frmIntvl : reflesh : etlf = -slfreq						//画面更新
			if usrRqstStat : break
			await slfreq
			if simuStat {
				etls += 0.001*slfreq
				etlts += 0.001*slfreq
			}
			etlf += slfreq
		loop
		goto *boot@mod_simuCfg

	*btnIntrrpt	/* ボタン割り込みへの対応 */
		switch stat
			case 0
				simuStat = 0
				objenable 0,0 : objenable 1,1
				swbreak
			case 1
				simuStat = 1
				objenable 0,1 : objenable 1,0
				swbreak
			case 2
				usrRqstStat = 1
		swend
		return

	#deffunc local initUI	/* UI初期化 */
		cls
		font msgothic,fontsize
		/* 各種ボタン */
		#define sxBtn	180
		#define syBtn	20
		objsize sxBtn,syBtn
		pos wsx-sxBtn-5,wsy-5-syBtn*3-2*2 : button gosub "停止",*btnIntrrpt
		pos wsx-sxBtn-5,wsy-5-syBtn*2-2 : button gosub "再開",*btnIntrrpt : objenable 1,0
		pos wsx-sxBtn-5,wsy-5-syBtn : button gosub "シミュレーション終了",*btnIntrrpt
		return

	#deffunc local 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 + 2.0*k2 + 2.0*k3 + k4)
		θ2 += dt/6*(k1(1) + 2.0*k2(1) + 2.0*k3(1) + k4(1))
		ω1 += dt/6*(k1(2) + 2.0*k2(2) + 2.0*k3(2) + k4(2))
		ω2 += dt/6*(k1(3) + 2.0*k2(3) + 2.0*k3(3) + k4(3))
		x1 = l1*sin(θ1) : y1 = l1*cos(θ1)
		x2 = x1+l2*sin(θ2) : y2 = y1+l2*cos(θ2)
		return

	#deffunc local reflesh	/* 画面更新 */
		redraw 0
			color : boxf
			drawCdntAx	//座標軸
			drawPndlms	//振り子
			drawTrack	//軌跡
			drawInfos : drawIndo2	//各種情報
		redraw 1
		return

	#deffunc local drawIndo2	/* 各種情報の表示 */
		color 200,200,200
		U = calcU() : K = calcK() : E = U+K
		pos 5,wsy-5-fontsize*9 : mes "U = "+strf("%6.2f", U)+" [J]"
		pos 5,wsy-5-fontsize*8 : mes "K = "+strf("%6.2f", K)+" [J]"
		pos 5,wsy-5-fontsize*7 : mes "E = "+strf("%6.2f", E)+" [J]"
		return

	#deffunc local handleTrack	/* おもり2の軌跡の管理 */
		TRACK(frntTRACK,0) = x2 : TRACK(frntTRACK,1) = y2
		if frntTRACK = numPtTRACK-1 : frntTRACK = 0 : else : frntTRACK ++
		return

	#deffunc local drawTrack	/* おもり2の軌跡の描画 */
		i = frntTRACK
		repeat numPtTrack
			xtmp = Oxw+TRACK(i,0)*mtr2px-3,xtmp+6,xtmp+6,xtmp
			ytmp = Oyw+TRACK(i,1)*mtr2px-3,ytmp,ytmp+6,ytmp+6
			gmode 3, , ,256*cnt/numPtTRACK
			color 50,100,255 : gsquare -1,xtmp,ytmp
			if i = numPtTRACK-1 : i = 0 : else : i ++
		loop
		return

	#defcfunc local calcU	/* 全位置エネルギーの計算 */
		return -(m1+m2)*g*l1*cos(θ1) - m2*g*l2*cos(θ2)

	#defcfunc local calcK	/* 全運動エネルギーの計算 */
		return 0.5*(m1+m2)*l1*l1*ω1*ω1 + 0.5*m2*l2*l2*ω2*ω2 + m2*l1*l2*ω1*ω2*cos(θ1-θ2)
#global

#module mod_simu_2
	#define dt		dt@
	#define hdt		hdt@
	#define g		g@
	#define l1		l@
	#define l2		l@(1)
	#define η		η@mod_simu

	#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-θ2 : cosφ = cos(φ)
		return (g*η*(cosφ*sin(θ1)-sin(θ2))+(η*l1*ω1*ω1+l2*ω2*ω2*cosφ)*sin(φ))/l2/(η-cosφ*cosφ)
#global

#module mod_commonDraw
	#define wsx	wsx@
	#define wsy	wsy@
	#define Oxw	Oxw@
	#define Oyw	Oyw@
	#define m1		m@
	#define m2		m@(1)
	#define l1		l@
	#define l2		l@(1)
	#define θ1	θ@
	#define θ2	θ@(1)
	#define r1w	rw@
	#define r2w	rw@(1)
	#define l1w	lw@
	#define l2w	lw@(1)
	#deffunc local modDetect
		return

	#deffunc drawCdntAx	/* ("drawCoordinateAxis") 座標軸の描画 */
		color 100,100,100
		line -1,Oyw,wsx,Oyw
		line Oxw,-1,Oxw,wsy
		return

	#deffunc drawPndlms	/* ("drawPendulums") 振り子の描画 */
		// 注意 hsp3dish では circle で中空円が描けないので一旦塗り潰してから背景色で一回り小さい円を塗り潰して対応する。
		/* おもり1 */
		xtmp = Oxw+l1w*sin(θ1) : ytmp = Oyw+l1w*cos(θ1)
		color 255,255,255 : circle xtmp-r1w,ytmp-r1w, xtmp+r1w,ytmp+r1w, 1
		color : circle xtmp-r1w+1,ytmp-r1w+1, xtmp+r1w-1,ytmp+r1w-1, 1
		color 255,255,255 : line Oxw,Oyw,xtmp,ytmp
		/* おもり2 */
		xtmp2 = xtmp + l2w*sin(θ2) : ytmp2 = ytmp + l2w*cos(θ2)
		color 255,255,255 : circle xtmp2-r2w,ytmp2-r2w, xtmp2+r2w,ytmp2+r2w, 1
		color : circle xtmp2-r2w+1,ytmp2-r2w+1, xtmp2+r2w-1,ytmp2+r2w-1, 1
		color 255,255,255 : line xtmp,ytmp,xtmp2,ytmp2
		return

	#deffunc drawInfos	/* 各種情報の表示 */
		/* 注意
			font 設定の影響を受ける。推奨は msgoshic,16 。
		*/
		fontsize = 16
		color 200,200,200
		pos 5,wsy-5-fontsize*6 : mes "l1 = "+strf("%3.2f",l1)+" [m]"
		pos 5,wsy-5-fontsize*5 : mes "l2 = "+strf("%3.2f",l2)+" [m]"
		pos 5,wsy-5-fontsize*4 : mes "θ1 = "+strf("%6.2f",rad2deg(θ1))+" [deg]"
		pos 5,wsy-5-fontsize*3 : mes "θ2 = "+strf("%6.2f",rad2deg(θ2))+" [deg]"
		pos 5,wsy-5-fontsize*2 : mes "m1 = "+strf("%3.2f",m1)+" [kg]"
		pos 5,wsy-5-fontsize : mes "m2 = "+strf("%3.2f",m2)+" [kg]"
		return
#global

#module mod_loadPreset
	#define wsx			wsx@
	#define wsy			wsy@
	#define frmIntvl	frmIntvl@*3
	#define mtr2px		mtr2px@
	#define fontsize	16
	#define PRESET		PRESET@
	#define σ	σ@
	#define m	m@
	#define m1	m@
	#define m2	m@(1)
	#define l	l@
	#define l1	l@
	#define l2	l@(1)
	#define θ	θ@
	#define θ1	θ@
	#define θ2	θ@(1)
	#define x1	x@
	#define x2	x@(1)
	#define y1	y@
	#define y2	y@(1)
	#define r1	r@
	#define r2	r@(1)
	#define r1w	rw@
	#define r2w	rw@(1)
	#define l1w	lw@
	#define l2w	lw@(1)

	#deffunc loadPreset
		/*
			プリセットからロードする処理をUIも含めて担当する。
			戻り値 : (-1,other) : (ロードせず,ロードされたプリセット番号)

			ウィンドウは cls されることに注意
		*/

		*boot
			/* 未初期化変数警告回避 */
			idSelPs = 0

			/* 直前の設定をバックアップ */
			mbak = m1,m2
			lbak = l1,l2
			θbak = θ1,θ2

			loadPresetFile
			initUI
			usrRqstStat = 0	//("userRequestStatus") ユーザーからの要求の状態。(0,1,2) = (通常,選択プリセット読込要求,読込キャンセル要求)

		*main
			repeat
				reflesh	//画面更新
				if usrRqstStat : break	//ユーザーから新たなアクションの要求があるなら
				await frmIntvl
			loop
			switch usrRqstStat
				case 1
					return idSelPs
				case 2	/* キャンセル */
					/* 設定復元 */
					m = mbak,mbak(1)
					l = lbak,lbak(1)
					θ = θbak,θbak(1)
					r1 = sqrt(m1/σ/4/m_pi) : r2 = sqrt(m2/σ/4/m_pi)
					x1 = l1*sin(θ1) : y1 = l1*cos(θ1)
					x2 = x1+l2*sin(θ2) : y2 = y1+l2*cos(θ2)
					return -1
			swend

		*PsBtnIntrrpt	/* プリセットボタン割り込みへの応答 */
			idSelPs = stat
			l1 = PRESET(stat,0) : l2 = PRESET(stat,1)
			m1 = PRESET(stat,2) : m2 = PRESET(stat,3)
			r1 = sqrt(m1/σ/4/m_pi) : r2 = sqrt(m2/σ/4/m_pi)
			θ1 = PRESET(stat,4) : θ2 = PRESET(stat,5)
			x1 = l1*sin(θ1) : y1 = l1*cos(θ1)
			x2 = x1+l2*sin(θ2) : y2 = y1+l2*cos(θ2)
			r1w = r1*mtr2px : r2w = r2*mtr2px
			l1w = l1*mtr2px : l2w = l2*mtr2px
			return

		*ActBtnIntrpt	/* アクションボタン割り込みへの対応 */
			usrRqstStat = stat-4
			return

	#deffunc local initUI	/* UI初期化 */
		cls
		font msgothic,fontsize

		/* プリセットボタン */
		#define sxPsBtn	50	//("sizexOfPresetButton") プリセットボタンの横幅
		#define syPsBtn	30
		objsize sxPsBtn,syPsBtn
		numVPs = 5	//("numberOfValidPreset") 有効なプリセットの個数
		repeat 5
			pos 200+(sxPsBtn+5)*cnt,wsy-syPsBtn-5 : button gosub ""+cnt+"",*PsBtnIntrrpt
			if PRESET(cnt,0) <= 0 : objenable cnt,0 : numVPs --	//プリセットが記録されていないならボタンを無効化
		loop

		/* アクションボタン */
		#define sxActBtn	100
		#define syActBtn	20
		objsize sxActBtn,syActBtn
		pos wsx-sxActBtn-5,wsy-(syActBtn+5)*2 : button gosub "読込",*ActBtnIntrpt
		if numVPs = 0 : objenable stat,0
		pos wsx-sxActBtn-5,wsy-syActBtn-5 : button gosub "キャンセル",*ActBtnIntrpt

		/* 初期選択 */
		if numVPs {
			repeat 5	/* 最初に見つかった有効なプリセットを選択する */
				if PRESET(cnt,0) > 0 {
					idSelPs = cnt	//選択されているプリセット番号
					l1 = PRESET(cnt,0) : l2 = PRESET(cnt,1)
					m1 = PRESET(cnt,2) : m2 = PRESET(cnt,3)
					r1 = sqrt(m1/σ/4/m_pi) : r2 = sqrt(m2/σ/4/m_pi)
					θ1 = PRESET(cnt,4) : θ2 = PRESET(cnt,5)
					x1 = l1*sin(θ1) : y1 = l1*cos(θ1)
					x2 = x1+l2*sin(θ2) : y2 = y1+l2*cos(θ2)
					r1w = r1*mtr2px : r2w = r2*mtr2px
					l1w = l1*mtr2px : l2w = l2*mtr2px
					break
				}
			loop
		}
		return

	#deffunc local reflesh /* 画面更新 */
		redraw 0
			color : boxf
			drawCdntAx	//座標軸
			drawInfos2	//各種情報
			if numVPs {
				drawPndlms	//振り子
				drawInfos	//各種情報
			}
		redraw 1
		return

	#deffunc local drawInfos2	/* 各種情報表示 */
		color 200,200,200
		pos 200,wsy-syPsBtn*2-15 : mes "プリセットを選択してください"
		if numVPs {
			color 200,150,100
			pos 200+(sxPsBtn+5)*idSelPs+(sxPsBtn-fontsize)/2,wsy-syPsBtn*2+5 : mes "▼"
		}
		return

	#deffunc loadPresetFile
		/*
			プリセットデータをファイルから読み込んでシステムに反映する。
			戻り値 (0,1) : (成功,失敗)
		*/
		exist "save/preset.dat"
		if strsize  = -1 : return 1
		bload "save/preset.dat", PRESET
		return 0
#global

#module mod_savePreset
	#define wsx			wsx@
	#define wsy			wsy@
	#define frmIntvl	frmIntvl@*3
	#define mtr2px		mtr2px@
	#define fontsize	16
	#define PRESET		PRESET@
	#define σ	σ@
	#define m	m@
	#define m1	m@
	#define m2	m@(1)
	#define l	l@
	#define l1	l@
	#define l2	l@(1)
	#define θ	θ@
	#define θ1	θ@
	#define θ2	θ@(1)
	#define x1	x@
	#define x2	x@(1)
	#define y1	y@
	#define y2	y@(1)
	#define r1	r@
	#define r2	r@(1)
	#define r1w	rw@
	#define r2w	rw@(1)
	#define l1w	lw@
	#define l2w	lw@(1)

	#deffunc savePreset
		/*
			プリセットにセーブする処理をUIも含めて担当する。
			戻り値 : (-1,other) : (セーブせず,セーブ先プリセット番号)

			ウィンドウは cls されることに注意
		*/

		*boot
			/* 未初期化変数警告回避 */
			idSelPs = 0

			/* 直前の設定をバックアップ */
			mbak = m1,m2
			lbak = l1,l2
			θbak = θ1,θ2

			loadPresetFile
			initUI
			usrRqstStat = 0	//("userRequestStatus") ユーザーからの要求の状態。(0,1,2) = (通常,選択プリセットへ保存要求,保存キャンセル要求)

		*main
			repeat
				reflesh	//画面更新
				if usrRqstStat : break	//ユーザーから新たなアクションの要求があるなら
				await frmIntvl
			loop

			/* 設定復元 */
			m = mbak,mbak(1)
			l = lbak,lbak(1)
			θ = θbak,θbak(1)
			r1 = sqrt(m1/σ/4/m_pi) : r2 = sqrt(m2/σ/4/m_pi)
			x1 = l1*sin(θ1) : y1 = l1*cos(θ1)
			x2 = x1+l2*sin(θ2) : y2 = y1+l2*cos(θ2)

			switch usrRqstStat
				case 1
					savePresetFile idSelPs
					return idSelPs
				case 2	//キャンセル
					return -1
			swend
			stop

		*PsBtnIntrrpt	/* プリセットボタン割り込みへの応答 */
			idSelPs = stat
			if PRESET(idSelPs,0) > 0 {	//選択プリセットのデータが有効なら
				l1 = PRESET(idSelPs,0) : l2 = PRESET(idSelPs,1)
				m1 = PRESET(idSelPs,2) : m2 = PRESET(idSelPs,3)
				r1 = sqrt(m1/σ/4/m_pi) : r2 = sqrt(m2/σ/4/m_pi)
				θ1 = PRESET(idSelPs,4) : θ2 = PRESET(idSelPs,5)
				x1 = l1*sin(θ1) : y1 = l1*cos(θ1)
				x2 = x1+l2*sin(θ2) : y2 = y1+l2*cos(θ2)
				r1w = r1*mtr2px : r2w = r2*mtr2px
				l1w = l1*mtr2px : l2w = l2*mtr2px
			}
			return

		*ActBtnIntrpt	/* アクションボタン割り込みへの対応 */
			usrRqstStat = stat-4
			return

	#deffunc local initUI	/* UI初期化 */
		cls
		font msgothic,fontsize

		/* プリセットボタン */
		#define sxPsBtn	50	//("sizexOfPresetButton") プリセットボタンの横幅
		#define syPsBtn	30
		objsize sxPsBtn,syPsBtn
		repeat 5 : pos 200+(sxPsBtn+5)*cnt,wsy-syPsBtn-5 : button gosub ""+cnt+"",*PsBtnIntrrpt : loop
		idSelPs = 0	//選択プリセット番号

		/* アクションボタン */
		#define sxActBtn	100
		#define syActBtn	20
		objsize sxActBtn,syActBtn
		pos wsx-sxActBtn-5,wsy-(syActBtn+5)*2 : button gosub "保存",*ActBtnIntrpt
		pos wsx-sxActBtn-5,wsy-syActBtn-5 : button gosub "キャンセル",*ActBtnIntrpt
	return

	#deffunc local reflesh /* 画面更新 */
		redraw 0
			color : boxf
			drawInfos2
			drawCdntAx	//座標軸
			if PRESET(idSelPs,0) > 0 {	//選択プリセットが有効なら
				drawPndlms	//振り子
				drawInfos	//各種情報
			}
		redraw 1
		return

	#deffunc local drawInfos2	/* 各種情報表示 */
		color 200,200,200
		pos 200,wsy-syPsBtn*2-15 : mes "プリセットを選択してください"
		color 200,150,100
		pos 200+(sxPsBtn+5)*idSelPs+(sxPsBtn-fontsize)/2,wsy-syPsBtn*2+5 : mes "▼"
		return

	#deffunc savePresetFile int i
		/*
			選択プリセットに現在の設定を書き込んでファイルに保存する。
			i : 選択プリセット番号
		*/
		PRESET(i,0) = l1 : PRESET(i,1) = l2
		PRESET(i,2) = m1 : PRESET(i,3) = m2
		PRESET(i,4) = θ1 : PRESET(i,5) = θ2
		bsave "save/preset.dat",PRESET
		#ifdef hsproom
		devcontrol "syncfs"
		#endif
		return
#global

#module mod_initPreset
	#define PRESET	@PRESET@

	#deffunc initPreset	/* プリセットデータを初期化してファイルに保存 */
		ddim PRESET,5,6
		PRESET(0,0) = 1.3 : PRESET(0,1) = 2.05 : PRESET(0,2) = 0.68 : PRESET(0,3) = 1.26 : PRESET(0,4) = 0.115715 : PRESET(0,5) = 2.953097
		PRESET(1,0) = 0.86 : PRESET(1,1) = 1.8 : PRESET(1,2) = 2.01 : PRESET(1,3) = 0.10 : PRESET(1,4) = -2.09148 : PRESET(1,5) = 1.82928
		PRESET(3,0) = 2.5 : PRESET(3,1) = 1.5 : PRESET(3,2) = 1.0 : PRESET(3,3) = 3.0 : PRESET(3,4) = 1.570796 : PRESET(3,5) = -2.356194
		PRESET(4,0) = 0.7 : PRESET(4,1) = 2.67 : PRESET(4,2) = 1.0 : PRESET(4,3) = 3.0 : PRESET(4,4) = 0.393746 : PRESET(4,5) = -1.99275
		bsave "save/preset.dat",PRESET
		#ifdef hsproom
		devcontrol "syncfs"
		#endif
		return
#global

#module mod_fatalError
	#deffunc fatalError
		dialog "A fatal error occured.\nThis program will end on closing this window."
		end
#global

HSP 二重振り子RK4シミュonHSP(修正版1)

#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

HSP 着弾点の予想と目標距離まで飛ばすための発射角の計算

;
;	着弾予測
;
;弾道、飛距離、の予想。弾丸の発射が行えます。
;

#include "hspmath.as"	; asinで使用

;	定数
#const BULLET_FRAME  50
groundTop = 400.0	; 地面の高さ
gravity = -1.0		; 重力
velocity = 20.0		; 初速

;	初期値
bulletPosX = 50.0	; 弾丸座標
bulletPosY = 0.0	; 弾丸座標
firingAngle = 45.0 * M_PI / 180.0	; 発射角度[rad]
setHikyori = 200.0	;設定飛距離(この飛距離を出すための発射角度を算出します。)

flgShot = 0			; ショットフラグ
vx = 0.0
vy = 0.0

ddim courseOfBullet, BULLET_FRAME, 2	; 弾道座標データ

;-----------------------------------------------------------
;
;	メインループ
;
*main
	redraw 1 : await 16 : redraw 0 : color 255, 255, 255 : boxf : color 0,0,0 : pos 0,0

	stick key, 1|2|4|8|128

	; --------------
	;	終了
	; --------------
	if key&128 : end

	; --------------
	;	発射角度
	; --------------
	if key&2 : firingAngle += 1.0 * M_PI / 180.0
	if key&8 : firingAngle -= 1.0 * M_PI / 180.0

	; --------------
	;	発射
	; --------------
	if key&16 : flgShot = 1

	; --------------
	;	物理計算
	; --------------

	;	重力
	if int(bulletPosY) > 0 {
		vy += gravity
	} else {
		vx = 0.0
		vy = 0.0
	}

	;	ショット
	if flgShot = 1 {
		;	初速
		vx = velocity * cos(firingAngle)
		vy = velocity * sin(firingAngle) + gravity / 2.0
			;微調整のため適当に1/2しています。最初だけ1/2倍しないと計算が大きくずれるようです。
			;射出時は地面に付いているのでってことで、詳細な考察は省略。
		flgShot = 0
	}

	;	予備計算
	px = bulletPosX
	py = bulletPosY
	px += vx
	py += vy

	;	地面
	if py < 0.0 {
		;予測地点が地面にめり込む場合
		bulletPosX += vx * bulletPosY/-vy
		bulletPosY = 0.0
	} else {
		bulletPosX += vx
		bulletPosY += vy
	}

	;	弾道計算
	gosub *CalcDandou

	;	飛距離予測計算
	gosub *CalcHikyori

	; --------------
	;	描画
	; --------------
	line 0, groundTop, ginfo_winx, groundTop
	gosub *PlotDandou
	gosub *PlotBullet
	pos 0, 0 : color 0,0,0
	mes "現在の座標:" + bulletPosX + ", " + bulletPosY
	mes "予想飛距離:" + yosouHikyoriX
	mes "予想座標  :" + (bulletPosX + yosouHikyoriX) + ", 0.0"
	mes "予想角度(距離 " + setHikyori + "):" + rad2deg(yosouKaku) + " [deg] or " + (90.0-rad2deg(yosouKaku)) + " [deg]"

	goto *main

;-----------------------------------------------------------
;
;	弾道軌道描画
;
*PlotDandou
	; --------------
	;	弾道軌道
	; --------------
	color 200, 200, 200
	x  = courseOfBullet(0, 0)
	y  = courseOfBullet(0, 1) * -1 + groundTop
	pos x, y
	repeat BULLET_FRAME-1, 1
		x = courseOfBullet(cnt, 0)
		y = courseOfBullet(cnt, 1) * -1 + groundTop
		line x, y
	loop
	return

;-----------------------------------------------------------
;
;	弾丸描画
;
*PlotBullet
	; --------------
	;	発射角度
	; --------------
	x = bulletPosX
	y = -bulletPosY + groundTop
	r = 30.0
	dx =  r * cos(firingAngle) + x
	dy = -r * sin(firingAngle) + y
	color 0,0,255
	line x, y, dx, dy
	mes strf("%.2f", (firingAngle*180.0/M_PI) ) + " [deg]"
	mes "Up:↑/Down:↓"

	; --------------
	;	弾丸
	; --------------
	x = bulletPosX
	y = -bulletPosY + groundTop
	r = 5
	color 0,0,0
	circle x-r, y-r,  x+r, y+r
	return

;-----------------------------------------------------------
;
;	弾道予想計算
;
*CalcDandou
	cvx = velocity * cos(firingAngle)
	cvy = velocity * sin(firingAngle) + gravity/2.0
	;0フレーム目
	courseOfBullet(0, 0) = bulletPosX
	courseOfBullet(0, 1) = bulletPosY
	;1フレーム目
	repeat BULLET_FRAME-1, 1
		courseOfBullet(cnt, 0) = courseOfBullet(cnt-1, 0) + cvx
		courseOfBullet(cnt, 1) = courseOfBullet(cnt-1, 1) + cvy + gravity * (cnt-1)
	loop
	return

;-----------------------------------------------------------
;
;	飛距離予想計算
;
*CalcHikyori
	;重力加速度:g
	;t秒後の移動距離:x,y
	;ただし上向きを正とする
	;したがって重力加速度gは g<0 である。
	;着弾点は射出点との高低差0とする。
	;
	;時刻tのときの距離座標x,yは次式のようになる。
	;
	;	x = vx0 * t
	;	y = 1/2*g*t^2 + vy0 * t
	;
	;着弾点と射出点は高低差0なので y = 0 のときの t を求める
	;(なお高低差0以外ならy=高低差として計算すれば良い。)
	;
	;	0 = 1/2*g*t^2 + vy0 * t
	;	  = t(1/2*g*t + vy0)
	;
	;	(1/2*g*t + vy0) = 0
	;	1/2*g*t = -vy0
	;	t = -vy0 * 2/g
	;
	;y = 0となるtは、
	;	t = 0
	;	t = -vy0 * 2/g
	;t = 0は射出時なので、t = -vy0 * 2/g となる。
	;このときxは
	;
	;	x = vx0 * t
	;	  = vx0 * -vy0 * 2/g
	;
	;射出角をrとしたとき
	;	vx0 = v0 * cos(r)
	;	vy0 = v0 * sin(r)
	;
	;したがってxは次のようになる。
	;
	;	x = -v0^2 * cos(r) * sin(r) * 2/g
	;
	;xがそのまま距離なので

	yosouHikyoriX = -1.0 * velocity * velocity * cos(firingAngle) * sin(firingAngle) * 2.0 / gravity

	;
	;xを任意の値に決めたい場合のrを求める。
	;	x = -v0^2 * cos(r) * sin(r) * 2/g
	;	cos(r) * sin(r) = x / (-v0^2) * g/2
	;
	;ここで加法定理
	;	sin(x+y) = sin(x) * cos(y) + cos(x) * sin(y)
	;	y=xの場合
	;	sin(2*x) = 2 * sin(x) * cos(x)
	;	1/2 * sin(2*x) = sin(x) * cos(x)
	;
	;したがって
	;	cos(r) * sin(r) = x / (-v0^2) * g/2
	;	1/2 * sin(2*r) = x / (-v0^2) * g/2
	;	sin(2*r) = x / (-v0^2) * g
	;	2 * r = asin( x / (-v0^2) * g )
	;	r = 1/2 * asin( x / (-v0^2) * g )
	;となり角度を求めることができる。rは複数でるがどれを用いても良い。
	;

	yosouKaku = asin( setHikyori / (-velocity * velocity) * gravity ) / 2.0

	return

HSP OBAQで「ニュートンのゆりかご」

;
;	OBAQ 「ニュートンのゆりかご」
;
;	スペースキーで動かし始める。
;	調整がうまく行っていないのか途中でバラバラになってしまいます。
;
#include "obaq.as"

;内積
#define global ctype DotProduct2D(%1,%2,%3,%4) (double(%1)*(%3) + double(%2)*(%4))

#module
;	振り子	pendulum
;	num      : オブジェクトID
;	x,y      : アンカー座標
;	distance : 距離
;	const_k  : バネ定数
;	damping  : 減衰
#deffunc qAttach int p_num, double p_x, double p_y, double p_distance, double p_const_k, double p_damping
	qgetreq@ rpr, REQ_PHYSICS_RATE	; 1フレームあたりの物理計算回数

	;	オブジェクトの状態を取得
	qgetpos@    p_num, hx1, hy1, hr1
	qgetspeed@  p_num, vx, vy, vr
	qgetweight@ p_num, weiht, mt

	;	オブジェクトからアンカーまでの距離ベクトル
	dx = p_x - hx1
	dy = p_y - hy1
	;cos	-(dx,dy)と(vx,vy)とがなす角のcos値
	dd = sqrt(dx*dx + dy*dy)
	vv = sqrt(vx*vx + vy*vy)
	if (dd=0.0) | (vv=0.0) {
		c = 1.0
	} else {
		c = DotProduct2D( vx, vy, -dx, -dy ) / vv / dd
	}
	;梁正規化
	ix = dx / dd
	iy = dy / dd

	;	減衰
	v = vv * c
	bvx = ix * -v
	bvy = iy * -v
	cvx = 0.0
	cvy = 0.0
	if bvx>0 {
		cvx = -p_damping
		if (bvx - p_damping)<0 : cvx = -bvx
	} else {
		if bvx<0 {
			cvx = p_damping
			if (bvx + p_damping)>0 : cvx = -bvx
		}
	}
	if bvy>0 {
		cvy = -p_damping
		if (bvy - p_damping)<0 : cvy = -bvy
	} else {
		if bvy<0 {
			cvy = p_damping
			if (bvy + p_damping)>0 : cvy = -bvy
		}
	}

	;	バネ
	;バネの伸び
	dl = sqrt(dx*dx + dy*dy) - p_distance
	; a = x * k / m
	vx = ix * dl * p_const_k / weiht
	vy = iy * dl * p_const_k / weiht
	vx *= rpr	;加速度を速度に置き換え
	vy *= rpr
	qspeed@ p_num, vx + cvx, vy + cvy, 0.0

	return

#global

	qreset			; OBAQの初期化

	;	オブジェクト配置
	dim myball, 5
	repeat 5
		qaddpoly myball(cnt), 100, 60.0+10.0*cnt, 60.0, 0, 5.0,5.0, 0
		qweight  myball(cnt), 6.0
		qdamper  myball(cnt), 0.0, 0.0
		qinertia myball(cnt), 1.0
		qtype    myball(cnt), 0x100
	loop

	;----------
	;	梁
	;----------
	; 硬いバネと大きな減衰で梁を表現します。

	; 梁長さ
	lg = 30.0

	; バネ定数
	; 値が小さいほどバネが柔らかく、よく伸びるようになります。
	;k = 0.01
	k = 0.4

	; 減衰
	; 値が大きいほど振動が速く収まります。
	;cv = 0.0001
	cv = 0.2

	;----------
	;	環境
	;----------
	qgravity 0, 0.005

;	メインループ
qgetpos mybox, px,py,pr
*main
	redraw 0		; 画面の更新を開始
	color 0,0,0:boxf	; 画面をクリア
	qexec			; OBAQによるオブジェクトの更新

	;	最初に動かす
	stick key
	if key&16 : qspeed myball(0), -0.5
	color 255,255,255
	pos 50,50
	mes "スペースキーを押してください。"

	;----------
	;	振り子
	;----------
	repeat 5
		qAttach myball(cnt), 60.0+10.0*cnt, 30.0, lg, k, cv
	loop

	;	紐を描画
	color 255,255,255
	repeat 5
		qgetpos myball(cnt), px,py,pa
		qcnvaxis lpx, lpy, px, py, 0
		x = 60.0+10.0*cnt
		y = 30.0
		qcnvaxis lx, ly, x, y, 0
		line lx, ly, lpx, lpy
	loop

	qdraw			; オブジェクトの描画
	redraw 1		; 画面の更新を終了
	await 12		; 一定時間待つ
	goto *main

HSP OBAQで振り子

#include "obaq.as"

;内積
#define ctype DotProduct2D(%1,%2,%3,%4) (double(%1)*(%3) + double(%2)*(%4))

	qreset			; OBAQの初期化

	;	オブジェクト追加
	qaddpoly mybox,   4, 80,30,0, 3.0,3.0
	qaddpoly myball, 20, 80,60,0, 3.0,3.0

	;qgravity 0,0
	qtype mybox, type_bind
	qinertia myball, 1.0

	qgetpos mybox, px,py,pr

*main
	redraw 0		; 画面の更新を開始
	color 0,0,0:boxf	; 画面をクリア
	qexec			; OBAQによるオブジェクトの更新

	;	キー入力
	stick key,15		; キーの取得
	if key&128 : end	; [ESC]キーで終了
	if key&1 : px -= 0.5
	if key&4 : px += 0.5
	if key&2 : py -= 0.5
	if key&8 : py += 0.5

	;	操作用オブジェクトを移動
	qpos mybox, px,py,0

	;	振り子
	qgetpos mybox,  hx0, hy0, hr0
	qgetpos myball, hx1, hy1, hr1
	qgetspeed myball, vx,vy,vr
	;ballからbox
	dx = hx0 - hx1
	dy = hy0 - hy1
	;cos
	dd = sqrt(dx*dx + dy*dy)
	vv = sqrt(vx*vx + vy*vy)
	c = DotProduct2D( vx, vy, -dx, -dy ) / vv / dd
	;紐正規化
	ix = dx / dd
	iy = dy / dd

	;減衰
	v = vv * c
	bvx = ix * -v
	bvy = iy * -v
	cv = 0.1
	cvx = 0.0
	cvy = 0.0
	if bvx>0 {
		cvx = -cv
		bvx -= cv
		if bvx<0 : cvx = 0.0
	} else {
		if bvx<0 {
			cvx = cv
			bvx += cv
			if bvx>0 : cvx = 0.0
		}
	}
	if bvy>0 {
		cvy = -cv
		bvy -= cv
		if bvy<0 : cvy = 0.0
	} else {
		if bvy<0 {
			cvy = cv
			bvy += cv
			if bvy>0 : cvy = 0.0
		}
	}
	qspeed myball, cvx, cvy, 0.0

	;	バネ
	;バネの伸び
	dl = sqrt(dx*dx + dy*dy) - 10.0
	vx = ix * dl / 4
	vy = iy * dl / 4
	qspeed myball, vx, vy

/*
	pos 50,50
	color 255,255,255
	mes ""+dy
	mes ""+vy
	mes vv
	mes c
	mes "cvx = " + cvx
	mes "cvy = " + cvy
*/

	qdraw			; オブジェクトの描画
	redraw 1		; 画面の更新を終了
	await 12		; 一定時間待つ
	goto *main

よく投稿されているコード

タグ

最近投稿されたコード