/*
/ MATRIX.H
/ ********
/ Matrix definition and operation class.
/
/ NOTE: Aspects of this code were borrowed
/ from http://irrlicht.sourceforge.net/ in
/ order to gain a mathematical understanding
/ of various matrix operations.
/
/ Will Beddow 2010 - http://343gs.com/brainstem
/
/ This software is currently under development
/ and is subject to change and improvement. Some
/ aspects of the system are incomplete and/or 
/ unstable due to ongoing implementation.
/
*/

#ifndef __MATRIX_H__
#define __MATRIX_H__

#include "Compile.h"
#include "Types.h"

namespace Brainstem
{
	template <class T>
	class Matrix4
	{
		public:

			enum eConstructor
			{
				EM4CONST_NOTHING = 0,
				EM4CONST_COPY,
				EM4CONST_IDENTITY,
				EM4CONST_TRANSPOSED,
				EM4CONST_INVERSE,
				EM4CONST_INVERSE_TRANSPOSED
			};

			Matrix4(eConstructor constructor = EM4CONST_IDENTITY);
			Matrix4(const Matrix4<T>& other, eConstructor constructor = EM4CONST_COPY);

			T& operator()(const int row, const int col) { return M[row * 4 + col]; }
			const T& operator()(const int row, const int col) const { return M[row * 4 + col]; }
			T& operator[](int index) { return M[index]; }

			const T& operator[](int index) const { return M[index]; }
			inline Matrix4<T>& operator=(const Matrix4<T> &other);
			inline Matrix4<T>& operator=(const T& scalar);

			const T* Ptr() const { return M; }
			T* Ptr() { return M; }

			bool operator==(const Matrix4<T> &other) const;
			bool operator!=(const Matrix4<T> &other) const;

			Matrix4<T> operator+(const Matrix4<T>& other) const;
			Matrix4<T>& operator+=(const Matrix4<T>& other);
			Matrix4<T> operator-(const Matrix4<T>& other) const;
			Matrix4<T>& operator-=(const Matrix4<T>& other);

			Matrix4<T> operator*(const Matrix4<T>& other) const;
			Matrix4<T>& operator*=(const Matrix4<T>& other);
			Matrix4<T> operator*(const T& scalar) const;
			Matrix4<T>& operator*=(const T& scalar);

			Matrix4<T>& SetByProduct(const Matrix4<T>& other_a,const Matrix4<T>& other_b);

			inline Matrix4<T>& SetIdentity();

			inline bool IsIdentity() const;
			Matrix4<T>& SetTranslation(const Vector3<T>& translation);

			Vector3<T> GetTranslation() const;
			Matrix4<T>& SetInverseTranslation(const Vector3<T>& translation);

			inline Matrix4<T>& SetRotationRadians(const Vector3<T>& rotation);
			inline Matrix4<T>& SetInverseRotationRadians(const Vector3<T>& rotation);

			Matrix4<T>& SetRotationDegrees(const Vector3<T>& rotation);
			Vector3<T> GetRotationDegrees() const;
			Matrix4<T>& SetInverseRotationDegrees( const Vector3<T>& rotation );

			Matrix4<T>& SetScale(const Vector3<T>& scale);
			Matrix4<T>& SetScale(const T scale) { return SetScale(Vector3<T>(scale,scale,scale)); }
			Vector3<T> GetScale() const;

			void InverseTranslateVector(Vector3<float>& vect) const;
			void InverseRotateVector(Vector3<float>& vect) const;

			void RotateVector(Vector3<float>& vect) const;
			void RotateVector(Vector3<float>& out, const Vector3<float>& in) const;
			void RotateVector(T *out,const Vector3<float> &in) const;

			void TransformVector(Vector3<float>& vect) const;
			void TransformVector(Vector3<float>& out, const Vector3<float>& in) const;
			void TransformVector(T *out,const Vector3<float> &in) const;
			void TranslateVector(Vector3<float>& vect) const;

			void MultiplyWith1x4Matrix(T* matrix) const;
			bool MakeInverse();

			bool GetInversePrimitive (Matrix4<T>& out) const;
			bool GetInverse(Matrix4<T>& out) const;

			T GetAt(int row, int col);

			Matrix4<T>& CreateProjectionMatrixPerspectiveFovLH(float fieldOfViewRadians, float aspectRatio, float zNear, float zFar);
			Matrix4<T>& CreateProjectionMatrixPerspectiveLH(float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar);
			Matrix4<T>& CreateProjectionMatrixOrthoLH(float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar);
			Matrix4<T>& CreateCameraLookAtMatrixLH(const Vector3<float>& position, const Vector3<float>& target, const Vector3<float>& upVector);

			Matrix4<T> Interpolate(const Matrix4<T>& b, float time) const;

			Matrix4<T> GetTransposed() const;
			inline void GetTransposed(Matrix4<T>& dest) const;

			void SetRotationCenter(const Vector3<float>& center, const Vector3<float>& translate);

			void CreateAxisAlignedBillboard(const Vector3<float>& camPos,
				const Vector3<float>& center,
				const Vector3<float>& translation,
				const Vector3<float>& axis,
				const Vector3<float>& from);

			Matrix4<T>& SetData(const T* data);
			bool Equals(const Matrix4<T>& other) const;

			T M[16];
	};

	template <class T>
	inline Matrix4<T>::Matrix4( eConstructor constructor )
	{
		switch (constructor)
		{
			case EM4CONST_NOTHING:
			case EM4CONST_COPY:
				break;
			case EM4CONST_IDENTITY:
			case EM4CONST_INVERSE:
			default:
				SetIdentity();
				break;
		}
	}

	template <class T>
	inline Matrix4<T>::Matrix4(const Matrix4<T>& other, eConstructor constructor)
	{
		switch (constructor)
		{
			case EM4CONST_IDENTITY:
				SetIdentity();
				break;
			case EM4CONST_NOTHING:
				break;
			case EM4CONST_COPY:
				*this = other;
				break;
			case EM4CONST_TRANSPOSED:
				other.GetTransposed(*this);
				break;
			case EM4CONST_INVERSE:
				if (!other.GetInverse(*this))
					memset(M, 0, 16*sizeof(T));
				break;
			case EM4CONST_INVERSE_TRANSPOSED:
				if (!other.GetInverse(*this))
					memset(M, 0, 16*sizeof(T));
				else
					*this = GetTransposed();
				break;
		}
	}

	template <class T>
	inline Matrix4<T> Matrix4<T>::operator+(const Matrix4<T>& other) const
	{
		Matrix4<T> temp (EM4CONST_NOTHING);

		temp[0] = M[0]+other[0];
		temp[1] = M[1]+other[1];
		temp[2] = M[2]+other[2];
		temp[3] = M[3]+other[3];
		temp[4] = M[4]+other[4];
		temp[5] = M[5]+other[5];
		temp[6] = M[6]+other[6];
		temp[7] = M[7]+other[7];
		temp[8] = M[8]+other[8];
		temp[9] = M[9]+other[9];
		temp[10] = M[10]+other[10];
		temp[11] = M[11]+other[11];
		temp[12] = M[12]+other[12];
		temp[13] = M[13]+other[13];
		temp[14] = M[14]+other[14];
		temp[15] = M[15]+other[15];

		return temp;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::operator+=(const Matrix4<T>& other)
	{
		M[0]+=other[0];
		M[1]+=other[1];
		M[2]+=other[2];
		M[3]+=other[3];
		M[4]+=other[4];
		M[5]+=other[5];
		M[6]+=other[6];
		M[7]+=other[7];
		M[8]+=other[8];
		M[9]+=other[9];
		M[10]+=other[10];
		M[11]+=other[11];
		M[12]+=other[12];
		M[13]+=other[13];
		M[14]+=other[14];
		M[15]+=other[15];

		return *this;
	}

	template <class T>
	inline Matrix4<T> Matrix4<T>::operator-(const Matrix4<T>& other) const
	{
		Matrix4<T> temp (EM4CONST_NOTHING);

		temp[0] = M[0]-other[0];
		temp[1] = M[1]-other[1];
		temp[2] = M[2]-other[2];
		temp[3] = M[3]-other[3];
		temp[4] = M[4]-other[4];
		temp[5] = M[5]-other[5];
		temp[6] = M[6]-other[6];
		temp[7] = M[7]-other[7];
		temp[8] = M[8]-other[8];
		temp[9] = M[9]-other[9];
		temp[10] = M[10]-other[10];
		temp[11] = M[11]-other[11];
		temp[12] = M[12]-other[12];
		temp[13] = M[13]-other[13];
		temp[14] = M[14]-other[14];
		temp[15] = M[15]-other[15];

		return temp;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::operator-=(const Matrix4<T>& other)
	{
		M[0]-=other[0];
		M[1]-=other[1];
		M[2]-=other[2];
		M[3]-=other[3];
		M[4]-=other[4];
		M[5]-=other[5];
		M[6]-=other[6];
		M[7]-=other[7];
		M[8]-=other[8];
		M[9]-=other[9];
		M[10]-=other[10];
		M[11]-=other[11];
		M[12]-=other[12];
		M[13]-=other[13];
		M[14]-=other[14];
		M[15]-=other[15];

		return *this;
	}

	template <class T>
	inline Matrix4<T> Matrix4<T>::operator*(const T& scalar) const
	{
		Matrix4<T> temp (EM4CONST_NOTHING);

		temp[0] = M[0]*scalar;
		temp[1] = M[1]*scalar;
		temp[2] = M[2]*scalar;
		temp[3] = M[3]*scalar;
		temp[4] = M[4]*scalar;
		temp[5] = M[5]*scalar;
		temp[6] = M[6]*scalar;
		temp[7] = M[7]*scalar;
		temp[8] = M[8]*scalar;
		temp[9] = M[9]*scalar;
		temp[10] = M[10]*scalar;
		temp[11] = M[11]*scalar;
		temp[12] = M[12]*scalar;
		temp[13] = M[13]*scalar;
		temp[14] = M[14]*scalar;
		temp[15] = M[15]*scalar;

		return temp;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::operator*=(const T& scalar)
	{
		M[0]*=scalar;
		M[1]*=scalar;
		M[2]*=scalar;
		M[3]*=scalar;
		M[4]*=scalar;
		M[5]*=scalar;
		M[6]*=scalar;
		M[7]*=scalar;
		M[8]*=scalar;
		M[9]*=scalar;
		M[10]*=scalar;
		M[11]*=scalar;
		M[12]*=scalar;
		M[13]*=scalar;
		M[14]*=scalar;
		M[15]*=scalar;

		return *this;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::operator*=(const Matrix4<T>& other)
	{
		Matrix4<T> temp (*this);
		return SetByProduct(temp, other);
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetByProduct(const Matrix4<T>& other_a,const Matrix4<T>& other_b )
	{
		const T *m1 = other_a.M;
		const T *m2 = other_b.M;

		M[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
		M[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
		M[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
		M[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];

		M[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
		M[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
		M[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
		M[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];

		M[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
		M[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
		M[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
		M[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];

		M[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
		M[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
		M[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
		M[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
		return *this;
	}

	template <class T>
	inline Matrix4<T> Matrix4<T>::operator*(const Matrix4<T>& m2) const
	{
		Matrix4<T> m3 (EM4CONST_NOTHING);

		const T *m1 = M;

		m3[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
		m3[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
		m3[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
		m3[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];

		m3[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
		m3[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
		m3[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
		m3[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];

		m3[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
		m3[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
		m3[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
		m3[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];

		m3[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
		m3[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
		m3[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
		m3[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
		return m3;
	}

	template <class T>
	inline Vector3<T> Matrix4<T>::GetTranslation() const
	{
		return Vector3<T>(M[12], M[13], M[14]);
	}


	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetTranslation(const Vector3<T>& translation)
	{
		M[12] = translation.X;
		M[13] = translation.Y;
		M[14] = translation.Z;
		return *this;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetInverseTranslation(const Vector3<T>& translation)
	{
		M[12] = -translation.X;
		M[13] = -translation.Y;
		M[14] = -translation.Z;
		return *this;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetScale(const Vector3<T>& scale)
	{
		M[0] = scale.X;
		M[5] = scale.Y;
		M[10] = scale.Z;
		return *this;
	}

	template <class T>
	inline Vector3<T> Matrix4<T>::GetScale() const
	{
		if(iszero(M[1]) && iszero(M[2]) &&
			iszero(M[4]) && iszero(M[6]) &&
			iszero(M[8]) && iszero(M[9]))
			return Vector3<T>(M[0], M[5], M[10]);

		return Vector3<T>(sqrtf(M[0] * M[0] + M[1] * M[1] + M[2] * M[2]),
							sqrtf(M[4] * M[4] + M[5] * M[5] + M[6] * M[6]),
							sqrtf(M[8] * M[8] + M[9] * M[9] + M[10] * M[10]));
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetRotationDegrees(const Vector3<T>& rotation)
	{
		return SetRotationRadians(BSTM_DEG_TO_RAD(rotation));
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetInverseRotationDegrees(const Vector3<T>& rotation)
	{
		return SetInverseRotationRadians(BSTM_DEG_TO_RAD(rotation));
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetRotationRadians(const Vector3<T>& rotation)
	{
		const float cr = cos(rotation.X);
		const float sr = sin(rotation.X);
		const float cp = cos(rotation.Y);
		const float sp = sin(rotation.Y);
		const float cy = cos(rotation.Z);
		const float sy = sin(rotation.Z);

		M[0] = (T)(cp * cy);
		M[1] = (T)(cp * sy);
		M[2] = (T)(-sp);

		const float srsp = sr * sp;
		const float crsp = cr * sp;

		M[4] = (T)(srsp * cy- cr * sy);
		M[5] = (T)(srsp * sy + cr * cy);
		M[6] = (T)(sr * cp);

		M[8] = (T)(crsp * cy + sr * sy);
		M[9] = (T)(crsp * sy - sr * cy);
		M[10] = (T)(cr * cp);
		return *this;
	}

	template <class T>
	inline Vector3<T> Matrix4<T>::GetRotationDegrees() const
	{
		const Matrix4<T> &mat = *this;
		const Vector3<T> scale = GetScale();
		const vector3d<float> invScale(reciprocal(scale.X), reciprocal(scale.Y), reciprocal(scale.Z));

		float Y = -asin(mat[2] * invScale.X);
		const float C = cos(Y);
		Y *= (BSTM_PI / 180.0f);

		float rotx, roty, X, Z;

		if (!iszero(C))
		{
			const float invC = reciprocal(C);
			rotx = mat[10] * invC * invScale.Z;
			roty = mat[6] * invC * invScale.Y;
			X = atan2(roty, rotx) * (BSTM_PI / 180.0f);
			rotx = mat[0] * invC * invScale.X;
			roty = mat[1] * invC * invScale.X;
			Z = atan2(roty, rotx) * (BSTM_PI / 180.0f);
		}
		else
		{
			X = 0.0f;
			rotx = mat[5] * invScale.Y;
			roty = -mat[4] * invScale.Y;
			Z = atan2(roty, rotx) * (BSTM_PI / 180.0f);
		}

		if (X < 0.0f) X += 360.0f;
		if (Y < 0.0f) Y += 360.0f;
		if (Z < 0.0f) Z += 360.0f;

		return Vector3<T>((T)X,(T)Y,(T)Z);
	}


	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetInverseRotationRadians(const Vector3<T>& rotation)
	{
		float cr = cos(rotation.X);
		float sr = sin(rotation.X);
		float cp = cos(rotation.Y);
		float sp = sin(rotation.Y);
		float cy = cos(rotation.Z);
		float sy = sin(rotation.Z);

		M[0] = (T)(cp * cy);
		M[4] = (T)(cp * sy);
		M[8] = (T)(-sp);

		float srsp = sr * sp;
		float crsp = cr * sp;

		M[1] = (T)(srsp * cy - cr * sy);
		M[5] = (T)(srsp * sy + cr * cy);
		M[9] = (T)(sr * cp);

		M[2] = (T)(crsp * cy + sr * sy);
		M[6] = (T)(crsp * sy - sr * cy);
		M[10] = (T)(cr * cp);
		return *this;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetIdentity()
	{
		memset(M, 0, 16*sizeof(T));
		M[0] = M[5] = M[10] = M[15] = (T)1;
		return *this;
	}

	template <class T>
	inline bool Matrix4<T>::IsIdentity() const
	{
		if (!Equals(M[0], (T)1) ||
				!Equals(M[5], (T)1) ||
				!Equals(M[10], (T)1) ||
				!Equals(M[15], (T)1))
			return false;

		for (int i = 0; i < 4; ++i)
			for (int j = 0; j < 4; ++j)
				if ((j != i) && (!iszero((*this)(i,j))))
					return false;

		return true;
	}

	template <class T>
	inline void Matrix4<T>::RotateVector(Vector3<float>& vect) const
	{
		Vector3<float> tmp = vect;
		vect.X = tmp.X * M[0] + tmp.Y * M[4] + tmp.Z * M[8];
		vect.Y = tmp.X * M[1] + tmp.Y * M[5] + tmp.Z * M[9];
		vect.Z = tmp.X * M[2] + tmp.Y * M[6] + tmp.Z * M[10];
	}

	template <class T>
	inline void Matrix4<T>::RotateVector(Vector3<float>& out, const Vector3<float>& in) const
	{
		out.X = in.X * M[0] + in.Y * M[4] + in.Z * M[8];
		out.Y = in.X * M[1] + in.Y * M[5] + in.Z * M[9];
		out.Z = in.X * M[2] + in.Y * M[6] + in.Z * M[10];
	}

	template <class T>
	inline void Matrix4<T>::RotateVector(T *out, const Vector3<float>& in) const
	{
		out[0] = in.X * M[0] + in.Y * M[4] + in.Z * M[8];
		out[1] = in.X * M[1] + in.Y * M[5] + in.Z * M[9];
		out[2] = in.X * M[2] + in.Y * M[6] + in.Z * M[10];
	}

	template <class T>
	inline void Matrix4<T>::InverseRotateVector(Vector3<float>& vect) const
	{
		Vector3<float> tmp = vect;
		vect.X = tmp.X * M[0] + tmp.Y * M[1] + tmp.Z * M[2];
		vect.Y = tmp.X * M[4] + tmp.Y * M[5] + tmp.Z * M[6];
		vect.Z = tmp.X * M[8] + tmp.Y * M[9] + tmp.Z * M[10];
	}

	template <class T>
	inline void Matrix4<T>::TransformVector(Vector3<float>& vect) const
	{
		float vector[3];

		vector[0] = vect.X * M[0] + vect.Y * M[4] + vect.Z * M[8] + M[12];
		vector[1] = vect.X * M[1] + vect.Y * M[5] + vect.Z * M[9] + M[13];
		vector[2] = vect.X * M[2] + vect.Y * M[6] + vect.Z * M[10] + M[14];

		vect.X = vector[0];
		vect.Y = vector[1];
		vect.Z = vector[2];
	}

	template <class T>
	inline void Matrix4<T>::TransformVector(Vector3<float>& out, const Vector3<float>& in) const
	{
		out.X = in.X * M[0] + in.Y * M[4] + in.Z * M[8] + M[12];
		out.Y = in.X * M[1] + in.Y * M[5] + in.Z * M[9] + M[13];
		out.Z = in.X * M[2] + in.Y * M[6] + in.Z * M[10] + M[14];
	}


	template <class T>
	inline void Matrix4<T>::TransformVector(T *out, const Vector3<float> &in) const
	{
		out[0] = in.X * M[0] + in.Y * M[4] + in.Z * M[8] + M[12];
		out[1] = in.X * M[1] + in.Y * M[5] + in.Z * M[9] + M[13];
		out[2] = in.X * M[2] + in.Y * M[6] + in.Z * M[10] + M[14];
		out[3] = in.X * M[3] + in.Y * M[7] + in.Z * M[11] + M[15];
	}

	template <class T>
	inline void Matrix4<T>::MultiplyWith1x4Matrix(T* matrix) const
	{
		T mat[4];
		mat[0] = matrix[0];
		mat[1] = matrix[1];
		mat[2] = matrix[2];
		mat[3] = matrix[3];

		matrix[0] = M[0] * mat[0] + M[4] * mat[1] + M[8] * mat[2] + M[12] * mat[3];
		matrix[1] = M[1] * mat[0] + M[5] * mat[1] + M[9] * mat[2] + M[13] * mat[3];
		matrix[2] = M[2] * mat[0] + M[6] * mat[1] + M[10] * mat[2] + M[14] * mat[3];
		matrix[3] = M[3] * mat[0] + M[7] * mat[1] + M[11] * mat[2] + M[15] * mat[3];
	}

	template <class T>
	inline void Matrix4<T>::InverseTranslateVector(Vector3<float>& vect) const
	{
		vect.X = vect.X - M[12];
		vect.Y = vect.Y - M[13];
		vect.Z = vect.Z - M[14];
	}

	template <class T>
	inline void Matrix4<T>::TranslateVector(Vector3<float>& vect) const
	{
		vect.X = vect.X + M[12];
		vect.Y = vect.Y + M[13];
		vect.Z = vect.Z + M[14];
	}

	template <class T>
	inline bool Matrix4<T>::GetInverse(Matrix4<T>& out) const
	{
		const Matrix4<T> &m = *this;

		float d = (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) * (m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2)) -
			(m(0, 0) * m(1, 2) - m(0, 2) * m(1, 0)) * (m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1)) +
			(m(0, 0) * m(1, 3) - m(0, 3) * m(1, 0)) * (m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1)) +
			(m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * (m(2, 0) * m(3, 3) - m(2, 3) * m(3, 0)) -
			(m(0, 1) * m(1, 3) - m(0, 3) * m(1, 1)) * (m(2, 0) * m(3, 2) - m(2, 2) * m(3, 0)) +
			(m(0, 2) * m(1, 3) - m(0, 3) * m(1, 2)) * (m(2, 0) * m(3, 1) - m(2, 1) * m(3, 0));

		if(fabs(d))
			return false;

		d = 1.0f / d;

		out(0, 0) = d * (m(1, 1) * (m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2)) +
				m(1, 2) * (m(2, 3) * m(3, 1) - m(2, 1) * m(3, 3)) +
				m(1, 3) * (m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1)));
		out(0, 1) = d * (m(2, 1) * (m(0, 2) * m(3, 3) - m(0, 3) * m(3, 2)) +
				m(2, 2) * (m(0, 3) * m(3, 1) - m(0, 1) * m(3, 3)) +
				m(2, 3) * (m(0, 1) * m(3, 2) - m(0, 2) * m(3, 1)));
		out(0, 2) = d * (m(3, 1) * (m(0, 2) * m(1, 3) - m(0, 3) * m(1, 2)) +
				m(3, 2) * (m(0, 3) * m(1, 1) - m(0, 1) * m(1, 3)) +
				m(3, 3) * (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)));
		out(0, 3) = d * (m(0, 1) * (m(1, 3) * m(2, 2) - m(1, 2) * m(2, 3)) +
				m(0, 2) * (m(1, 1) * m(2, 3) - m(1, 3) * m(2, 1)) +
				m(0, 3) * (m(1, 2) * m(2, 1) - m(1, 1) * m(2, 2)));
		out(1, 0) = d * (m(1, 2) * (m(2, 0) * m(3, 3) - m(2, 3) * m(3, 0)) +
				m(1, 3) * (m(2, 2) * m(3, 0) - m(2, 0) * m(3, 2)) +
				m(1, 0) * (m(2, 3) * m(3, 2) - m(2, 2) * m(3, 3)));
		out(1, 1) = d * (m(2, 2) * (m(0, 0) * m(3, 3) - m(0, 3) * m(3, 0)) +
				m(2, 3) * (m(0, 2) * m(3, 0) - m(0, 0) * m(3, 2)) +
				m(2, 0) * (m(0, 3) * m(3, 2) - m(0, 2) * m(3, 3)));
		out(1, 2) = d * (m(3, 2) * (m(0, 0) * m(1, 3) - m(0, 3) * m(1, 0)) +
				m(3, 3) * (m(0, 2) * m(1, 0) - m(0, 0) * m(1, 2)) +
				m(3, 0) * (m(0, 3) * m(1, 2) - m(0, 2) * m(1, 3)));
		out(1, 3) = d * (m(0, 2) * (m(1, 3) * m(2, 0) - m(1, 0) * m(2, 3)) +
				m(0, 3) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
				m(0, 0) * (m(1, 2) * m(2, 3) - m(1, 3) * m(2, 2)));
		out(2, 0) = d * (m(1, 3) * (m(2, 0) * m(3, 1) - m(2, 1) * m(3, 0)) +
				m(1, 0) * (m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1)) +
				m(1, 1) * (m(2, 3) * m(3, 0) - m(2, 0) * m(3, 3)));
		out(2, 1) = d * (m(2, 3) * (m(0, 0) * m(3, 1) - m(0, 1) * m(3, 0)) +
				m(2, 0) * (m(0, 1) * m(3, 3) - m(0, 3) * m(3, 1)) +
				m(2, 1) * (m(0, 3) * m(3, 0) - m(0, 0) * m(3, 3)));
		out(2, 2) = d * (m(3, 3) * (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) +
				m(3, 0) * (m(0, 1) * m(1, 3) - m(0, 3) * m(1, 1)) +
				m(3, 1) * (m(0, 3) * m(1, 0) - m(0, 0) * m(1, 3)));
		out(2, 3) = d * (m(0, 3) * (m(1, 1) * m(2, 0) - m(1, 0) * m(2, 1)) +
				m(0, 0) * (m(1, 3) * m(2, 1) - m(1, 1) * m(2, 3)) +
				m(0, 1) * (m(1, 0) * m(2, 3) - m(1, 3) * m(2, 0)));
		out(3, 0) = d * (m(1, 0) * (m(2, 2) * m(3, 1) - m(2, 1) * m(3, 2)) +
				m(1, 1) * (m(2, 0) * m(3, 2) - m(2, 2) * m(3, 0)) +
				m(1, 2) * (m(2, 1) * m(3, 0) - m(2, 0) * m(3, 1)));
		out(3, 1) = d * (m(2, 0) * (m(0, 2) * m(3, 1) - m(0, 1) * m(3, 2)) +
				m(2, 1) * (m(0, 0) * m(3, 2) - m(0, 2) * m(3, 0)) +
				m(2, 2) * (m(0, 1) * m(3, 0) - m(0, 0) * m(3, 1)));
		out(3, 2) = d * (m(3, 0) * (m(0, 2) * m(1, 1) - m(0, 1) * m(1, 2)) +
				m(3, 1) * (m(0, 0) * m(1, 2) - m(0, 2) * m(1, 0)) +
				m(3, 2) * (m(0, 1) * m(1, 0) - m(0, 0) * m(1, 1)));
		out(3, 3) = d * (m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) +
				m(0, 1) * (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) +
				m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)));

		return true;
	}

	template <class T>
	inline bool Matrix4<T>::GetInversePrimitive (Matrix4<T>& out) const
	{
		out.M[0] = M[0];
		out.M[1] = M[4];
		out.M[2] = M[8];
		out.M[3] = 0;

		out.M[4] = M[1];
		out.M[5] = M[5];
		out.M[6] = M[9];
		out.M[7] = 0;

		out.M[8] = M[2];
		out.M[9] = M[6];
		out.M[10] = M[10];
		out.M[11] = 0;

		out.M[12] = (T)-(M[12] * M[0] + M[13] * M[1] + M[14] * M[2]);
		out.M[13] = (T)-(M[12] * M[4] + M[13] * M[5] + M[14] * M[6]);
		out.M[14] = (T)-(M[12] * M[8] + M[13] * M[9] + M[14] * M[10]);
		out.M[15] = 1;

		return true;
	}

	template <class T>
	inline bool Matrix4<T>::MakeInverse()
	{
		Matrix4<T> temp (EM4CONST_NOTHING);

		if (GetInverse(temp))
		{
			*this = temp;
			return true;
		}

		return false;
	}

	template <class T>
	inline T Matrix4<T>::GetAt(int row, int col)
	{
		return M[col + (row * 4)];
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::operator=(const Matrix4<T> &other)
	{
		if (this == &other)
			return *this;

		memcpy(M, other.M, 16*sizeof(T));
		return *this;
	}


	template <class T>
	inline Matrix4<T>& Matrix4<T>::operator=(const T& scalar)
	{
		for (int i = 0; i < 16; ++i)
			M[i] = scalar;

		return *this;
	}


	template <class T>
	inline bool Matrix4<T>::operator==(const Matrix4<T> &other) const
	{
		for (int i = 0; i < 16; ++i)
			if (M[i] != other.M[i])
				return false;

		return true;
	}


	template <class T>
	inline bool Matrix4<T>::operator!=(const Matrix4<T> &other) const
	{
		return !(*this == other);
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::CreateProjectionMatrixPerspectiveFovLH(
			float fieldOfViewRadians, float aspectRatio, float zNear, float zFar)
	{
		const float h = 1.0f / (tan(fieldOfViewRadians*0.5f));
		const T w = (T)(h / aspectRatio);
		M[0] = w;
		M[1] = 0;
		M[2] = 0;
		M[3] = 0;

		M[4] = 0;
		M[5] = (T)h;
		M[6] = 0;
		M[7] = 0;

		M[8] = 0;
		M[9] = 0;
		M[10] = (T)(zFar / (zFar - zNear));
		M[11] = 1;

		M[12] = 0;
		M[13] = 0;
		M[14] = (T)(-zNear * zFar / (zFar - zNear));
		M[15] = 0;

		return *this;
	}

	template <class T>
	inline void Matrix4<T>::CreateAxisAlignedBillboard(const Vector3<float>& camPos,
											const Vector3<float>& center,
											const Vector3<float>& translation,
											const Vector3<float>& axis,
											const Vector3<float>& from
										)
	{
		Vector3<float> up = axis;
		up.Normalize();

		Vector3<float> forward = camPos - center;
		forward.Normalize();

		Vector3<float> right = up.CrossProduct(forward);
		right.Normalize();

		Vector3<float> look = right.CrossProduct(up);

		Vector3<float> vs = look.CrossProduct(from);

		float ca = from.DotProduct(look);	
		Vector3<float> vt (up * (1.0f - ca));

		M[0] = vt.X * up.X + ca;
		M[5] = vt.Y * up.Y + ca;
		M[10] = vt.Z * up.Z + ca;

		vt.X *= up.Y;
		vt.Z *= up.X;
		vt.Y *= up.Z;

		M[1] = vt.X - vs.Z;
		M[2] = vt.Z + vs.Y;
		M[3] = (T) 0;

		M[4] = vt.X + vs.Z;
		M[6] = vt.Y - vs.X;
		M[7] = (T) 0;

		M[8] = vt.Z - vs.Y;
		M[9] = vt.Y + vs.X;
		M[11] = (T) 0;

		SetRotationCenter(center, translation);
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::CreateProjectionMatrixOrthoLH(
			float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar)
	{
		M[0] = (T)(2 / widthOfViewVolume);
		M[1] = 0;
		M[2] = 0;
		M[3] = 0;

		M[4] = 0;
		M[5] = (T)(2 / heightOfViewVolume);
		M[6] = 0;
		M[7] = 0;

		M[8] = 0;
		M[9] = 0;
		M[10] = (T)(1 / (zFar - zNear));
		M[11] = 0;

		M[12] = 0;
		M[13] = 0;
		M[14] = (T)(zNear / (zNear - zFar));
		M[15] = 1;

		return *this;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::CreateProjectionMatrixPerspectiveLH(
			float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar)
	{
		M[0] = (T)(2 * zNear / widthOfViewVolume);
		M[1] = 0;
		M[2] = 0;
		M[3] = 0;

		M[4] = 0;
		M[5] = (T)(2 * zNear / heightOfViewVolume);
		M[6] = 0;
		M[7] = 0;

		M[8] = 0;
		M[9] = 0;
		M[10] = (T)(zFar / (zFar - zNear));
		M[11] = 1;

		M[12] = 0;
		M[13] = 0;
		M[14] = (T)(zNear * zFar / (zNear - zFar));
		M[15] = 0;
		return *this;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::CreateCameraLookAtMatrixLH(
				const Vector3<float>& position,
				const Vector3<float>& target,
				const Vector3<float>& upVector)
	{
		Vector3<float> zaxis = target - position;
		zaxis.Normalize();

		Vector3<float> xaxis = upVector.CrossProduct(zaxis);
		xaxis.Normalize();

		Vector3<float> yaxis = zaxis.CrossProduct(xaxis);

		M[0] = (T)xaxis.X;
		M[1] = (T)yaxis.X;
		M[2] = (T)zaxis.X;
		M[3] = 0;

		M[4] = (T)xaxis.Y;
		M[5] = (T)yaxis.Y;
		M[6] = (T)zaxis.Y;
		M[7] = 0;

		M[8] = (T)xaxis.Z;
		M[9] = (T)yaxis.Z;
		M[10] = (T)zaxis.Z;
		M[11] = 0;

		M[12] = (T)-xaxis.DotProduct(position);
		M[13] = (T)-yaxis.DotProduct(position);
		M[14] = (T)-zaxis.DotProduct(position);
		M[15] = 1;
		return *this;
	}

	template <class T>
	inline Matrix4<T> Matrix4<T>::Interpolate(const Matrix4<T>& b, float time) const
	{
		Matrix4<T> mat (EM4CONST_NOTHING);

		for (int i = 0; i < 16; i += 4)
		{
			mat.M[i + 0] = (T)(M[i + 0] + ( b.M[i + 0] - M[i + 0]) * time);
			mat.M[i + 1] = (T)(M[i + 1] + ( b.M[i + 1] - M[i + 1]) * time);
			mat.M[i + 2] = (T)(M[i + 2] + ( b.M[i + 2] - M[i + 2]) * time);
			mat.M[i + 3] = (T)(M[i + 3] + ( b.M[i + 3] - M[i + 3]) * time);
		}
		return mat;
	}

	template <class T>
	inline Matrix4<T> Matrix4<T>::GetTransposed() const
	{
		Matrix4<T> t (EM4CONST_NOTHING);
		GetTransposed (t);
		return t;
	}

	template <class T>
	inline void Matrix4<T>::GetTransposed(Matrix4<T>& o) const
	{
		o[0] = M[0];
		o[1] = M[4];
		o[2] = M[8];
		o[3] = M[12];

		o[4] = M[1];
		o[5] = M[5];
		o[6] = M[9];
		o[7] = M[13];

		o[8] = M[2];
		o[9] = M[6];
		o[10] = M[10];
		o[11] = M[14];

		o[12] = M[3];
		o[13] = M[7];
		o[14] = M[11];
		o[15] = M[15];
	}

	template <class T>
	inline void Matrix4<T>::SetRotationCenter(const Vector3<float>& center, const Vector3<float>& translation)
	{
		M[12] = -M[0] * center.X - M[4] * center.Y - M[8] * center.Z + (center.X - translation.X);
		M[13] = -M[1] * center.X - M[5] * center.Y - M[9] * center.Z + (center.Y - translation.Y);
		M[14] = -M[2] * center.X - M[6] * center.Y - M[10] * center.Z + (center.Z - translation.Z);
		M[15] = (T) 1.0f;
	}

	template <class T>
	inline Matrix4<T>& Matrix4<T>::SetData(const T* data)
	{
		memcpy(M, data, 16 * sizeof(T));
		return *this;
	}

	template <class T>
	inline bool Matrix4<T>::Equals(const Matrix4<T>& other) const
	{
		for (int i = 0; i < 16; i++)
			if (M[i] != other.M[i])
				return false;

		return true;
	}

	template <class T>
	inline Matrix4<T> operator*(const T scalar, const Matrix4<T>& mat)
	{
		return mat * scalar;
	}
}

#endif