// 回転角度取得関数検証用モデル「おいら〜君Ver.2」

// Z軸：青
// X軸：緑
// Y軸：赤

// 【操作法】
//	 X軸回転：↑/↓、Y軸回転：←/→、Z軸回転：Z/C
//	 回転加速：+X、回転リセット：Z+C
//	 ズーム：A/D、ズーム加速：+S、ズームリセット：X+S
//	 直方体長さ：Q/E、直方体長さ加速：+W

val{
	rx(default=0,min=-180,max=180,step=0)
	ry(default=0,min=-180,max=180,step=0)
	rz(default=0,min=-180,max=180,step=0)
}

key{
}

body{
	core(name=C_0){
		s:trimf(name=C_Z,angle=rz,option=1){
			n:frame(option=1){
				n:trimf(option=1){}
				w:frame(option=1){}
				e:frame(option=1){
					w:trimf(name=C_X,angle=rx,option=1){
						n:frame(option=1){}
						s:frame(name=C_Y,option=1){
							n:trimf(angle=90,option=1){
								e:trimf(angle=-ry,option=1){
									e:frame(option=1){
										e:frame(option=1){
											e:frame(option=1){
												n:frame(option=1){}
												s:frame(option=1){
													n:trimf(angle=90,option=1){
														n:weight(){}
														s:weight(){}
														w:weight(){}
														e:weight(){}
													}
												}
											}
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}
}

lua{

reset=0
zmini=17.18873405456543
zlen=0.3

function decrease(x,t,l)
	l=math.abs(l)
	return x+math.min(l,math.max(-l,t-x))
end

function math.angle_deg(a)
	while(a<-180) do a=a+360 end
	while(a> 180) do a=a-360 end
	return a
end

function getwpos(lx,ly,lz,cn)
	local wx=_XX(cn)*lx+_YX(cn)*ly+_ZX(cn)*lz+_X(cn)
	local wy=_XY(cn)*lx+_YY(cn)*ly+_ZY(cn)*lz+_Y(cn)
	local wz=_XZ(cn)*lx+_YZ(cn)*ly+_ZZ(cn)*lz+_Z(cn)
	return wx,wy,wz
end

function getlpos(wx,wy,wz,cn)
	wx=wx-_X(cn)
	wy=wy-_Y(cn)
	wz=wz-_Z(cn)
	local lx=_XX(cn)*wx+_XY(cn)*wy+_XZ(cn)*wz
	local ly=_YX(cn)*wx+_YY(cn)*wy+_YZ(cn)*wz
	local lz=_ZX(cn)*wx+_ZY(cn)*wy+_ZZ(cn)*wz
	return lx,ly,lz
end

function rgb_col(r,g,b)
	return r*65536+g*256+b
end

function draw_axis(cn,cx,cy,cz,l)
	local x,y,z=_X(cn),_Y(cn),_Z(cn)
	local xx,xy,xz=_XX(cx)*l,_XY(cx)*l,_XZ(cx)*l
	local yx,yy,yz=_YX(cy)*l,_YY(cy)*l,_YZ(cy)*l
	local zx,zy,zz=_ZX(cz)*l,_ZY(cz)*l,_ZZ(cz)*l
	_SETCOLOR(rgb_col(0,0,255))
	_MOVE3D(x+zx,y+zy,z+zz)
	_LINE3D(x-zx,y-zy,z-zz)
	_SETCOLOR(rgb_col(0,255,0))
	_MOVE3D(x+xx,y+xy,z+xz)
	_LINE3D(x-xx,y-xy,z-xz)
	_SETCOLOR(rgb_col(255,0,0))
	_MOVE3D(x+yx,y+yy,z+yz)
	_LINE3D(x-yx,y-yy,z-yz)
end

function draw_hexa(cn,k,h)
	local x={ k, k, k, k,-k,-k,-k,-k}
	local y={ k, k,-k,-k, k, k,-k,-k}
	local z={ h,-h,-h, h, h,-h,-h, h}
	for i=1,8 do x[i],y[i],z[i]=getwpos(x[i],y[i],z[i],cn) end
	_SETCOLOR(rgb_col(255,255,255))
	_MOVE3D(x[4],y[4],z[4])
	for i=1,4 do _LINE3D(x[i],y[i],z[i]) end
	_MOVE3D(x[8],y[8],z[8])
	for i=5,8 do _LINE3D(x[i],y[i],z[i]) end
	for i=1,4 do
		_MOVE3D(x[i],y[i],z[i])
		_LINE3D(x[i+4],y[i+4],z[i+4])
	end
end

function draw_cross(cn,d,r)
	local z=_ZOOM(0)
	local w=math.tan(math.rad(z/2))*(d+0.75)*r
	local x={ w, 0,-w, 0}
	local y={ 0, w, 0,-w}
	local z={-d,-d,-d,-d}
	for i=1,4 do x[i],y[i],z[i]=getwpos(x[i],y[i],z[i],cn) end
	_SETCOLOR(rgb_col(255,255,0))
	_MOVE3D(x[1],y[1],z[1])
	_LINE3D(x[3],y[3],z[3])
	_MOVE3D(x[2],y[2],z[2])
	_LINE3D(x[4],y[4],z[4])
	for i=1,4 do _LINE3D(x[i],y[i],z[i]) end
end

function zoom()
	local z=_ZOOM(0)
	local s=0.1
	if(_KEY(8)>0) then s=1 end
	if(_KEY(7)>0) then z=z-s end
	if(_KEY(9)>0) then z=z+s end
	if(_KEY(5)>0)and(_KEY(8)>0) then z=zmini end
	z=math.min(100,math.max(1,z))
	local t=_ZOOM(z)
	return z
end

function main()
	if(BX==nil) then
		BX=_I(C_0,0,0)
		BY=_I(C_0,1,1)
		BZ=_I(C_0,2,2)
	end

	local s=1
	if(_KEY(5)>0) then s=5 end
	if(_KEY(0)>0) then RX=RX-s end
	if(_KEY(1)>0) then RX=RX+s end
	if(_KEY(2)>0) then RY=RY-s end
	if(_KEY(3)>0) then RY=RY+s end
	if(_KEY(4)>0) then RZ=RZ-s end
	if(_KEY(6)>0) then RZ=RZ+s end
	if(_KEY(4)>0)and(_KEY(6)>0) then reset=1 end
	if(reset>0) then
		RX=decrease(RX,0,5)
		RY=decrease(RY,0,5)
		RZ=decrease(RZ,0,5)
		if(RX==0)and(RY==0)and(RZ==0) then reset=0 end
	end
	RX=math.angle_deg(RX)
	RY=math.angle_deg(RY)
	RZ=math.angle_deg(RZ)
	draw_axis(C_0,C_X,C_Y,C_Z,1.5)

	s=0.01
	if(_KEY(15)>0) then s=0.05 end
	if(_KEY(14)>0) then zlen=zlen*(1-s) end
	if(_KEY(16)>0) then zlen=zlen*(1+s) end
	zlen=math.min(300,math.max(0.25,zlen))
	draw_hexa(C_0,0.15,zlen)

	draw_cross(C_0,zlen,0.5)

	local  zm=zoom()

	out( 0,"FPS=",_FPS(),",  CHIPS=".._CHIPS(),",  WEIGHT=",_WEIGHT(),"kg")
	out( 1,"&↑↓:ピッチ, ←→:ヨー, Z/C:ロール, +X:回転加速, Z+C:回転リセット")
	out( 2,"&A/D:ズーム, +S:ズーム加速, S+X:ズームリセット")
	out( 3,"&Q/E:直方体長さ, +W:直方体長さ加速")
	out( 4,string.format("ZOOM=%.2f [deg], Z_LEN=%.2f [m]",zm,zlen))
	out( 5,"AX=",math.deg(_AX(C_0)),", AY=",math.deg(_AY(C_0)),", AZ=",math.deg(_AZ(C_0)))
	out( 6,"EX=",math.deg(_EX(C_0)),", EY=",math.deg(_EY(C_0)),", EZ=",math.deg(_EZ(C_0)))
	out( 7,"WX=",_WX(C_0),", WY=",_WY(C_0),", WZ=",_WZ(C_0))
	out( 8,"XX=",_XX(C_0),", XY=",_XY(C_0),", XZ=",_XZ(C_0))
	out( 9,"YX=",_YX(C_0),", YY=",_YY(C_0),", YZ=",_YZ(C_0))
	out(10,"ZX=",_ZX(C_0),", ZY=",_ZY(C_0),", ZZ=",_ZZ(C_0))
	out(11,"QW=",_QW(C_0),", QX=",_QX(C_0),", QY=",_QY(C_0),", QZ=",_QZ(C_0))
	out(12,"I00=",_I(C_0,0,0),", I01=",_I(C_0,1,0),", I02=",_I(C_0,2,0))
	out(13,"I10=",_I(C_0,0,1),", I11=",_I(C_0,1,1),", I12=",_I(C_0,2,1))
	out(14,"I20=",_I(C_0,0,2),", I21=",_I(C_0,1,2),", I22=",_I(C_0,2,2))
	local xx,xy,xz=_XX(C_0)^2,_XY(C_0)^2,_XZ(C_0)^2
	local yx,yy,yz=_YX(C_0)^2,_YY(C_0)^2,_YZ(C_0)^2
	local zx,zy,zz=_ZX(C_0)^2,_ZY(C_0)^2,_ZZ(C_0)^2
	out(15,"IX=",BX*xx+BY*yx+BZ*zx)
	out(16,"IY=",BX*xy+BY*yy+BZ*zy)
	out(17,"IZ=",BX*xz+BY*yz+BZ*zz)
end
}