OBAQで「ニュートンのゆりかご」 | Codetter（こーどったー）

;
;	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