bluecore/engine/Math/Quaternion.h

#ifndef BLUECORE_QUATERNION_H
#define BLUECORE_QUATERNION_H

#include "Vector.h"

namespace BlueCore
{
template<class T> class QuaternionTemplate
{
public:
	T w, x, y, z;

	inline QuaternionTemplate()
	{
		w = static_cast<T>(1.0);
		x = static_cast<T>(0.0);
		y = static_cast<T>(0.0);
		z = static_cast<T>(0.0);
	}

	template<class S> inline QuaternionTemplate(S w, S x, S y, S z)
	{
		this->w = static_cast<T>(w);
		this->x = static_cast<T>(x);
		this->y = static_cast<T>(y);
		this->z = static_cast<T>(z);
	}

	template<class S> inline QuaternionTemplate(Vector3Template<S> axis, S angle)
	{
		T half_angle = static_cast<T>(angle)/static_cast<T>(2.0);
		T sin_half_angle = static_cast<T>(sin(half_angle) );

		w = static_cast<T>(cos(half_angle) );
		x = sin_half_angle * static_cast<T>(axis.x);
		y = sin_half_angle * static_cast<T>(axis.y);
		z = sin_half_angle * static_cast<T>(axis.z);
	}

	template<class S> inline QuaternionTemplate(S h, S a, S b)
	{
		T c1 = static_cast<T>(cos(h / 2.0) );
		T c2 = static_cast<T>(cos(a / 2.0) );
		T c3 = static_cast<T>(cos(b / 2.0) );
		T s1 = static_cast<T>(sin(h / 2.0) );
		T s2 = static_cast<T>(sin(a / 2.0) );
		T s3 = static_cast<T>(sin(b / 2.0) );

		w = c1 * c2 * c3 - s1 * s2 * s3;
		x = s1 * s2 * c3 + c1 * c2 * s3;
		y = s1 * c2 * c3 + c1 * s2 * s3;
		z = c1 * s2 * c3 - s1 * c2 * s3;
	}

	inline void identity()
	{
		w = static_cast<T>(1.0);
		x = static_cast<T>(0.0);
		y = static_cast<T>(0.0);
		z = static_cast<T>(0.0);
	}

	template<class S> inline QuaternionTemplate<T> operator +(
			const QuaternionTemplate<S> &a) const
	{
		return QuaternionTemplate<T>(w + static_cast<T>(a.w), x
				+ static_cast<T>(a.x), y + static_cast<T>(a.y), z
				+ static_cast<T>(a.z) );
	}

	template<class S> inline QuaternionTemplate<T> &operator +=(
			const QuaternionTemplate<S> &a)
	{
		w += static_cast<T>(a.w);
		x += static_cast<T>(a.x);
		y += static_cast<T>(a.y);
		z += static_cast<T>(a.z);

		return *this;
	}

	template<class S> inline QuaternionTemplate<T> operator -(
			const QuaternionTemplate<S> &a) const
	{
		return QuaternionTemplate<T>(w - static_cast<T>(a.w), x
				- static_cast<T>(a.x), y - static_cast<T>(a.y), z
				- static_cast<T>(a.z) );
	}

	template<class S> inline QuaternionTemplate<T> operator *(
			const QuaternionTemplate<S> &a) const
	{
		return QuaternionTemplate<T>(w * static_cast<T>(a.w) - x
				* static_cast<T>(a.x) - y * static_cast<T>(a.y) - z
				* static_cast<T>(a.z), w * static_cast<T>(a.x) + x
				* static_cast<T>(a.w) + y * static_cast<T>(a.z) - z
				* static_cast<T>(a.y), w * static_cast<T>(a.y) - x
				* static_cast<T>(a.z) + y * static_cast<T>(a.w) + z
				* static_cast<T>(a.x), w * static_cast<T>(a.z) + x
				* static_cast<T>(a.y) - y * static_cast<T>(a.x) + z
				* static_cast<T>(a.w) );
	}

	template<class S> inline QuaternionTemplate<T> &operator *=(
			const QuaternionTemplate<S> &a)
	{
		w = w * static_cast<T>(a.w) - x * static_cast<T>(a.x) - y
				* static_cast<T>(a.y) - z * static_cast<T>(a.z);
		x = w * static_cast<T>(a.x) + x * static_cast<T>(a.w) + y
				* static_cast<T>(a.z) - z * static_cast<T>(a.y);
		y = w * static_cast<T>(a.y) - x * static_cast<T>(a.z) + y
				* static_cast<T>(a.w) + z * static_cast<T>(a.x);
		z = w * static_cast<T>(a.z) + x * static_cast<T>(a.y) - y
				* static_cast<T>(a.x) + z * static_cast<T>(a.w);

		return *this;
	}

	template<class S> inline QuaternionTemplate<T> &operator -=(
			const QuaternionTemplate<S> &a)
	{
		w -= static_cast<T>(a.w);
		x -= static_cast<T>(a.x);
		y -= static_cast<T>(a.y);
		z -= static_cast<T>(a.z);

		return *this;
	}

	template<class S> inline QuaternionTemplate<T> &operator =(
			const QuaternionTemplate<S> &a)
	{
		w = static_cast<T>(a.w);
		x = static_cast<T>(a.x);
		y = static_cast<T>(a.y);
		z = static_cast<T>(a.z);

		return *this;
	}

	inline QuaternionTemplate<T> unit() const
	{
		T d = 1/sqrt(w*w + x*x + y*y + z*z);
		return QuaternionTemplate<T>(w * d, x * d, y * d, z * d);
	}

	inline QuaternionTemplate<T> inversed() const
	{
		return QuaternionTemplate<T>(w, -x, -y, -z);
	}

	inline void inverse()
	{
		x = -x;
		y = -y;
		z = -z;
	}

	inline QuaternionTemplate<T> &normalize()
	{
		T d = 1/sqrt(w*w + x*x + y*y + z*z);
		w *= d;
		x *= d;
		y *= d;
		z *= d;

		return *this;
	}

	template<class S> inline const Vector3Template<T> apply(
			const Vector3Template<S> &a) const
	{
		T xx = x*x, xy = x*y, xz = x*z, xw = x*w, yy = y*y, yz = y*z, yw = y*w,
				zz = z*z, zw = z*w;

		return Vector3Template<T>( 2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy
				- zz ) + static_cast<T>(a.y) * (xy - zw ) + static_cast<T>(a.z)
				* (xz + yw ) ),

		2.0 * ( static_cast<T>(a.x) * (xy + zw ) + static_cast<T>(a.y) * ( 0.5
				- xx - zz ) + static_cast<T>(a.z) * (yz - xw ) ),

		2.0 * ( static_cast<T>(a.x) * (xz - yw ) + static_cast<T>(a.y) * (yz
				+ xw ) + static_cast<T>(a.z) * ( 0.5 - xx - yy ) ));
	}

	template<class S> inline const Vector3Template<T> operator *(
			const Vector3Template<S> &a) const
	{
		T xx = x*x, xy = x*y, xz = x*z, xw = x*w, yy = y*y, yz = y*z, yw = y*w,
				zz = z*z, zw = z*w;

		return Vector3Template<T>( 2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy
				- zz ) + static_cast<T>(a.y) * (xy - zw ) + static_cast<T>(a.z)
				* (xz + yw ) ),

		2.0 * ( static_cast<T>(a.x) * (xy + zw ) + static_cast<T>(a.y) * ( 0.5
				- xx - zz ) + static_cast<T>(a.z) * (yz - xw ) ),

		2.0 * ( static_cast<T>(a.x) * (xz - yw ) + static_cast<T>(a.y) * (yz
				+ xw ) + static_cast<T>(a.z) * ( 0.5 - xx - yy ) ));
	}

	template<class S> inline Vector3Template<T> applyInversed(
			const Vector3Template<S> &a) const
	{
		T xx = x*x, xy = x*y, xz = x*z, xw = -x*w, yy = y*y, yz = y*z, yw = -y
				*w, zz = z*z, zw = -z*w;

		return Vector3Template<T>( 2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy
				- zz ) + static_cast<T>(a.y) * (xy - zw ) + static_cast<T>(a.z)
				* (xz + yw ) ),

		2.0 * ( static_cast<T>(a.x) * (xy + zw ) + static_cast<T>(a.y) * ( 0.5
				- xx - zz ) + static_cast<T>(a.z) * (yz - xw ) ),

		2.0 * ( static_cast<T>(a.x) * (xz - yw ) + static_cast<T>(a.y) * (yz
				+ xw ) + static_cast<T>(a.z) * ( 0.5 - xx - yy ) ));
	}

	inline QuaternionTemplate<T> slerp(const QuaternionTemplate<T> &q,
			const Scalar &t)
	{
		T phi = acos(w*q.w + x*q.x + y*q.y + z*q.z);
		T s = 1 / sin(phi);
		T a = sin(phi*(1-t)) * s;
		T b = sin(phi*t) * s;

		return QuaternionTemplate<T>(a*w+b*q.w, a*x+b*q.x, a*y+b*q.y, a*z+b*q.z);
	}
};

typedef QuaternionTemplate<float> QuaternionFloat;
typedef QuaternionTemplate<double> QuaternionDouble;
typedef QuaternionTemplate<Scalar> Quaternion;
}

#endif