#
# Author:: Mark Harrison-Ball <Mark.Harrison-Ball@rockstargames.com>
# Date:: 20 Februray 2013 (AP3)
# Purpose:
#			Vector Conversion utils
#


include RSG::Base::Math
include System

class MathUtils

	def initialize()
		@rad = 57.2957795
	end
	
	# Create a Quaternion from input values
	def quaternionf(x,y,z,w)
		return RSG::Base::Math::Quaternionf.new(x,y,z,w)	
	end
	
	# Return Matrix from Quat
	def Matrix34f(quatIn)
		return RSG::Base::Math::Matrix34f.new(quatIn)
	end
	
	def QuaternionToEulerAngles2(pInput)
		puts pInput.methods
	end
	
	def RadiansToDegrees(pInput)
		return pInput*@rad 
	end
	
	def QuaternionToEulerAngles(pInput)
		puts pInput.W
		puts pInput
		if pInput.W > 1 then			
			pInput.Normalize();
			puts pInput
		end
		
		x =  pInput.X
		y =  pInput.Y
		z =  pInput.Z
		w =  pInput.W
		
		pitch = 0.0
		yaw = 0.0
		roll = 0.0
		puts yaw
		halfPi = System::Math.PI / 2
		test = x * y + z * w
		if test > 0.499 then
			yaw = 2 * System::Math.Atan2(x, w)
			roll = halfPi
			pitch = 0
		elsif test < -0.499 then
			yaw = -2 * System::Math.Atan2(x, w)
			roll = -halfPi
			pitch = 0
		else
			sqx = x * x
			sqy = y * y
			sqz = z * z
			yaw = System::Math.Atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz)
			roll = System::Math.Asin(2 * test)
			pitch = System::Math.Atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz)
		end
		
		pitch = RadiansToDegrees(pitch)
		yaw = RadiansToDegrees(yaw)
		roll = RadiansToDegrees(roll)
		
		puts "#{yaw}, #{roll}, #{pitch}"
	end

end