#ifndef LifeMath_h__
#define LifeMath_h__

//#include "LifeDebug.h"

#include "Math.h"
#include <cmath>
#include <algorithm>
#include "Vector2D.h"
#include "Vector3D.h"
#include "Vector4D.h"
#include "Matrix4x4.h"
#include "Quaternion.h"
#include "Plane.h"
#include <intrin.h>

inline int GetFlag(int val, int flag)
{
	return val & flag;
}
inline void RemoveFlag(int& val, int flag)
{
	val = val & (~flag);
}
inline void SetFlag(int& val, int flag)
{
	val |= flag;
}
namespace LifeMath
{
#undef INLINE
#ifdef _WIN32
#ifdef _MSC_VER
#define INLINE __forceinline
#endif
#else
#ifdef __GNUC__
#define INLINE __attribute__((always_inline))
#endif
#endif

#ifndef INLINE
#define INLINE inline
#endif

	/*#define min(x, y) \
	(x < y ? x : y)

	#define max(x, y) \
	(x > y ? x : y)*/

	/*template<typename T>
	INLINE const T& (min)(const T& left, const T& right)
	{
	return left < right ? left : right;
	}

	template<typename T>
	INLINE T max(T left, T right)
	{
	return left > right ? left : right;
	}*/

	static const float PI = 3.14159265358979f;
	enum Axis
	{
		X, Y, Z, W
	};

	/*template<typename T>
	T SetFlag(T val, T flag)
	{
	return val | flag;
	}

	template<typename T>
	T GetFlag(T val, T flag)
	{
	return val & flag;
	}*/

	/*template<typename T>
	T RemoveFlag(T val, T flag)
	{
	return val & (~flag);
	}*/

	extern void Mat4x4Invert(const Matrix4x4* matrix, Matrix4x4* result);
	extern void QuatRotationYawPitchRoll(float yaw, float pitch, float roll, Quaternion& result);
	extern void QuatRotationMatrix(const Matrix4x4& matrix, Quaternion& dst);

	//----- Vector3D-----
	INLINE float Vec3Length(Vec3f* src)
	{
		return sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
	}

	INLINE float Vec3Length(const Vec3f& src)
	{
		return sqrtf(src.X * src.X + src.Y * src.Y + src.Z * src.Z);
	}

	INLINE void Vec3Normalize(Vec3f* src)
	{
		float num2 = Vec3Length(src);
		if (num2 != 0.0f)
		{
			float num = 1.0f / num2;
			src->X *= num;
			src->Y *= num;
			src->Z *= num;
		}
	}

	INLINE void Vec3Normalize(Vec3f* vector, Vec3f* result)
	{
		float num2 = Vec3Length(vector);
		if (num2 != 0.0f)
		{
			float num = 1.0f / num2;
			result->X = vector->X * num;
			result->Y = vector->Y * num;
			result->Z = vector->Z * num;
		}
		else
		{
			result->X = result->Y = result->Z = 0.0f;
		}
	}

	INLINE void Vec3Add(Vec3f& src, Vec3f& val)
	{
		src.X += val.X;
		src.Y += val.Y;
		src.Z += val.Z;
	}

	INLINE void Vec3Reflect(Vec3f& vector, Vec3f& normal, Vec3f& result)
	{
		float num2 = ((vector.Y * normal.Y) + (vector.X * normal.X)) + (vector.Z * normal.Z);
		float num = num2 * 2.0f;
		result.X = vector.X - (normal.X * num);
		result.Y = vector.Y - (normal.Y * num);
		result.Z = vector.Z - (normal.Z * num);
	}

	INLINE void Vec3Reflect(Vec3f& vector, Vec3f& normal)
	{
		float num2 = ((vector.Y * normal.Y) + (vector.X * normal.X)) + (vector.Z * normal.Z);
		float num = num2 * 2.0f;
		vector.X = vector.X - (normal.X * num);
		vector.Y = vector.Y - (normal.Y * num);
		vector.Z = vector.Z - (normal.Z * num);
	}

	INLINE void Vec3Transform(Vec3f* vector, Matrix4x4* transformation, Vec4f* result)
	{
		result->X = (((vector->Y * transformation->M21) + (vector->X * transformation->M11)) + (vector->Z * transformation->M31)) + transformation->M41;
		result->Y = (((vector->Y * transformation->M22) + (vector->X * transformation->M12)) + (vector->Z * transformation->M32)) + transformation->M42;
		result->Z = (((vector->Y * transformation->M23) + (vector->X * transformation->M13)) + (vector->Z * transformation->M33)) + transformation->M43;
		result->W = (((transformation->M24 * vector->Y) + (transformation->M14 * vector->X)) + (vector->Z * transformation->M34)) + transformation->M44;
	}

	INLINE float Vec3Dot(const Vec3f* left, const Vec3f* right)
	{
		return (((left->Y * right->Y) + (left->X * right->X)) + (left->Z * right->Z));
	}

	INLINE Vec3f Vec3TransformCoordinate(const Matrix4x4& transformation, const Vec3f& coordinate)
	{
		Vec3f vector;
		Vec4f vector2;
		vector2.X = (((transformation.M21 * coordinate.Y) + (transformation.M11 * coordinate.X)) + (transformation.M31 * coordinate.Z)) + transformation.M41;
		vector2.Y = (((transformation.M22 * coordinate.Y) + (transformation.M12 * coordinate.X)) + (transformation.M32 * coordinate.Z)) + transformation.M42;
		vector2.Z = (((transformation.M23 * coordinate.Y) + (transformation.M13 * coordinate.X)) + (transformation.M33 * coordinate.Z)) + transformation.M43;
		float num = (1.0f / ((((transformation.M24 * coordinate.Y) + (transformation.M14 * coordinate.X)) + (transformation.M34 * coordinate.Z)) + transformation.M44));
		vector2.W = num;
		vector.X = vector2.X * num;
		vector.Y = vector2.Y * num;
		vector.Z = vector2.Z * num;
		return vector;
	}

	INLINE void Vec3TransformCoordinate(Vec3f& coordinate, const Matrix4x4& transformation)
	{
		Vec4f vector2;
		vector2.X = (((transformation.M21 * coordinate.Y) + (transformation.M11 * coordinate.X)) + (transformation.M31 * coordinate.Z)) + transformation.M41;
		vector2.Y = (((transformation.M22 * coordinate.Y) + (transformation.M12 * coordinate.X)) + (transformation.M32 * coordinate.Z)) + transformation.M42;
		vector2.Z = (((transformation.M23 * coordinate.Y) + (transformation.M13 * coordinate.X)) + (transformation.M33 * coordinate.Z)) + transformation.M43;
		float num = (float)(1.0 / ((((transformation.M24 * coordinate.Y) + (transformation.M14 * coordinate.X)) + (transformation.M34 * coordinate.Z)) + transformation.M44));
		vector2.W = num;
		coordinate.X = vector2.X * num;
		coordinate.Y = vector2.Y * num;
		coordinate.Z = vector2.Z * num;
	}

	INLINE Vec3f Vec3TransformNoW(const Vec3f& coordinate, const Matrix4x4& transformation)
	{
		Vec3f vector;
		vector.X = (((transformation.M21 * coordinate.Y) + (transformation.M11 * coordinate.X)) + (transformation.M31 * coordinate.Z)) + transformation.M41;
		vector.Y = (((transformation.M22 * coordinate.Y) + (transformation.M12 * coordinate.X)) + (transformation.M32 * coordinate.Z)) + transformation.M42;
		vector.Z = (((transformation.M23 * coordinate.Y) + (transformation.M13 * coordinate.X)) + (transformation.M33 * coordinate.Z)) + transformation.M43;
		return vector;
	}

	INLINE void Vec3Minimize(const Vec3f& value1, const Vec3f& value2, Vec3f& result)
	{
		float z;
		float y;
		float x;
		if (value1.X < value2.X)
		{
			x = value1.X;
		}
		else
		{
			x = value2.X;
		}
		result.X = x;
		if (value1.Y < value2.Y)
		{
			y = value1.Y;
		}
		else
		{
			y = value2.Y;
		}
		result.Y = y;
		if (value1.Z < value2.Z)
		{
			z = value1.Z;
		}
		else
		{
			z = value2.Z;
		}
		result.Z = z;
	}

	INLINE void Vec3Minimize(Vec4f& value1, Vec3f& value2, Vec3f& result)
	{
		float z;
		float y;
		float x;
		if (value1.X < value2.X)
		{
			x = value1.X;
		}
		else
		{
			x = value2.X;
		}
		result.X = x;
		if (value1.Y < value2.Y)
		{
			y = value1.Y;
		}
		else
		{
			y = value2.Y;
		}
		result.Y = y;
		if (value1.Z < value2.Z)
		{
			z = value1.Z;
		}
		else
		{
			z = value2.Z;
		}
		result.Z = z;
	}

	INLINE void Vec3Maximize(const Vec3f& value1, const Vec3f& value2, Vec3f& result)
	{
		float z;
		float y;
		float x;
		if (value1.X > value2.X)
		{
			x = value1.X;
		}
		else
		{
			x = value2.X;
		}
		result.X = x;
		if (value1.Y > value2.Y)
		{
			y = value1.Y;
		}
		else
		{
			y = value2.Y;
		}
		result.Y = y;
		if (value1.Z > value2.Z)
		{
			z = value1.Z;
		}
		else
		{
			z = value2.Z;
		}
		result.Z = z;
	}

	INLINE void Vec3Maximize(const Vec4f& value1, const Vec3f& value2, Vec3f& result)
	{
		float z;
		float y;
		float x;
		if (value1.X > value2.X)
		{
			x = value1.X;
		}
		else
		{
			x = value2.X;
		}
		result.X = x;
		if (value1.Y > value2.Y)
		{
			y = value1.Y;
		}
		else
		{
			y = value2.Y;
		}
		result.Y = y;
		if (value1.Z > value2.Z)
		{
			z = value1.Z;
		}
		else
		{
			z = value2.Z;
		}
		result.Z = z;
	}

	INLINE void Vec3TransformCoordinate(const Vec3f& coordinate, const Matrix4x4& transformation, Vec3f& result)
	{
		Vec4f vector2;
		vector2.X = (((coordinate.Y * transformation.M21) + (coordinate.X * transformation.M11)) + (coordinate.Z * transformation.M31)) + transformation.M41;
		vector2.Y = (((coordinate.Y * transformation.M22) + (coordinate.X * transformation.M12)) + (coordinate.Z * transformation.M32)) + transformation.M42;
		vector2.Z = (((coordinate.Y * transformation.M23) + (coordinate.X * transformation.M13)) + (coordinate.Z * transformation.M33)) + transformation.M43;
		float num = (float)(1.0 / ((((transformation.M24 * coordinate.Y) + (transformation.M14 * coordinate.X)) + (coordinate.Z * transformation.M34)) + transformation.M44));
		vector2.W = num;
		result.X = vector2.X * num;
		result.Y = vector2.Y * num;
		result.Z = vector2.Z * num;
	}

	INLINE void Vec3Cross(const Vec3f& left, const Vec3f& right, Vec3f& result)
	{
		result.X = (left.Y * right.Z) - (left.Z * right.Y);
		result.Y = (left.Z * right.X) - (left.X * right.Z);
		result.Z = (left.X * right.Y) - (left.Y * right.X);
	}

	INLINE Vec3f Vec3Cross(const Vec3f& vector1, const Vec3f& vector2)
	{
		Vec3f vector;
		vector.X = (vector1.Y * vector2.Z) - (vector1.Z * vector2.Y);
		vector.Y = (vector1.Z * vector2.X) - (vector1.X * vector2.Z);
		vector.Z = (vector1.X * vector2.Y) - (vector1.Y * vector2.X);
		return vector;
	}

	INLINE Vec3f Vec3Lerp(const Vec3f& start, const Vec3f& end, float amount)
	{
		Vec3f vector;
		vector.X = ((end.X - start.X) * amount) + start.X;
		vector.Y = ((end.Y - start.Y) * amount) + start.Y;
		vector.Z = ((end.Z - start.Z) * amount) + start.Z;
		return vector;
	}

	INLINE void Vec3Lerp(const Vec3f& start, const Vec3f& end, float amount, Vec3f& result)
	{
		result.X = ((end.X - start.X) * amount) + start.X;
		result.Y = ((end.Y - start.Y) * amount) + start.Y;
		result.Z = ((end.Z - start.Z) * amount) + start.Z;
	}

	INLINE void Vec3Move(Vec3f& src, const Vec3f& direction, float amount)
	{
		src.X += direction.X * amount;
		src.Y += direction.Y * amount;
		src.Z += direction.Z * amount;
	}

	INLINE void Vec3Mul(Vec3f& vector, float val)
	{
		vector.X *= val;
		vector.Y *= val;
		vector.Z *= val;
	}

	INLINE void Vec3TransformCoordinate(Vec3f& coord, Matrix4x4& transform)
	{
		Vec4f vector;
		vector.X = (((coord.X * transform.M11) + (coord.Y * transform.M21)) + (coord.Z * transform.M31)) + transform.M41;
		vector.Y = (((coord.X * transform.M12) + (coord.Y * transform.M22)) + (coord.Z * transform.M32)) + transform.M42;
		vector.Z = (((coord.X * transform.M13) + (coord.Y * transform.M23)) + (coord.Z * transform.M33)) + transform.M43;
		vector.W = 1.0f / ((((coord.X * transform.M14) + (coord.Y * transform.M24)) + (coord.Z * transform.M34)) + transform.M44);
		//vector.W = 1.0f;
		coord.X = vector.X * vector.W;
		coord.Y = vector.Y * vector.W;
		coord.Z = vector.Z * vector.W;
	}

	INLINE float Vec3Distance(const Vec3f& left, const Vec3f& right)
	{
		float x = right.X - left.X;
		float y = right.Y - left.Y;
		float z = right.Z - left.Z;

		return (float)sqrtf(x * x + y * y + z * z);
	}

	INLINE float Vec3DistanceSquared(const Vec3f& left, const Vec3f& right)
	{
		float x = right.X - left.X;
		float y = right.Y - left.Y;
		float z = right.Z - left.Z;

		return x * x + y * y + z * z;
	}

	INLINE float Vec3DistanceSquared(const Vec3f& left, const Vec4f& right)
	{
		float x = right.X - left.X;
		float y = right.Y - left.Y;
		float z = right.Z - left.Z;

		return x * x + y * y + z * z;
	}

	INLINE Vec3f Vec3Unproject(const Vec3f& vector, float x, float y, float width, float height, float minZ, float maxZ, const Matrix4x4& worldViewProjection)
	{
		Vec3f coordinate;
		Matrix4x4 transformation;
		Mat4x4Invert(&worldViewProjection, &transformation);

		coordinate.X = ((((vector.X - x) / (width)) * 2.0f) - 1.0f);
		coordinate.Y = -((((vector.Y - y) / (height)) * 2.0f) - 1.0f);
		coordinate.Z = (vector.Z - minZ) / (maxZ - minZ);
		Vec3TransformCoordinate(coordinate, transformation);

		return coordinate;
	}
	//-----Vector4D-----
	INLINE void Vec4Move( Vec3f& source, Vec3f& direction, float amount)
	{
		source.X += direction.X * amount;
		source.Y += direction.Y * amount;
		source.Z += direction.Z * amount;
	}
	INLINE void Vec4Minimize(Vec4f& value1, Vec4f& value2, Vec4f& result)
	{
		float z;
		float y;
		float x;
		float w;
		if (value1.X < value2.X)
		{
			x = value1.X;
		}
		else
		{
			x = value2.X;
		}
		result.X = x;
		if (value1.Y < value2.Y)
		{
			y = value1.Y;
		}
		else
		{
			y = value2.Y;
		}
		result.Y = y;
		if (value1.Z < value2.Z)
		{
			z = value1.Z;
		}
		else
		{
			z = value2.Z;
		}
		result.Z = z;
		if (value1.W < value2.W)
		{
			w = value1.W;
		}
		else
		{
			w = value2.W;
		}
		result.W = w;
	}
	INLINE void Vec4Maximize(Vec4f& value1, Vec4f& value2, Vec4f& result)
	{
		float z;
		float y;
		float x;
		float w;
		if (value1.X > value2.X)
		{
			x = value1.X;
		}
		else
		{
			x = value2.X;
		}
		result.X = x;
		if (value1.Y > value2.Y)
		{
			y = value1.Y;
		}
		else
		{
			y = value2.Y;
		}
		result.Y = y;
		if (value1.Z > value2.Z)
		{
			z = value1.Z;
		}
		else
		{
			z = value2.Z;
		}
		result.Z = z;

		if (value1.W > value2.W)
		{
			w = value1.W;
		}
		else
		{
			w = value2.W;
		}
		result.W = w;
	}
	//-----Matrix4x4-----
	INLINE Matrix4x4* Mat4x4Identity(Matrix4x4* dst)
	{
		memset(&dst->M12, 0, sizeof(Matrix4x4) - 2 * sizeof(float));
		dst->M11 = dst->M22 = dst->M33 = dst->M44 = 1.0f;
		return dst;
	}

	INLINE void Mat4x4Ortho(float width, float height, float zNear, float zFar, Matrix4x4& result)
	{
		result.M12 = result.M13 = result.M14 =
			result.M21 = result.M23 = result.M24 =
			result.M31 = result.M32 = result.M34 =
			result.M41 = result.M42 = 0.0f;
		result.M44 = 1.0f;

		result.M11 = 2.0f / width;
		result.M22 = 2.0f / height;
		result.M33 = 1.0f / (zFar - zNear);
		result.M43 = zNear / (zNear - zFar);
	}

	INLINE Matrix4x4 Mat4x4Invert(const Matrix4x4* matrix)
	{
		Matrix4x4 matrix2;
		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));
		matrix2.M11 = num39 * num;
		matrix2.M21 = num38 * num;
		matrix2.M31 = num37 * num;
		matrix2.M41 = num36 * num;
		matrix2.M12 = -(((num4 * num23) - (num3 * num22)) + (num2 * num21)) * num;
		matrix2.M22 = (((num5 * num23) - (num3 * num20)) + (num2 * num19)) * num;
		matrix2.M32 = -(((num5 * num22) - (num4 * num20)) + (num2 * num18)) * num;
		matrix2.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);
		matrix2.M13 = (((num4 * num35) - (num3 * num34)) + (num2 * num33)) * num;
		matrix2.M23 = -(((num5 * num35) - (num3 * num32)) + (num2 * num31)) * num;
		matrix2.M33 = (((num5 * num34) - (num4 * num32)) + (num2 * num30)) * num;
		matrix2.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);
		matrix2.M14 = -(((num4 * num29) - (num3 * num28)) + (num2 * num27)) * num;
		matrix2.M24 = (((num5 * num29) - (num3 * num26)) + (num2 * num25)) * num;
		matrix2.M34 = -(((num5 * num28) - (num4 * num26)) + (num2 * num24)) * num;
		matrix2.M44 = (((num5 * num27) - (num4 * num25)) + (num3 * num24)) * num;
		return matrix2;
	}

	INLINE void Mat4x4MulSSE(Matrix4x4* m1, Matrix4x4* m2, Matrix4x4* m3)
	{
		__m128 rowW = *((__m128*)(&m1->M11));
		__m128 rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
		__m128 rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
		__m128 rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
		rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));

		rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
		rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
		rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
		rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
		rowX = _mm_add_ps(rowX,rowZ);
		rowY = _mm_add_ps(rowY,rowW);
		rowX = _mm_add_ps(rowX,rowY);
		*((__m128*)(&m3->M11)) = rowX;
		rowW = *((__m128*)(&m1->M21));

		rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
		rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
		rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
		rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));

		rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
		rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
		rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
		rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
		rowX = _mm_add_ps(rowX,rowZ);
		rowY = _mm_add_ps(rowY,rowW);
		rowX = _mm_add_ps(rowX,rowY);
		*((__m128*)(&m3->M21)) = rowX;
		rowW = *((__m128*)(&m1->M31));

		rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
		rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
		rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
		rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));

		rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
		rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
		rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
		rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
		rowX = _mm_add_ps(rowX,rowZ);
		rowY = _mm_add_ps(rowY,rowW);
		rowX = _mm_add_ps(rowX,rowY);
		*((__m128*)(&m3->M31)) = rowX;
		rowW = *((__m128*)(&m1->M41));

		rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
		rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
		rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
		rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));

		rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
		rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
		rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
		rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
		rowX = _mm_add_ps(rowX,rowZ);
		rowY = _mm_add_ps(rowY,rowW);
		rowX = _mm_add_ps(rowX,rowY);
		*((__m128*)(&m3->M41)) = rowX;
	}

	INLINE Matrix4x4* Mat4x4RotationAxis(Vec3f* axis, float angle, Matrix4x4* pResult)
	{
		float y = axis->Y;
		float x = axis->X;
		float z = axis->Z;
		if (((float)(((x * x) + (y * y)) + (z * z))) != 1.0f)
		{
			Vec3Normalize(axis);
			// axis.Normalize();
		}
		float num4 = axis->X;
		float num3 = axis->Y;
		float num2 = axis->Z;
		float num = cosf(angle);
		float num5 = sinf(angle);
		float num23 = num4;
		float num17 = (num23 * num23);
		float num22 = num3;
		float num16 = (num22 * num22);
		float num21 = num2;
		float num15 = (num21 * num21);
		float num20 = num3 * num4;
		float num19 = num2 * num4;
		float num18 = num2 * num3;
		pResult->M11 = (((1.0f - num17) * num)) + num17;
		float num14 = num20;
		float num13 = num14 - (num * num14);
		float num12 = num5 * num2;
		pResult->M12 = (num12 + num13);
		float num11 = num19;
		float num10 = num11 - (num * num11);
		float num9 = num5 * num3;
		pResult->M13 = (num10 - num9);
		pResult->M14 = 0.0f;
		pResult->M21 = (num13 - num12);
		pResult->M22 = (((1.0f - num16) * num)) + num16;
		float num8 = num18;
		float num7 = num8 - (num * num8);
		float num6 = num5 * num4;
		pResult->M23 = (float)(num6 + num7);
		pResult->M24 = 0.0f;
		pResult->M31 = (float)(num9 + num10);
		pResult->M32 = (float)(num7 - num6);
		pResult->M33 = ((float)((1.0f - num15) * num)) + num15;
		pResult->M34 = 0.0f;
		pResult->M41 = 0.0f;
		pResult->M42 = 0.0f;
		pResult->M43 = 0.0f;
		pResult->M44 = 1.0f;

		return pResult;
	}

	INLINE Matrix4x4 Mat4x4RotationX(float angle)
	{
		Matrix4x4 matrix;
		float num2 = cosf(angle);
		float num = sinf(angle);
		matrix.M11 = 1.0f;
		matrix.M12 = 0.0f;
		matrix.M13 = 0.0f;
		matrix.M14 = 0.0f;
		matrix.M21 = 0.0f;
		matrix.M22 = num2;
		matrix.M23 = num;
		matrix.M24 = 0.0f;
		matrix.M31 = 0.0f;
		matrix.M32 = -num;
		matrix.M33 = num2;
		matrix.M34 = 0.0f;
		matrix.M41 = 0.0f;
		matrix.M42 = 0.0f;
		matrix.M43 = 0.0f;
		matrix.M44 = 1.0f;
		return matrix;
	}

	INLINE void Mat4x4RotationX(float angle, Matrix4x4& matrix)
	{
		float num2 = cosf(angle);
		float num = sinf(angle);
		matrix.M11 = 1.0f;
		matrix.M12 = 0.0f;
		matrix.M13 = 0.0f;
		matrix.M14 = 0.0f;
		matrix.M21 = 0.0f;
		matrix.M22 = num2;
		matrix.M23 = num;
		matrix.M24 = 0.0f;
		matrix.M31 = 0.0f;
		matrix.M32 = -num;
		matrix.M33 = num2;
		matrix.M34 = 0.0f;
		matrix.M41 = 0.0f;
		matrix.M42 = 0.0f;
		matrix.M43 = 0.0f;
		matrix.M44 = 1.0f;
	}

	INLINE Matrix4x4 Mat4x4RotationY(float angle)
	{
		Matrix4x4 matrix;
		float num2 = cosf(angle);
		float num = sinf(angle);
		matrix.M11 = num2;
		matrix.M12 = 0.0f;
		matrix.M13 = -num;
		matrix.M14 = 0.0f;
		matrix.M21 = 0.0f;
		matrix.M22 = 1.0f;
		matrix.M23 = 0.0f;
		matrix.M24 = 0.0f;
		matrix.M31 = num;
		matrix.M32 = 0.0f;
		matrix.M33 = num2;
		matrix.M34 = 0.0f;
		matrix.M41 = 0.0f;
		matrix.M42 = 0.0f;
		matrix.M43 = 0.0f;
		matrix.M44 = 1.0f;
		return matrix;
	}

	INLINE void Mat4x4RotationY(float angle, Matrix4x4& matrix)
	{
		float num2 = cosf(angle);
		float num = sinf(angle);
		matrix.M11 = num2;
		matrix.M12 = 0.0f;
		matrix.M13 = -num;
		matrix.M14 = 0.0f;
		matrix.M21 = 0.0f;
		matrix.M22 = 1.0f;
		matrix.M23 = 0.0f;
		matrix.M24 = 0.0f;
		matrix.M31 = num;
		matrix.M32 = 0.0f;
		matrix.M33 = num2;
		matrix.M34 = 0.0f;
		matrix.M41 = 0.0f;
		matrix.M42 = 0.0f;
		matrix.M43 = 0.0f;
		matrix.M44 = 1.0f;
	}

	INLINE Matrix4x4 Mat4x4RotationZ(float angle)
	{
		Matrix4x4 matrix;
		float num2 = cosf(angle);
		float num = sinf(angle);
		matrix.M11 = num2;
		matrix.M12 = num;
		matrix.M13 = 0.0f;
		matrix.M14 = 0.0f;
		matrix.M21 = -num;
		matrix.M22 = num2;
		matrix.M23 = 0.0f;
		matrix.M24 = 0.0f;
		matrix.M31 = 0.0f;
		matrix.M32 = 0.0f;
		matrix.M33 = 1.0f;
		matrix.M34 = 0.0f;
		matrix.M41 = 0.0f;
		matrix.M42 = 0.0f;
		matrix.M43 = 0.0f;
		matrix.M44 = 1.0f;
		return matrix;
	}

	INLINE void Mat4x4RotationZ(float angle, Matrix4x4& matrix)
	{
		float num2 = cosf(angle);
		float num = sinf(angle);
		matrix.M11 = num2;
		matrix.M12 = num;
		matrix.M13 = 0.0f;
		matrix.M14 = 0.0f;
		matrix.M21 = -num;
		matrix.M22 = num2;
		matrix.M23 = 0.0f;
		matrix.M24 = 0.0f;
		matrix.M31 = 0.0f;
		matrix.M32 = 0.0f;
		matrix.M33 = 1.0f;
		matrix.M34 = 0.0f;
		matrix.M41 = 0.0f;
		matrix.M42 = 0.0f;
		matrix.M43 = 0.0f;
		matrix.M44 = 1.0f;
	}

	INLINE void Mat4x4RotationQuaternion(const Quaternion& rotation, Matrix4x4& result)
	{
		float x = rotation.X;
		float num9 = (x * x);
		float y = rotation.Y;
		float num8 = (y * y);
		float z = rotation.Z;
		float num7 = (z * z);
		float num6 = rotation.Y * rotation.X;
		float num5 = rotation.W * rotation.Z;
		float num4 = rotation.Z * rotation.X;
		float num3 = rotation.W * rotation.Y;
		float num2 = rotation.Z * rotation.Y;
		float num = rotation.W * rotation.X;
		result.M11 = (1.0f - ((num7 + num8) * 2.0f));
		result.M12 = ((num5 + num6) * 2.0f);
		result.M13 = ((num4 - num3) * 2.0f);
		result.M14 = 0.0f;
		result.M21 = ((num6 - num5) * 2.0f);
		result.M22 = (1.0f - ((num7 + num9) * 2.0f));
		result.M23 = ((num + num2) * 2.0f);
		result.M24 = 0.0f;
		result.M31 = ((num3 + num4) * 2.0f);
		result.M32 = ((num2 - num) * 2.0f);
		result.M33 = (1.0f - ((num8 + num9) * 2.0f));
		result.M34 = 0.0f;
		result.M41 = 0.0f;
		result.M42 = 0.0f;
		result.M43 = 0.0f;
		result.M44 = 1.0f;
	}

	INLINE void Mat4x4RotationYawPitchRoll(float yaw, float pitch, float roll, Matrix4x4& out)
	{
		Quaternion quaternion;
		QuatRotationYawPitchRoll(yaw, pitch, roll, quaternion);
		Mat4x4RotationQuaternion(quaternion, out);
	}

	INLINE void Mat4x4Scaling(float xScale, float yScale, float zScale, Matrix4x4& out)
	{
		out.M11 = xScale;
		out.M12 = 0.0f;
		out.M13 = 0.0f;
		out.M14 = 0.0f;
		out.M21 = 0.0f;
		out.M22 = yScale;
		out.M23 = 0.0f;
		out.M24 = 0.0f;
		out.M31 = 0.0f;
		out.M32 = 0.0f;
		out.M33 = zScale;
		out.M34 = 0.0f;
		out.M41 = 0.0f;
		out.M42 = 0.0f;
		out.M43 = 0.0f;
		out.M44 = 1.0f;
	}

	INLINE void Mat4x4Scaling(Vec3f& amt, Matrix4x4& matrix)
	{
		matrix.M11 = amt.X;
		matrix.M12 = 0.0f;
		matrix.M13 = 0.0f;
		matrix.M14 = 0.0f;
		matrix.M21 = 0.0f;
		matrix.M22 = amt.Y;
		matrix.M23 = 0.0f;
		matrix.M24 = 0.0f;
		matrix.M31 = 0.0f;
		matrix.M32 = 0.0f;
		matrix.M33 = amt.Z;
		matrix.M34 = 0.0f;
		matrix.M41 = 0.0f;
		matrix.M42 = 0.0f;
		matrix.M43 = 0.0f;
		matrix.M44 = 1.0f;
	}

	INLINE void Mat4x4Translation(const Vec3f& amount, Matrix4x4& matrix)
	{
		matrix.M11 = 1.0f;
		matrix.M12 = 0.0f;
		matrix.M13 = 0.0f;
		matrix.M14 = 0.0f;
		matrix.M21 = 0.0f;
		matrix.M22 = 1.0f;
		matrix.M23 = 0.0f;
		matrix.M24 = 0.0f;
		matrix.M31 = 0.0f;
		matrix.M32 = 0.0f;
		matrix.M33 = 1.0f;
		matrix.M34 = 0.0f;
		matrix.M41 = amount.X;
		matrix.M42 = amount.Y;
		matrix.M43 = amount.Z;
		matrix.M44 = 1.0f;
	}

	INLINE void Mat4x4Translation(float _x, float _y, float _z, Matrix4x4& matrix)
	{
		matrix.M11 = 1.0f;
		matrix.M12 = 0.0f;
		matrix.M13 = 0.0f;
		matrix.M14 = 0.0f;
		matrix.M21 = 0.0f;
		matrix.M22 = 1.0f;
		matrix.M23 = 0.0f;
		matrix.M24 = 0.0f;
		matrix.M31 = 0.0f;
		matrix.M32 = 0.0f;
		matrix.M33 = 1.0f;
		matrix.M34 = 0.0f;
		matrix.M41 = _x;
		matrix.M42 = _y;
		matrix.M43 = _z;
		matrix.M44 = 1.0f;
	}

	INLINE void Mat4x4Transpose(const Matrix4x4& src, Matrix4x4& dst)
	{
		Matrix4x4 tmp = src;
		dst.M11 = tmp.M11;
		dst.M12 = tmp.M21;
		dst.M13 = tmp.M31;
		dst.M14 = tmp.M41;
		dst.M21 = tmp.M12;
		dst.M22 = tmp.M22;
		dst.M23 = tmp.M32;
		dst.M24 = tmp.M42;
		dst.M31 = tmp.M13;
		dst.M32 = tmp.M23;
		dst.M33 = tmp.M33;
		dst.M34 = tmp.M43;
		dst.M41 = tmp.M14;
		dst.M42 = tmp.M24;
		dst.M43 = tmp.M34;
		dst.M44 = tmp.M44;
	}

	INLINE void Mat4x4Lerp(const Matrix4x4& start, const Matrix4x4& end, Matrix4x4& result, float amount)
	{
		result.M11 = ((end.M11 - start.M11) * amount) + start.M11;
		result.M12 = ((end.M12 - start.M12) * amount) + start.M12;
		result.M13 = ((end.M13 - start.M13) * amount) + start.M13;
		result.M14 = ((end.M14 - start.M14) * amount) + start.M14;
		result.M21 = ((end.M21 - start.M21) * amount) + start.M21;
		result.M22 = ((end.M22 - start.M22) * amount) + start.M22;
		result.M23 = ((end.M23 - start.M23) * amount) + start.M23;
		result.M24 = ((end.M24 - start.M24) * amount) + start.M24;
		result.M31 = ((end.M31 - start.M31) * amount) + start.M31;
		result.M32 = ((end.M32 - start.M32) * amount) + start.M32;
		result.M33 = ((end.M33 - start.M33) * amount) + start.M33;
		result.M34 = ((end.M34 - start.M34) * amount) + start.M34;
		result.M41 = ((end.M41 - start.M41) * amount) + start.M41;
		result.M42 = ((end.M42 - start.M42) * amount) + start.M42;
		result.M43 = ((end.M43 - start.M43) * amount) + start.M43;
		result.M44 = ((end.M44 - start.M44) * amount) + start.M44;
	}

	INLINE void Mat4x4Billboard(const Vec3f& objectPosition, const Vec3f& cameraPosition, const Vec3f& cameraUpVector, const Vec3f& cameraForwardVector, Matrix4x4& matrix)
	{
		Vec3f eyeDir = objectPosition - cameraPosition;
		Vec3f right = eyeDir;
		Vec3f result;
		Vec3f vector3;

		float x = right.X;
		float y = right.Y;
		float z = right.Z;

		float num = (((x * x) + (y * y)) + (z * z));
		if (num < 9.99999974737875f)
		{
			right = -(cameraForwardVector);
		}
		else
		{
			right = (Vec3f)(eyeDir * ((1.0f / sqrtf(num))));
		}
		Vec3Cross(cameraUpVector, right, result);
		Vec3Normalize(&result);
		Vec3Cross(right, result, vector3);

		matrix.M11 = vector3.X;
		matrix.M12 = vector3.Y;
		matrix.M13 = vector3.Z;
		matrix.M14 = 0.0f;
		matrix.M21 = result.X;
		matrix.M22 = result.Y;
		matrix.M23 = result.Z;
		matrix.M24 = 0.0f;
		matrix.M31 = right.X;
		matrix.M32 = right.Y;
		matrix.M33 = right.Z;
		matrix.M34 = 0.0f;
		matrix.M41 = objectPosition.X;
		matrix.M42 = objectPosition.Y;
		matrix.M43 = objectPosition.Z;
		matrix.M44 = 1.0f;
	}

	INLINE void Mat4x4LookAtLH(const Vec3f& cameraPosition, const Vec3f& cameraTarget, const Vec3f& cameraUpVector, Matrix4x4& matrix)
	{
		Vec3f camDir = cameraTarget - cameraPosition;
		Vec3Normalize(&camDir);

		// Vec3f right = Vector3D.Normalize(Vector3D.Cross(cameraUpVector, camDir));

		Vec3f right;
		Vec3Cross(cameraUpVector, camDir, right);
		Vec3Normalize(&right);

		Vec3f up;
		Vec3Cross(camDir, right, up);

		matrix.M11 = right.X;
		matrix.M12 = up.X;
		matrix.M13 = camDir.X;
		matrix.M14 = 0.0f;
		matrix.M21 = right.Y;
		matrix.M22 = up.Y;
		matrix.M23 = camDir.Y;
		matrix.M24 = 0.0f;
		matrix.M31 = right.Z;
		matrix.M32 = up.Z;
		matrix.M33 = camDir.Z;
		matrix.M34 = 0.0f;
		matrix.M41 = -Vec3Dot(&right, &cameraPosition);
		matrix.M42 = -Vec3Dot(&up, &cameraPosition);
		matrix.M43 = -Vec3Dot(&camDir, &cameraPosition);
		matrix.M44 = 1.0f;
	}

	INLINE void Mat4x4Decompose(Matrix4x4& matrix, Vec3f& position, Vec3f& scale, Quaternion& rot)
	{
		//compute scaling
		Vec3f Row0(matrix.M11, matrix.M12, matrix.M13);
		Vec3f Row1(matrix.M21, matrix.M22, matrix.M23);
		Vec3f Row2(matrix.M31, matrix.M32, matrix.M33);
		scale.X = Vec3Length(&Row0);
		scale.Y = Vec3Length(&Row1);
		scale.Z = Vec3Length(&Row2);

		//translation
		position.X = matrix.M41;
		position.Y = matrix.M42;
		position.Z = matrix.M43;

		//rotation
		QuatRotationMatrix(matrix,rot);
	}

	INLINE void Mat4x4PerspectiveFOV(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, Matrix4x4& matrix)
	{
		/*if ((fieldOfView <= 0.0f) || (fieldOfView >= (2.0f * 3.1415926535f)))
		{
		throw new ArgumentOutOfRangeException("FieldOfView is not correct!");
		}
		if (nearPlaneDistance <= 0.0f)
		{
		throw new ArgumentOutOfRangeException("Near plane distance must be greater than zero");
		}
		if (farPlaneDistance <= 0.0f)
		{
		throw new ArgumentOutOfRangeException("Far plane distance must be greater than zero");
		}
		if (nearPlaneDistance >= farPlaneDistance)
		{
		throw new ArgumentOutOfRangeException("Distance to the far plane must be greater than distance to the near plane!");
		}*/
		float num = 1.0f / (tanf(fieldOfView * 0.5f));
		float num9 = num / aspectRatio;
		matrix.M11 = num9;
		matrix.M12 = matrix.M13 = matrix.M14 = 0.0f;
		matrix.M22 = num;
		matrix.M21 = matrix.M23 = matrix.M24 = 0.0f;
		matrix.M31 = matrix.M32 = 0.0f;
		matrix.M33 = farPlaneDistance / (farPlaneDistance - nearPlaneDistance);
		matrix.M34 = 1.0f;
		matrix.M41 = matrix.M42 = matrix.M44 = 0.0f;
		matrix.M43 = -nearPlaneDistance * farPlaneDistance / (farPlaneDistance - nearPlaneDistance);
	}
	//-----Quaternion-----
	INLINE void QuatIdentity(Quaternion& pDst)
	{
		pDst.W = pDst.Y = pDst.Z = 0.0f;
		pDst.X = 1.0f;
	}

	INLINE void QuatLerp(const Quaternion& quaternion1, const Quaternion& quaternion2, float amount, Quaternion& result)
	{
		float num = amount;
		float num2 = 1.0f - num;
		float num5 = (((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W);
		if (num5 >= 0.0f)
		{
			result.X = (num2 * quaternion1.X) + (num * quaternion2.X);
			result.Y = (num2 * quaternion1.Y) + (num * quaternion2.Y);
			result.Z = (num2 * quaternion1.Z) + (num * quaternion2.Z);
			result.W = (num2 * quaternion1.W) + (num * quaternion2.W);
		}
		else
		{
			result.X = (num2 * quaternion1.X) - (num * quaternion2.X);
			result.Y = (num2 * quaternion1.Y) - (num * quaternion2.Y);
			result.Z = (num2 * quaternion1.Z) - (num * quaternion2.Z);
			result.W = (num2 * quaternion1.W) - (num * quaternion2.W);
		}
		float num4 = (((result.X * result.X) + (result.Y * result.Y)) + (result.Z * result.Z)) + (result.W * result.W);

		if(num4 <=0)
			printf("OLOLOOOOOOOOOOOOOOOOO");
		float num3 = 1.0f / sqrtf(num4);
		result.X *= num3;
		result.Y *= num3;
		result.Z *= num3;
		result.W *= num3;
	}

	INLINE void  QuatSLerp( const Quaternion &from, const Quaternion &to, float t, Quaternion& result )
	{
		Quaternion  temp;
		float  omega, cosom, sinom, scale0, scale1;

		if ( t <= 0.0f )
		{
			result = from;
			return;
		}

		if ( t >= 1.0f )
		{
			result = to;
			return;
		}

		if ( from == to )
		{
			result = to;
			return;
		}

		cosom = from.X * to.X + from.Y * to.Y + from.Z * to.Z + from.W * to.W;
		if ( cosom < 0.0f )
		{
			temp = -to;
			cosom = -cosom;
		} else
		{
			temp = to;
		}

		if ( ( 1.0f - cosom ) > 1e-6f ) {
			omega = acos( cosom );
			sinom = 1.0f / sin( omega );
			scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
			scale1 = sin( t * omega ) * sinom;
		} else {
			scale0 = 1.0f - t;
			scale1 = t;
		}

		result = ( from * scale0) + ( temp * scale1);
	}

	INLINE float QuatLength(const Quaternion& src)
	{
		return sqrtf((src.X * src.X) + (src.Y * src.Y) + (src.Z * src.Z) + (src.W * src.W));
	}

	INLINE void QuatNormalize(Quaternion& src)
	{
		float num = (1.0f /  QuatLength(src));
		src.X *= num;
		src.Y *= num;
		src.Z *= num;
		src.W *= num;
	}

	INLINE void QuatSetAxisAngle(const Vec3f& axis, float theta, Quaternion& dst)
	{
		dst.X = sinf(theta * 0.5f) * axis.X;
		dst.Y = sinf(theta * 0.5f) * axis.Y;
		dst.Z = sinf(theta * 0.5f) * axis.Z;
		dst.W = cosf(theta * 0.5f);
	}

	INLINE void QuatInvert(Quaternion& src)
	{
		float num2 = (((src.X * src.X) + (src.Y * src.Y)) + (src.Z * src.Z)) + (src.W * src.W);
		float num = 1.0f / num2;
		src.X = -src.X * num;
		src.Y = -src.Y * num;
		src.Z = -src.Z * num;
		src.W = src.W * num;
	}
	//-----Plane-----
	INLINE bool PlaneIntersectLine(const Plane& plane, const Vec3f& start, const Vec3f& dir, float& dist)
	{
		dist = 0;
		float num2 = ((plane.Normal.X * dir.X) + (plane.Normal.Y * dir.Y)) + (plane.Normal.Z * dir.Z);
		if (fabs(num2) < 1E-05f)
		{
			return false;
		}
		float num3 = ((plane.Normal.X * start.X) + (plane.Normal.Y * start.Y)) + (plane.Normal.Z * start.Z);
		float num = (-plane.D - num3) / num2;
		if (num < 0.0f)
		{
			if (num < -1E-05f)
			{
				return false;
			}
			num = 0.0f;
		}
		dist = num;
		return true;
	}
	INLINE void PlaneNormalize(Plane& plane)
	{
		float num2 = ((plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y)) + (plane.Normal.Z * plane.Normal.Z);
		if (fabs(num2 - 1.0f) >= 1.192093E-07f)
		{
			float num = 1.0f / sqrtf(num2);
			plane.Normal.X *= num;
			plane.Normal.Y *= num;
			plane.Normal.Z *= num;
			plane.D *= num;
		}
	}
	INLINE void PlaneTransform(Plane& plane, const Matrix4x4& matrix)
	{
		Matrix4x4 matrix2 = matrix;
		matrix2 = Mat4x4Invert(&matrix2);
		float x = plane.Normal.X;
		float y = plane.Normal.Y;
		float z = plane.Normal.Z;
		float d = plane.D;
		plane.Normal.X = (((x * matrix2.M11) + (y * matrix2.M12)) + (z * matrix2.M13)) + (d * matrix2.M14);
		plane.Normal.Y = (((x * matrix2.M21) + (y * matrix2.M22)) + (z * matrix2.M23)) + (d * matrix2.M24);
		plane.Normal.Z = (((x * matrix2.M31) + (y * matrix2.M32)) + (z * matrix2.M33)) + (d * matrix2.M34);
		plane.D = (((x * matrix2.M41) + (y * matrix2.M42)) + (z * matrix2.M43)) + (d * matrix2.M44);
	}
	INLINE void PlaneTransform(const Plane& plane, const Matrix4x4& matrix, Plane& result)
	{
		Matrix4x4 matrix2 = matrix;
		matrix2 = Mat4x4Invert(&matrix2);
		float x = plane.Normal.X;
		float y = plane.Normal.Y;
		float z = plane.Normal.Z;
		float d = plane.D;
		result.Normal.X = (((x * matrix2.M11) + (y * matrix2.M12)) + (z * matrix2.M13)) + (d * matrix2.M14);
		result.Normal.Y = (((x * matrix2.M21) + (y * matrix2.M22)) + (z * matrix2.M23)) + (d * matrix2.M24);
		result.Normal.Z = (((x * matrix2.M31) + (y * matrix2.M32)) + (z * matrix2.M33)) + (d * matrix2.M34);
		result.D = (((x * matrix2.M41) + (y * matrix2.M42)) + (z * matrix2.M43)) + (d * matrix2.M44);
	}
	INLINE float PlaneDot(const Plane& plane, const Vec4f& value)
	{
		return ((((plane.Normal.X * value.X) + (plane.Normal.Y * value.Y)) + (plane.Normal.Z * value.Z)) + (plane.D * value.W));
	}
	INLINE float PlaneDotCoordinate(const Plane& plane, const Vec3f& value)
	{
		return ((((plane.Normal.X * value.X) + (plane.Normal.Y * value.Y)) + (plane.Normal.Z * value.Z)) + plane.D);
	}

	INLINE float PlaneDotNormal(const Plane& plane, const Vec3f& value)
	{
		return (((plane.Normal.X * value.X) + (plane.Normal.Y * value.Y)) + (plane.Normal.Z * value.Z));
	}
	INLINE float PlaneDot(const Plane& plane, const Vec3f& point)
	{
		return ((((plane.Normal.Y * point.Y) + (plane.Normal.X * point.X)) + (plane.Normal.Z * point.Z)) + plane.D);
	}

	INLINE void Get4x4Offsets(int destTextureWidth, int destTextureHeight, Vec4f* output)
	{
		float tu = 1.0f / destTextureWidth;
		float tv = 1.0f / destTextureHeight;

		int index = 0;
		for (int x = 0; x < 4; x++)
		{
			for (int y = 0; y < 4; y++)
			{
				output[index].X = (x - 1.5f) * tu;
				output[index].Y = (y - 1.5f) * tv;

				index++;
			}
		}
	}
	INLINE void Get3x3Offsets(int destTextureWidth, int destTextureHeight, Vec4f* output)
	{
		float sampleOffset = 1.0f;
		float tu = 1.0f / destTextureWidth * sampleOffset;
		float tv = 1.0f / destTextureHeight * sampleOffset;

		int index = 0;
		for (int x = 0; x < 3; x++)
		{
			for (int y = 0; y < 3; y++)
			{
				output[index].X = (x - 1.5f) * tu;
				output[index].Y = (y - 1.5f) * tv;

				index++;
			}
		}
	}
	INLINE void Get2x2Offsets(int destTextureWidth, int destTextureHeight, Vec4f* output)
	{
		float sampleOffset = 1.0f;
		float tu = 1.0f / destTextureWidth * sampleOffset;
		float tv = 1.0f / destTextureHeight * sampleOffset;

		//Vector4D[] offsets = new Vector4D[4];
		int index = 0;
		for (int x = 0; x < 2; x++)
		{
			for (int y = 0; y < 2; y++)
			{
				output[index].X = (x - 0.5f) * tu;
				output[index].Y = (y - 0.5f) * tv;

				index++;
			}
		}
	}
	INLINE void GetBlurWeightedOffsets(float numSamples, int destTextureDimensionLength, Vec4f* output)
	{
		float tuv = 1.0f / destTextureDimensionLength;

		float offset = -(numSamples - 1.0f) / 2.0f;
		for (int i = 0; i < numSamples; i++)
		{
			output[i].X = offset * tuv;
			offset++;
			float x = 2.0f * i / (numSamples - 1.0f) - 1.0f;
			output[i].Y = -0.25f * x * x + 0.225f;
		}
	}
}
#endif // LifeMath_h__