OBAQで振り子 | Codetter（こーどったー）

#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