nuclear@13: #ifndef QUATERNION_H_
nuclear@13: #define	QUATERNION_H_
nuclear@13: 
nuclear@13: class Quat;
nuclear@13: 
nuclear@13: inline Quat operator *(const Quat &a, const Quat &b);
nuclear@13: 
nuclear@13: class Quat{
nuclear@13: public:
nuclear@13: 	float x, y, z, w;
nuclear@13: 
nuclear@13: 	Quat() : x(0), y(0), z(0), w(1) {}
nuclear@13: 	Quat(float xx, float yy, float zz, float ww) : x(xx), y(yy), z(zz), w(ww) {}
nuclear@13: 
nuclear@13: 	void set_identity()
nuclear@13: 	{
nuclear@13: 		x = y = z = 0.0f;
nuclear@13: 		w = 1.0f;
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	Quat conjugate() const
nuclear@13: 	{
nuclear@13: 		return Quat(-x, -y, -z, w);
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	float length() const
nuclear@13: 	{
nuclear@13: 		return (float)sqrt(x * x + y * y + z * z + w * w);
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	float length_sq() const
nuclear@13: 	{
nuclear@13: 		return x * x + y * y + z * z + w * w;
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	void normalize()
nuclear@13: 	{
nuclear@13: 		float len = length();
nuclear@13: 		if(len != 0.0) {
nuclear@13: 			x /= len;
nuclear@13: 			y /= len;
nuclear@13: 			z /= len;
nuclear@13: 			w /= len;
nuclear@13: 		}
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	Quat inverse() const
nuclear@13: 	{
nuclear@13: 		Quat inv = conjugate();
nuclear@13: 		float len_sq = length_sq();
nuclear@13: 		if(len_sq != 0.0) {
nuclear@13: 			inv.x /= len_sq;
nuclear@13: 			inv.y /= len_sq;
nuclear@13: 			inv.z /= len_sq;
nuclear@13: 			inv.w /= len_sq;
nuclear@13: 		}
nuclear@13: 		return inv;
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	void set_rotation(float angle, float axis_x, float axis_y, float axis_z)
nuclear@13: 	{
nuclear@13: 		float half_angle = angle * 0.5;
nuclear@13: 		float sin_half = sin(half_angle);
nuclear@13: 
nuclear@13: 		w = cos(half_angle);
nuclear@13: 		x = axis_x * sin_half;
nuclear@13: 		y = axis_y * sin_half;
nuclear@13: 		z = axis_z * sin_half;
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	void rotate(float angle, float x, float y, float z)
nuclear@13: 	{
nuclear@13: 		Quat q;
nuclear@13: 		q.set_rotation(angle, x, y, z);
nuclear@13: 		*this = *this * q;
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	void rotate(const Quat &q)
nuclear@13: 	{
nuclear@13: 		*this = q * *this * q.conjugate();
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	Matrix4x4 get_matrix() const
nuclear@13: 	{
nuclear@13: 		return Matrix4x4(
nuclear@13: 			1.0 - 2.0 * y*y - 2.0 * z*z,	2.0 * x * y - 2.0 * w * z,		2.0 * z * x + 2.0 * w * y, 0,
nuclear@13: 			2.0 * x * y + 2.0 * w * z,		1.0 - 2.0 * x*x - 2.0 * z*z,	2.0 * y * z - 2.0 * w * x, 0,
nuclear@13: 			2.0 * z * x - 2.0 * w * y,		2.0 * y * z + 2.0 * w * x,		1.0 - 2.0 * x*x - 2.0 * y*y, 0,
nuclear@13: 			0, 0, 0, 1);
nuclear@13: 	}
nuclear@13: };
nuclear@13: 
nuclear@13: inline Quat operator *(const Quat &a, const Quat &b)
nuclear@13: {
nuclear@13: 	float dot = a.x * b.x + a.y * b.y + a.z * b.z;
nuclear@13: 	float cross_x = a.y * b.z - a.z * b.y;
nuclear@13: 	float cross_y = a.z * b.x - a.x * b.z;
nuclear@13: 	float cross_z = a.x * b.y - a.y * b.x;
nuclear@13: 
nuclear@13: 	float w = a.w * b.w - dot;
nuclear@13: 	float x = a.x * b.w + b.x * a.w + cross_x;
nuclear@13: 	float y = a.y * b.w + b.y * a.w + cross_y;
nuclear@13: 	float z = a.z * b.w + b.z * a.w + cross_z;
nuclear@13: 
nuclear@13: 	return Quat(x, y, z, w);
nuclear@13: }
nuclear@13: 
nuclear@13: #endif	// QUATERNION_H_