g_asteroid = {}

g_height = 8  -- offset from origin to centre for crate

function asteroid_init(e)
end

function asteroid_update(vect)
	for k,_ in pairs(g_asteroid) do
	
		vars = g_asteroid[k]
		vars.x = vars.x - vect.x
		vars.y = vars.y - vect.y
		vars.z = vars.z - vect.z
	end
end

function asteroid_main(e)

	vars = g_asteroid[e]
	
	if vars == nil then
	
		local Ent = g_Entity[e]
	
		if Ent ~= nil then
		
			local angle = math.random() * 2 * math.pi
			local dist  = math.random() * 3000 + 500
			
			g_asteroid[e] = 
				-- initialise quaternions
			   {entity_qat  = EulerToQuat(0, 0, 0),
				turning_qat = EulerToQuat(math.rad(math.random() * 0.2), 
										  math.rad(math.random() * 0.2), 
										  math.rad(math.random() * 0.2)),
				-- position
				x = g_PlayerPosX + math.sin(angle) * dist,
				y = g_PlayerPosY + math.random(dist) * math.random(-1, 1),
				z = g_PlayerPosZ + math.cos(angle) * dist};
				
			Scale(e, math.random() * 500 + 100)
		end
		return
	else	
		-- this bit rotates the entity
		vars.entity_qat = QuatMul(vars.entity_qat, vars.turning_qat)
												 
		-- get the new Euler angles
		local xr, yr, zr = QuatToEuler(vars.entity_qat)		
		
		SendMessageI("collisionoff",e)
		SetPosition(e, vars.x, vars.y, vars.z)
		SetRotation(e, math.deg(xr), math.deg(yr), math.deg(zr))
		SendMessageI("collisionoon",e)
	end
end

function asteroid_exit (e)
	g_asteroid[e] = nil
end
	
function Rotate3D (x, y, z, xrot, yrot, zrot) 

	function RotatePoint2D (x, y, Ang)  -- Ang in radians
		local Sa, Ca = math.sin(Ang), math.cos(Ang)
		return x*Ca - y*Sa, x*Sa + y*Ca
	end

 
	local NX, NY, NZ = x, y, z

	-- X
	NZ, NY = RotatePoint2D (NZ, NY, -xrot)
	
	-- Y
	NX, NZ = RotatePoint2D (NX, NZ, -yrot)
	
	-- Z
	NY, NX = RotatePoint2D (NY, NX, -zrot)

	return NX, NY, NZ
end

function EulerToQuat(pitch, yaw, roll)    -- radians

	local cr = math.cos(roll/2)
	local cp = math.cos(pitch/2)
	local cy = math.cos(yaw/2)
	local sr = math.sin(roll/2)
	local sp = math.sin(pitch/2)
	local sy = math.sin(yaw/2)
		
	local cycp = cy * cp
	local sysp = sy * sp
	local cysp = cy * sp
	local sycp = sy * cp
	
	return {w = (cr * cycp) + (sr * sysp),
			x = (cr * cysp) - (sr * sycp),
			y = (cr * sycp) + (sr * cysp),
			z = (sr * cycp) - (cr * sysp)}

end

function QuatToEuler(q)
 
	local sqw = q.w*q.w
	local sqx = q.x*q.x
	local sqy = q.y*q.y
	local sqz = q.z*q.z
	
	local h = -2.0 * (q.x*q.z - q.y*q.w)
	
	if math.abs(h) < 0.99999 then
	
		return  math.atan2(2.0 * (q.y*q.z + q.x*q.w),(-sqx - sqy + sqz + sqw)),  -- x ang
				math.asin(-2.0 * (q.x*q.z - q.y*q.w)),                           -- y ang
				math.atan2(2.0 * (q.x*q.y + q.z*q.w),(sqx - sqy - sqz + sqw))    -- z ang
				
	else
		return  math.atan2(2.0 * (q.y*q.z + q.x*q.w),(-sqx - sqy + sqz + sqw)),  -- x ang
				(math.pi / 2) * h,                                               -- y ang
				math.atan2(2.0 * (q.x*q.y + q.z*q.w),(sqx - sqy - sqz + sqw))    -- z ang				
	end
end
		
function QuatMul(q1, q2)

	local A = (q1.w + q1.x)*(q2.w + q2.x)
	local B = (q1.z - q1.y)*(q2.y - q2.z)
	local C = (q1.w - q1.x)*(q2.y + q2.z)
	local D = (q1.y + q1.z)*(q2.w - q2.x)
	local E = (q1.x + q1.z)*(q2.x + q2.y)
	local F = (q1.x - q1.z)*(q2.x - q2.y)
	local G = (q1.w + q1.y)*(q2.w - q2.z)
	local H = (q1.w - q1.y)*(q2.w + q2.z)

	return {w = B + (-E - F + G + H)/2,
		    x = A - ( E + F + G + H)/2,
			y = C + ( E - F + G - H)/2,
			z = D + ( E - F - G + H)/2}
end