/** @file
 * PC specific implementation of the aligned 3-vector and 3x3-matrix
 * classes 'Asura_Vector_3' and 'Asura_Matrix_3x3'.
 *
 * It is an implementation of the APIs defined in 'Asura_Maths.h', and
 * it is included by the 'Asura_PlatformMaths.h' header.
 *
 * Copyright 2006 Rebellion Developments Ltd.
 */

#ifdef ASURA_USE_PRAGMA_ONCE
	#pragma once
#endif

#ifndef ___ASURA_PC_VECTORMATHS_H___
#define ___ASURA_PC_VECTORMATHS_H___ 1

#if defined ASURA_PC_SSE
	#include <xmmintrin.h>
#endif

//////////////////////////////////////////////////////////////////////////
// Include files...
//////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h"

// Asura Platform-independent
#include "Core/Asura_HalfFloat.h"

// Asura PC
#include "Core/Asura_PlatformBasics.h"

//////////////////////////////////////////////////////////////////////////
// Defines...
//////////////////////////////////////////////////////////////////////////

// Asura_AVector3...
#define Asura_AVector3_InParam		const Asura_AVector3&
#define Asura_AVector3_OutParam		Asura_AVector3&
#define Asura_AVector3_InOutParam	Asura_AVector3&

// Asura_AMatrix3x3...
#define Asura_AMatrix3x3_InParam	const Asura_AMatrix3x3&
#define Asura_AMatrix3x3_OutParam	Asura_AMatrix3x3&
#define Asura_AMatrix3x3_InOutParam	Asura_AMatrix3x3&

// Asura_AVector4...
#define Asura_AVector4_InParam		const Asura_AVector4&
#define Asura_AVector4_OutParam		Asura_AVector4&
#define Asura_AVector4_InOutParam	Asura_AVector4&

// Asura_AMatrix4x4...
#define Asura_AMatrix4x4_InParam	const Asura_AMatrix4x4&
#define Asura_AMatrix4x4_OutParam	Asura_AMatrix4x4&
#define Asura_AMatrix4x4_InOutParam	Asura_AMatrix4x4&

#if defined ASURA_PC_SSE
	#if 0
		/*
			This is probably a bit dangerous: it does 3-vector loads and
			stores as 4-vectors, handling specially the 4th ''tail'' element.
		*/
		#define ASURA_PC_SSE_TAIL 1
	#endif
#endif

// Comment in the line below to check alignment of Asura_AVector3s 
// #define ASURA_AVECTOR3_CHECK_ALIGNMENT (1)


//////////////////////////////////////////////////////////////////////////
// Inlined MMX wrapper functions...
//////////////////////////////////////////////////////////////////////////

#if defined ASURA_PC_SSE
/** Add together the first 3 elements of the parameter */
static inline __m128 _mm_asura_hadd3_ps(register const __m128 afQ)
{
    return _mm_add_ss(
            _mm_shuffle_ps(afQ,afQ,_MM_SHUFFLE(0,0,0,0)),
                _mm_add_ss(
                    _mm_shuffle_ps(afQ,afQ,_MM_SHUFFLE(1,1,1,1)),
                    _mm_shuffle_ps(afQ,afQ,_MM_SHUFFLE(2,2,2,2))));
}

/** Add together all the elements of the parameter */
static inline __m128 _mm_asura_hadd4_ps(register __m128 afQ)
{
	/* Inspired by Klimovitski */
	afQ = _mm_add_ps(afQ,_mm_movehl_ps(afQ,afQ));
	return _mm_add_ps(afQ,_mm_shuffle_ps(afQ,afQ,_MM_SHUFFLE(3,2,1,1)));
}

/** Zero an SSE register */
static inline __m128 _mm_asura_zero_ps(void)
{
	register const __m128 afZero = _mm_setzero_ps();
	return afZero;
}

/** Load 3 values from unaligned address */
static inline __m128 _mm_asura_loadu3_ps(register const float* const afV)
{
	/* Klimovitski hints: scalar load and store better */
	register const __m128 afX = _mm_load_ss(&afV[0]);
	register const __m128 afY = _mm_load_ss(&afV[1]);
	register const __m128 afZ = _mm_load_ss(&afV[2]);

	return _mm_shuffle_ps(_mm_movelh_ps(afX,afY),afZ,_MM_SHUFFLE(3,0,2,0));
}

/** Load 3 non-sequential values from unaligned address */
static inline __m128 _mm_asura_loadnsu3_ps(register const float afV0, register const float afV1, register const float afV2)
{
	register const __m128 afX = _mm_load_ss(&afV0);
	register const __m128 afY = _mm_load_ss(&afV1);
	register const __m128 afZ = _mm_load_ss(&afV2);

	return _mm_shuffle_ps(_mm_movelh_ps(afX,afY),afZ,_MM_SHUFFLE(3,0,2,0));
}

/** Store 3 values to an unaligned address */
static inline void _mm_asura_storeu3_ps(
	register float* const afV,register const __m128 afQ
)
{
	/* Klimovitski hints: scalar load and store better */
	register const __m128 xY = _mm_shuffle_ps(afQ,afQ,_MM_SHUFFLE(3,3,3,1));
	register const __m128 xZ = _mm_movehl_ps(afQ,afQ);

	_mm_store_ss(&afV[0],afQ);
	_mm_store_ss(&afV[1],xY);
	_mm_store_ss(&afV[2],xZ);
}

#if defined ASURA_PC_SSE_TAIL
/**
 *	Return the 1st parameter with the fourth element set to
 *	the first element of the second parameter.
 */
static inline __m128 _mm_asura_move4_ps(
	register const __m128 afQ,register __m128 afF
)
{
	afF = _mm_move_ss(afQ,afF);
	afF = _mm_shuffle_ps(afF,afF,_MM_SHUFFLE(0,2,1,3));
	return _mm_move_ss(afF,afQ);
}
#endif

static inline __m128 _mm_asura_load3_ps(register const float* const afV)
{
#if defined ASURA_PC_SSE_TAIL
	/*
		Fetch 4 values, but move a zero into the fourth element.
	*/
	register __m128 afZ = _mm_asura_zero_ps();
	register __m128 afQ = _mm_load_ps(afV);

	return _mm_asura_move4_ps(afQ,afZ);
#else
	return _mm_asura_loadu3_ps(afV);
#endif
}

static inline void _mm_asura_store3_ps(
	register float* const afV,register __m128 afQ
)
{
#if defined ASURA_PC_SSE_TAIL
	register __m128 afT = _mm_load_ss(&afV[3]);
	afQ = _mm_asura_move4_ps(afQ,afT);

	_mm_store_ps(afV,afQ);
#else
	_mm_asura_storeu3_ps(afV,afQ);
#endif
}

#endif


//////////////////////////////////////////////////////////////////////////
// Class declarations...
//////////////////////////////////////////////////////////////////////////

class Asura_AVector3
{
private:

    friend class Asura_AMatrix3x3;
    friend class Asura_SoA_Vector_3;

	friend Asura_Vector_3
        ::Asura_Vector_3(const Asura_AVector3& xV);
    friend void Asura_Vector_3
        ::operator =(const Asura_AVector3& xV);
    friend void Asura_Quat
        ::FromRotation(Asura_Angle fAngle,const Asura_AVector3& xAxis);

#if defined ASURA_AVECTOR3_CHECK_ALIGNMENT
	inline void Debug_CheckAlignment( void)
	{


#if defined ASURA_PC_SSE && defined ASURA_ASSERT
		Asura_Assertf( __alignof( __m128) <= __alignof( Asura_AVector3), ("Asura_AVector3's memory aligment is too small. It is %d but to be at least %d", __alignof( Asura_AVector3), __alignof( __m128)));
		
		u_int uPointerValue = reinterpret_cast< u_int>( this);
		u_int uPointerValueMod = uPointerValue % __alignof( Asura_AVector3);
		Asura_Assertf( 0 == uPointerValueMod, ("An Asura_AVector3 has been allocated with an incorrect memory aligment. 0x%08X %d", uPointerValue, uPointerValueMod));
#endif // ASURA_AVECTOR3_CHECK_ALIGNMENT
	}
#endif


    union
    {
        struct
        {
            float m_fX;
            float m_fY;
            float m_fZ;
			/* Dummy */
			float m_fW;
        };

        float m_afV[4];

#if defined ASURA_PC_SSE
        __m128 m_afQ;
#endif
    };

public:

    Asura_AVector3()
    {
	#if defined ASURA_PC_SSE && defined ASURA_AVECTOR3_CHECK_ALIGNMENT
		Debug_CheckAlignment();
	#endif
    }

    Asura_AVector3(
        register const float fX,
        register const float fY,
        register const float fZ
    )
    {
    #if defined ASURA_PC_SSE

	#if defined ASURA_AVECTOR3_CHECK_ALIGNMENT
		Debug_CheckAlignment();
	#endif
		
        m_afQ = _mm_set_ps(0.0f,fZ,fY,fX);
    #else
        m_fX = fX;
        m_fY = fY;
        m_fZ = fZ;
    #endif
    }

    inline void Zero(void)
    {
    #if defined ASURA_PC_SSE
        m_afQ = _mm_asura_zero_ps();
    #else
        m_fX = m_fY = m_fZ = 0.0f;
    #endif
    }

    explicit Asura_AVector3(register const Asura_Vector_3& xV)
    {
    #if defined ASURA_PC_SSE
        m_afQ = _mm_asura_loadu3_ps(&xV.x);
    #else
        m_fX = xV.x;
        m_fY = xV.y;
        m_fZ = xV.z;
    #endif
    }

    inline void operator =(register const Asura_Vector_3& xV) 
    {
    #if defined ASURA_PC_SSE
        m_afQ = _mm_asura_loadu3_ps(&xV.x);
    #else
        m_fX = xV.x;
        m_fY = xV.y;
        m_fZ = xV.z;
    #endif
    }

	inline void operator =(Asura_AVector3_InParam xV) 
	{
#if defined ASURA_PC_SSE
		m_afQ = xV.m_afQ;
#else
		m_fX = xV.m_fX;
		m_fY = xV.m_fY;
		m_fZ = xV.m_fZ;
#endif
	}

    inline float GetX(void) const { return m_fX; }
    inline float GetY(void) const { return m_fY; }
    inline float GetZ(void) const { return m_fZ; }

    inline void SetX(register const float fX) { m_fX = fX; }
    inline void SetY(register const float fY) { m_fY = fY; }
    inline void SetZ(register const float fZ) { m_fZ = fZ; }

    inline float GetElement(const unsigned fI) const
    {
        return *(reinterpret_cast<const float* const>(&m_afV) + fI);
    }

    inline void SetElement(const unsigned fI,register const float fN)
    {
        *(reinterpret_cast<float* const>(&m_afV) + fI) = fN;
    }

    inline void LoadAligned(register const float afT[3])
    {
    #if defined ASURA_PC_SSE
		m_afQ = _mm_asura_load3_ps(afT);
	#else
        m_fX = afT[0];
        m_fY = afT[1];
        m_fZ = afT[2];
        m_fW = 0.0f;
    #endif
    }

    inline void StoreAligned(register float afT[3]) const 
    {
    #if defined ASURA_PC_SSE
		_mm_asura_store3_ps(afT,m_afQ);
    #else
        afT[0] = m_fX;
        afT[1] = m_fY;
        afT[2] = m_fZ;
    #endif
    }

    inline void LoadUnaligned(register const float afT[3])
    {
    #if defined ASURA_PC_SSE
		m_afQ = _mm_asura_loadu3_ps(afT);
    #else
        m_fX = afT[0];
        m_fY = afT[1];
        m_fZ = afT[2];
        m_fW = 0.0f;
    #endif
    }

    inline void StoreUnaligned(register float afT[3]) const
    {
    #if defined ASURA_PC_SSE
		_mm_asura_storeu3_ps(afT,m_afQ);
	#else
        afT[0] = m_fX;
        afT[1] = m_fY;
        afT[2] = m_fZ;
    #endif
    }

	inline void StoreAlignedHalf(register Asura_HalfFloat afT[3]) const
	{
		afT[0] = m_fX;
		afT[1] = m_fY;
		afT[2] = m_fZ;
	}
    
	#if defined ASURA_PC_SSE
    inline __m128 GetM128( void) const
    {
		return( this->m_afQ);
    }
	#endif
    
    /*
      Unary operators
    */

    inline Asura_AVector3 operator -() const
    {		
    #if defined ASURA_PC_SSE
		Asura_AVector3 xO;
        xO.m_afQ = _mm_sub_ps(_mm_asura_zero_ps(),m_afQ);
		return xO;
    #else
        return Asura_AVector3(-m_fX,-m_fY,-m_fZ);
    #endif
    }

    /*
        ''and becomes'' operators.
    */

    inline void operator +=(register Asura_AVector3_InParam xV)
    {
    #if defined ASURA_PC_SSE
        m_afQ = _mm_add_ps(m_afQ,xV.m_afQ);
    #else
        m_fX += xV.m_fX;
        m_fY += xV.m_fY;
        m_fZ += xV.m_fZ;
    #endif
    }

    inline void operator -=(register Asura_AVector3_InParam xV)
    {
    #if defined ASURA_PC_SSE
        m_afQ = _mm_sub_ps(m_afQ,xV.m_afQ);
    #else
        m_fX -= xV.m_fX;
        m_fY -= xV.m_fY;
        m_fZ -= xV.m_fZ;
    #endif
    }

    inline void operator *=(const float fN)
    {
    #if defined ASURA_PC_SSE
        m_afQ = _mm_mul_ps(m_afQ,_mm_load_ps1(&fN));
    #else
        m_fX *= fN;
        m_fY *= fN;
        m_fZ *= fN;
    #endif
    }

    inline void operator *=(register Asura_AVector3_InParam xV) 
    {
    #if defined ASURA_PC_SSE
        m_afQ = _mm_mul_ps(m_afQ,xV.m_afQ);
    #else
        m_fX *= xV.m_fX;
        m_fY *= xV.m_fY;
        m_fZ *= xV.m_fZ;
    #endif
    }

    inline void operator /=(const float fN)
    {
    #if defined ASURA_PC_SSE
		#if defined ASURA_PC_SSE_RCP
			/* Klimovitski hints it may be faster to do 3 multiplies */
			register const __m128 afInv = _mm_rcp_ss(_mm_load_ss(&fN));
			m_afQ = _mm_mul_ps(m_afQ,
						_mm_shuffle_ps(afInv,afInv,_MM_SHUFFLE(0,0,0,0)));
		#else
			m_afQ = _mm_div_ps(m_afQ,_mm_load_ps1(&fN));
		#endif
    #else
        register const float fInv = 1.0f / fN;

        m_fX *= fInv;
        m_fY *= fInv;
        m_fZ *= fInv;
    #endif
    }

    /*
      Binary additive operators
    */

    inline Asura_AVector3 operator +(register Asura_AVector3_InParam xV) const
    {
    #if defined ASURA_PC_SSE
        Asura_AVector3 xO;
        xO.m_afQ = _mm_add_ps(m_afQ,xV.m_afQ);
        return xO;
    #else
        return Asura_AVector3(
            m_fX+xV.m_fX,m_fY+xV.m_fY,m_fZ+xV.m_fZ
        );
    #endif
    }

    inline Asura_AVector3 operator -(register Asura_AVector3_InParam xV) const
    {
    #if defined ASURA_PC_SSE
        Asura_AVector3 xO;
        xO.m_afQ = _mm_sub_ps(m_afQ,xV.m_afQ);
        return xO;
    #else
        return Asura_AVector3(
            m_fX-xV.m_fX,m_fY-xV.m_fY,m_fZ-xV.m_fZ
        );
    #endif
    }

    /*
      Binary multiplicative operators
    */

    inline Asura_AVector3 operator *(const float fN) const
    {
    #if defined ASURA_PC_SSE
        Asura_AVector3 xO;
		xO.m_afQ = _mm_mul_ps(m_afQ,_mm_load_ps1(&fN));
        return xO;
    #else
        return Asura_AVector3(m_fX*fN,m_fY*fN,m_fZ*fN);
    #endif
    }

    inline Asura_AVector3 operator /(const float fN) const
    {
    #if defined ASURA_PC_SSE
        Asura_AVector3 xO;
		#if defined ASURA_PC_SSE_RCP
			register const __m128 afInv = _mm_rcp_ss(_mm_load_ss(&fN));
			xO.m_afQ = _mm_mul_ps(m_afQ,
						_mm_shuffle_ps(afInv,afInv,_MM_SHUFFLE(0,0,0,0)));
		#else
			xO.m_afQ = _mm_div_ps(m_afQ,_mm_load_ps1(&fN));
		#endif
        return xO;
    #else
        register const float fInv = 1.0f / fN;
        return Asura_AVector3(m_fX*fInv,m_fY*fInv,m_fZ*fInv );
    #endif
    }

    /* Dot product */
    inline float operator *(register Asura_AVector3_InParam xV) const
    {
    #if defined ASURA_PC_SSE
		/* Klimovitski hints it may be faster to do 3 multiplies */
		float fN;
        _mm_store_ss(&fN,_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,xV.m_afQ)));
		return fN;
    #else
        return (m_fX*xV.m_fX + m_fY*xV.m_fY + m_fZ*xV.m_fZ);
    #endif
    }

    /* Cross product */
    inline Asura_AVector3 operator ^(register Asura_AVector3_InParam xV) const
    {
    #if defined ASURA_PC_SSE
        Asura_AVector3 xO;

        xO.m_afQ
			= _mm_sub_ps(
				/* m_fY*xV.m_fZ, m_fZ*xV.m_fX, m_fX*xV.m_fY */
				_mm_mul_ps(
					_mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(3,0,2,1)),
					_mm_shuffle_ps(xV.m_afQ,xV.m_afQ,_MM_SHUFFLE(3,1,0,2))),
				/* m_fZ*xV.m_fY, m_fX*xV.m_fZ, m_fY*xV.m_fX */
				_mm_mul_ps(
					_mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(3,1,0,2)),
					_mm_shuffle_ps(xV.m_afQ,xV.m_afQ,_MM_SHUFFLE(3,0,2,1))));

        return xO;
    #else
        return Asura_AVector3(
            m_fY*xV.m_fZ - m_fZ*xV.m_fY,
            m_fZ*xV.m_fX - m_fX*xV.m_fZ,
            m_fX*xV.m_fY - m_fY*xV.m_fX
        );
    #endif
    }

    /*
        Special case: aligned vector times unaligned matrix.
    */
    inline Asura_AVector3 operator *(const Asura_Matrix_3x3& xM) const;
    inline Asura_AVector3 operator *(Asura_AMatrix3x3_InParam xM) const;

#if 0
    inline Asura_AVector3 operator *(const Asura_Matrix_3x3& xM) const
    {
        class Asura_AVector3 result;

        result.m_fX = xM.e11 * m_fX + xM.e12 * m_fY + xM.e13 * m_fZ;
        result.m_fY = xM.e21 * m_fX + xM.e22 * m_fY + xM.e23 * m_fZ;
        result.m_fZ = xM.e31 * m_fX + xM.e32 * m_fY + xM.e33 * m_fZ;

        return result;
    }
#endif

    /*
        Equality operators
    */

    inline bool operator ==(register Asura_AVector3_InParam xV) const
    {
    #if defined ASURA_PC_SSE
        return (_mm_movemask_ps(_mm_cmpeq_ps(m_afQ,xV.m_afQ)) & 0x07) == 0x07;
    #else
        return m_fX == xV.m_fX && m_fY == xV.m_fY && m_fZ == xV.m_fZ;
    #endif
    }

    inline bool operator !=(register Asura_AVector3_InParam xV) const
    {
    #if defined ASURA_PC_SSE
        return (_mm_movemask_ps(_mm_cmpneq_ps(m_afQ,xV.m_afQ)) & 0x07) != 0x00;
    #else
        return m_fX != xV.m_fX || m_fY != xV.m_fY || m_fZ != xV.m_fZ;
    #endif
    }

    inline bool ApproximatelyEquals(register Asura_AVector3_InParam xV,
        register const float fFuzz = 1.0e-6f) const
    {
        Asura_AVector3 xDelta(*this - xV);

        return xDelta.MagnitudeSquared() < fFuzz;
    }

    /*
        Order operators
    */

#if defined ASURA_PC_SSE
    inline bool operator >(register Asura_AVector3_InParam xV) const
    {
        return (_mm_movemask_ps(_mm_cmpgt_ps(m_afQ,xV.m_afQ)) & 0x07) == 0x07;
    }

    inline bool operator <(register Asura_AVector3_InParam xV) const
    {
        return (_mm_movemask_ps(_mm_cmplt_ps(m_afQ,xV.m_afQ)) & 0x07) == 0x07;
    }

    inline bool operator >=(register Asura_AVector3_InParam xV) const
    {
        return (_mm_movemask_ps(_mm_cmpge_ps(m_afQ,xV.m_afQ)) & 0x07) == 0x07;
    }

    inline bool operator <=(register Asura_AVector3_InParam xV) const
    {
        return (_mm_movemask_ps(_mm_cmple_ps(m_afQ,xV.m_afQ)) & 0x07) == 0x07;
    }
#else
    inline bool operator >(register const Asura_AVector3& xV) const
    {
        return (m_fX > xV.m_fX) && (m_fY > xV.m_fY) && (m_fZ > xV.m_fZ);
    }

    inline bool operator <(const Asura_AVector3& xV) const
    {
        return (m_fX < xV.m_fX) && (m_fY < xV.m_fY) && (m_fZ < xV.m_fZ);
    }

    inline bool operator >=(register const Asura_AVector3& xV) const
    {
        return (m_fX >= xV.m_fX) && (m_fY >= xV.m_fY) && (m_fZ >= xV.m_fZ);
    }

    inline bool operator <=(const Asura_AVector3& xV) const
    {
        return (m_fX <= xV.m_fX) && (m_fY <= xV.m_fY) && (m_fZ <= xV.m_fZ);
    }
#endif

    /*
        Vector operations
    */

    inline float MagnitudeSquared(void) const
    {   
    #if defined ASURA_PC_SSE
		float fN;
        _mm_store_ss(&fN,_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ)));
		return fN;
    #else
        return (m_fX*m_fX) + (m_fY*m_fY) + (m_fZ*m_fZ);
    #endif
    }

    inline float Magnitude(void) const
    {
    #if defined ASURA_PC_SSE
		float fN;
        _mm_store_ss(&fN,
			_mm_sqrt_ss(_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ))));
		return fN;
    #else
        return Asura_Maths::Sqrt((m_fX*m_fX) + (m_fY*m_fY) + (m_fZ*m_fZ));
    #endif
    }

    inline void Normalise(void)
    {
        Asura_Assert(this->MagnitudeSquared() != 0.0f, "trying to Normalise Zero vector");

    #if defined ASURA_PC_SSE
		#if defined ASURA_PC_SSE_RCP
			/* Klimovitski hints it may be faster to do 3 multiplies */
			register const __m128 afInvMag
				 = _mm_rsqrt_ss(_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ)));

			m_afQ = _mm_mul_ps(m_afQ,
                    _mm_shuffle_ps(afInvMag,afInvMag,_MM_SHUFFLE(0,0,0,0)));
		#else
			register const __m128 afMag
				 = _mm_sqrt_ss(_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ)));
			m_afQ = _mm_div_ps(m_afQ,
                    _mm_shuffle_ps(afMag,afMag,_MM_SHUFFLE(0,0,0,0)));
		#endif
    #else
        register const float fInvMag = 1.0f/Magnitude();

        m_fX *= fInvMag;
        m_fY *= fInvMag;
        m_fZ *= fInvMag;
    #endif
    }

    inline float NormaliseReturningMagnitude(void)
    {
    #if defined ASURA_PC_SSE
		/* Klimovitski hints it may be faster to do 3 multiplies */
        register const __m128 afMag
             = _mm_sqrt_ss(_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ)));

        /* Iff magnitude nonzero, scale vector by it */
        if (_mm_ucomineq_ss(afMag,_mm_cmpneq_ss(afMag,afMag)))
        {
		#if defined ASURA_PC_SSE_RCP
			/* Klimovitski hints it may be faster to do 3 multiplies */
			register const __m128 afInvMag = _mm_rcp_ss(afMag);
            m_afQ = _mm_mul_ps(m_afQ,
				_mm_shuffle_ps(afInvMag,afInvMag,_MM_SHUFFLE(0,0,0,0)));
		#else
            m_afQ = _mm_div_ps(m_afQ,
				_mm_shuffle_ps(afMag,afMag,_MM_SHUFFLE(0,0,0,0)));
		#endif
        }

        {
            float fMag;

            _mm_store_ss(&fMag,afMag);

            return fMag;
        }
    #else
        register const float fMag = Magnitude();

        if (fMag != 0.0f)
        {
            register const float fInvMag = 1.0f/fMag;

            m_fX *= fInvMag;
            m_fY *= fInvMag;
            m_fZ *= fInvMag;
        }

        return fMag;
    #endif
    }

    inline void ApproximateNormalise(void)
    {
        Asura_Assert(this->MagnitudeSquared() != 0.0f, "trying to Normalise Zero vector");

    #if defined ASURA_PC_SSE
		#if defined ASURA_PC_SSE_RCP
			/* Klimovitski hints it may be faster to do 3 multiplies */
			register const __m128 afInvMag
				 = _mm_rsqrt_ss(_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ)));

			m_afQ = _mm_mul_ps(m_afQ,
                    _mm_shuffle_ps(afInvMag,afInvMag,_MM_SHUFFLE(0,0,0,0)));
		#else
			register const __m128 afMag
				 = _mm_sqrt_ss(_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ)));
			m_afQ = _mm_div_ps(m_afQ,
                    _mm_shuffle_ps(afMag,afMag,_MM_SHUFFLE(0,0,0,0)));
		#endif
    #else
        register const float fInvMag = 1.0f/Magnitude();

        m_fX *= fInvMag;
        m_fY *= fInvMag;
        m_fZ *= fInvMag;
    #endif
    }

    inline float ApproximateNormaliseReturningMagnitude(void)
    {
    #if defined ASURA_PC_SSE
		/* Klimovitski hints it may be faster to do 3 multiplies */
        register const __m128 afMag
             = _mm_sqrt_ss(_mm_asura_hadd3_ps(_mm_mul_ps(m_afQ,m_afQ)));

        /* Iff magnitude nonzero, scale vector by it */
        if (_mm_ucomineq_ss(afMag,_mm_cmpneq_ss(afMag,afMag)))
        {
		#if defined ASURA_PC_SSE_RCP
			/* Klimovitski hints it may be faster to do 3 multiplies */
			register const __m128 afInvMag = _mm_rcp_ss(afMag);
            m_afQ = _mm_mul_ps(m_afQ,
				_mm_shuffle_ps(afInvMag,afInvMag,_MM_SHUFFLE(0,0,0,0)));
		#else
            m_afQ = _mm_div_ps(m_afQ,
				_mm_shuffle_ps(afMag,afMag,_MM_SHUFFLE(0,0,0,0)));
		#endif
        }

        {
            float fMag;

            _mm_store_ss(&fMag,afMag);

            return fMag;
        }
    #else
        register const float fMag = Magnitude();

        if (fMag != 0.0f)
        {
            register const float fInvMag = 1.0f/fMag;

            m_fX *= fInvMag;
            m_fY *= fInvMag;
            m_fZ *= fInvMag;
        }

        return fMag;
    #endif
    }

	inline void Lerp(Asura_AVector3_InParam xTo, const float fWeight)
	{
		const float	fFromWeight	= 1.0f - fWeight;

		// Interpolate...
		m_fX	= (m_fX * fFromWeight) + (xTo.m_fX * fWeight);
		m_fY	= (m_fY * fFromWeight) + (xTo.m_fY * fWeight);
		m_fZ	= (m_fZ * fFromWeight) + (xTo.m_fZ * fWeight);
	}

	inline void Lerp(Asura_AVector3_InParam xFrom, Asura_AVector3_InParam xTo, const float fWeight)
	{
		const float	fFromWeight	= 1.0f - fWeight;

		// Interpolate...
		m_fX	= (xFrom.m_fX * fFromWeight) + (xTo.m_fX * fWeight);
		m_fY	= (xFrom.m_fY * fFromWeight) + (xTo.m_fY * fWeight);
		m_fZ	= (xFrom.m_fZ * fFromWeight) + (xTo.m_fZ * fWeight);
	}

	inline void Max(Asura_AVector3_InParam xV1, Asura_AVector3_InParam xV2)
	{
		m_fX	= Asura_Maths::Max(xV1.m_fX, xV2.m_fX);
		m_fY	= Asura_Maths::Max(xV1.m_fY, xV2.m_fY);
		m_fZ	= Asura_Maths::Max(xV1.m_fZ, xV2.m_fZ);
	}

	inline void Min(Asura_AVector3_InParam xV1, Asura_AVector3_InParam xV2)
	{
		m_fX	= Asura_Maths::Min(xV1.m_fX, xV2.m_fX);
		m_fY	= Asura_Maths::Min(xV1.m_fY, xV2.m_fY);
		m_fZ	= Asura_Maths::Min(xV1.m_fZ, xV2.m_fZ);
	}

	/*
      Compare *elementwise* 'xLHS' with 'xRHS', and for each element where
      the comparison is true return the corresponding elements of
      'xTrue', else that of 'xFalse'.

      Therefore 'SelectLT(a,b,a,b)' is then elementwise minimum,
      'SelectGT(a,b,a,b)' is elementwise maximum.
    */

    inline void SelectGE(
        register Asura_AVector3_InParam xLHS,
        register Asura_AVector3_InParam xRHS,
        register Asura_AVector3_InParam xTrue,
        register Asura_AVector3_InParam xFalse
    )
    {
    #if defined ASURA_PC_SSE
        register const __m128 aiMask = _mm_cmpge_ps(xLHS.m_afQ,xRHS.m_afQ);

        m_afQ = _mm_or_ps(
            _mm_and_ps(aiMask,xTrue.m_afQ),
            _mm_andnot_ps(aiMask,xFalse.m_afQ)
        );
    #else
        m_fX = xLHS.m_fX >= xRHS.m_fX ? xTrue.m_fX : xFalse.m_fX;
        m_fY = xLHS.m_fY >= xRHS.m_fY ? xTrue.m_fY : xFalse.m_fY;
        m_fZ = xLHS.m_fZ >= xRHS.m_fZ ? xTrue.m_fZ : xFalse.m_fZ;
    #endif
    }

    inline void SelectGT(
        register Asura_AVector3_InParam xLHS,
        register Asura_AVector3_InParam xRHS,
        register Asura_AVector3_InParam xTrue,
        register Asura_AVector3_InParam xFalse
    )
    {
    #if  defined ASURA_PC_SSE
        register const __m128 aiMask = _mm_cmpgt_ps(xLHS.m_afQ,xRHS.m_afQ);

        m_afQ = _mm_or_ps(
            _mm_and_ps(aiMask,xTrue.m_afQ),
            _mm_andnot_ps(aiMask,xFalse.m_afQ)
        );
    #else
        m_fX = xLHS.m_fX > xRHS.m_fX ? xTrue.m_fX : xFalse.m_fX;
        m_fY = xLHS.m_fY > xRHS.m_fY ? xTrue.m_fY : xFalse.m_fY;
        m_fZ = xLHS.m_fZ > xRHS.m_fZ ? xTrue.m_fZ : xFalse.m_fZ;
    #endif
    }

    inline void SelectLE(
        register Asura_AVector3_InParam xLHS,
        register Asura_AVector3_InParam xRHS,
        register Asura_AVector3_InParam xTrue,
        register Asura_AVector3_InParam xFalse
    )
    {
    #if  defined ASURA_PC_SSE
        register const __m128 aiMask = _mm_cmple_ps(xLHS.m_afQ,xRHS.m_afQ);

        m_afQ = _mm_or_ps(
            _mm_and_ps(aiMask,xTrue.m_afQ),
            _mm_andnot_ps(aiMask,xFalse.m_afQ)
        );
    #else
        m_fX = xLHS.m_fX <= xRHS.m_fX ? xTrue.m_fX : xFalse.m_fX;
        m_fY = xLHS.m_fY <= xRHS.m_fY ? xTrue.m_fY : xFalse.m_fY;
        m_fZ = xLHS.m_fZ <= xRHS.m_fZ ? xTrue.m_fZ : xFalse.m_fZ;
    #endif
    }

    inline void SelectLT(
        register Asura_AVector3_InParam xLHS,
        register Asura_AVector3_InParam xRHS,
        register Asura_AVector3_InParam xTrue,
        register Asura_AVector3_InParam xFalse
    )
    {
    #if  defined ASURA_PC_SSE
        register const __m128 aiMask = _mm_cmplt_ps(xLHS.m_afQ,xRHS.m_afQ);

        m_afQ = _mm_or_ps(
            _mm_and_ps(aiMask,xTrue.m_afQ),
            _mm_andnot_ps(aiMask,xFalse.m_afQ)
        );
    #else
        m_fX = xLHS.m_fX < xRHS.m_fX ? xTrue.m_fX : xFalse.m_fX;
        m_fY = xLHS.m_fY < xRHS.m_fY ? xTrue.m_fY : xFalse.m_fY;
        m_fZ = xLHS.m_fZ < xRHS.m_fZ ? xTrue.m_fZ : xFalse.m_fZ;
    #endif
    }

    inline void SelectEQ(
        register Asura_AVector3_InParam xLHS,
        register Asura_AVector3_InParam xRHS,
        register Asura_AVector3_InParam xTrue,
        register Asura_AVector3_InParam xFalse
    )
    {
    #if defined ASURA_PC_SSE
        register const __m128 aiMask = _mm_cmpeq_ps(xLHS.m_afQ,xRHS.m_afQ);

        m_afQ = _mm_or_ps(
            _mm_and_ps(aiMask,xTrue.m_afQ),
            _mm_andnot_ps(aiMask,xFalse.m_afQ)
        );
    #else
        m_fX = xLHS.m_fX == xRHS.m_fX ? xTrue.m_fX : xFalse.m_fX;
        m_fY = xLHS.m_fY == xRHS.m_fY ? xTrue.m_fY : xFalse.m_fY;
        m_fZ = xLHS.m_fZ == xRHS.m_fZ ? xTrue.m_fZ : xFalse.m_fZ;
    #endif
    }

    inline void SelectNE(
        register Asura_AVector3_InParam xLHS,
        register Asura_AVector3_InParam xRHS,
        register Asura_AVector3_InParam xTrue,
        register Asura_AVector3_InParam xFalse
    )
    {
    #if defined ASURA_PC_SSE
        register const __m128 aiMask = _mm_cmpneq_ps(xLHS.m_afQ,xRHS.m_afQ);

        m_afQ = _mm_or_ps(
            _mm_and_ps(aiMask,xTrue.m_afQ),
            _mm_andnot_ps(aiMask,xFalse.m_afQ)
        );
    #else
        m_fX = xLHS.m_fX != xRHS.m_fX ? xTrue.m_fX : xFalse.m_fX;
        m_fY = xLHS.m_fY != xRHS.m_fY ? xTrue.m_fY : xFalse.m_fY;
        m_fZ = xLHS.m_fZ != xRHS.m_fZ ? xTrue.m_fZ : xFalse.m_fZ;
    #endif
    }
    
};

/*
    These are 'Asura_Vector3' (unaligned) operations that have
    to be defined after 'Asura_AVector3' is fully defined.
*/

inline Asura_Vector_3::Asura_Vector_3(register const Asura_AVector3& xV)
{
#if defined ASURA_PC_SSE
    _mm_storel_pi(reinterpret_cast<__m64*>(this),xV.m_afQ);
    z = xV.GetZ();
#else
    x = xV.GetX();
    y = xV.GetY();
    z = xV.GetZ();
#endif
}

inline void Asura_Vector_3::operator =(
    register const Asura_AVector3& xV
)
{
#if defined ASURA_PC_SSE
    _mm_storel_pi(reinterpret_cast<__m64*>(this),xV.m_afQ);
    z = xV.GetZ();
#else
    x = xV.GetX();
    y = xV.GetY();
    z = xV.GetZ();
#endif

}

/*
    Note that in Asura_Matrix_3x3 the data is stored as 3 rows. For
    efficiency when using vector instructions it's better for us to
    store 3 columns. Since the old 3x3 matrix use e11 to e33 I'm keeping
    the convention of starting named columns at 1 rather than 0.
*/

class Asura_AMatrix3x3
{
    friend Asura_AVector3 Asura_AVector3
        ::operator *(const Asura_AMatrix3x3& xM) const;

    Asura_AVector3 m_xC0;
    Asura_AVector3 m_xC1;
    Asura_AVector3 m_xC2;

public:

    inline Asura_AMatrix3x3(void)
    {
    }

    explicit Asura_AMatrix3x3( const Asura_Matrix_3x3& xMat3 )
    {
        FromMat3( xMat3 );
    }

    inline void Zero(void)
    {
        m_xC0.Zero();
        m_xC1.Zero();
        m_xC2.Zero();
    }

    inline void Identity(void)
    {
    #if defined ASURA_PC_SSE
		register __m128 af0010 = _mm_set_ps(0.0f,1.0f,0.0f,0.0f);

		m_xC0.m_afQ = _mm_shuffle_ps(af0010,af0010,_MM_SHUFFLE(0,0,0,2));
		m_xC1.m_afQ = _mm_shuffle_ps(af0010,af0010,_MM_SHUFFLE(0,0,2,0));
		m_xC2.m_afQ = af0010;
    #else
        m_xC0 = Asura_AVector3(1.0f,0.0f,0.0f);
        m_xC1 = Asura_AVector3(0.0f,1.0f,0.0f);
        m_xC2 = Asura_AVector3(0.0f,0.0f,1.0f);
    #endif
    }

    Asura_AMatrix3x3(
        register const float e11,
        register const float e12,
        register const float e13,

        register const float e21,
        register const float e22,
        register const float e23,

        register const float e31,
        register const float e32,
        register const float e33
    )
    {
        m_xC0 = Asura_AVector3(e11,e21,e31);
        m_xC1 = Asura_AVector3(e12,e22,e32);
        m_xC2 = Asura_AVector3(e13,e23,e33);
    }

    Asura_AMatrix3x3(
        register const Asura_AVector3& xC0,
        register const Asura_AVector3& xC1,
        register const Asura_AVector3& xC2
    )
    {
        m_xC0 = xC0;
        m_xC1 = xC1;
        m_xC2 = xC2;
    }

    /*
        Accessors and mutators
    */

    inline Asura_AVector3 GetColumn0(void) const { return m_xC0; }
    inline Asura_AVector3 GetColumn1(void) const { return m_xC1; }
    inline Asura_AVector3 GetColumn2(void) const { return m_xC2; }

    inline Asura_AVector3 GetColumn(u_int u) const
    {
        return *(&m_xC0 + u);
    }

    float GetElement(register const unsigned uR,
        register const unsigned uC) const
    {
        return GetColumn(uC).GetElement(uR);
    }

    void SetColumn0(Asura_AVector3_InParam xC0) { m_xC0 = xC0; }
    void SetColumn1(Asura_AVector3_InParam xC1) { m_xC1 = xC1; }
    void SetColumn2(Asura_AVector3_InParam xC2) { m_xC2 = xC2; }

    /*
        Conversions
    */

    void ToMat3(register Asura_Matrix_3x3& xM3) const
    {
        xM3.e11 = GetElement(0,0);
        xM3.e12 = GetElement(0,1);
        xM3.e13 = GetElement(0,2);

        xM3.e21 = GetElement(1,0);
        xM3.e22 = GetElement(1,1);
        xM3.e23 = GetElement(1,2);

        xM3.e31 = GetElement(2,0);
        xM3.e32 = GetElement(2,1);
        xM3.e33 = GetElement(2,2);
    }

    void FromMat3(register const Asura_Matrix_3x3& xM3)
    {
    #if defined ASURA_PC_SSE
        m_xC0.m_afQ = _mm_set_ps(0.0f,xM3.e31,xM3.e21,xM3.e11);
        m_xC1.m_afQ = _mm_set_ps(0.0f,xM3.e32,xM3.e22,xM3.e12);
        m_xC2.m_afQ = _mm_set_ps(0.0f,xM3.e33,xM3.e23,xM3.e13);
    #else
        m_xC0 = Asura_AVector3(xM3.e11,xM3.e21,xM3.e31);
        m_xC1 = Asura_AVector3(xM3.e12,xM3.e22,xM3.e32);
        m_xC2 = Asura_AVector3(xM3.e13,xM3.e23,xM3.e33);
    #endif
    }

    /*
        Equality operators.
    */

    inline bool operator ==(register Asura_AMatrix3x3_InParam xM) const
    {
        return m_xC0 == xM.m_xC0 && m_xC1 == xM.m_xC1 && m_xC2 == xM.m_xC2;
    }

    inline bool operator != (register Asura_AMatrix3x3_InParam xM) const
    {
        return m_xC0 != xM.m_xC0 || m_xC1 != xM.m_xC1 || m_xC2 != xM.m_xC2;
    }

    /*
        Binary multiplicative operators.
    */

    inline Asura_AMatrix3x3 operator *(
        register Asura_AMatrix3x3_InParam xM
    ) const
    {
        return Asura_AMatrix3x3(m_xC0*xM,m_xC1*xM,m_xC2*xM);
    }

    inline Asura_AMatrix3x3 operator *(register const float fN) const
    {
        return Asura_AMatrix3x3(m_xC0*fN,m_xC1*fN,m_xC2*fN);
    }

    /*
        Matrix operations.
    */

	// Tilde operator to make a skew-symmetric matrix from a vector
	inline void Tilde( Asura_AVector3_InParam xVector )
	{
		m_xC0 = Asura_AVector3( 0.0f,				+xVector.GetZ(),	-xVector.GetY() );
		m_xC1 = Asura_AVector3( -xVector.GetZ(),	0.0f,				+xVector.GetX() );
		m_xC2 = Asura_AVector3( +xVector.GetY(),	-xVector.GetX(),	0.0f			);
	}

	// Try to make the matrix orthogonal by forcing the axes represented by the matrix
	// columns to be a) unit-length and b) perpendicular.
	inline void Orthogonalise( void )
	{
		// normalise "down" column
		m_xC1.Normalise();

		// make "forwards" column perpendicular to this, and normalise
		m_xC2 -= m_xC1 * ( m_xC2 * m_xC1 );
		m_xC2.Normalise();

		// and now the "right" column must be perpendicular to both of these: x = + y^z
		m_xC0 = m_xC1 ^ m_xC2;

	}

	// matrix-matrix per component addition - used in integration
	inline void Add( Asura_AMatrix3x3_InParam xM ) 
	{
		m_xC0 += xM.m_xC0;
		m_xC1 += xM.m_xC1;
		m_xC2 += xM.m_xC2;
	}

    inline void Transpose(void)
    {
	#if defined ASURA_PC_SSE
		/* Not sure SSE is a win here */
		register __m128 C4 = _mm_asura_zero_ps();

		_MM_TRANSPOSE4_PS(m_xC0.m_afQ,m_xC1.m_afQ,m_xC2.m_afQ,C4);
	#else
        {
            register const float fN = m_xC0.GetY();
            m_xC0.SetY(m_xC1.GetX());
            m_xC1.SetX(fN);
        }

        {
            register const float fN = m_xC0.GetZ();
            m_xC0.SetZ(m_xC2.GetX());
            m_xC2.SetX(fN);
        }

        {
            register const float fN = m_xC1.GetZ();
            m_xC1.SetZ(m_xC2.GetY());
            m_xC2.SetY(fN);
        }
	#endif
    }

    inline Asura_AVector3 ApplyTranspose(
        register Asura_AVector3_InParam xV
    ) const
    {
        register Asura_AMatrix3x3 xT(*this);

        xT.Transpose();

        return xV*xT;
    }

};

inline Asura_AVector3 Asura_AVector3::operator *(
    register Asura_AMatrix3x3_InParam xM
) const
{
#if defined ASURA_PC_SSE
    Asura_AVector3 xO;

    xO.m_afQ
        = _mm_add_ps(
            _mm_mul_ps(
                _mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(0,0,0,0)),
				xM.m_xC0.m_afQ),
            _mm_add_ps(
                _mm_mul_ps(
                    _mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(1,1,1,1)),
					xM.m_xC1.m_afQ),
                _mm_mul_ps(
                    _mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(2,2,2,2)),
					xM.m_xC2.m_afQ)));

    return xO;
#else
	return Asura_AVector3(
		(xM.GetElement(0,0)*m_fX
			+ xM.GetElement(0,1)*m_fY + xM.GetElement(0,2)*m_fZ),
		(xM.GetElement(1,0)*m_fX
			+ xM.GetElement(1,1)*m_fY + xM.GetElement(1,2)*m_fZ),
		(xM.GetElement(2,0)*m_fX
			+ xM.GetElement(2,1)*m_fY + xM.GetElement(2,2)*m_fZ)
	);
#endif
}

/*
    Special case: aligned vector times unaligned matrix
*/
inline Asura_AVector3 Asura_AVector3::operator *(
    register const Asura_Matrix_3x3& xM) const
{
#ifdef ASURA_PC_SSE
	register const __m128 afC0 = _mm_asura_loadnsu3_ps(xM.Element[0][0], xM.Element[1][0], xM.Element[2][0]);
	register const __m128 afC1 = _mm_asura_loadnsu3_ps(xM.Element[0][1], xM.Element[1][1], xM.Element[2][1]);
	register const __m128 afC2 = _mm_asura_loadnsu3_ps(xM.Element[0][2], xM.Element[1][2], xM.Element[2][2]);

	Asura_AVector3 xO;

	xO.m_afQ
		= _mm_add_ps(
			_mm_mul_ps(
				_mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(0,0,0,0)),afC0),
				_mm_add_ps(
					_mm_mul_ps(
						_mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(1,1,1,1)),afC1),
					_mm_mul_ps(
						_mm_shuffle_ps(m_afQ,m_afQ,_MM_SHUFFLE(2,2,2,2)),afC2)));

	return xO;
#else
    return Asura_AVector3(
		xM.e11*m_fX + xM.e12*m_fY + xM.e13*m_fZ,
		xM.e21*m_fX + xM.e22*m_fY + xM.e23*m_fZ,
		xM.e31*m_fX + xM.e32*m_fY + xM.e33*m_fZ
	);
#endif
}

class Asura_SoA_Float
{
private:
    friend class Asura_SoA_Vector_3;

    union
    {
        struct
        {
            float m_fX;
            float m_fY;
            float m_fZ;
            float m_fW;
        };

        float m_afV[4];

#if defined ASURA_PC_SSE
        __m128 m_afQ;
#endif
    };

public:

    Asura_SoA_Float() {}

    Asura_SoA_Float(const float fN)
    {
    #if defined ASURA_PC_SSE
		m_afQ = _mm_set_ps1(fN);
    #else
        m_afV[0] = m_afV[1] = m_afV[2] = m_afV[3] = fN;
    #endif
    }

    Asura_SoA_Float(
        register const float fV0,register const float fV1,
        register const float fV2,register const float fV3)
    {
    #if defined ASURA_PC_SSE
	m_afQ = _mm_set_ps(fV3,fV2,fV1,fV0);
    #else
        m_afV[0] = fV0;
        m_afV[1] = fV1;
        m_afV[2] = fV2;
        m_afV[3] = fV3;
    #endif
    }

    Asura_SoA_Float(register const Asura_SoA_Float& xB)
    {
    #if defined ASURA_PC_SSE
	m_afQ = xB.m_afQ;
    #else
        m_afV[0] = xB.m_afV[0];
        m_afV[1] = xB.m_afV[1];
        m_afV[2] = xB.m_afV[2];
        m_afV[3] = xB.m_afV[3];
    #endif
    }

    inline void operator =(register const Asura_SoA_Float& xB)
    {
    #if defined ASURA_PC_SSE
	m_afQ = xB.m_afQ;
    #else
        m_afV[0] = xB.m_afV[0];
        m_afV[1] = xB.m_afV[1];
        m_afV[2] = xB.m_afV[2];
        m_afV[3] = xB.m_afV[3];
    #endif
    }

    inline Asura_SoA_Float operator +(register const Asura_SoA_Float& xB) const
    {
    #if defined ASURA_PC_SSE
		Asura_SoA_Float xO;
		xO.m_afQ = _mm_add_ps(m_afQ,xB.m_afQ);
		return xO;
    #else
        return Asura_SoA_Float(
            m_afV[0]+xB.m_afV[0],m_afV[1]+xB.m_afV[1],
            m_afV[2]+xB.m_afV[2],m_afV[3]+xB.m_afV[3]      
        );
    #endif
    }

    inline Asura_SoA_Float operator *(const float fN) const
    {
    #if defined ASURA_PC_SSE
		Asura_SoA_Float xO;
		xO.m_afQ = _mm_mul_ps(m_afQ,_mm_load_ps1(&fN));
		return xO;
    #else
        return Asura_SoA_Float(
			m_afV[0]*fN,m_afV[1]*fN,m_afV[2]*fN,m_afV[3]*fN
		);
    #endif
    }

    inline Asura_SoA_Float operator *(register const Asura_SoA_Float& xB) const
    {
    #if defined ASURA_PC_SSE
		Asura_SoA_Float xO;

		xO.m_afQ = _mm_mul_ps(m_afQ,xB.m_afQ);

		return xO;
    #else
        return Asura_SoA_Float(
            m_afV[0]*xB.m_afV[0],m_afV[1]*xB.m_afV[1],
            m_afV[2]*xB.m_afV[2],m_afV[3]*xB.m_afV[3]
        );
    #endif
    }

    inline bool operator ==(register const Asura_SoA_Float& xB) const
    {
    #if defined ASURA_PC_SSE
        return _mm_movemask_ps(_mm_cmpeq_ps(m_afQ,xB.m_afQ)) == 0x0f;
    #else
        return m_afV[0] == xB.m_afV[0] && m_afV[1] == xB.m_afV[1]
            && m_afV[2] == xB.m_afV[2] && m_afV[3] == xB.m_afV[3];
    #endif
    }

    inline bool operator !=(register const Asura_SoA_Float& xB ) const
    {
    #if defined ASURA_PC_SSE
        return _mm_movemask_ps(_mm_cmpneq_ps(m_afQ,xB.m_afQ)) != 0x00;
    #else
        return m_afV[0] != xB.m_afV[0] || m_afV[1] != xB.m_afV[1]
            || m_afV[2] != xB.m_afV[2] || m_afV[3] != xB.m_afV[3];
    #endif
    }
};

class Asura_SoA_Vector_3
{
private:

    Asura_SoA_Float m_xX;
    Asura_SoA_Float m_xY;
    Asura_SoA_Float m_xZ;

public:

    Asura_SoA_Vector_3() {}
    Asura_SoA_Vector_3(
        register const Asura_SoA_Float& xX,
        register const Asura_SoA_Float& xY,
        register const Asura_SoA_Float& xZ
    ) 
    {
        m_xX = xX;
        m_xY = xY;
        m_xZ = xZ;
    }

    Asura_SoA_Vector_3(register const Asura_SoA_Vector_3& xB)
    {
        m_xX = xB.m_xX;
        m_xY = xB.m_xY;
        m_xZ = xB.m_xZ;
    }

    Asura_SoA_Vector_3& operator =(register const Asura_SoA_Vector_3& xB)
    {
        m_xX = xB.m_xX;

        m_xY = xB.m_xY;
        m_xZ = xB.m_xZ;

        return *this;
    }

    /*
        Create new 'SoA_Float's from members.
    */
    inline Asura_SoA_Float GetXXXX() const { return m_xX; }
    inline Asura_SoA_Float GetYYYY() const { return m_xY; }
    inline Asura_SoA_Float GetZZZZ() const { return m_xZ; }

    /*
        Dot product.
    */
    inline Asura_SoA_Float operator *(
        register const Asura_SoA_Vector_3& xB
    ) const
    {
    #if defined ASURA_PC_SSE
		Asura_SoA_Float xO;

		xO.m_afQ
			= _mm_add_ps(
				_mm_mul_ps(m_xX.m_afQ,xB.m_xX.m_afQ),
				_mm_add_ps(
					_mm_mul_ps(m_xY.m_afQ,xB.m_xY.m_afQ),
					_mm_mul_ps(m_xZ.m_afQ,xB.m_xZ.m_afQ)));

		return xO;
    #else
        register float fOut0,fOut1,fOut2,fOut3;

        {
            register const float fX0 = m_xX.m_afV[0];
            register const float fX2 = m_xX.m_afV[1];

            register const float fY0 = m_xY.m_afV[0];
            register const float fY2 = m_xY.m_afV[1];

            register const float fZ0 = m_xZ.m_afV[0];
            register const float fZ2 = m_xZ.m_afV[1];

            register const float fX1 = xB.m_xX.m_afV[0];
            register const float fX3 = xB.m_xX.m_afV[1];

            register const float fY1 = xB.m_xY.m_afV[0];
            register const float fY3 = xB.m_xY.m_afV[1];

            register const float fZ1 = xB.m_xZ.m_afV[0];
            register const float fZ3 = xB.m_xZ.m_afV[1];

            fOut0 = fX0*fX1 + fY0*fY1 + fZ0*fZ1;
            fOut1 = fX2*fX3 + fY2*fY3 + fZ2*fZ3;
        }

        {
            register const float fX0 = m_xX.m_afV[2];
            register const float fX2 = m_xX.m_afV[3];

            register const float fY0 = m_xY.m_afV[2];
            register const float fY2 = m_xY.m_afV[3];

            register const float fZ0 = m_xZ.m_afV[2];
            register const float fZ2 = m_xZ.m_afV[3];

            register const float fX1 = xB.m_xX.m_afV[2];
            register const float fX3 = xB.m_xX.m_afV[3];

            register const float fY1 = xB.m_xY.m_afV[2];
            register const float fY3 = xB.m_xY.m_afV[3];

            register const float fZ1 = xB.m_xZ.m_afV[2];
            register const float fZ3 = xB.m_xZ.m_afV[3];

            fOut2 = fX0*fX1 + fY0*fY1 + fZ0*fZ1;
            fOut3 = fX2*fX3 + fY2*fY3 + fZ2*fZ3;
        }

        return Asura_SoA_Float(fOut0,fOut1,fOut2,fOut3);
    #endif
    }

    /*
        Transform by matrix.
    */
    inline const Asura_SoA_Vector_3 operator *(
        register const Asura_AMatrix3x3& xM
    ) const
    {
        register Asura_SoA_Float xOutX;
        register Asura_SoA_Float xOutY;
        register Asura_SoA_Float xOutZ;

		{
            register const Asura_AVector3 xC0 = xM.GetColumn0();

            register const Asura_SoA_Float xM11(xC0.GetX());
            register const Asura_SoA_Float xM21(xC0.GetY());
            register const Asura_SoA_Float xM31(xC0.GetZ());

            xOutX = m_xX*xM11;
            xOutY = m_xX*xM21;
            xOutZ = m_xX*xM31;
        }

        {
            register const Asura_AVector3 xC1 = xM.GetColumn1();

            register const Asura_SoA_Float xM12(xC1.GetX());
            register const Asura_SoA_Float xM22(xC1.GetY());
            register const Asura_SoA_Float xM32(xC1.GetZ());

            xOutX = xOutX + m_xY*xM12;
            xOutY = xOutY + m_xY*xM22;
            xOutZ = xOutZ + m_xY*xM32;
        }

        {
            register const Asura_AVector3 xC2 = xM.GetColumn2();

            register const Asura_SoA_Float xM13(xC2.GetX());
            register const Asura_SoA_Float xM23(xC2.GetY());
            register const Asura_SoA_Float xM33(xC2.GetZ());

            xOutX = xOutX + m_xZ*xM13;
            xOutY = xOutY + m_xZ*xM23;
            xOutZ = xOutZ + m_xZ*xM33;
        }

        return Asura_SoA_Vector_3(xOutX,xOutY,xOutZ);
    }
};

inline void Asura_Quat::ToMatrix(Asura_AMatrix3x3_OutParam m) const
{
	/* Implementation from Graphics Gems IV */

	float s = 2.0f;///Magnitude();      

	float xs = x*s;
	float ys = y*s;
	float zs = z*s;
	
	float wx = w*xs;
	float wy = w*ys;
	float wz = w*zs;

	float xx = x*xs;
	float xy = x*ys;
	float xz = x*zs;

	float yy = y*ys;
	float yz = y*zs;
	float zz = z*zs;
	    
	m.SetColumn0(Asura_AVector3( 1.0f - (yy+zz), xy + wz, xz - wy ));
	m.SetColumn1(Asura_AVector3( xy - wz, 1.0f - (xx + zz), yz + wx ));
	m.SetColumn2(Asura_AVector3( xz + wy, yz - wx, 1.0f - (xx+yy) ));
}

inline void Asura_Quat::FromMatrix(Asura_AMatrix3x3_InParam xM4)
{
	Asura_Matrix_3x3 xM3;
	xM4.ToMat3(xM3);
	FromMatrix(xM3);
}

/**
 * This function rotates a vector p by the quaternion q.
 *
 * The formula is: p' = (q)(p)(q*)
 *
 * But we calculate it using a vector formula which is an
 * expansion of this that Kev worked out:
 *
 *   for q = (v,w): v' = 2w(v^p) + (2(v.p)v + (w^2-v^2)p
 */
inline Asura_AVector3 Asura_Quat::RotateVector(Asura_AVector3_InParam xP) const
{
	/* the vector part of 'q' and its squared magnitude */
	register const Asura_AVector3 xQV(x,y,z);
	register const float fQVMagSq = xQV.MagnitudeSquared();

	register const Asura_AVector3 xR1 = (xQV^xP) * (2.0f*w);
	register const Asura_AVector3 xR2 = xQV * (2.0f*(xQV*xP));
	register const Asura_AVector3 xR3 = xP * (w*w - fQVMagSq);

	return Asura_AVector3(xR1+xR2+xR3);
}

// creates a quaternion from an angle and axis of rotation
inline void Asura_Quat::FromRotation(const Asura_Angle fAngle,
	Asura_AVector3_InParam xAxis)
{
	register float fSine;

	Asura_Assertf( fAngle == 0.0f ||
		Asura_Maths::Abs(xAxis.MagnitudeSquared() - 1.0f) < 0.0001f,
		( "axis should be normalised, mag. sq. is %.6f", xAxis.MagnitudeSquared() )
	);

	Asura_Maths::SinCos(fAngle*0.5f, fSine, w);

#if defined ASURA_PC_SSE
	register const __m128 afQ = _mm_mul_ps(xAxis.m_afQ,_mm_set_ps1(fSine));
	_mm_asura_storeu3_ps(reinterpret_cast<float* const>(this),afQ);
#else
	x = xAxis.GetX()*fSine;
	y = xAxis.GetY()*fSine;
	z = xAxis.GetZ()*fSine;
#endif
}

//////////////////////////////////////////////////////////////////////////
// Asura_AVector4
//////////////////////////////////////////////////////////////////////////
class Asura_AVector4
{
public:
	// Constructors...
	Asura_AVector4(void);
	Asura_AVector4(const float fX, const float fY, const float fZ, const float fW);
	Asura_AVector4(const Asura_AVector4& xOther);
	Asura_AVector4(const Asura_AVector3& xOther, const float fW);
	Asura_AVector4(const Asura_Vector_3& xOther, const float fW);

	// Zero functions...
	void ZeroW0(void);
	void ZeroW1(void);

	// Selection functions...
	void SelectGE(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS, Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse);
	void SelectGT(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS, Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse);
	void SelectLE(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS, Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse);
	void SelectLT(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS, Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse);
	void SelectEQ(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS, Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse);
	void SelectNE(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS, Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse);

	// Load/store functions...
	void LoadAligned(const float* pfFloats);
	void LoadUnaligned(const float* pfFloats);
	void StoreAligned(float* pfFloats) const;
	void StoreUnaligned(float* pfFloats) const;
	void StoreAlignedHalf(Asura_HalfFloat* pxHalves) const;

	// Manipulation functions...
	Asura_AVector4 ComponentwiseProduct(Asura_AVector4_InParam xOther) const;
	float DotProduct3(Asura_AVector4_InParam xOther) const;
	float DotProduct4(Asura_AVector4_InParam xOther) const;

	float MagnitudeSquared(void) const;
	float Magnitude(void) const;

	void Normalise(void);
	float NormaliseReturningMagnitude(void);
	void ApproximateNormalise(void);
	float ApproximateNormaliseReturningMagnitude(void);

	// Assignment operators...
	void operator=(Asura_AVector4_InParam xOther);

	void operator+=(Asura_AVector4_InParam xOther);
	void operator-=(Asura_AVector4_InParam xOther);

	void operator*=(const float fScale);
	void operator/=(const float fScale);

	// Unary operators...
	Asura_AVector4 operator-(void) const;

	// Binary operators...
	Asura_AVector4 operator+(Asura_AVector4_InParam xOther) const;
	Asura_AVector4 operator-(Asura_AVector4_InParam xOther) const;
	Asura_AVector4 operator^(Asura_AVector4_InParam xOther) const;

	float operator*(Asura_AVector4_InParam xOther) const;

	Asura_AVector4 operator*(const float fScale) const;
	Asura_AVector4 operator/(const float fScale) const;

	// Comparison operators...
	bool operator==(Asura_AVector4_InParam xOther) const;
	bool ApproximatelyEquals(Asura_AVector4_InParam xOther, const float fEpsilon = 1.0e-6f) const;
	bool operator!=(Asura_AVector4_InParam xOther) const;

	// Minimum/maximum functions...
	void Max(Asura_AVector4_InParam xV1, Asura_AVector4_InParam xV2);
	void Min(Asura_AVector4_InParam xV1, Asura_AVector4_InParam xV2);

	// Properties...
	float GetX(void) const;
	float GetY(void) const;
	float GetZ(void) const;
	float GetW(void) const;

	void SetX(const float fX);
	void SetY(const float fY);
	void SetZ(const float fZ);
	void SetW(const float fW);

	float GetElement(const u_int uIndex) const;
	void SetElement(const u_int uIndex, const float fValue);
	void SetElements(const float fX, const float fY, const float fZ, const float fW);

private:
	union
	{
		float	m_afElements[4];

		struct 
		{
			float m_fX;
			float m_fY;
			float m_fZ;
			float m_fW;
		};
	};
};


//////////////////////////////////////////////////////////////////////////
// Asura_AMatrix4x4
//////////////////////////////////////////////////////////////////////////
class Asura_AMatrix4x4
{
public:
	// Constructors/initializers
	Asura_AMatrix4x4(void);
	Asura_AMatrix4x4(float e00, float e01, float e02, float e03,
		float e10, float e11, float e12, float e13,
		float e20, float e21, float e22, float e23,
		float e30, float e31, float e32, float e33);
	Asura_AMatrix4x4(const Asura_AMatrix4x4& xOther);
	Asura_AMatrix4x4(const Asura_Matrix_3x3& xOrientation, const Asura_Vector_3& xTranslation);
	Asura_AMatrix4x4(const Asura_Quat& xOrientation, const Asura_Vector_3& xTranslation);

	// Constant functions...
	void Identity(void);
	void Zero(void);

	// Orientation/translation functions...
	void ToMatrix(Asura_Matrix_3x3& xOrientation) const;
	void FromMatrix(const Asura_Matrix_3x3& xOrientation);

	Asura_Vector_3 GetTranslation() const;
	void SetTranslation(const Asura_Vector_3& xTranslation);

	Asura_AVector3 GetATranslation() const;
	void SetTranslation(Asura_AVector3_InParam xTranslation);

	Asura_AVector4 GetA4Translation() const;
	void SetTranslation(Asura_AVector4_InParam xTranslation);

	// Manipulation functions...
	Asura_AVector4 Apply(Asura_AVector4_InParam xInput) const;
	void ApplyToArray(const Asura_AVector4* pxInput, const u_int uNumElements, Asura_AVector4* pxOutput) const;

	void Apply(Asura_AMatrix4x4_InParam xInput, Asura_AMatrix4x4_OutParam xOutput) const;
	void ApplyToArray(const Asura_AMatrix4x4* pxInput, const u_int uNumElements, Asura_AMatrix4x4* pxOutput) const;

	float Cofactor(const u_int uRow, const u_int uCol) const;
	float Determinant(void) const;

	void Invert(void);
	void InvertOrthogonal(void);
	void Transpose(void);

	void Invert(Asura_AMatrix4x4_OutParam xOther) const;
	void InvertOrthogonal(Asura_AMatrix4x4_OutParam xOther) const;
	void Transpose(Asura_AMatrix4x4_OutParam xOther) const;

	// Unary operators
	Asura_AMatrix4x4 operator-(void) const;

	// Binary operators
	Asura_AMatrix4x4 operator*(const float fScale) const;
	Asura_AMatrix4x4 operator/(const float fScale) const;

	// Assignment operators
	void operator=(Asura_AMatrix4x4_InParam xOther);
	void operator*=(const float fScale);
	void operator/=(const float fScale);

	// Conditional operators
	bool operator==(Asura_AMatrix4x4_InParam xOther) const;
	bool ApproximatelyEquals(Asura_AMatrix4x4_InParam, float fEpsilon = 1.0e-6f) const;
	bool operator!=(Asura_AMatrix4x4_InParam xOther) const;

	// Elementary properties...
	float GetElement(const u_int uRow, const u_int uCol) const;
	void SetElement(const u_int uRow, const u_int uCol, const float fValue);

	Asura_AVector4 GetColumn(const u_int uColumnIndex) const;
	Asura_AVector4 GetColumn0() const;
	Asura_AVector4 GetColumn1() const;
	Asura_AVector4 GetColumn2() const;
	Asura_AVector4 GetColumn3() const;

	void SetColumn(const u_int uColumnIndex, Asura_AVector4_InParam xColumn);
	void SetColumn0(Asura_AVector4_InParam xCol0);
	void SetColumn1(Asura_AVector4_InParam xCol1);
	void SetColumn2(Asura_AVector4_InParam xCol2);
	void SetColumn3(Asura_AVector4_InParam xCol3);

	Asura_AVector4 GetRow(const u_int uRowIndex) const;
	Asura_AVector4 GetRow0() const;
	Asura_AVector4 GetRow1() const;
	Asura_AVector4 GetRow2() const;
	Asura_AVector4 GetRow3() const;
	
	void SetRow(const u_int uRowIndex, Asura_AVector4_InParam xRow);
	void SetRow0(Asura_AVector4_InParam xRow0);
	void SetRow1(Asura_AVector4_InParam xRow1);
	void SetRow2(Asura_AVector4_InParam xRow2);
	void SetRow3(Asura_AVector4_InParam xRow3);

	void SetColumns(Asura_AVector4_InParam xCol0, Asura_AVector4_InParam xCol1, Asura_AVector4_InParam xCol2, Asura_AVector4_InParam xCol3);
	void SetRows(Asura_AVector4_InParam xRow0, Asura_AVector4_InParam xRow1, Asura_AVector4_InParam xRow2, Asura_AVector4_InParam xRow3);

	// Load/store functions...
	void LoadColsAligned(const float* pfFloats);
	void LoadRowsAligned(const float* pfFloats);
	void LoadColsUnaligned(const float* pfFloats);
	void LoadRowsUnaligned(const float* pfFloats);

	void StoreColsAligned(float* pfFloats) const;
	void StoreRowsAligned(float* pfFloats) const;
	void StoreColsUnaligned(float* pfFloats) const;
	void StoreRowsUnaligned(float* pfFloats) const;

private:
	Asura_AVector4 m_axColumns[4];
};


//////////////////////////////////////////////////////////////////////////
// Inline Function Definitions
//////////////////////////////////////////////////////////////////////////

// scalar * 4x1 Vector - to match the member function in Asura_AVector4
__forceinline Asura_AVector4 operator*(const float fScale, const Asura_AVector4& xOther)
{
	return(xOther * fScale);
}

__forceinline Asura_AVector4 operator*(Asura_AVector4_InParam xVec, Asura_AMatrix4x4_InParam xMat)
{
	return xMat.Apply(xVec);
}

// To match the backwards vector-matrix multiply, we do a backwards matrix-vector multiply too.
__forceinline Asura_AMatrix4x4 operator*(Asura_AMatrix4x4_InParam xM1, Asura_AMatrix4x4_InParam xM2)
{
	Asura_AMatrix4x4	xResult;

	xM2.Apply(xM1, xResult);
	return(xResult);
}


//////////////////////////////////////////////////////////////////////////
// Asura_AVector4
//////////////////////////////////////////////////////////////////////////
__forceinline Asura_AVector4::Asura_AVector4(void)
{
}

__forceinline Asura_AVector4::Asura_AVector4(const float fX, const float fY, const float fZ, const float fW)
: m_fX(fX)
, m_fY(fY)
, m_fZ(fZ)
, m_fW(fW)
{
}

__forceinline Asura_AVector4::Asura_AVector4(Asura_AVector4_InParam xOther)
: m_fX(xOther.m_fX)
, m_fY(xOther.m_fY)
, m_fZ(xOther.m_fZ)
, m_fW(xOther.m_fW)
{
}

__forceinline Asura_AVector4::Asura_AVector4(const Asura_AVector3& xOther, const float fW)
: m_fX(xOther.GetX())
, m_fY(xOther.GetY())
, m_fZ(xOther.GetZ())
, m_fW(fW)
{
}

__forceinline Asura_AVector4::Asura_AVector4(const Asura_Vector_3& xOther, const float fW)
: m_fX(xOther.x)
, m_fY(xOther.y)
, m_fZ(xOther.z)
, m_fW(fW)
{
}

__forceinline void Asura_AVector4::ZeroW0(void) 
{
	m_fX = m_fY = m_fZ = m_fW = 0.0f;
}

__forceinline void Asura_AVector4::ZeroW1(void) 
{
	m_fX = m_fY = m_fZ = 0.0f;
	m_fW = 1.0f;
}

__forceinline void Asura_AVector4::SelectGE(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS,
											Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse)
{
	m_fX 	= (xLHS.m_fX >= xRHS.m_fX) ? xTrue.m_fX : xFalse.m_fX;
	m_fY 	= (xLHS.m_fY >= xRHS.m_fY) ? xTrue.m_fY : xFalse.m_fY;
	m_fZ 	= (xLHS.m_fZ >= xRHS.m_fZ) ? xTrue.m_fZ : xFalse.m_fZ;
	m_fW 	= (xLHS.m_fW >= xRHS.m_fW) ? xTrue.m_fW : xFalse.m_fW;
}

__forceinline void Asura_AVector4::SelectGT(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS,
											Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse)
{
	m_fX 	= (xLHS.m_fX > xRHS.m_fX) ? xTrue.m_fX : xFalse.m_fX;
	m_fY 	= (xLHS.m_fY > xRHS.m_fY) ? xTrue.m_fY : xFalse.m_fY;
	m_fZ 	= (xLHS.m_fZ > xRHS.m_fZ) ? xTrue.m_fZ : xFalse.m_fZ;
	m_fW 	= (xLHS.m_fW > xRHS.m_fW) ? xTrue.m_fW : xFalse.m_fW;
}

__forceinline void Asura_AVector4::SelectLE(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS,
											Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse)
{
	m_fX 	= (xLHS.m_fX <= xRHS.m_fX) ? xTrue.m_fX : xFalse.m_fX;
	m_fY 	= (xLHS.m_fY <= xRHS.m_fY) ? xTrue.m_fY : xFalse.m_fY;
	m_fZ 	= (xLHS.m_fZ <= xRHS.m_fZ) ? xTrue.m_fZ : xFalse.m_fZ;
	m_fW 	= (xLHS.m_fW <= xRHS.m_fW) ? xTrue.m_fW : xFalse.m_fW;
}

__forceinline void Asura_AVector4::SelectLT(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS,
											Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse)
{
	m_fX 	= (xLHS.m_fX < xRHS.m_fX) ? xTrue.m_fX : xFalse.m_fX;
	m_fY 	= (xLHS.m_fY < xRHS.m_fY) ? xTrue.m_fY : xFalse.m_fY;
	m_fZ 	= (xLHS.m_fZ < xRHS.m_fZ) ? xTrue.m_fZ : xFalse.m_fZ;
	m_fW 	= (xLHS.m_fW < xRHS.m_fW) ? xTrue.m_fW : xFalse.m_fW;
}

__forceinline void Asura_AVector4::SelectEQ(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS,
											Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse)
{
	m_fX 	= (xLHS.m_fX == xRHS.m_fX) ? xTrue.m_fX : xFalse.m_fX;
	m_fY 	= (xLHS.m_fY == xRHS.m_fY) ? xTrue.m_fY : xFalse.m_fY;
	m_fZ 	= (xLHS.m_fZ == xRHS.m_fZ) ? xTrue.m_fZ : xFalse.m_fZ;
	m_fW 	= (xLHS.m_fW == xRHS.m_fW) ? xTrue.m_fW : xFalse.m_fW;
}

__forceinline void Asura_AVector4::SelectNE(Asura_AVector4_InParam xLHS, Asura_AVector4_InParam xRHS,
											Asura_AVector4_InParam xTrue, Asura_AVector4_InParam xFalse)
{
	m_fX 	= (xLHS.m_fX != xRHS.m_fX) ? xTrue.m_fX : xFalse.m_fX;
	m_fY 	= (xLHS.m_fY != xRHS.m_fY) ? xTrue.m_fY : xFalse.m_fY;
	m_fZ 	= (xLHS.m_fZ != xRHS.m_fZ) ? xTrue.m_fZ : xFalse.m_fZ;
	m_fW 	= (xLHS.m_fW != xRHS.m_fW) ? xTrue.m_fW : xFalse.m_fW;
}

__forceinline void Asura_AVector4::operator=(Asura_AVector4_InParam xOther)
{
	m_fX	= xOther.m_fX;
	m_fY	= xOther.m_fY;
	m_fZ	= xOther.m_fZ;
	m_fW	= xOther.m_fW;
}

__forceinline void Asura_AVector4::LoadAligned(const float* pfFloats)
{
	m_fX	= pfFloats[0];
	m_fY	= pfFloats[1];
	m_fZ	= pfFloats[2];
	m_fW	= pfFloats[3];
}

__forceinline void Asura_AVector4::LoadUnaligned(const float* pfFloats)
{
	m_fX	= pfFloats[0];
	m_fY	= pfFloats[1];
	m_fZ	= pfFloats[2];
	m_fW	= pfFloats[3];
}

__forceinline void Asura_AVector4::StoreAligned(float* pfFloats) const
{
	pfFloats[0]	= m_fX;
	pfFloats[1]	= m_fY;
	pfFloats[2]	= m_fZ;
	pfFloats[3]	= m_fW;
}

__forceinline void Asura_AVector4::StoreUnaligned(float* pfFloats) const
{
	pfFloats[0]	= m_fX;
	pfFloats[1]	= m_fY;
	pfFloats[2]	= m_fZ;
	pfFloats[3]	= m_fW;
}

__forceinline void Asura_AVector4::StoreAlignedHalf(Asura_HalfFloat* pxHalves) const
{
	pxHalves[0] = m_fX;
	pxHalves[1] = m_fY;
	pxHalves[2] = m_fZ;
	pxHalves[3] = m_fW;
}

__forceinline Asura_AVector4 Asura_AVector4::ComponentwiseProduct(Asura_AVector4_InParam xOther) const
{
	return(Asura_AVector4((m_fX * xOther.m_fX), (m_fY * xOther.m_fY), (m_fZ * xOther.m_fZ), (m_fW * xOther.m_fW)));
}

__forceinline float Asura_AVector4::DotProduct3(Asura_AVector4_InParam xOther) const
{
	return((m_fX * xOther.m_fX) + (m_fY * xOther.m_fY) + (m_fZ * xOther.m_fZ));
}

__forceinline float Asura_AVector4::DotProduct4(Asura_AVector4_InParam xOther) const
{
	return((m_fX * xOther.m_fX) + (m_fY * xOther.m_fY) + (m_fZ * xOther.m_fZ) + (m_fW * xOther.m_fW));
}

__forceinline float Asura_AVector4::MagnitudeSquared(void) const 
{
	return((m_fX * m_fX) + (m_fY * m_fY) + (m_fZ * m_fZ) + (m_fW * m_fW));
}

__forceinline float Asura_AVector4::Magnitude(void) const 
{
	return(Asura_Maths::Sqrt(MagnitudeSquared()));
}

__forceinline void Asura_AVector4::Normalise(void)
{
	const float	fMagnitude	= Magnitude();

	*this	/= fMagnitude;
}

__forceinline float Asura_AVector4::NormaliseReturningMagnitude(void) 
{
	const float	fMagnitude = Magnitude();

	if(fMagnitude == 0.0f)
	{
		return(0.0f);
	}
	else
	{
		*this	/= fMagnitude;
		return(fMagnitude);
	}
}

__forceinline void Asura_AVector4::ApproximateNormalise(void)
{
	Normalise();
}

__forceinline float Asura_AVector4::ApproximateNormaliseReturningMagnitude(void) 
{
	return(NormaliseReturningMagnitude());
}

__forceinline void Asura_AVector4::operator+=(Asura_AVector4_InParam xOther)
{
	m_fX	+= xOther.m_fX;
	m_fY	+= xOther.m_fY;
	m_fZ	+= xOther.m_fZ;
	m_fW	+= xOther.m_fW;
}

__forceinline void Asura_AVector4::operator-=(Asura_AVector4_InParam xOther)
{
	m_fX	-= xOther.m_fX;
	m_fY	-= xOther.m_fY;
	m_fZ	-= xOther.m_fZ;
	m_fW	-= xOther.m_fW;
}

__forceinline void Asura_AVector4::operator*=(const float fScale) 
{
	m_fX	*= fScale;
	m_fY	*= fScale;
	m_fZ	*= fScale;
	m_fW	*= fScale;
}

__forceinline void Asura_AVector4::operator/=(const float fScale) 
{
	m_fX	/= fScale;
	m_fY	/= fScale;
	m_fZ	/= fScale;
	m_fW	/= fScale;
}

__forceinline Asura_AVector4 Asura_AVector4::operator-(void) const 
{
	return Asura_AVector4(-m_fX, -m_fY, -m_fZ, -m_fW);
}

__forceinline Asura_AVector4 Asura_AVector4::operator+(Asura_AVector4_InParam xOther) const
{
	return(Asura_AVector4((m_fX + xOther.m_fX), (m_fY + xOther.m_fY), (m_fZ + xOther.m_fZ), (m_fW + xOther.m_fW)));
}

__forceinline Asura_AVector4 Asura_AVector4::operator-( Asura_AVector4_InParam xOther) const
{
	return(Asura_AVector4((m_fX - xOther.m_fX), (m_fY - xOther.m_fY), (m_fZ - xOther.m_fZ), (m_fW - xOther.m_fW)));
}

__forceinline Asura_AVector4 Asura_AVector4::operator^(Asura_AVector4_InParam xOther) const
{
	return(Asura_AVector4((m_fY * xOther.m_fZ) - (m_fZ * xOther.m_fY), (m_fZ * xOther.m_fX) - (m_fX * xOther.m_fZ), (m_fX * xOther.m_fY) - (m_fY * xOther.m_fX), 0.0f));
}

__forceinline float Asura_AVector4::operator*(Asura_AVector4_InParam xOther) const
{
	return(DotProduct4(xOther));
}

__forceinline Asura_AVector4 Asura_AVector4::operator*(const float fScale) const 
{
	return(Asura_AVector4((m_fX * fScale), (m_fY * fScale), (m_fZ * fScale), (m_fW * fScale)));
}

__forceinline Asura_AVector4 Asura_AVector4::operator/(const float fScale) const 
{
	return(Asura_AVector4((m_fX / fScale), (m_fY / fScale), (m_fZ / fScale), (m_fW / fScale)));
}

__forceinline bool Asura_AVector4::operator==(Asura_AVector4_InParam xOther) const
{
	return((m_fX == xOther.m_fX) && (m_fY == xOther.m_fY) && (m_fZ == xOther.m_fZ) && (m_fW == xOther.m_fW));
}

__forceinline bool Asura_AVector4::ApproximatelyEquals(Asura_AVector4_InParam xOther, float fEpsilon) const
{
	Asura_AVector4	xDifference(*this - xOther);

	return(xDifference.MagnitudeSquared() < fEpsilon);
}

__forceinline bool Asura_AVector4::operator!=(Asura_AVector4_InParam xOther) const
{
	return((m_fX != xOther.m_fX) || (m_fY != xOther.m_fY) || (m_fZ != xOther.m_fZ) || (m_fW != xOther.m_fW));
}

__forceinline void Asura_AVector4::Max(Asura_AVector4_InParam xV1, Asura_AVector4_InParam xV2)
{
	m_fX	= Asura_Maths::Max(xV1.m_fX, xV2.m_fX);
	m_fY	= Asura_Maths::Max(xV1.m_fY, xV2.m_fY);
	m_fZ	= Asura_Maths::Max(xV1.m_fZ, xV2.m_fZ);
	m_fW	= Asura_Maths::Max(xV1.m_fW, xV2.m_fW);
}
__forceinline void Asura_AVector4::Min(Asura_AVector4_InParam xV1, Asura_AVector4_InParam xV2)
{
	m_fX	= Asura_Maths::Min(xV1.m_fX, xV2.m_fX);
	m_fY	= Asura_Maths::Min(xV1.m_fY, xV2.m_fY);
	m_fZ	= Asura_Maths::Min(xV1.m_fZ, xV2.m_fZ);
	m_fW	= Asura_Maths::Min(xV1.m_fW, xV2.m_fW);
}

__forceinline float Asura_AVector4::GetX(void) const
{
	return(m_fX);
}

__forceinline float Asura_AVector4::GetY(void) const
{ 
	return(m_fY);
}

__forceinline float Asura_AVector4::GetZ(void) const
{
	return(m_fZ);
}

__forceinline float Asura_AVector4::GetW(void) const
{
	return(m_fW);
}

__forceinline void Asura_AVector4::SetX(const float fX)
{
	m_fX	= fX;
}

__forceinline void Asura_AVector4::SetY(const float fY)
{
	m_fY	= fY;
}

__forceinline void Asura_AVector4::SetZ(const float fZ)
{
	m_fZ	= fZ;
}

__forceinline void Asura_AVector4::SetW(const float fW)
{
	m_fW	= fW;
}

__forceinline float Asura_AVector4::GetElement(const u_int uIndex) const
{ 
	return(m_afElements[uIndex]);
}

__forceinline void Asura_AVector4::SetElement(const u_int uIndex, const float fValue)
{
	m_afElements[uIndex]	= fValue;
}

__forceinline void Asura_AVector4::SetElements(const float fX, const float fY, const float fZ, const float fW)
{
	m_fX	= fX;
	m_fY	= fY;
	m_fZ	= fZ;
	m_fW	= fW;
}


//////////////////////////////////////////////////////////////////////////
// Asura_AMatrix4x4
//////////////////////////////////////////////////////////////////////////
__forceinline Asura_AMatrix4x4::Asura_AMatrix4x4(void)
{
}

__forceinline Asura_AMatrix4x4::Asura_AMatrix4x4(const float fE11, const float fE12, const float fE13, const float fE14,
												 const float fE21, const float fE22, const float fE23, const float fE24,
												 const float fE31, const float fE32, const float fE33, const float fE34,
												 const float fE41, const float fE42, const float fE43, const float fE44)
{
	m_axColumns[0].SetElements(fE11, fE21, fE31, fE41);
	m_axColumns[1].SetElements(fE12, fE22, fE32, fE42);
	m_axColumns[2].SetElements(fE13, fE23, fE33, fE43);
	m_axColumns[3].SetElements(fE14, fE24, fE34, fE44);
}

__forceinline Asura_AMatrix4x4::Asura_AMatrix4x4(const Asura_AMatrix4x4& xOther)
{
	m_axColumns[0]	= xOther.m_axColumns[0];
	m_axColumns[1]	= xOther.m_axColumns[1];
	m_axColumns[2]	= xOther.m_axColumns[2];
	m_axColumns[3]	= xOther.m_axColumns[3];
}

__forceinline Asura_AMatrix4x4::Asura_AMatrix4x4(const Asura_Matrix_3x3& xRotation, const Asura_Vector_3& xPosition)
{
	m_axColumns[0].SetElements(xRotation.e11,		xRotation.e21,		xRotation.e31,		0.0f);
	m_axColumns[1].SetElements(xRotation.e12,		xRotation.e22,		xRotation.e32,		0.0f);
	m_axColumns[2].SetElements(xRotation.e13,		xRotation.e23,		xRotation.e33,		0.0f);
	m_axColumns[3].SetElements(xPosition.GetX(),	xPosition.GetY(),	xPosition.GetZ(),	1.0f);
}

__forceinline Asura_AMatrix4x4::Asura_AMatrix4x4(const Asura_Quat& xRotation, const Asura_Vector_3& xPosition)
{
	const float	fS	= 2.0f;	//xRotation.Magnitude();

	const float	fXS	= xRotation.x * fS;
	const float	fYS	= xRotation.y * fS;
	const float	fZS	= xRotation.z * fS;

	const float	fWX	= xRotation.w * fXS;
	const float	fWY	= xRotation.w * fYS;
	const float	fWZ	= xRotation.w * fZS;

	const float	fXX	= xRotation.x * fXS;
	const float	fXY	= xRotation.x * fYS;
	const float	fXZ	= xRotation.x * fZS;

	const float	fYY	= xRotation.y * fYS;
	const float	fYZ	= xRotation.y * fZS;
	const float	fZZ	= xRotation.z * fZS;

	// Implementation from Graphics Gems IV...
	m_axColumns[0].SetElements(1.0f - (fYY + fZZ),	fXY + fWZ,			fXZ - fWY,			0.0f);
	m_axColumns[1].SetElements(fXY - fWZ,			1.0f - (fXX + fZZ),	fYZ + fWX,			0.0f);
	m_axColumns[2].SetElements(fXZ + fWY,			fYZ - fWX,			1.0f - (fXX + fYY),	0.0f);
	m_axColumns[3].SetElements(xPosition.GetX(),	xPosition.GetY(),	xPosition.GetZ(),	1.0f);
}

__forceinline void Asura_AMatrix4x4::SetColumns(Asura_AVector4_InParam xCol0, Asura_AVector4_InParam xCol1, Asura_AVector4_InParam xCol2, Asura_AVector4_InParam xCol3)
{
	m_axColumns[0]	= xCol0;
	m_axColumns[1]	= xCol1;
	m_axColumns[2]	= xCol2;
	m_axColumns[3]	= xCol3;
}

__forceinline void Asura_AMatrix4x4::SetRows(Asura_AVector4_InParam xRow0, Asura_AVector4_InParam xRow1, Asura_AVector4_InParam xRow2, Asura_AVector4_InParam xRow3)
{
	SetRow0(xRow0);
	SetRow1(xRow1);
	SetRow2(xRow2);
	SetRow3(xRow3);
}

__forceinline void Asura_AMatrix4x4::Identity(void)
{
	m_axColumns[0].SetElements(1.0f, 0.0f, 0.0f, 0.0f);
	m_axColumns[1].SetElements(0.0f, 1.0f, 0.0f, 0.0f);
	m_axColumns[2].SetElements(0.0f, 0.0f, 1.0f, 0.0f);
	m_axColumns[3].ZeroW1();
}

__forceinline void Asura_AMatrix4x4::Zero(void)
{
	m_axColumns[0].ZeroW0();
	m_axColumns[1].ZeroW0();
	m_axColumns[2].ZeroW0();
	m_axColumns[3].ZeroW0();
}

__forceinline void Asura_AMatrix4x4::ToMatrix(Asura_Matrix_3x3& xOrientation) const
{
	xOrientation.e11	= m_axColumns[0].GetX();
	xOrientation.e21	= m_axColumns[0].GetY();
	xOrientation.e31	= m_axColumns[0].GetZ();

	xOrientation.e12	= m_axColumns[1].GetX();
	xOrientation.e22	= m_axColumns[1].GetY();
	xOrientation.e32	= m_axColumns[1].GetZ();

	xOrientation.e13	= m_axColumns[2].GetX();
	xOrientation.e23	= m_axColumns[2].GetY();
	xOrientation.e33	= m_axColumns[2].GetZ();
}

__forceinline void Asura_AMatrix4x4::FromMatrix(const Asura_Matrix_3x3& xOrientation)
{
	m_axColumns[0].SetElements(xOrientation.e11, xOrientation.e21, xOrientation.e31, 0.0f);
	m_axColumns[1].SetElements(xOrientation.e12, xOrientation.e22, xOrientation.e32, 0.0f);
	m_axColumns[2].SetElements(xOrientation.e13, xOrientation.e23, xOrientation.e33, 0.0f);
	m_axColumns[3].ZeroW1();
}

__forceinline Asura_Vector_3 Asura_AMatrix4x4::GetTranslation() const
{
	return Asura_Vector_3(m_axColumns[3].GetX(),m_axColumns[3].GetY(),m_axColumns[3].GetZ());
}

__forceinline void Asura_AMatrix4x4::SetTranslation(const Asura_Vector_3& xTranslation)
{
	m_axColumns[3]	= Asura_AVector4(xTranslation, 1.0f);
}

__forceinline Asura_AVector3 Asura_AMatrix4x4::GetATranslation() const
{
	return Asura_AVector3(m_axColumns[3].GetX(),m_axColumns[3].GetY(),m_axColumns[3].GetZ());
}

__forceinline void Asura_AMatrix4x4::SetTranslation(Asura_AVector3_InParam xTranslation)
{
	m_axColumns[3].SetElements(xTranslation.GetX(), xTranslation.GetY(), xTranslation.GetZ(), 1.0f);
}

__forceinline Asura_AVector4 Asura_AMatrix4x4::GetA4Translation() const
{
	return m_axColumns[3];
}

__forceinline void Asura_AMatrix4x4::SetTranslation(Asura_AVector4_InParam xTranslation)
{
	m_axColumns[3]	= xTranslation;
}

__forceinline Asura_AMatrix4x4 Asura_AMatrix4x4::operator-(void) const
{
	Asura_AMatrix4x4	xTemp;

	xTemp.SetColumns(-m_axColumns[0], -m_axColumns[1], -m_axColumns[2], -m_axColumns[3]);
	return(xTemp);
}

__forceinline Asura_AVector4 Asura_AMatrix4x4::Apply(Asura_AVector4_InParam xInput) const
{
	Asura_AVector4	axRows[4];

	// Extract the data...
	axRows[0] = GetRow0();
	axRows[1] = GetRow1();
	axRows[2] = GetRow2();
	axRows[3] = GetRow3();

	// Set each element of the result in turn...
	return Asura_AVector4((axRows[0] * xInput), (axRows[1] * xInput), (axRows[2] * xInput), (axRows[3] * xInput));
}

__forceinline void Asura_AMatrix4x4::ApplyToArray(const Asura_AVector4* pxInput, const u_int uNumElements, Asura_AVector4* pxOutput) const
{
	Asura_AVector4	axRows[4];

	// Extract the data...
	axRows[0] = GetRow0();
	axRows[1] = GetRow1();
	axRows[2] = GetRow2();
	axRows[3] = GetRow3();

	for(u_int u = 0; u < uNumElements; u++, pxInput++, pxOutput++)
	{
		// Set each element of the result in turn...
		pxOutput->SetElements((axRows[0] * *pxInput), (axRows[1] * *pxInput), (axRows[2] * *pxInput), (axRows[3] * *pxInput));
	}
}

__forceinline void Asura_AMatrix4x4::Apply(Asura_AMatrix4x4_InParam xInput, Asura_AMatrix4x4_OutParam xOutput) const
{
	Asura_AVector4	axRows[4];
	Asura_AVector4	axColumns[4];

	// Extract the data...
	axRows[0] = GetRow0();
	axRows[1] = GetRow1();
	axRows[2] = GetRow2();
	axRows[3] = GetRow3();
	axColumns[0] = xInput.GetColumn0();
	axColumns[1] = xInput.GetColumn1();
	axColumns[2] = xInput.GetColumn2();
	axColumns[3] = xInput.GetColumn3();

	// Set each element of the result in turn...
	xOutput.SetColumn0(Asura_AVector4(axColumns[0] * axRows[0], axColumns[0] * axRows[1], axColumns[0] * axRows[2], axColumns[0] * axRows[3]));
	xOutput.SetColumn1(Asura_AVector4(axColumns[1] * axRows[0], axColumns[1] * axRows[1], axColumns[1] * axRows[2], axColumns[1] * axRows[3]));
	xOutput.SetColumn2(Asura_AVector4(axColumns[2] * axRows[0], axColumns[2] * axRows[1], axColumns[2] * axRows[2], axColumns[2] * axRows[3]));
	xOutput.SetColumn3(Asura_AVector4(axColumns[3] * axRows[0], axColumns[3] * axRows[1], axColumns[3] * axRows[2], axColumns[3] * axRows[3]));
}

__forceinline void Asura_AMatrix4x4::ApplyToArray(const Asura_AMatrix4x4* pxInput, const u_int uNumElements, Asura_AMatrix4x4* pxOutput) const
{
	Asura_AVector4	axRows[4];

	// Extract the row-data...
	axRows[0] = GetRow0();
	axRows[1] = GetRow1();
	axRows[2] = GetRow2();
	axRows[3] = GetRow3();

	for(u_int u = 0; u < uNumElements; u++, pxInput++, pxOutput++)
	{
		Asura_AVector4	axColumns[4];

		// Extract the column-data...
		axColumns[0] = pxInput->GetColumn0();
		axColumns[1] = pxInput->GetColumn1();
		axColumns[2] = pxInput->GetColumn2();
		axColumns[3] = pxInput->GetColumn3();

		// Set each element of the result in turn...
		pxOutput->SetColumn0(Asura_AVector4(axColumns[0] * axRows[0], axColumns[0] * axRows[1], axColumns[0] * axRows[2], axColumns[0] * axRows[3]));
		pxOutput->SetColumn1(Asura_AVector4(axColumns[1] * axRows[0], axColumns[1] * axRows[1], axColumns[1] * axRows[2], axColumns[1] * axRows[3]));
		pxOutput->SetColumn2(Asura_AVector4(axColumns[2] * axRows[0], axColumns[2] * axRows[1], axColumns[2] * axRows[2], axColumns[2] * axRows[3]));
		pxOutput->SetColumn3(Asura_AVector4(axColumns[3] * axRows[0], axColumns[3] * axRows[1], axColumns[3] * axRows[2], axColumns[3] * axRows[3]));
	}
}

__forceinline float Asura_AMatrix4x4::Cofactor(const u_int uRow, const u_int uCol) const
{
	static const u_int	aauIndices[4][3]	= { {1, 2, 3}, {0, 2, 3}, {0, 1, 3}, {0, 1, 2} };
	const u_int*		puRowsToUse			= aauIndices[uRow];
	const u_int*		puColsToUse			= aauIndices[uCol];

	// Calculate the determinant of the sub-matrix produced by ignoring row uRow and column uCol
	const float	fA	= GetElement(puRowsToUse[0], puColsToUse[0]);
	const float	fB	= GetElement(puRowsToUse[0], puColsToUse[1]);
	const float	fC	= GetElement(puRowsToUse[0], puColsToUse[2]);

	const float	fD	= GetElement(puRowsToUse[1], puColsToUse[0]);
	const float	fE	= GetElement(puRowsToUse[1], puColsToUse[1]);
	const float	fF	= GetElement(puRowsToUse[1], puColsToUse[2]);

	const float	fG	= GetElement(puRowsToUse[2], puColsToUse[0]);
	const float	fH	= GetElement(puRowsToUse[2], puColsToUse[1]);
	const float	fI	= GetElement(puRowsToUse[2], puColsToUse[2]);

	return( ((fA * fE * fI) + (fD * fH * fC) + (fG * fB * fF)) -
		((fA * fH * fF) + (fD * fB * fI) + (fG * fE * fC)) );
}

__forceinline float Asura_AMatrix4x4::Determinant(void) const
{
	const float	fElement00	= GetElement(0, 0);
	const float	fElement01	= GetElement(0, 1);
	const float	fElement02	= GetElement(0, 2);
	const float	fElement03	= GetElement(0, 3);

	const float fCofactor00	= Cofactor(0, 0);
	const float fCofactor01	= Cofactor(0, 1);
	const float fCofactor02	= Cofactor(0, 2);
	const float fCofactor03	= Cofactor(0, 3);

	return((fElement00 * fCofactor00) - (fElement01 * fCofactor01) + (fElement02 * fCofactor02) - (fElement03 * fCofactor03));
}

__forceinline void Asura_AMatrix4x4::Invert(Asura_AMatrix4x4_OutParam xOther) const
{
	const float			fDeterminant		= Determinant();
	const float			fOneOverDeterminant	= 1.0f / fDeterminant;
	Asura_AMatrix4x4	xResult;

	// Calculate using the adjoint matrix approach...
	Asura_Assert(fDeterminant != 0.f, "Trying to calculate the inverse of a non-invertable matrix");
	for(int iRow = 0; iRow < 4; ++iRow)
	{
		for(int iCol = 0; iCol < 4; ++iCol)
		{
			const float	fSign		= (((iRow + iCol) & 1) == 0) ? 1.0f : -1.0f;
			const float	fCofactor	= Cofactor(iRow, iCol);	// note flipped uCol/uRow

			xOther.SetElement(iCol, iRow, fSign * fCofactor * fOneOverDeterminant);
		}
	}
}

__forceinline void Asura_AMatrix4x4::Invert(void)
{
	const Asura_AMatrix4x4	xTemp(*this);

	xTemp.Invert(*this);
}

__forceinline void Asura_AMatrix4x4::InvertOrthogonal(Asura_AMatrix4x4_OutParam xOther) const
{
	// Create the transposed orientation matrix...
	xOther.m_axColumns[0].SetElements(m_axColumns[0].GetX(), m_axColumns[1].GetX(), m_axColumns[2].GetX(), 0.0f);
	xOther.m_axColumns[1].SetElements(m_axColumns[0].GetY(), m_axColumns[1].GetY(), m_axColumns[2].GetY(), 0.0f);
	xOther.m_axColumns[2].SetElements(m_axColumns[0].GetZ(), m_axColumns[1].GetZ(), m_axColumns[2].GetZ(), 0.0f);
	xOther.m_axColumns[3].ZeroW1();

	// ...apply the transpose to the position and subtract this from the final translation...
	xOther.m_axColumns[3]	-= m_axColumns[3] * xOther;
	xOther.m_axColumns[3].SetW(1.0f);
}

__forceinline void Asura_AMatrix4x4::InvertOrthogonal(void)
{
	const Asura_AMatrix4x4	xTemp(*this);

	xTemp.InvertOrthogonal(*this);
}

__forceinline void Asura_AMatrix4x4::Transpose(Asura_AMatrix4x4_OutParam xOther) const
{
	xOther.SetRows(m_axColumns[0], m_axColumns[1], m_axColumns[2], m_axColumns[3]);
}

__forceinline void Asura_AMatrix4x4::Transpose(void)
{
	const Asura_AMatrix4x4	xTemp(*this);

	xTemp.Transpose(*this);
}

__forceinline Asura_AMatrix4x4 Asura_AMatrix4x4::operator*(const float fScale) const
{
	Asura_AMatrix4x4	xTemp(*this);

	xTemp.m_axColumns[0]	*= fScale;
	xTemp.m_axColumns[1]	*= fScale;
	xTemp.m_axColumns[2]	*= fScale;
	xTemp.m_axColumns[3]	*= fScale;
	return(xTemp);
}

__forceinline Asura_AMatrix4x4 Asura_AMatrix4x4::operator/(const float fScale) const
{
	const float			fOneOverScale	= 1.0f / fScale;
	Asura_AMatrix4x4	xTemp(*this);

	xTemp.m_axColumns[0]	*= fOneOverScale;
	xTemp.m_axColumns[1]	*= fOneOverScale;
	xTemp.m_axColumns[2]	*= fOneOverScale;
	xTemp.m_axColumns[3]	*= fOneOverScale;
	return(xTemp);
}

__forceinline void Asura_AMatrix4x4::operator=(Asura_AMatrix4x4_InParam xM)
{
	m_axColumns[0]	= xM.m_axColumns[0];
	m_axColumns[1]	= xM.m_axColumns[1];
	m_axColumns[2]	= xM.m_axColumns[2];
	m_axColumns[3]	= xM.m_axColumns[3];
}

__forceinline void Asura_AMatrix4x4::operator*=(const float fScale)
{
	m_axColumns[0]	*= fScale;
	m_axColumns[1]	*= fScale;
	m_axColumns[2]	*= fScale;
	m_axColumns[3]	*= fScale;
}

__forceinline void Asura_AMatrix4x4::operator/=(const float fScale)
{
	const float	fOneOverScale	= 1.0f / fScale;

	m_axColumns[0]	*= fOneOverScale;
	m_axColumns[1]	*= fOneOverScale;
	m_axColumns[2]	*= fOneOverScale;
	m_axColumns[3]	*= fOneOverScale;
}

__forceinline bool Asura_AMatrix4x4::operator==(Asura_AMatrix4x4_InParam xOther) const
{
	return (m_axColumns[0] == xOther.m_axColumns[0])
		&& (m_axColumns[1] == xOther.m_axColumns[1])
		&& (m_axColumns[2] == xOther.m_axColumns[2])
		&& (m_axColumns[3] == xOther.m_axColumns[3]);
}

__forceinline bool Asura_AMatrix4x4::ApproximatelyEquals(Asura_AMatrix4x4_InParam xOther, const float fEpsilon) const
{
	return (m_axColumns[0].ApproximatelyEquals(xOther.m_axColumns[0], fEpsilon))
		&& (m_axColumns[1].ApproximatelyEquals(xOther.m_axColumns[1], fEpsilon))
		&& (m_axColumns[2].ApproximatelyEquals(xOther.m_axColumns[2], fEpsilon))
		&& (m_axColumns[3].ApproximatelyEquals(xOther.m_axColumns[3], fEpsilon));
}

__forceinline bool Asura_AMatrix4x4::operator!=(Asura_AMatrix4x4_InParam xOther) const
{
	return (m_axColumns[0] != xOther.m_axColumns[0])
		|| (m_axColumns[1] != xOther.m_axColumns[1])
		|| (m_axColumns[2] != xOther.m_axColumns[2])
		|| (m_axColumns[3] != xOther.m_axColumns[3]);
}

__forceinline float Asura_AMatrix4x4::GetElement(const u_int uRow, const u_int uCol) const
{
	Asura_Assert(uRow <= 3, "Invalid matrix row index passed to Asura_AMatrix4x4::GetElement");
	Asura_Assert(uCol <= 3, "Invalid matrix column index passed to Asura_AMatrix4x4::GetElement");
	return(m_axColumns[uCol].GetElement(uRow));
}

__forceinline void Asura_AMatrix4x4::SetElement(const u_int uRow, const u_int uCol, const float fValue)
{ 
	Asura_Assert(uRow <= 3, "Invalid matrix row index passed to Asura_AMatrix4x4::SetElement");
	Asura_Assert(uCol <= 3, "Invalid matrix column index passed to Asura_AMatrix4x4::SetElement");
	m_axColumns[uCol].SetElement(uRow, fValue);
}

__forceinline Asura_AVector4 Asura_AMatrix4x4::GetColumn(const u_int uColumnIndex) const
{
	Asura_Assert(uColumnIndex <= 3, "Invalid matrix column index passed to Asura_AMatrix4x4::GetColumn");
	return m_axColumns[uColumnIndex];
}

__forceinline Asura_AVector4 Asura_AMatrix4x4::GetColumn0() const
{
	return m_axColumns[0];
}
__forceinline Asura_AVector4 Asura_AMatrix4x4::GetColumn1() const
{
	return m_axColumns[1];
}
__forceinline Asura_AVector4 Asura_AMatrix4x4::GetColumn2() const
{
	return m_axColumns[2];
}
__forceinline Asura_AVector4 Asura_AMatrix4x4::GetColumn3() const
{
	return m_axColumns[3];
}

__forceinline void Asura_AMatrix4x4::SetColumn(const u_int uColumnIndex, Asura_AVector4_InParam xColumn)
{
	Asura_Assert(uColumnIndex <= 3, "Invalid matrix column index passed to Asura_AMatrix4x4::SetColumn");
	m_axColumns[uColumnIndex] = xColumn;
}

__forceinline void Asura_AMatrix4x4::SetColumn0(Asura_AVector4_InParam xCol0)
{
	m_axColumns[0] = xCol0;
}

__forceinline void Asura_AMatrix4x4::SetColumn1(Asura_AVector4_InParam xCol1)
{
	m_axColumns[1] = xCol1;
}

__forceinline void Asura_AMatrix4x4::SetColumn2(Asura_AVector4_InParam xCol2)
{
	m_axColumns[2] = xCol2;
}

__forceinline void Asura_AMatrix4x4::SetColumn3(Asura_AVector4_InParam xCol3)
{
	m_axColumns[3] = xCol3;
}

__forceinline Asura_AVector4 Asura_AMatrix4x4::GetRow(const u_int uRowIndex) const
{
	Asura_Assert(uRowIndex <= 3, "Invalid matrix row index passed to Asura_AMatrix4x4::GetRow");
	return Asura_AVector4(m_axColumns[0].GetElement(uRowIndex), m_axColumns[1].GetElement(uRowIndex),
						  m_axColumns[2].GetElement(uRowIndex), m_axColumns[3].GetElement(uRowIndex));
}

__forceinline Asura_AVector4 Asura_AMatrix4x4::GetRow0() const
{
	return Asura_AVector4(m_axColumns[0].GetElement(0), m_axColumns[1].GetElement(0),
						  m_axColumns[2].GetElement(0), m_axColumns[3].GetElement(0));
}
__forceinline Asura_AVector4 Asura_AMatrix4x4::GetRow1() const
{
	return Asura_AVector4(m_axColumns[0].GetElement(1), m_axColumns[1].GetElement(1),
						  m_axColumns[2].GetElement(1), m_axColumns[3].GetElement(1));
}
__forceinline Asura_AVector4 Asura_AMatrix4x4::GetRow2() const
{
	return Asura_AVector4(m_axColumns[0].GetElement(2), m_axColumns[1].GetElement(2),
						  m_axColumns[2].GetElement(2), m_axColumns[3].GetElement(2));
}
__forceinline Asura_AVector4 Asura_AMatrix4x4::GetRow3() const
{
	return Asura_AVector4(m_axColumns[0].GetElement(3), m_axColumns[1].GetElement(3),
						  m_axColumns[2].GetElement(3), m_axColumns[3].GetElement(3));
}

__forceinline void Asura_AMatrix4x4::SetRow(const u_int uRowIndex, Asura_AVector4_InParam xRow)
{
	Asura_Assert(uRowIndex <= 3, "Invalid matrix row index passed to Asura_AMatrix4x4::SetRow");
	m_axColumns[0].SetElement(uRowIndex, xRow.GetElement(0));
	m_axColumns[1].SetElement(uRowIndex, xRow.GetElement(1));
	m_axColumns[2].SetElement(uRowIndex, xRow.GetElement(2));
	m_axColumns[3].SetElement(uRowIndex, xRow.GetElement(3));
}

__forceinline void Asura_AMatrix4x4::SetRow0(Asura_AVector4_InParam xRow0)
{
	m_axColumns[0].SetElement(0, xRow0.GetElement(0));
	m_axColumns[1].SetElement(0, xRow0.GetElement(1));
	m_axColumns[2].SetElement(0, xRow0.GetElement(2));
	m_axColumns[3].SetElement(0, xRow0.GetElement(3));
}

__forceinline void Asura_AMatrix4x4::SetRow1(Asura_AVector4_InParam xRow1)
{
	m_axColumns[0].SetElement(1, xRow1.GetElement(0));
	m_axColumns[1].SetElement(1, xRow1.GetElement(1));
	m_axColumns[2].SetElement(1, xRow1.GetElement(2));
	m_axColumns[3].SetElement(1, xRow1.GetElement(3));
}

__forceinline void Asura_AMatrix4x4::SetRow2(Asura_AVector4_InParam xRow2)
{
	m_axColumns[0].SetElement(2, xRow2.GetElement(0));
	m_axColumns[1].SetElement(2, xRow2.GetElement(1));
	m_axColumns[2].SetElement(2, xRow2.GetElement(2));
	m_axColumns[3].SetElement(2, xRow2.GetElement(3));
}

__forceinline void Asura_AMatrix4x4::SetRow3(Asura_AVector4_InParam xRow3)
{
	m_axColumns[0].SetElement(3, xRow3.GetElement(0));
	m_axColumns[1].SetElement(3, xRow3.GetElement(1));
	m_axColumns[2].SetElement(3, xRow3.GetElement(2));
	m_axColumns[3].SetElement(3, xRow3.GetElement(3));
}

__forceinline void Asura_AMatrix4x4::LoadColsAligned(const float* pfFloats)
{
	m_axColumns[0].LoadAligned(&pfFloats[0]);
	m_axColumns[1].LoadAligned(&pfFloats[4]);
	m_axColumns[2].LoadAligned(&pfFloats[8]);
	m_axColumns[3].LoadAligned(&pfFloats[12]);
}

__forceinline void Asura_AMatrix4x4::LoadRowsAligned(const float* pfFloats) 
{
	LoadColsAligned(pfFloats);
	Transpose();
}

__forceinline void Asura_AMatrix4x4::LoadColsUnaligned(const float* pfFloats)
{
	m_axColumns[0].LoadUnaligned(&pfFloats[0]);
	m_axColumns[1].LoadUnaligned(&pfFloats[4]);
	m_axColumns[2].LoadUnaligned(&pfFloats[8]);
	m_axColumns[3].LoadUnaligned(&pfFloats[12]);
}

__forceinline void Asura_AMatrix4x4::LoadRowsUnaligned(const float* pfFloats)
{
	LoadColsUnaligned(pfFloats);
	Transpose();
}

__forceinline void Asura_AMatrix4x4::StoreColsAligned(float* pfFloats) const
{
	m_axColumns[0].StoreAligned(&pfFloats[0]);
	m_axColumns[1].StoreAligned(&pfFloats[4]);
	m_axColumns[2].StoreAligned(&pfFloats[8]);
	m_axColumns[3].StoreAligned(&pfFloats[12]);
}

__forceinline void Asura_AMatrix4x4::StoreRowsAligned(float* pfFloats) const
{
	Asura_AMatrix4x4	xTemp;

	Transpose(xTemp);
	xTemp.StoreColsAligned(pfFloats);
}

__forceinline void Asura_AMatrix4x4::StoreColsUnaligned(float* pfFloats) const
{
	m_axColumns[0].StoreUnaligned(&pfFloats[0]);
	m_axColumns[1].StoreUnaligned(&pfFloats[4]);
	m_axColumns[2].StoreUnaligned(&pfFloats[8]);
	m_axColumns[3].StoreUnaligned(&pfFloats[12]);
}

__forceinline void Asura_AMatrix4x4::StoreRowsUnaligned(float* pfFloats) const
{
	Asura_AMatrix4x4	xTemp;

	Transpose(xTemp);
	xTemp.StoreColsUnaligned(pfFloats);
}

#endif


/// End of file. ///
