#include "LifeMath.h"

namespace LifeMath
{
	extern INLINE void Mat4x4Invert(const Matrix4x4* matrix, Matrix4x4* result)
	{
		float num5 = matrix->M11;
		float num4 = matrix->M12;
		float num3 = matrix->M13;
		float num2 = matrix->M14;
		float num9 = matrix->M21;
		float num8 = matrix->M22;
		float num7 = matrix->M23;
		float num6 = matrix->M24;
		float num17 = matrix->M31;
		float num16 = matrix->M32;
		float num15 = matrix->M33;
		float num14 = matrix->M34;
		float num13 = matrix->M41;
		float num12 = matrix->M42;
		float num11 = matrix->M43;
		float num10 = matrix->M44;
		float num23 = (num15 * num10) - (num14 * num11);
		float num22 = (num16 * num10) - (num14 * num12);
		float num21 = (num16 * num11) - (num15 * num12);
		float num20 = (num17 * num10) - (num14 * num13);
		float num19 = (num17 * num11) - (num15 * num13);
		float num18 = (num17 * num12) - (num16 * num13);
		float num39 = ((num8 * num23) - (num7 * num22)) + (num6 * num21);
		float num38 = -(((num9 * num23) - (num7 * num20)) + (num6 * num19));
		float num37 = ((num9 * num22) - (num8 * num20)) + (num6 * num18);
		float num36 = -(((num9 * num21) - (num8 * num19)) + (num7 * num18));
		float num = 1.0f / ((((num5 * num39) + (num4 * num38)) + (num3 * num37)) + (num2 * num36));
		result->M11 = num39 * num;
		result->M21 = num38 * num;
		result->M31 = num37 * num;
		result->M41 = num36 * num;
		result->M12 = -(((num4 * num23) - (num3 * num22)) + (num2 * num21)) * num;
		result->M22 = (((num5 * num23) - (num3 * num20)) + (num2 * num19)) * num;
		result->M32 = -(((num5 * num22) - (num4 * num20)) + (num2 * num18)) * num;
		result->M42 = (((num5 * num21) - (num4 * num19)) + (num3 * num18)) * num;
		float num35 = (num7 * num10) - (num6 * num11);
		float num34 = (num8 * num10) - (num6 * num12);
		float num33 = (num8 * num11) - (num7 * num12);
		float num32 = (num9 * num10) - (num6 * num13);
		float num31 = (num9 * num11) - (num7 * num13);
		float num30 = (num9 * num12) - (num8 * num13);
		result->M13 = (((num4 * num35) - (num3 * num34)) + (num2 * num33)) * num;
		result->M23 = -(((num5 * num35) - (num3 * num32)) + (num2 * num31)) * num;
		result->M33 = (((num5 * num34) - (num4 * num32)) + (num2 * num30)) * num;
		result->M43 = -(((num5 * num33) - (num4 * num31)) + (num3 * num30)) * num;
		float num29 = (num7 * num14) - (num6 * num15);
		float num28 = (num8 * num14) - (num6 * num16);
		float num27 = (num8 * num15) - (num7 * num16);
		float num26 = (num9 * num14) - (num6 * num17);
		float num25 = (num9 * num15) - (num7 * num17);
		float num24 = (num9 * num16) - (num8 * num17);
		result->M14 = -(((num4 * num29) - (num3 * num28)) + (num2 * num27)) * num;
		result->M24 = (((num5 * num29) - (num3 * num26)) + (num2 * num25)) * num;
		result->M34 = -(((num5 * num28) - (num4 * num26)) + (num2 * num24)) * num;
		result->M44 = (((num5 * num27) - (num4 * num25)) + (num3 * num24)) * num;
	}

	extern INLINE void QuatRotationYawPitchRoll(float yaw, float pitch, float roll, Quaternion& result)
	{
		float num13 = roll * 0.5f;
		float num2 = sinf(num13);
		float num = cosf(num13);
		float num12 = pitch * 0.5f;
		float num11 = sinf(num12);
		float num10 = cosf(num12);
		float num9 = yaw * 0.5f;
		float num8 = sinf(num9);
		float num7 = cosf(num9);
		float num6 = num7 * num11;
		float num5 = num8 * num10;
		result.X = ((num2 * num5) + (num * num6));
		result.Y = ((num * num5) - (num2 * num6));
		float num4 = num7 * num10;
		float num3 = num8 * num11;
		result.Z = ((num2 * num4) - (num * num3));
		result.W = ((num2 * num3) + (num * num4));
	}

	extern INLINE void QuatRotationMatrix(const Matrix4x4& matrix, Quaternion& dst)
	{
		float num8 = (matrix.M11 + matrix.M22) + matrix.M33;
		if (num8 > 0.0f)
		{
			float num = sqrtf(num8 + 1.0f);
			dst.W = num * 0.5f;
			num = 0.5f / num;
			dst.X = (matrix.M23 - matrix.M32) * num;
			dst.Y = (matrix.M31 - matrix.M13) * num;
			dst.Z = (matrix.M12 - matrix.M21) * num;
		}
		else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
		{
			float num7 = sqrtf((((1.0f + matrix.M11) - matrix.M22) - matrix.M33));
			float num4 = 0.5f / num7;
			dst.X = 0.5f * num7;
			dst.Y = (matrix.M12 + matrix.M21) * num4;
			dst.Z = (matrix.M13 + matrix.M31) * num4;
			dst.W = (matrix.M23 - matrix.M32) * num4;
		}
		else if (matrix.M22 > matrix.M33)
		{
			float num6 = sqrtf((((1.0f + matrix.M22) - matrix.M11) - matrix.M33));
			float num3 = 0.5f / num6;
			dst.X = (matrix.M21 + matrix.M12) * num3;
			dst.Y = 0.5f * num6;
			dst.Z = (matrix.M32 + matrix.M23) * num3;
			dst.W = (matrix.M31 - matrix.M13) * num3;
		}
		else
		{
			float num5 = sqrtf((((1.0f + matrix.M33) - matrix.M11) - matrix.M22));
			float num2 = 0.5f / num5;
			dst.X = (matrix.M31 + matrix.M13) * num2;
			dst.Y = (matrix.M32 + matrix.M23) * num2;
			dst.Z = 0.5f * num5;
			dst.W = (matrix.M12 - matrix.M21) * num2;
		}
	}
}