ストワで始めるLua 第12回 物理センサー

outputは適当に追加してください

pi=math.pi
pi2=2*pi

--multiplier matrix
function multiplier(A, B)
	if #A[1] ~= #B then return nil end
	local r = {}
	for i = 1, #A do r[i] = {} end
	for i = 1, #A do
		for j = 1, #B[1] do
			local s = 0
			for k = 1, #B do s = s + A[i][k] * B[k][j] end
			r[i][j] = s
		end
	end
	return r
end

--transporse matrix
function transpose(M)
	local r = {}
	for i = 1, #M[1] do
		r[i] = {}
		for j = 1, #M do r[i][j] = M[j][i] end
	end
	return r
end

--rotation matrix
function rotation(T)
	local RX, RY, RZ, qx, qy, qz
	qx, qy, qz = T[1], T[2], T[3]
	RX = { { 1, 0, 0 }, { 0, math.cos(qx), -math.sin(qx) }, { 0, math.sin(qx), math.cos(qx) } }
	RY = { { math.cos(qy), 0, math.sin(qy) }, { 0, 1, 0 }, { -math.sin(qy), 0, math.cos(qy) } }
	RZ = { { math.cos(qz), -math.sin(qz), 0 }, { math.sin(qz), math.cos(qz), 0 }, { 0, 0, 1 } }
	return multiplier(RZ, multiplier(RY, RX))
end

function onTick()

	--gps position (m)
	gps={}
	gps.x=input.getNumber(1)
	gps.y=input.getNumber(3)
	gps.z=input.getNumber(2)--height
	
	--liner speed (m/s)
	liner={}
	liner.x=input.getNumber(7)
	liner.y=input.getNumber(9)
	liner.z=input.getNumber(8)--height

	--tilt angle (rotation)
	tilt={}
	tilt.x=input.getNumber(15)--pitch
	tilt.y=-input.getNumber(16)--roll
	tilt.z=input.getNumber(17)--yaw(compass)


	--angular
	local euler = { -input.getNumber(4), -input.getNumber(5), -input.getNumber(6) }
	local rT = rotation(euler)
	--local rT = transpose(r)
	
	globalAngular={ { input.getNumber(10) }, { input.getNumber(11) }, { input.getNumber(12) } }
	local angular = multiplier(rT, globalAngular)
	angularVelocity={ x=angular[1][1], y=angular[3][1], z=angular[2][1] }

end
function onDraw()
	w=screen.getWidth()
	h=screen.getHeight()
	
end