//////////////////////////////////////////////////////////////////////////
// Author: Ben Vanik - noxa@nomorals.org
// File: rnd_Math.h
// Created: 5/4/2001 1:01:12 PM
// Class: .H C++ Header
//////////////////////////////////////////////////////////////////////////
/* Special notes:                                                       *\

\*                                                                      */

#pragma once

//========================================================================
// Includes
#include "eng_Main.h"

//========================================================================
// Definitions

#define MAX_VERTEX_TEX_LVLS		3	// 4 total vertex texture levels

#define PI			(3.14159265359f)
#define DEG2RAD(a)	( PI  / 180 * (a) )
#define RAD2DEG(a)	( 180 / PI  * (a) )

//========================================================================
// Macros

//========================================================================
// Typedefs

//========================================================================
// Globals

//========================================================================
// Prototypes
void Mat_Identify_4x4(float **fMatrix);
void Mat_Mul_4x4_4x4(float **fA, float **fB, float **fDest);

//////////////////////////////////////////////////////////////////////////
// Body
//////////////////////////////////////////////////////////////////////////

// 2 float value
struct pt
{
	float x, y;
};

// 3 float value
struct pt3
{
	float x, y, z;
};

#define vec pt3

// 4 float value
struct pt4
{
	float x, y, z, a;
};

//////////////////////////////////////////////////////////////////////////
// Struct: vertex
// Notes: Vertex used in calculations and hardware operations
//========================================================================
struct vertex
{
	float x, y, z;
	float nx, ny, nz;											// Normal
	float tu, tv;												// Base texture level
};

//////////////////////////////////////////////////////////////////////////
// Struct: mvertex
// Notes: Multi-textured vertex
//========================================================================
struct mvertex
{
	float x, y, z;
	float nx, ny, nz;											// Normal
	float tu, tv;												// Base texture level
	float _tu[MAX_VERTEX_TEX_LVLS], _tv[MAX_VERTEX_TEX_LVLS];	// Extra texture levels (can be left blank)
};

//////////////////////////////////////////////////////////////////////////
// Class: IVector
// Notes: Advanced vector class
//========================================================================
// Code for this class taken without permission *grin* from http://www.baskuenen.myweb.nl/
// Optimized by noxa@nomorals.org
class IVector
{
public:
	float	x, y, z;

	IVector()
	{
	}
	IVector(float _x, float _y, float _z)
	{
		/*
		x	= _x;
		y	= _y;
		z	= _z;
		*/
		__asm
		{
			mov ebx, dword ptr [this]
			mov eax, _x
			mov dword ptr [ebx], eax
			mov eax, _y
			mov dword ptr [ebx+4], eax
			mov eax, _z
			mov dword ptr [ebx+8], eax
		}
	}
	IVector(const IVector &_v)
	{
		/*
		x	= _v.x;
		y	= _v.y;
		z	= _v.z;
		*/
		__asm
		{
			mov ebx, dword ptr [this]
			mov ecx, dword ptr [_v]

			mov eax, dword ptr [ecx]
			mov dword ptr [ebx], eax
			mov eax, dword ptr [ecx+4]
			mov dword ptr [ebx+4], eax
			mov eax, dword ptr [ecx+8]
			mov dword ptr [ebx+8], eax
		}
	}
	const IVector& operator = (const IVector& _v)
	{
		/*
		x	= _v.x;
		y	= _v.y;
		z	= _v.z;
		*/
		__asm
		{
			mov ebx, dword ptr [this]
			mov ecx, dword ptr [_v]
			
			mov eax, dword ptr [ecx]
			mov dword ptr [ebx], eax
			mov eax, dword ptr [ecx+4]
			mov dword ptr [ebx+4], eax
			mov eax, dword ptr [ecx+8]
			mov dword ptr [ebx+8], eax
		}
		return(*this);
	}

	// Boolean stuff
	const bool operator == (const IVector  &_v) const
	{
		//return( (x == _v.x) && (y == _v.y) && (z == _v.z) );
		bool	bEqual;
		__asm
		{
			mov ebx, dword ptr [this]
			mov ecx, dword ptr [_v]
			fld [ebx]				; x
			fcomp [ecx]				; _v.x
			fnstsw ax   
			test ah, 44h
			jp end_no
			fld [ebx+4]				; y
			fcomp [ecx+4]			; _v.y
			fnstsw ax   
			test ah, 44h
			jp end_no
			fld [ebx+8]				; z
			fcomp [ecx+8]			; _v.z
			fnstsw ax   
			test ah, 44h
			jp end_no
			mov bEqual, 1
			jmp end
		end_no:
			mov bEqual, 0
		end:
		}
		return( bEqual );
	}
	const bool operator != (const IVector &_v) const
	{
		//return( !((*this) == _v) );
		bool	bEqual;
		__asm
		{
			mov ebx, dword ptr [this]
			mov ecx, dword ptr [_v]
			fld [ebx]				; x
			fcomp [ecx]				; _v.x
			fnstsw ax   
			test ah, 44h
			jp end_no
			fld [ebx+4]				; y
			fcomp [ecx+4]			; _v.y
			fnstsw ax   
			test ah, 44h
			jp end_no
			fld [ebx+8]				; z
			fcomp [ecx+8]			; _v.z
			fnstsw ax   
			test ah, 44h
			jp end_no
			mov bEqual, 1
			jmp end
		end_no:
			mov bEqual, 0
		end:
		}
		return( !bEqual );
	}

	// Add & Substract, Positive & Negative
	const IVector operator + (const IVector &_v) const
	{
		IVector vta;
		/*
		vta.x	= x + _v.x;
		vta.y	= y + _v.y;
		vta.z	= z + _v.z;
		*/
		__asm
		{
			; save needed pointers
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]

			; x
			fld dword ptr [ecx]		; load x of _v
			fadd dword ptr [edx]	; load x of this
			fst dword ptr [vta]		; store to x of vta

			; y
			fld dword ptr [ecx+4]	; load y of _v
			fadd dword ptr [edx+4]	; load y of this
			fst dword ptr [vta+4]	; store to y of vta
			
			; z
			fld dword ptr [ecx+8]	; load z of _v
			fadd dword ptr [edx+8]	; load z of this
			fst dword ptr [vta+8]	; store to z of vta
		}
		return( vta );
	}
	const IVector operator + () const
	{
		IVector vta;
		/*
		vta.x	= x;
		vta.y	= y;
		vta.z	= z;
		*/
		__asm
		{
			mov ebx, dword ptr [this]
			;x
			mov ecx, dword ptr [ebx]
			mov dword ptr [vta], ecx
			;y
			mov ecx, dword ptr [ebx+4]
			mov dword ptr [vta+4], ecx
			;z
			mov ecx, dword ptr [ebx+8]
			mov dword ptr [vta+8], ecx
		}
		return( vta );
	}
	const IVector& operator += (const IVector &_v)
	{
		/*
		x += Vector.x;
		y += Vector.y;
		z += Vector.z;
		*/
		__asm
		{
			mov ebx, dword ptr [this]
			mov ecx, dword ptr [_v]
			;x
			fld [ebx]
			fadd [ecx]
			fst [ebx]
			;y
			fld [ebx+4]
			fadd [ecx+4]
			fst [ebx+4]
			;z
			fld [ebx+8]
			fadd [ecx+8]
			fst [ebx+8]
		}
		return( *this );
	}
	const IVector operator - (const IVector &_v) const
	{
		//return( IVector(x - _v.x, y - _v.y, z - _v.z) );
		IVector vta;
		__asm
		{
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]
			
			; x
			fld dword ptr [ecx]		; load x of _v
			fsub dword ptr [edx]	; load x of this
			fst dword ptr [vta]		; store to x of vta

			; y
			fld dword ptr [ecx+4]	; load y of _v
			fsub dword ptr [edx+4]	; load y of this
			fst dword ptr [vta+4]	; store to y of vta
			
			; z
			fld dword ptr [ecx+8]	; load z of _v
			fsub dword ptr [edx+8]	; load z of this
			fst dword ptr [vta+8]	; store to z of vta

		}
		return( vta );
	}
	const IVector operator - () const
	{
		//return( IVector(-x, -y, -z) );
		IVector vta;
		__asm
		{
			mov ebx, dword ptr [this]
			;x
			fld [ebx]
			fchs
			fst dword ptr [vta]
			;y
			fld [ebx+4]
			fchs
			fst dword ptr [vta+4]
			;z
			fld [ebx+8]
			fchs
			fst dword ptr [vta+8]
		}
		return( vta );
	}
	const IVector& operator -= (const IVector &_v)
	{
		/*
		x -= Vector.x;
		y -= Vector.y;
		z -= Vector.z;
		*/
		/*
		x -= y
		x = x - y
		*/
		__asm
		{
			mov ebx, dword ptr [this]
			mov ecx, dword ptr [_v]
			;x
			fld [ecx]
			fld [ebx]
			fsub
			fst [ebx]
			;y
			fld [ecx+4]
			fld [ebx+4]
			fsub
			fst [ebx+4]
			;z
			fld [ecx+8]
			fld [ebx+8]
			fsub
			fst [ebx+8]
		}
		return( (*this) );
	}

	// Multiply & Divide
	const IVector operator * (const IVector &_v) const
	{
		//return IVector(x * _v.x, y * _v.y, z * _v.z);
		IVector	vta;
		__asm
		{
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]
			
			; x
			fld dword ptr [ecx]		; load x of _v
			fmul dword ptr [edx]	; load x of this
			fst dword ptr [vta]		; store to x of vta

			; y
			fld dword ptr [ecx+4]	; load y of _v
			fmul dword ptr [edx+4]	; load y of this
			fst dword ptr [vta+4]	; store to y of vta
			
			; z
			fld dword ptr [ecx+8]	; load z of _v
			fmul dword ptr [edx+8]	; load z of this
			fst dword ptr [vta+8]	; store to z of vta
		}
		return( vta );
	}
	const IVector& operator *= (const IVector &_v)
	{
		/*
		x *= _v.x;
		y *= _v.y;
		z *= _v.z;
		*/
		IVector	vta;
		__asm
		{
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]
			
			; x
			fld dword ptr [ecx]		; load x of _v
			fmul dword ptr [edx]	; load x of this
			fst dword ptr [edx]		; store to x of this

			; y
			fld dword ptr [ecx+4]	; load y of _v
			fmul dword ptr [edx+4]	; load y of this
			fst dword ptr [edx+4]	; store to y of this
			
			; z
			fld dword ptr [ecx+8]	; load z of _v
			fmul dword ptr [edx+8]	; load z of this
			fst dword ptr [edx+8]	; store to z of this
		}
		return( (*this) );
	}
	const IVector operator * (const float d) const
	{
		//return IVector(x * d, y * d, z * d);
		IVector vta;
		__asm
		{
			mov edx, dword ptr [this]
			fld d					; load d
			fld st					; dup it
			fld st					; dup it

			; x
			fmul dword ptr [edx]	; load x of this
			fstp dword ptr [vta]	; store to x of vta

			; y
			fmul dword ptr [edx+4]	; load y of this
			fstp dword ptr [vta+4]	; store to y of vta
			
			; z
			fmul dword ptr [edx+8]	; load z of this
			fstp dword ptr [vta+8]	; store to z of vta
		}
		return( vta );
	}
	const IVector& operator *= (const float d)
	{
		/*
		x *= d;
		y *= d;
		z *= d;
		*/
		IVector	vta;
		__asm
		{
			mov edx, dword ptr [this]
			fld d					; load d
			fld st
			fld st
			
			; x
			fmul dword ptr [edx]	; load x of this
			fstp dword ptr [edx]	; store to x of this

			; y
			fmul dword ptr [edx+4]	; load y of this
			fstp dword ptr [edx+4]	; store to y of this
			
			; z
			fmul dword ptr [edx+8]	; load z of this
			fstp dword ptr [edx+8]	; store to z of this
		}
		return( (*this) );
	}
	const IVector operator / (const IVector &_v) const
	{
		//return IVector(x / _v.x, y / _v.y, z / _v.z);
		IVector vta;
		__asm
		{
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]
			
			; x
			fld dword ptr [ecx]		; load x of _v
			fdiv dword ptr [edx]	; load x of this
			fstp dword ptr [vta]	; store to x of vta

			; y
			fld dword ptr [ecx+4]	; load y of _v
			fdiv dword ptr [edx+4]	; load y of this
			fstp dword ptr [vta+4]	; store to y of vta
			
			; z
			fld dword ptr [ecx+8]	; load z of _v
			fdiv dword ptr [edx+8]	; load z of this
			fstp dword ptr [vta+8]	; store to z of vta
		}
		return( vta );
	}
	const IVector& operator /= (const IVector &_v)
	{
		/*
		x /= _v.x;
		y /= _v.y;
		z /= _v.z;
		*/
		__asm
		{
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]
			
			; x
			fld dword ptr [ecx]		; load x of _v
			fdiv dword ptr [edx]	; load x of this
			fstp dword ptr [edx]	; store to x of this

			; y
			fld dword ptr [ecx+4]	; load y of _v
			fdiv dword ptr [edx+4]	; load y of this
			fstp dword ptr [edx+4]	; store to y of this
			
			; z
			fld dword ptr [ecx+8]	; load z of _v
			fdiv dword ptr [edx+8]	; load z of this
			fstp dword ptr [edx+8]	; store to z of this
		}
		return( (*this) );
	}
	const IVector operator / (const float d) const
	{
		const float _d = 1.0f / d;
		//return IVector(x * _d, y * _d, z * _d);
		IVector vta;
		__asm
		{
			mov edx, dword ptr [this]
			fld _d					; load _d
			fld st
			fld st

			; x
			fmul dword ptr [edx]	; load x of this
			fstp dword ptr [vta]	; store to x of vta

			; y
			fmul dword ptr [edx+4]	; load y of this
			fstp dword ptr [vta+4]	; store to y of vta
			
			; z
			fmul dword ptr [edx+8]	; load z of this
			fstp dword ptr [vta+8]	; store to z of vta
		}
		return( vta );
	}
	const IVector& operator /= (const float d)
	{
		const float _d = 1.0f / d;
		/*
		x *= _d;
		y *= _d;
		z *= _d;
		*/
		__asm
		{
			mov edx, dword ptr [this]
			fld _d					; load _d
			fld st
			fld st
			
			; x
			fmul dword ptr [edx]	; load x of this
			fstp dword ptr [edx]	; store to x of this

			; y
			fmul dword ptr [edx+4]	; load y of this
			fstp dword ptr [edx+4]	; store to y of this
			
			; z
			fmul dword ptr [edx+8]	; load z of this
			fstp dword ptr [edx+8]	; store to z of this
		}
		return( (*this) );
	}

	// Dot Product
	const float operator % (const IVector &_v) const
	{
		//return( (x * _v.x) + (y * _v.y) + (z * _v.z) );
		float fTemp;
		__asm
		{
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]

			; (x * _v.x)
			fld dword ptr [edx]
			fld dword ptr [ecx]
			fmul
			
			; (y * _v.y)
			fld dword ptr [edx+4]
			fld dword ptr [ecx+4]
			fmul

			; (z * _v.z)
			fld dword ptr [edx+8]
			fld dword ptr [ecx+8]
			fmul

			; (a) + (b) + (c)
			fadd
			fadd
			fst fTemp
		}
		return( fTemp );
	}
	// Cross Product
	const IVector operator ^ (const IVector &_v) const
	{
		/*
		return IVector(
				y * _v.z - _v.y * z,
				z * _v.x - _v.z * x,
				x * _v.y - _v.x * y);
				*/
		IVector vta;
		__asm
		{
			mov ecx, dword ptr [_v]
			mov edx, dword ptr [this]

			;x
			;= (_v.y * z)
			fld dword ptr [ecx+4]
			fld dword ptr [edx+8]
			fmul
			;= (y * _v.z)
			fld dword ptr [edx+4]
			fld dword ptr [ecx+8]
			fmul
			;= (a) - (b)
			fsub
			fstp dword ptr [vta]

			;y
			;= (_v.z * x)
			fld dword ptr [ecx+8]
			fld dword ptr [edx]
			fmul
			;= (z * _v.x)
			fld dword ptr [edx+8]
			fld dword ptr [ecx]
			fmul
			;= (a) - (b)
			fsub
			fstp dword ptr [vta+4]

			;z
			;= (_v.x * y)
			fld dword ptr [ecx]
			fld dword ptr [edx+4]
			fmul
			;= (x * _v.y)
			fld dword ptr [edx]
			fld dword ptr [ecx+4]
			fmul
			;= (a) - (b)
			fsub
			fstp dword ptr [vta+8]
		}
		return( vta );
	}
	const IVector& operator ^= (const IVector &_v)
	{
		//TODO: Optimize this - IVector::operator ^=
		return( (*this) = (*this) ^ _v );
	}

	// Set length (Normalize if 1)
	const float operator ! () const
	{
		//return sqrtf(x*x + y*y + z*z);
		float fTemp;
		__asm
		{
			mov ebx, dword ptr [this]

			;x
			fld dword ptr [ebx]
			fld st
			fmul

			;y
			fld dword ptr [ebx+4]
			fld st
			fmul

			;z
			fld dword ptr [ebx+8]
			fld st
			fmul

			; add them up
			fadd
			fadd

			; square root
			fsqrt

			fstp fTemp
		}
		return( fTemp );
	}
	const IVector operator | (const float dLength) const
	{
		//TODO: Optimize this - IVector::operator |
		return( (*this) * (dLength / !(*this)) );
	}
	const IVector& operator |= (const float dLength)
	{
		//TODO: Optimize this - IVector::operator |=
		return *this = *this | dLength;
	}

	// The angle between two vectors in radians
	const float inline Angle(const IVector& Normal) const
	{
		//TODO: Optimize this - IVector::Angle
		return( acosf( (*this) % Normal ) );
	}
	// Reflect in Normal _v
	const IVector inline Reflection(const IVector& PlaneNormal) const
	{
		//TODO: Optimize this - IVector::Reflection
		return( ((*this) - PlaneNormal * 2.0 * ((*this) % PlaneNormal)) * !(*this) );
	}
	// Rotate fAngle Degrees (radians) Around Normal
	const IVector inline Rotate(const float fAngle, const IVector& Normal) const
	{
		float fCos0;
		__asm
		{
			fld fAngle
			fcos
			fstp fCos0
		}
		float fSin0;
		__asm
		{
			fld fAngle
			fsin
			fstp fSin0
		}
		//TODO: Optimize this - IVector::Rotate
		return( IVector(
				(*this) * fCos0 + ((Normal * (*this)) * (1.0f - fCos0)) * Normal + ((*this) ^ Normal) * fSin0
				) );
	}
};

//////////////////////////////////////////////////////////////////////////
// Class: IMatrix
// Notes: Advanced matrix class
//========================================================================
// Code for this class taken without permission *grin* from http://www.baskuenen.myweb.nl/
// Optimized by noxa@nomorals.org
class IMatrix
{
public:
	union
	{
		struct
		{
			IVector RotX;
			float m03;
			IVector RotY;
			float m13;
			IVector RotZ;
			float m23;
			union
			{
				struct
				{
					IVector Pos;
				};
				struct
				{
					float x, y, z;
				};
			};
			float m33;
		};
		float m4x4[4][4];
		float m16[16];
	};

	IMatrix(const float x = 0, const float y = 0, const float z = 0) : Pos(x,y,z), RotX(1,0,0), RotY(0,1,0), RotZ(0,0,1), m33(1)
	{
	}
	IMatrix(const IVector& _Pos, const IVector& _RotX = IVector(1,0,0), const IVector& _RotY = IVector(0,1,0), const IVector& _RotZ = IVector(0,0,1)) : Pos(_Pos), RotX(_RotX), RotY(_RotY), RotZ(_RotZ), m33(1)
	{
	}
	IMatrix(const IMatrix& Matrix) : Pos(Matrix.Pos), RotX(Matrix.RotX), RotY(Matrix.RotY), RotZ(Matrix.RotZ), m33(1)
	{
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: Identify
	// Notes: Identify the matrix
	//========================================================================
	void __inline Identify()
	{
		// Set everything to 0
		memset( (void*)m4x4, 0, sizeof(float) * 16 );

		// Set diagonals to 1
		m4x4[0][0] = m4x4[1][1] = m4x4[2][2] = m4x4[3][3] = 1;
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: operator =
	// Notes: Equal operator
	//========================================================================
	const IMatrix& operator = (const IMatrix& Matrix)
	{
		//Copy the matrix data
		memcpy( (void*)m16, (void*)Matrix.m16, sizeof(float) * 16 );
		
		return( (*this) );
	}
	
	//////////////////////////////////////////////////////////////////////////
	// Function: operator =
	// Notes: Transform equal operator
	//========================================================================
	const IMatrix& operator = (const IVector &_v)
	{
		__asm
		{
			; Pos	= _v;
			mov ebx, dword ptr [Pos]
			mov ecx, dword ptr [_v]

			mov eax, dword ptr [ecx]
			mov dword ptr [ebx], eax
			mov eax, dword ptr [ecx+4]
			mov dword ptr [ebx+4], eax
			mov eax, dword ptr [ecx+8]
			mov dword ptr [ebx+8], eax
		}

		return( (*this) );
	}

	const bool operator == (const IMatrix& Matrix) const
	{
		return( ( Pos == Matrix.Pos ) &&
				( RotX == Matrix.RotX ) &&
				( RotY == Matrix.RotY ) &&
				( RotZ == Matrix.RotZ ) );
	}
	const bool operator != (const IMatrix& Matrix) const
	{
		return( !( (*this) == Matrix) );
	}

	const IMatrix operator + (const IVector &_v) const
	{
		return( IMatrix( Pos + IVector(_v % RotX, _v % RotY, _v % RotZ), RotX, RotY, RotZ) );
	}
	const IMatrix& operator += (const IVector &_v)
	{
		return( (*this) = (*this) + _v );
	}
	const IMatrix operator - (const IVector &_v) const
	{
		return( IMatrix( Pos - IVector(_v % RotX, _v % RotY, _v % RotZ), RotX, RotY, RotZ) );
	}
	const IMatrix& operator -= (const IVector &_v)
	{
		return( (*this) = (*this) - _v );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: operator *
	// Notes: Multiply the matrix by another
	//========================================================================
	const IMatrix operator * (const IMatrix& Matrix) const
	{
		IMatrix	mat;

		mat.m4x4[0][0]	= m4x4[0][0] * Matrix.m4x4[0][0] + m4x4[0][1] * Matrix.m4x4[1][0] + m4x4[0][2] * Matrix.m4x4[2][0];
		mat.m4x4[0][1]	= m4x4[0][0] * Matrix.m4x4[0][1] + m4x4[0][1] * Matrix.m4x4[1][1] + m4x4[0][2] * Matrix.m4x4[2][1];
		mat.m4x4[0][2]	= m4x4[0][0] * Matrix.m4x4[0][2] + m4x4[0][1] * Matrix.m4x4[1][2] + m4x4[0][2] * Matrix.m4x4[2][2];
		mat.m4x4[0][3]	= 0;

		mat.m4x4[1][0]	= m4x4[1][0] * Matrix.m4x4[0][0] + m4x4[1][1] * Matrix.m4x4[1][0] + m4x4[1][2] * Matrix.m4x4[2][0];
		mat.m4x4[1][1]	= m4x4[1][0] * Matrix.m4x4[0][1] + m4x4[1][1] * Matrix.m4x4[1][1] + m4x4[1][2] * Matrix.m4x4[2][1];
		mat.m4x4[1][2]	= m4x4[1][0] * Matrix.m4x4[0][2] + m4x4[1][1] * Matrix.m4x4[1][2] + m4x4[1][2] * Matrix.m4x4[2][2];
		mat.m4x4[1][3]	= 0;

		mat.m4x4[2][0]	= m4x4[2][0] * Matrix.m4x4[0][0] + m4x4[2][1] * Matrix.m4x4[1][0] + m4x4[2][2] * Matrix.m4x4[2][0];
		mat.m4x4[2][1]	= m4x4[2][0] * Matrix.m4x4[0][1] + m4x4[2][1] * Matrix.m4x4[1][1] + m4x4[2][2] * Matrix.m4x4[2][1];
		mat.m4x4[2][2]	= m4x4[2][0] * Matrix.m4x4[0][2] + m4x4[2][1] * Matrix.m4x4[1][2] + m4x4[2][2] * Matrix.m4x4[2][2];
		mat.m4x4[2][3]	= 0;

		mat.m4x4[3][0]	= m4x4[3][0] * Matrix.m4x4[0][0] + m4x4[3][1] * Matrix.m4x4[1][0] + m4x4[3][2] * Matrix.m4x4[2][0];
		mat.m4x4[3][1]	= m4x4[3][0] * Matrix.m4x4[0][1] + m4x4[3][1] * Matrix.m4x4[1][1] + m4x4[3][2] * Matrix.m4x4[2][1];
		mat.m4x4[3][2]	= m4x4[3][0] * Matrix.m4x4[0][2] + m4x4[3][1] * Matrix.m4x4[1][2] + m4x4[3][2] * Matrix.m4x4[2][2];
		mat.m4x4[3][3]	= 1;

		return( mat );

	}

	//////////////////////////////////////////////////////////////////////////
	// Function: operator *=
	// Notes: Multiply and equal
	//========================================================================
	const IMatrix& operator *= (const IMatrix& Matrix)
	{
		// (*this) = (*this) * Matrix

		float	_m4x4[4][4];

		_m4x4[0][0]	= m4x4[0][0] * Matrix.m4x4[0][0] + m4x4[0][1] * Matrix.m4x4[1][0] + m4x4[0][2] * Matrix.m4x4[2][0];
		_m4x4[0][1]	= m4x4[0][0] * Matrix.m4x4[0][1] + m4x4[0][1] * Matrix.m4x4[1][1] + m4x4[0][2] * Matrix.m4x4[2][1];
		_m4x4[0][2]	= m4x4[0][0] * Matrix.m4x4[0][2] + m4x4[0][1] * Matrix.m4x4[1][2] + m4x4[0][2] * Matrix.m4x4[2][2];
		_m4x4[0][3]	= 0;

		_m4x4[1][0]	= m4x4[1][0] * Matrix.m4x4[0][0] + m4x4[1][1] * Matrix.m4x4[1][0] + m4x4[1][2] * Matrix.m4x4[2][0];
		_m4x4[1][1]	= m4x4[1][0] * Matrix.m4x4[0][1] + m4x4[1][1] * Matrix.m4x4[1][1] + m4x4[1][2] * Matrix.m4x4[2][1];
		_m4x4[1][2]	= m4x4[1][0] * Matrix.m4x4[0][2] + m4x4[1][1] * Matrix.m4x4[1][2] + m4x4[1][2] * Matrix.m4x4[2][2];
		_m4x4[1][3]	= 0;

		_m4x4[2][0]	= m4x4[2][0] * Matrix.m4x4[0][0] + m4x4[2][1] * Matrix.m4x4[1][0] + m4x4[2][2] * Matrix.m4x4[2][0];
		_m4x4[2][1]	= m4x4[2][0] * Matrix.m4x4[0][1] + m4x4[2][1] * Matrix.m4x4[1][1] + m4x4[2][2] * Matrix.m4x4[2][1];
		_m4x4[2][2]	= m4x4[2][0] * Matrix.m4x4[0][2] + m4x4[2][1] * Matrix.m4x4[1][2] + m4x4[2][2] * Matrix.m4x4[2][2];
		_m4x4[2][3]	= 0;

		_m4x4[3][0]	= m4x4[3][0] * Matrix.m4x4[0][0] + m4x4[3][1] * Matrix.m4x4[1][0] + m4x4[3][2] * Matrix.m4x4[2][0];
		_m4x4[3][1]	= m4x4[3][0] * Matrix.m4x4[0][1] + m4x4[3][1] * Matrix.m4x4[1][1] + m4x4[3][2] * Matrix.m4x4[2][1];
		_m4x4[3][2]	= m4x4[3][0] * Matrix.m4x4[0][2] + m4x4[3][1] * Matrix.m4x4[1][2] + m4x4[3][2] * Matrix.m4x4[2][2];
		_m4x4[3][3]	= 1;

		memcpy( (void*)this->m4x4, (void*)_m4x4, sizeof(float) * 16 );

		return( (*this) );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: operator *
	// Notes: Multiply the matrix by a vector
	//========================================================================
	const IMatrix operator * (const IVector &_v) const
	{
		return( (IMatrix)IMatrix(Pos * _v, RotX, RotY, RotZ) );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: operator !
	// Notes: Inverse matrix
	//========================================================================
	const IMatrix __fastcall operator ! () const
	{
		IMatrix Matrix( IVector(0,0,0),
						IVector(RotX.x, RotY.x, RotZ.x),
						IVector(RotX.y, RotY.y, RotZ.y),
						IVector(RotX.z, RotY.z, RotZ.z)	);
		return( Matrix - Pos );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: Inverse
	// Notes: Inverse matrix
	//========================================================================
	const IMatrix& Inverse() const
	{
		return( (*this) );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: RotateX
	// Notes: Rotate along the X axis
	//========================================================================
	const IMatrix __fastcall RotateX(float fAngle)
	{
		float fSin0;
		float fCos0;

		// Calculate sin/cos
		fSin0	= (float)sin(fAngle);
		fCos0	= (float)cos(fAngle);

		// Setup matrix
		this->m4x4[1][1]	= ( fCos0 );
		this->m4x4[1][2]	= ( fSin0 );
		this->m4x4[2][1]	= (-fSin0 );
		this->m4x4[2][2]	= ( fCos0 );

		return( (*this) );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: RotateY
	// Notes: Rotate along the Y axis
	//========================================================================
	const IMatrix __fastcall RotateY(float fAngle)
	{
		float fSin0;
		float fCos0;

		// Calculate sin/cos
		fSin0	= (float)sin(fAngle);
		fCos0	= (float)cos(fAngle);

		// Setup matrix
		m4x4[0][0]	= ( fCos0 );
		m4x4[0][2]	= (-fSin0 );
		m4x4[2][0]	= ( fSin0 );
		m4x4[2][2]	= ( fCos0 );

		return( (*this) );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: RotateZ
	// Notes: Rotate along the Z axis
	//========================================================================
	const IMatrix __fastcall RotateZ(float fAngle)
	{
		float fSin0;
		float fCos0;

		// Calculate sin/cos
		fSin0	= (float)sin(fAngle);
		fCos0	= (float)cos(fAngle);

		// Setup matrix
		m4x4[0][0]	= ( fCos0 );
		m4x4[0][1]	= ( fSin0 );
		m4x4[1][0]	= (-fSin0 );
		m4x4[1][1]	= ( fCos0 );

		return( (*this) );
	}

	//////////////////////////////////////////////////////////////////////////
	// Function: Rotate
	// Notes: Rotate the matrix
	//========================================================================
	void Rotate(float fX, float fY, float fZ)
	{
		float	fSin0, fSin1;
		float	fCos0, fCos1;
		int		nProduct	= 0;
		IMatrix	mat;
		float	fRotateX[4][4],
				fRotateY[4][4],
				fRotateZ[4][4],
				fTemp[4][4];

		// Identify the matrix
		mat.Identify();

		// Figure out what we need to do
		if( fX )
			nProduct	+= 4;
		if( fY )
			nProduct	+= 2;
		if( fZ )
			nProduct	+= 1;

		// Compute the matrix
		switch( nProduct )
		{
		case( 1 ): // Final matrix = z

			// Calculate sin/cos
			fSin0	= (float)sin(fZ);
			fCos0	= (float)cos(fZ);

			// Setup matrix
			mat.m4x4[0][0]	= ( fCos0 );
			mat.m4x4[0][1]	= ( fSin0 );
			mat.m4x4[1][0]	= (-fSin0 );
			mat.m4x4[1][1]	= ( fCos0 );
			
			break;

		case( 2 ): // Final matrix = y

			// Calculate sin/cos
			fSin0	= (float)sin(fY);
			fCos0	= (float)cos(fY);

			// Setup matrix
			mat.m4x4[0][0]	= ( fCos0 );
			mat.m4x4[0][2]	= (-fSin0 );
			mat.m4x4[2][0]	= ( fSin0 );
			mat.m4x4[2][2]	= ( fCos0 );
			
			break;

		case( 3 ): // Final matrix = y * z

			// Calculate sin/cos
			fSin0	= (float)sin(fY);
			fCos0	= (float)cos(fY);
			fSin1	= (float)sin(fZ);
			fCos1	= (float)cos(fZ);

			mat.m4x4[0][0]	= fCos0 * fCos1;
			mat.m4x4[0][1]	= fCos0 * fSin1;
			mat.m4x4[0][2]	= -fSin0;

			mat.m4x4[1][0]	= -fSin1;
			mat.m4x4[1][1]	= fCos1;
			
			mat.m4x4[2][0]	= fSin0 * fCos1;
			mat.m4x4[2][1]	= fSin0 * fSin1;
			mat.m4x4[2][2]	= fCos0;

			break;

		case( 4 ): // Final matrix = x

			// Calculate sin/cos
			fSin0	= (float)sin(fX);
			fCos0	= (float)cos(fX);

			// Setup matrix
			mat.m4x4[1][1]	= ( fCos0 );
			mat.m4x4[1][2]	= ( fSin0 );
			mat.m4x4[2][1]	= (-fSin0 );
			mat.m4x4[2][2]	= ( fCos0 );
		
			break;

		case( 5 ): // Final matrix = x * z

			// Calculate sin/cos
			fSin0	= (float)sin(fX);
			fCos0	= (float)cos(fX);
			fSin1	= (float)sin(fZ);
			fCos1	= (float)cos(fZ);

			mat.m4x4[0][0]	= fCos1;
			mat.m4x4[0][1]	= fSin1;

			mat.m4x4[1][0]	= -fCos0 * fSin1;
			mat.m4x4[1][1]	= fCos0 * fCos1;
			mat.m4x4[1][2]	= fSin0;
			
			mat.m4x4[2][0]	= fSin0 * fSin1;
			mat.m4x4[2][1]	= -fSin0 * fCos1;
			mat.m4x4[2][2]	= fCos0;

			break;

		case( 6 ): // Final matrix = x * y

			// Calculate sin/cos
			fSin0	= (float)sin(fX);
			fCos0	= (float)cos(fX);
			fSin1	= (float)sin(fY);
			fCos1	= (float)cos(fY);

			mat.m4x4[0][0]	= fCos1;
			mat.m4x4[0][2]	= -fSin1;

			mat.m4x4[1][0]	= fSin0 * fSin1;
			mat.m4x4[1][1]	= fCos0;
			mat.m4x4[1][2]	= fSin0 * fCos1;
			
			mat.m4x4[2][0]	= fCos0 * fSin1;
			mat.m4x4[2][1]	= -fSin0;
			mat.m4x4[2][2]	= fCos0 * fCos1;

			break;

		case( 7 ): // Final matrix = x * y * z

			// x
			Mat_Identify_4x4( (float**)fRotateX );
			fSin0			= (float)sin(fX);
			fCos0			= (float)cos(fX);
			fRotateX[1][1]	= ( fCos0 );
			fRotateX[1][2]	= ( fSin0 );
			fRotateX[2][1]	= (-fSin0 );
			fRotateX[2][2]	= ( fCos0 );

			// y
			Mat_Identify_4x4( (float**)fRotateY );
			fSin0			= (float)sin(fY);
			fCos0			= (float)cos(fY);
			fRotateY[0][0]	= ( fCos0 );
			fRotateY[0][2]	= (-fSin0 );
			fRotateY[2][0]	= ( fSin0 );
			fRotateY[2][2]	= ( fCos0 );

			// z
			Mat_Identify_4x4( (float**)fRotateZ );
			fSin0			= (float)sin(fZ);
			fCos0			= (float)cos(fZ);
			fRotateZ[0][0]	= ( fCos0 );
			fRotateZ[0][1]	= ( fSin0 );
			fRotateZ[1][0]	= (-fSin0 );
			fRotateZ[1][1]	= ( fCos0 );

			Mat_Mul_4x4_4x4( (float**)fRotateX, (float**)fRotateY, (float**)fTemp );
			Mat_Mul_4x4_4x4( (float**)fTemp, (float**)fRotateZ, (float**)mat.m4x4 );

			break;

		};
	}

	const IVector __fastcall Transform(const IVector &_v)
	{
		return( IVector(_v % RotX, _v % RotY, _v % RotZ) + Pos );
	}
};

//////////////////////////////////////////////////////////////////////////
// Function: IPlane
// Notes: Plane class
//========================================================================
class IPlane
{
public:
	IVector	vNormal;
	float	fDist;

	IPlane(float A = 1, float B = 0, float C = 0, float D = 0) : vNormal(A, B, C), fDist(D)
	{
	}
	
	IPlane(const IVector& Normal, float D = 0) : vNormal(Normal), fDist(D)
	{
	}

	IPlane(const IPlane& Plane) : vNormal(Plane.vNormal), fDist(Plane.fDist)
	{
	}

	IPlane(const IVector& VertexA, const IVector& VertexB, const IVector& VertexC)
	{
		IVector	NormalA( (VertexC - VertexA) | 1.0 );
		IVector	NormalB( (VertexC - VertexB) | 1.0 );
		fDist	= -VertexA % (vNormal = (NormalA ^ NormalB) | 1.0);
	}

	const IPlane& operator = (const IPlane& Plane)
	{
		vNormal	= Plane.vNormal;
		fDist	= Plane.fDist;
		return( (*this) );
	}

	// Boolean Operators
	const bool operator == (const IPlane& Plane) const
	{
		return( (vNormal == Plane.vNormal) && (fDist == Plane.fDist) );
	}
	const bool operator != (const IPlane& Plane) const
	{
		return( !( (*this) == Plane ) );
	}

	// Move plane
	const IPlane operator + (const IVector& Vector) const
	{
		return( IPlane(vNormal, fDist + Vector % vNormal) );
	}
	const IPlane& operator += (const IVector& Vector)
	{	return( (*this) = (*this) + Vector );
	}
	const IPlane operator - (const IVector& Vector) const
	{	return( IPlane(vNormal, fDist - Vector % vNormal) );
	}
	const IPlane& operator -= (const IVector& Vector)
	{	return( (*this) = (*this) - Vector );
	}

	// Just your average plane functions
	const bool inline PointOnPlane(const IVector& Point) const
	{
		return( DistanceToPlane(Point) == 0 );
	}
	const float inline DistanceToPlane(const IVector& Point) const
	{
		return( vNormal % Point + fDist );
	}
	const IVector inline RayIntersection(const IVector& RayPos, const IVector& RayDir) const
	{
		const float a = vNormal % RayDir;
		if( a == 0 )
			return( RayPos );	//error line parallel to plane
		
		return( RayPos - RayDir * (DistanceToPlane(RayPos) / a) );
	}
};

void Mat_Identify_4x4(float **fMatrix)
{
	// Set everything to 0
	memset( (void*)&fMatrix[0][0], 0, sizeof(float) * 16 );

	// Set diagonals to 1
	fMatrix[0][0] = fMatrix[1][1] = fMatrix[2][2] = fMatrix[3][3] = 1;
}

void Mat_Mul_4x4_4x4(float **fA, float **fB, float **fDest)
{
	fDest[0][0]	= fA[0][0] * fB[0][0] + fA[0][1] * fB[1][0] + fA[0][2] * fB[2][0];
	fDest[0][1]	= fA[0][0] * fB[0][1] + fA[0][1] * fB[1][1] + fA[0][2] * fB[2][1];
	fDest[0][2]	= fA[0][0] * fB[0][2] + fA[0][1] * fB[1][2] + fA[0][2] * fB[2][2];
	fDest[0][3]	= 0;

	fDest[1][0]	= fA[1][0] * fB[0][0] + fA[1][1] * fB[1][0] + fA[1][2] * fB[2][0];
	fDest[1][1]	= fA[1][0] * fB[0][1] + fA[1][1] * fB[1][1] + fA[1][2] * fB[2][1];
	fDest[1][2]	= fA[1][0] * fB[0][2] + fA[1][1] * fB[1][2] + fA[1][2] * fB[2][2];
	fDest[1][3]	= 0;

	fDest[2][0]	= fA[2][0] * fB[0][0] + fA[2][1] * fB[1][0] + fA[2][2] * fB[2][0];
	fDest[2][1]	= fA[2][0] * fB[0][1] + fA[2][1] * fB[1][1] + fA[2][2] * fB[2][1];
	fDest[2][2]	= fA[2][0] * fB[0][2] + fA[2][1] * fB[1][2] + fA[2][2] * fB[2][2];
	fDest[2][3]	= 0;

	fDest[3][0]	= fA[3][0] * fB[0][0] + fA[3][1] * fB[1][0] + fA[3][2] * fB[2][0];
	fDest[3][1]	= fA[3][0] * fB[0][1] + fA[3][1] * fB[1][1] + fA[3][2] * fB[2][1];
	fDest[3][2]	= fA[3][0] * fB[0][2] + fA[3][1] * fB[1][2] + fA[3][2] * fB[2][2];
	fDest[3][3]	= 1;
}