ES-DE/es-core/src/math/Transform4x4f.h

#pragma once
#ifndef ES_CORE_MATH_TRANSFORM4X4F_H
#define ES_CORE_MATH_TRANSFORM4X4F_H

#include "math/Vector4f.h"
#include "math/Vector3f.h"
#include <math.h>

class Transform4x4f
{
public:

	Transform4x4f()                                                                                                                    { }
	Transform4x4f(const Vector4f& r0, const Vector4f& r1, const Vector4f& r2, const Vector4f& r3) : mR0(r0), mR1(r1), mR2(r2), mR3(r3) { }

	const Transform4x4f operator*(const Transform4x4f& other) const
	{
		const float* tm = (float*)this;
		const float* om = (float*)&other;

		return
		{
			{
				tm[ 0] * om[ 0] + tm[ 1] * om[ 4] + tm[ 2] * om[ 8],
				tm[ 0] * om[ 1] + tm[ 1] * om[ 5] + tm[ 2] * om[ 9],
				tm[ 0] * om[ 2] + tm[ 1] * om[ 6] + tm[ 2] * om[10],
				0
			},
			{
				tm[ 4] * om[ 0] + tm[ 5] * om[ 4] + tm[ 6] * om[ 8],
				tm[ 4] * om[ 1] + tm[ 5] * om[ 5] + tm[ 6] * om[ 9],
				tm[ 4] * om[ 2] + tm[ 5] * om[ 6] + tm[ 6] * om[10],
				0
			},
			{
				tm[ 8] * om[ 0] + tm[ 9] * om[ 4] + tm[10] * om[ 8],
				tm[ 8] * om[ 1] + tm[ 9] * om[ 5] + tm[10] * om[ 9],
				tm[ 8] * om[ 2] + tm[ 9] * om[ 6] + tm[10] * om[10],
				0
			},
			{
				tm[ 0] * om[12] + tm[ 4] * om[13] + tm[ 8] * om[14] + tm[12],
				tm[ 1] * om[12] + tm[ 5] * om[13] + tm[ 9] * om[14] + tm[13],
				tm[ 2] * om[12] + tm[ 6] * om[13] + tm[10] * om[14] + tm[14],
				1
			}
		};
	}

	const Vector3f operator*(const Vector3f& other) const
	{
		const float* tm = (float*)this;
		const float* ov = (float*)&other;

		return
		{
			tm[ 0] * ov[0] + tm[ 4] * ov[1] + tm[ 8] * ov[2] + tm[12],
			tm[ 1] * ov[0] + tm[ 5] * ov[1] + tm[ 9] * ov[2] + tm[13],
			tm[ 2] * ov[0] + tm[ 6] * ov[1] + tm[10] * ov[2] + tm[14]
		};
	}

	Transform4x4f& operator*=(const Transform4x4f& other) { *this = *this * other; return *this; }

	inline       Vector4f& r0()       { return mR0; }
	inline       Vector4f& r1()       { return mR1; }
	inline       Vector4f& r2()       { return mR2; }
	inline       Vector4f& r3()       { return mR3; }
	inline const Vector4f& r0() const { return mR0; }
	inline const Vector4f& r1() const { return mR1; }
	inline const Vector4f& r2() const { return mR2; }
	inline const Vector4f& r3() const { return mR3; }

	inline Transform4x4f& invert(const Transform4x4f& other)
	{
		float*       tm = (float*)this;
		const float* om = (float*)&other;

		// Full invert
		// tm[ 0] =  ((om[ 5] * (om[10] * om[15] - om[11] * om[14])) - (om[ 9] * (om[ 6] * om[15] - om[ 7] * om[14])) + (om[13] * (om[ 6] * om[11] - om[ 7] * om[10])));
		// tm[ 1] = -((om[ 1] * (om[10] * om[15] - om[11] * om[14])) - (om[ 9] * (om[ 2] * om[15] - om[ 3] * om[14])) + (om[13] * (om[ 2] * om[11] - om[ 3] * om[10])));
		// tm[ 2] =  ((om[ 1] * (om[ 6] * om[15] - om[ 7] * om[14])) - (om[ 5] * (om[ 2] * om[15] - om[ 3] * om[14])) + (om[13] * (om[ 2] * om[ 7] - om[ 3] * om[ 6])));
		// tm[ 3] = -((om[ 1] * (om[ 6] * om[11] - om[ 7] * om[10])) - (om[ 5] * (om[ 2] * om[11] - om[ 3] * om[10])) + (om[ 9] * (om[ 2] * om[ 7] - om[ 3] * om[ 6])));
		// tm[ 4] = -((om[ 4] * (om[10] * om[15] - om[11] * om[14])) - (om[ 8] * (om[ 6] * om[15] - om[ 7] * om[14])) + (om[12] * (om[ 6] * om[11] - om[ 7] * om[10])));
		// tm[ 5] =  ((om[ 0] * (om[10] * om[15] - om[11] * om[14])) - (om[ 8] * (om[ 2] * om[15] - om[ 3] * om[14])) + (om[12] * (om[ 2] * om[11] - om[ 3] * om[10])));
		// tm[ 6] = -((om[ 0] * (om[ 6] * om[15] - om[ 7] * om[14])) - (om[ 4] * (om[ 2] * om[15] - om[ 3] * om[14])) + (om[12] * (om[ 2] * om[ 7] - om[ 3] * om[ 6])));
		// tm[ 7] =  ((om[ 0] * (om[ 6] * om[11] - om[ 7] * om[10])) - (om[ 4] * (om[ 2] * om[11] - om[ 3] * om[10])) + (om[ 8] * (om[ 2] * om[ 7] - om[ 3] * om[ 6])));
		// tm[ 8] =  ((om[ 4] * (om[ 9] * om[15] - om[11] * om[13])) - (om[ 8] * (om[ 5] * om[15] - om[ 7] * om[13])) + (om[12] * (om[ 5] * om[11] - om[ 7] * om[ 9])));
		// tm[ 9] = -((om[ 0] * (om[ 9] * om[15] - om[11] * om[13])) - (om[ 8] * (om[ 1] * om[15] - om[ 3] * om[13])) + (om[12] * (om[ 1] * om[11] - om[ 3] * om[ 9])));
		// tm[10] =  ((om[ 0] * (om[ 5] * om[15] - om[ 7] * om[13])) - (om[ 4] * (om[ 1] * om[15] - om[ 3] * om[13])) + (om[12] * (om[ 1] * om[ 7] - om[ 3] * om[ 5])));
		// tm[11] = -((om[ 0] * (om[ 5] * om[11] - om[ 7] * om[ 9])) - (om[ 4] * (om[ 1] * om[11] - om[ 3] * om[ 9])) + (om[ 8] * (om[ 1] * om[ 7] - om[ 3] * om[ 5])));
		// tm[12] = -((om[ 4] * (om[ 9] * om[14] - om[10] * om[13])) - (om[ 8] * (om[ 5] * om[14] - om[ 6] * om[13])) + (om[12] * (om[ 5] * om[10] - om[ 6] * om[ 9])));
		// tm[13] =  ((om[ 0] * (om[ 9] * om[14] - om[10] * om[13])) - (om[ 8] * (om[ 1] * om[14] - om[ 2] * om[13])) + (om[12] * (om[ 1] * om[10] - om[ 2] * om[ 9])));
		// tm[14] = -((om[ 0] * (om[ 5] * om[14] - om[ 6] * om[13])) - (om[ 4] * (om[ 1] * om[14] - om[ 2] * om[13])) + (om[12] * (om[ 1] * om[ 6] - om[ 2] * om[ 5])));
		// tm[15] =  ((om[ 0] * (om[ 5] * om[10] - om[ 6] * om[ 9])) - (om[ 4] * (om[ 1] * om[10] - om[ 2] * om[ 9])) + (om[ 8] * (om[ 1] * om[ 6] - om[ 2] * om[ 5])));

		// Optimized invert ( om[3, 7 and 11] is always 0, and om[15] is always 1 )
		tm[ 0] =  ((om[ 5] * om[10]) - (om[ 9] * om[ 6]));
		tm[ 1] = -((om[ 1] * om[10]) - (om[ 9] * om[ 2]));
		tm[ 2] =  ((om[ 1] * om[ 6]) - (om[ 5] * om[ 2]));
		tm[ 3] =  0;
		tm[ 4] = -((om[ 4] * om[10]) - (om[ 8] * om[ 6]));
		tm[ 5] =  ((om[ 0] * om[10]) - (om[ 8] * om[ 2]));
		tm[ 6] = -((om[ 0] * om[ 6]) - (om[ 4] * om[ 2]));
		tm[ 7] =  0;
		tm[ 8] =  ((om[ 4] * om[ 9]) - (om[ 8] * om[ 5]));
		tm[ 9] = -((om[ 0] * om[ 9]) - (om[ 8] * om[ 1]));
		tm[10] =  ((om[ 0] * om[ 5]) - (om[ 4] * om[ 1]));
		tm[11] =  0;
		tm[12] = -((om[ 4] * (om[ 9] * om[14] - om[10] * om[13])) - (om[ 8] * (om[ 5] * om[14] - om[ 6] * om[13])) + (om[12] * (om[ 5] * om[10] - om[ 6] * om[ 9])));
		tm[13] =  ((om[ 0] * (om[ 9] * om[14] - om[10] * om[13])) - (om[ 8] * (om[ 1] * om[14] - om[ 2] * om[13])) + (om[12] * (om[ 1] * om[10] - om[ 2] * om[ 9])));
		tm[14] = -((om[ 0] * (om[ 5] * om[14] - om[ 6] * om[13])) - (om[ 4] * (om[ 1] * om[14] - om[ 2] * om[13])) + (om[12] * (om[ 1] * om[ 6] - om[ 2] * om[ 5])));
		tm[15] =  1;

		float Determinant = om[ 0] * tm[ 0] +
		                    om[ 4] * tm[ 1] +
		                    om[ 8] * tm[ 2] +
		                    om[12] * tm[ 3];

		if(Determinant != 0)
			Determinant = 1 / Determinant;

		tm[ 0] *= Determinant;
		tm[ 1] *= Determinant;
		tm[ 2] *= Determinant;
		tm[ 4] *= Determinant;
		tm[ 5] *= Determinant;
		tm[ 6] *= Determinant;
		tm[ 8] *= Determinant;
		tm[ 9] *= Determinant;
		tm[10] *= Determinant;
		tm[12] *= Determinant;
		tm[13] *= Determinant;
		tm[14] *= Determinant;

		return *this;
	}

	inline Transform4x4f& scale(const Vector3f& scale)
	{
		float*       tm = (float*)this;
		const float* sv = (float*)&scale;

		tm[ 0] *= sv[0];
		tm[ 1] *= sv[1];
		tm[ 2] *= sv[2];
		tm[ 4] *= sv[0];
		tm[ 5] *= sv[1];
		tm[ 6] *= sv[2];
		tm[ 8] *= sv[0];
		tm[ 9] *= sv[1];
		tm[10] *= sv[2];

		return *this;
	}

	inline Transform4x4f& rotate(const float angle, const Vector3f& axis)
	{
		float*       tm      = (float*)this;
		const float* av      = (float*)&axis;
		const float  s       = sin(-angle);
		const float  c       = cos(-angle);
		const float  t       = 1 - c;
		const float  x       = av[0];
		const float  y       = av[1];
		const float  z       = av[2];
		const float  tx      = t * x;
		const float  ty      = t * y;
		const float  tz      = t * z;
		const float  sx      = s * x;
		const float  sy      = s * y;
		const float  sz      = s * z;
		const float  r[9]    = { tx * x + c,
		                         tx * y - sz,
		                         tx * z + sy,
		                         ty * x + sz,
		                         ty * y + c,
		                         ty * z - sx,
		                         tz * x - sy,
		                         tz * y + sx,
		                         tz * z + c };
		const float  temp[9] = { tm[ 0] * r[0] + tm[ 1] * r[3] + tm[ 2] * r[6],
		                         tm[ 0] * r[1] + tm[ 1] * r[4] + tm[ 2] * r[7],
		                         tm[ 0] * r[2] + tm[ 1] * r[5] + tm[ 2] * r[8],
		                         tm[ 4] * r[0] + tm[ 5] * r[3] + tm[ 6] * r[6],
		                         tm[ 4] * r[1] + tm[ 5] * r[4] + tm[ 6] * r[7],
		                         tm[ 4] * r[2] + tm[ 5] * r[5] + tm[ 6] * r[8],
		                         tm[ 8] * r[0] + tm[ 9] * r[3] + tm[10] * r[6],
		                         tm[ 8] * r[1] + tm[ 9] * r[4] + tm[10] * r[7],
		                         tm[ 8] * r[2] + tm[ 9] * r[5] + tm[10] * r[8] };

		tm[ 0] = temp[0];
		tm[ 1] = temp[1];
		tm[ 2] = temp[2];
		tm[ 4] = temp[3];
		tm[ 5] = temp[4];
		tm[ 6] = temp[5];
		tm[ 8] = temp[6];
		tm[ 9] = temp[7];
		tm[10] = temp[8];

		return *this;
	};

	inline Transform4x4f& rotateX(const float angle)
	{
		float*      tm      = (float*)this;
		const float s       = sin(-angle);
		const float c       = cos(-angle);
		const float temp[6] = { tm[ 1] *  c + tm[ 2] * s,
		                        tm[ 1] * -s + tm[ 2] * c,
		                        tm[ 5] *  c + tm[ 6] * s,
		                        tm[ 5] * -s + tm[ 6] * c,
		                        tm[ 9] *  c + tm[10] * s,
		                        tm[ 9] * -s + tm[10] * c };

		tm[ 1] = temp[0];
		tm[ 2] = temp[1];
		tm[ 5] = temp[2];
		tm[ 6] = temp[3];
		tm[ 9] = temp[4];
		tm[10] = temp[5];

		return *this;
	};

	inline Transform4x4f& rotateY(const float angle)
	{
		float*      tm      = (float*)this;
		const float s       = sin(-angle);
		const float c       = cos(-angle);
		const float temp[6] = { tm[ 0] * c + tm[ 2] * -s,
		                        tm[ 0] * s + tm[ 2] *  c,
		                        tm[ 4] * c + tm[ 6] * -s,
		                        tm[ 4] * s + tm[ 6] *  c,
		                        tm[ 8] * c + tm[10] * -s,
		                        tm[ 8] * s + tm[10] *  c };

		tm[ 0] = temp[0];
		tm[ 2] = temp[1];
		tm[ 4] = temp[2];
		tm[ 6] = temp[3];
		tm[ 8] = temp[4];
		tm[10] = temp[5];

		return *this;
	};

	inline Transform4x4f& rotateZ(const float angle)
	{
		float*      tm      = (float*)this;
		const float s       = sin(-angle);
		const float c       = cos(-angle);
		const float temp[6] = { tm[ 0] *  c + tm[ 1] * s,
		                        tm[ 0] * -s + tm[ 1] * c,
		                        tm[ 4] *  c + tm[ 5] * s,
		                        tm[ 4] * -s + tm[ 5] * c,
		                        tm[ 8] *  c + tm[ 9] * s,
		                        tm[ 8] * -s + tm[ 9] * c };

		tm[ 0] = temp[0];
		tm[ 1] = temp[1];
		tm[ 4] = temp[2];
		tm[ 5] = temp[3];
		tm[ 8] = temp[4];
		tm[ 9] = temp[5];

		return *this;
	};

	inline Transform4x4f& translate(const Vector3f& translation)
	{
		float*       tm = (float*)this;
		const float* tv = (float*)&translation;

		tm[12] += tm[ 0] * tv[0] + tm[ 4] * tv[1] + tm[ 8] * tv[2];
		tm[13] += tm[ 1] * tv[0] + tm[ 5] * tv[1] + tm[ 9] * tv[2];
		tm[14] += tm[ 2] * tv[0] + tm[ 6] * tv[1] + tm[10] * tv[2];

		return *this;
	}

	inline Transform4x4f& round()
	{
		float* tm = (float*)this;

		tm[12] = (int)(tm[12] + 0.5f);
		tm[13] = (int)(tm[13] + 0.5f);
		tm[14] = (int)(tm[14] + 0.5f);

		return *this;
	}

	inline       Vector3f& translation()       { return mR3.v3(); }
	inline const Vector3f& translation() const { return mR3.v3(); }

	static const Transform4x4f Identity() { return { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } }; }

protected:

	Vector4f mR0;
	Vector4f mR1;
	Vector4f mR2;
	Vector4f mR3;

};

#endif // ES_CORE_MATH_TRANSFORM4X4F_H