//
// Asura_PlatformMaths.h
//
// Created by KJFL - Wednesday, February 04, 2004
//
// Copyright (C) 2004 Rebellion, All Rights Reserved.
//

#ifdef ASURA_USE_PRAGMA_ONCE
#pragma once
#endif

#ifndef __ASURA_PLATFORMMATHS_H__
#define __ASURA_PLATFORMMATHS_H__

// I N C L U D E S ///////////////////////////////////////////////////////////

#include <limits.h>
#include <math.h>
#include <psptypes.h>

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

/*
	Here '_ALIGNED' means that 16-byte aligned, else
	load or store elementwise.
*/

#define	ASURA_PSP_AV3_ALIGNED				1
#undef	ASURA_PSP_Q4_ALIGNED

#if !defined ASURA_PSP_AV3_ALIGNED
	#error AVector3 should really be aligned on PSP.
#endif

#if defined ASURA_PSP_Q4_ALIGNED
	#error Quat cannot be aligned yet.
#endif

// RJT NOTE: Certain functions in this file don't compile correctly with GCC4 (psp-gcc 2.5.2 anyhow)
// future revisions may correct this in which case this define can be removed
#if defined __GNUC__ && (__GNUC__ > 3 )
	#define GCC4_BUGGED
#endif

#if defined __SNC__
typedef int SNMatrixType __attribute__((mode(MF),aligned(64)));
typedef int SNVectorType __attribute__((mode(VF),aligned(16)));
typedef int SNScalarType __attribute__((mode(VS)));
struct SNPokeVector { float x,y,z,w; } __attribute__((aligned(16))) ;
#endif

// F U N C T I O N   D E F I N I T I O N S ///////////////////////////////////

// return x raised to the y'th power
static inline float Asura_Maths::Power(float x, float y)
{
	// assume powf exists. if not then that platform is going to have to come up with an alternative
	return powf(x, y);
}

static inline Asura_Angle Asura_Maths::DegToRad(register const float deg)
{
	return (deg*DegToRadConstant);
}

static inline float Asura_Maths::RadToDeg(register const Asura_Angle rad)
{
	return (rad*RadToDegConstant);
}

// Kevin Lea - 10/09/2001 10:38:13 - max & min inlines
template< typename T >
static inline T Asura_Maths::Max( register const T& tA,register const T& tB )
{
#if defined __GNUC__ && !defined __SNC__
    return tA >? tB;
#else
	return (tA>tB ? tA:tB);
#endif
}

template< typename T >
static inline T Asura_Maths::Min( register const T& tA,register const T& tB )
{
#if defined __GNUC__ && !defined __SNC__
    return tA <? tB;
#else
	return (tA<tB ? tA:tB);
#endif
}

static inline float Asura_Maths::Abs(register const float f)
{
#if defined __SNC__
	return __builtin_allegrex_abs_s(f);
#else
	register Asura_IntOrFloat a;
	a.F = f;

	/// Kevin Lea - 04/02/2002 14:36:52 - mask out sign bit
	a.I &= 0x7fffffff;

	return a.F;
#endif
}

static inline int Asura_Maths::Abs(register const int a)
{
    return (a >= 0) ? a : -a;
}

static inline float Asura_Maths::Sign(register const float f)
{
#if defined __GNUC__ && ! defined __SNC__
    /* The assumption here is that we want to avoid branching */
    register const vfpu_v4 afT = vfpu_v4_s(f);
    return vfpu_extract_float_from_v4x(vfpu_vsgn_s(afT,0));
#else
    return (f > 0.0f) ? 1.0f : (f < 0.0f) ? -1.0f : f;
#endif
}

static inline int Asura_Maths::Sign(register const int a)
{
    return (a > 0) ? 1 : (a < 0) ? -1 : a;
}


static inline u_int Asura_Maths::GetFloorLog2( u_int uValue )
{
	u_int uLog2 = 0;
	u_int uNext = 2;

	while( uNext <= uValue )
	{
		uNext <<= 1;
		++uLog2;
	}

	return uLog2;
}

static inline u_int Asura_Maths::GetCeilLog2( u_int uValue )
{
	u_int uLog2 = 0;
	u_int uCmp  = 1;

	while( uCmp < uValue )
	{
		uCmp <<= 1;
		++uLog2;
	}

	return uLog2;
}

static inline u_int Asura_Maths::GetFloorPow2( u_int uValueOld )
{
	if( uValueOld <= 1)
	{
		return( uValueOld);
	}

	u_int uNShift = 0;
	u_int uValue = uValueOld;

	for( ; uValue != 0; uValue >>= 1, uNShift++);

	u_int uValueNew = 1 << (uNShift - 1);
	if( uValueOld < uValueNew)
	{
		uValueNew >>= 1;
	}

	return( uValueNew);
}

static inline u_int Asura_Maths::GetCeilPow2( u_int uValueOld )
{
	if( uValueOld <= 1)
	{
		return( uValueOld);
	}

	u_int uNShift = 0;
	u_int uValue = uValueOld;

	for( ; uValue != 0; uValue >>= 1, uNShift++);

	u_int uValueNew = 1 << (uNShift - 1);
	if( uValueNew < uValueOld)
	{
		uValueNew <<= 1;
	}

	return( uValueNew);
}

/// David Evans - 22/08/2002 16:10:20 - Mathematical functions, moved
/// from Asura_Maths.cpp

/*
	The PSP deals with angles in gradians, a French and British
	military measure. 100 gradians == 90deg == PI/2 radians.

	http://MathForum.org/library/drmath/view/55451.html
	http://en.WikiPedia.org/wiki/Gradians
*/

static inline float Asura_Maths::Sine(register const Asura_Angle fAngle)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	SNScalarType xTemp;
	SNScalarType xAngle = __builtin_ftovs(fAngle);

	_asm
	{
		vcst.s xTemp, VFPU_2_PI
		vmul.s xAngle, xAngle, xTemp
		vsin.s xTemp, xAngle
	};

	return __builtin_vstof(xTemp);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER
    register const vfpu_v4 af2PI = vfpu_vcst_s(VFPU_CONST_2_PI,0);
    register vfpu_v4 afT = vfpu_vmul_s(vfpu_v4_s(fAngle),af2PI,0);

    return vfpu_extract_float_from_v4x(vfpu_vsin_s(afT,0));
#elif defined __GNUC__ && !defined __SNC__
    return __builtin_sinf(fAngle);
#else
	return (sinf(fAngle));
#endif
}

static inline float Asura_Maths::Cosine(register const Asura_Angle fAngle)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	SNScalarType xTemp;
	SNScalarType xAngle = __builtin_ftovs(fAngle);

	_asm
	{
		vcst.s xTemp, VFPU_2_PI
		vmul.s xAngle, xAngle, xTemp
		vcos.s xTemp, xAngle
	};

	return __builtin_vstof(xTemp);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER
    register const vfpu_v4 af2PI = vfpu_vcst_s(VFPU_CONST_2_PI,0);
    register vfpu_v4 afT = vfpu_vmul_s(vfpu_v4_s(fAngle),af2PI,0);

    return vfpu_extract_float_from_v4x(vfpu_vcos_s(afT,0));
#elif defined __GNUC__ && !defined __SNC__
    return __builtin_cosf(fAngle);
#else
	return (cosf(fAngle));
#endif
}

static inline float Asura_Maths::Tangent(register const Asura_Angle fAngle)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	// TODO: use vrot, but we need subobject access
	SNScalarType xTemp1;
	SNScalarType xTemp2;
	SNScalarType xAngle = __builtin_ftovs(fAngle);

	_asm
	{
		vcst.s xTemp1, VFPU_2_PI
		vmul.s xAngle, xAngle, xTemp1
		vsin.s xTemp1, xAngle
		vcos.s xTemp2, xAngle
		vdiv.s xTemp1, xTemp1, xTemp2	// tan = sin/cos
	};

	return __builtin_vstof(xTemp1);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER
    register const vfpu_v4 af2PI = vfpu_vcst_s(VFPU_CONST_2_PI,0);
    register const vfpu_v4 afT = vfpu_vmul_s(vfpu_v4_s(fAngle),af2PI,0);

    register const vfpu_v4 afS = vfpu_vsin_s(afT,0);
    register const vfpu_v4 afC = vfpu_vcos_s(afT,0);

    return vfpu_extract_float_from_v4x(vfpu_vdiv_s(afS,afC,0));
#elif defined __GNUC__ && !defined __SNC__
    return __builtin_tanf(fAngle);
#else
	return (tanf(fAngle));
#endif
}

static inline void Asura_Maths::SinCos(register const Asura_Angle fAngle,
    float& fOutputSine, float& fOutputCos)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
  	SNScalarType xTemp1;
  	SNScalarType xAngle = __builtin_ftovs(fAngle);

  	_asm
  	{
  		vcst.s xTemp1, VFPU_2_PI
  		vmul.s xAngle, xAngle, xTemp1
  		vrot.p c700, xAngle, 1			// c700 = (sin, cos)
  		sv.s s700, 0(&fOutputSine)
		sv.s s701, 0(&fOutputCos)
  	};
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER
	#if defined ASURA_PSP_GCC_BUILTINS
		register const vfpu_v4 af2PI = vfpu_vcst_s(VFPU_CONST_2_PI,0);

		register const vfpu_v4 afT = vfpu_vmul_s(vfpu_v4_s(fAngle),af2PI,0);
		register const vfpu_v4 afS = vfpu_vsin_s(afT,0);
		register const vfpu_v4 afC = vfpu_vcos_s(afT,0);

		fOutputSine = vfpu_extract_float_from_v4x(afS);
		fOutputCos = vfpu_extract_float_from_v4x(afC);
	#else
		vfpu_v4 af2PI;
		vfpu_v4 afAngle;
		vfpu_v4 afSin;
		vfpu_v4 afCos;

		__asm__ (
			"mtv %6, %x3 \n"

			// Store 2 / Pi *WARNING* This is slightly different to the
			// Pi approximation used in Asura normally, so if values are
			// looking too different we need to load in our own value.
			"vcst.s %x2, VFPU_2_PI \n"

			 // Cos, Sin use 0..1 to correspond to our original angle
			 // 0..Pi/2, so scale our angles by 2/Pi
			"vmul.s %x3, %x3, %x2 \n"

			"vsin.s %x4, %x3 \n" // sin(fAngle)
			"vcos.s %x5, %x3 \n" // cos(fAngle)

			"mfv %0, %x4 \n" // Save sin(fAngle)
			"mfv %1, %x5 \n" // Save cos(fAngle)

			: "=r" (fOutputSine), "=r" (fOutputCos),
			  "=r" (af2PI), "=r" (afAngle),
			  "=r" (afSin), "=r" (afCos)
			: "r" (fAngle)
		);
	#endif
#elif defined __GNUC__ && !defined __SNC__
	fOutputSine = __builtin_sinf(fAngle);
	fOutputCos = __builtin_cosf(fAngle);
#else
	fOutputSine = sinf(fAngle);
	fOutputCos = cosf(fAngle);
#endif
}

static inline void Asura_Maths::SinCos2(
	register const Asura_Angle fAngle1,
	register const Asura_Angle fAngle2,
	float& fOutputSine1,float& fOutputCos1,
    float& fOutputSine2,float& fOutputCos2
)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	_asm
    {
		mfc1 $t0, fAngle1
		mtv $t0, s702
		mfc1 $t0, fAngle2
		mtv $t0, s712

		vcst.s s703, VFPU_2_PI
		vscl.p r702, r702, s703
		vsin.p r700, r702
		vcos.p r701, r702

		sv.s s700, 0(&fOutputSine1)
		sv.s s701, 0(&fOutputCos1)
		sv.s s710, 0(&fOutputSine2)
		sv.s s711, 0(&fOutputCos2)
	};
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_v4 af2PI = vfpu_vcst_s(VFPU_CONST_2_PI,0);
	register const vfpu_v4 afAngles
		= vfpu_insert_float_into_v4xy(afAngles,fAngle1,fAngle2);

	register const vfpu_v4 afT = vfpu_vscl_p(afAngles,af2PI,0);
	register const vfpu_v4 afS = vfpu_vsin_p(afT,0);
	register const vfpu_v4 afC = vfpu_vcos_p(afT,0);

	fOutputSine1 = vfpu_extract_float_from_v4x(afS);
	fOutputSine2 = vfpu_extract_float_from_v4y(afS);
	fOutputCos1 = vfpu_extract_float_from_v4x(afC);
	fOutputCos2 = vfpu_extract_float_from_v4y(afC);
#elif defined __GNUC__ && !defined __SNC__
	fOutputSine1 = __builtin_sinf(fAngle1);
	fOutputSine2 = __builtin_sinf(fAngle2);
	fOutputCos1 = __builtin_cosf(fAngle1);
	fOutputCos2 = __builtin_cosf(fAngle2);
#else
	fOutputSine1 = sinf(fAngle1); fOutputCos1 = cosf(fAngle1);
	fOutputSine2 = sinf(fAngle2); fOutputCos2 = cosf(fAngle2);
#endif
}

static inline Asura_Angle Asura_Maths::ArcSine(register const float fSine)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	SNScalarType xTemp1;
	SNScalarType xTemp2 = __builtin_ftovs(fSine);

	_asm
	{
		vsat1.s xTemp2, xTemp2			// clamp to range [-1,1]
		vasin.s xTemp1, xTemp2			// temp1 is arcsin
		vcst.s xTemp2, VFPU_PI_2		// load scale factor (Pi/2) in temp2
		vmul.s xTemp1, xTemp1, xTemp2	// scale angle
	};

	return __builtin_vstof(xTemp1);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER
	register const vfpu_v4 af2PI = vfpu_vcst_s(VFPU_CONST_PI_2,0);
	// clamp to range [-1,1]
	register const vfpu_v4 afSine = vfpu_vsat1_s(vfpu_v4_s(fSine),0);
	register const vfpu_v4 afAngleHG = vfpu_vasin_s(afSine,0);

	return vfpu_extract_float_from_v4x(vfpu_vmul_s(afAngleHG,af2PI,0));
#else
	return static_cast<Asura_Angle>
		(asinf(Asura_Maths::ClampToRange(fSine, -1.f, 1.f)));
#endif
}

static inline Asura_Angle Asura_Maths::ArcCosine(register const float fCosine)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	SNScalarType xTemp1;
	SNScalarType xTemp2 = __builtin_ftovs(fCosine);

	_asm
	{
		vsat1.s xTemp2, xTemp2			// clamp to range [-1,1]
		vasin.s xTemp1, xTemp2			// temp1 is arcsin
		vcst.s xTemp2, VFPU_PI_2		// load scale factor (Pi/2) in temp2
		vocp.s xTemp1, xTemp1			// in hectogradians, acos = 1 - asin
		vmul.s xTemp1, xTemp1, xTemp2	// scale angle
	};

	return __builtin_vstof(xTemp1);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER
	register const vfpu_v4 af2PI = vfpu_vcst_s(VFPU_CONST_PI_2,0);
	// clamp to range [-1,1]
	register const vfpu_v4 afCosine = vfpu_vsat1_s(vfpu_v4_s(fCosine),0);
	// in hectogradians, acos = 1 - asin
	register const vfpu_v4 afAngleHG = vfpu_vocp_s(vfpu_vasin_s(afCosine,0),0);

	return vfpu_extract_float_from_v4x(vfpu_vmul_s(afAngleHG,af2PI,0));
#else
	return static_cast<Asura_Angle>
		(acosf(Asura_Maths::ClampToRange(fCosine, -1.f, 1.f)));
#endif
}

/*
	ArcTangent(y/x). The magnitude and sign of the result depends on
	the magnitude and sign of both x and y. So while atan(1) is the
	same as atan2(1,1), and is PI/4, atan2(-1,-1) is -3*pi/4.

    http://www.cs.cmu.edu/Groups/AI/html/cltl/clm/node128.html

	The IEEE floating people people have decided the following special
	cases, counterclockwise:

		origin			y = 0	x = 0		NaN
		right X axis:	y = 0	x > 0		0
		1st quadrant:	y > 0	x > 0		0 < a < PI/2

		upper y axis:	y > 0	x = 0		PI/2
		2nd quadrant:	y > 0	x < 0		PI/2 < a < PI
		left x axis:	y = 0	x < 0		PI

		3rd quadrant:	y < 0	x < 0		-PI < a < -PI/2
		lower y axis:	y < 0	x = 0		-PI/2
		4th quadrant:	y < 0	x > 0		-PI/2 < a < 0

						y = +0	x = +0		+0
						y = -0 	x = +0		-0
						y = +0	x = -0		PI
						y = -0	x = -0		-PI
*/

#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	extern float atan2f_atan66vfpu(float y,float x);
#elif 0 && defined ASURA_PSP_USE_VFPU_ASSEMBLER
	extern float atan2f_gccvfpu(float y,float x);
#elif 0
	extern float atan2f_sunpro(float y,float x);
#endif

static inline Asura_Angle Asura_Maths::ArcTangent(
	const float y,const float x
)
{
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
    return atan2f_atan66vfpu(y,x);
#elif 0 && defined ASURA_PSP_USE_VFPU_ASSEMBLER
    return atan2f_gccvfpu(y,x);
#elif 0
    return atan2f_sunpro(y,x);
#else
	return static_cast<Asura_Angle>(atan2f(y,x));
#endif
}

static inline float Asura_Maths::Sqrt(register const float f)
{
	return __builtin_allegrex_sqrt_s(f);
}

static inline float Asura_Maths::InvSqrt(register const float f)
{
	register const float m = __builtin_allegrex_sqrt_s(f);

	return 1.0f/m;
}

/// David Evans - 22/08/2002 15:36:03 - Conversion functions - moved
/// from Asura_Maths.cpp

static inline int Asura_Maths::FloatToInt(register const float f)
{
	// KJFL - 2004/02/04 14:32:01 - TODO

	return static_cast< int >( f > 0.0f ? f + 0.5f : f - 0.5f );//(int)f;
}

static inline int Asura_Maths::FloatToInt_Near(register const float f)
{
	// KJFL - 2004/02/04 14:32:32 - TODO

	return static_cast< int >( f > 0.0f ? f + 0.5f : f - 0.5f );//(int)f;
}

/// David Evans - 22/08/2002 10:14:09 - Returns the integer part of the float
static inline int Asura_Maths::FloatToInt_Chop(float f)
{
	// AW - 08/04/04 - TODO. Will hardware round differently to
	// emulator? Is there a faster way?
#if 0
    return static_cast<int>( f > 0.0f ? f - 0.5f : f + 0.5f );
#else
	return static_cast<int>(f);
#endif
}

// (MH - 18.04.2002) - since vararg functions implicitly convert floats
// to doubles, (as well as chars to ints) centralise the conversion
// here. This prevents a warning every time vararg functions are used in
// the code.
static inline double Asura_Maths::FtoD(register const float f)
{
	return static_cast<double>(f);
}

static inline float Asura_Maths::CalculateTriArea(const Asura_Vector_3& xV0,
    const Asura_Vector_3& xV1, const Asura_Vector_3& xV2 )
{
	return( 0.5f * ((xV1 - xV0) ^ (xV2 - xV0)).Magnitude() );
}

// KOR - 28/11/2005 - floating point modulo (remainder of fX/fY); should
// be fmodf() on most platforms
static inline float Asura_Maths::FloatMod(float fX, float fY)
{
	Asura_Assert(Asura_Maths::Abs(fY) > 1.0e-30f, "tiny or zero (invalid) argument to fmodf");
	return fmodf(fX, fY);
}

// KOR - 28/11/2005 - Returns f where f is in the range [fMin, fMax) and
// f = fAngle mod (fMax - fMin)
static inline float Asura_Maths::AngleToRange(register const float fAngle,
	register const float fMin, register const float fMax)
{
	register const float fRange = fMax - fMin;

	// This gets us a fReturnAngle in the range (-fRange, fRange) where
	// fReturnAngle = fAngle mod fRange
	register const float fReturnAngle = FloatMod(fAngle, fRange);

	if (fReturnAngle < fMin)
	{
		// This deals with fReturnAngle in (-fRange, fMin)
		return fReturnAngle + fRange;
	}
	if (fReturnAngle >= fMax)
	{
		// This deals with fReturnAngle in [fMax, fRange)
		return fReturnAngle - fRange;
	}

	// Otherwise fReturnAngle is in [fMin, fMax) as desired.
	return fReturnAngle;
}

static inline float Asura_Maths::RadiansToZeroCentredRange(register const float fRadians)
{
	return AngleToRange(fRadians, -Pi, Pi);
}

static inline float Asura_Maths::RadiansToZeroMinimumRange(register const float fRadians)
{
	return AngleToRange(fRadians, 0.f, Pi * 2.f);
}

static inline bool Asura_Maths::IsNaN ( const float fNum )
{
	// If this platform conforms to IEEE754 try...
	//return (fNum != fNum);
	Asura_Assert( false, "Function not written" );
	return false;
}


// A S U R A _ M A T R I X _ 3 X 3 ///////////////////////////////////////////

/*
	Pseudo intrinsics for loading and saving unaligned 3-vectors
	and 3-matrices into VFPU 4-registers and 4x4registers.

	In the 'Load' and 'Save' functions below each element's load or
	store could as well be a separate statement, to signify that the
	order does not matter semantically, and that the compiler might
	rearrange the order.

	But on the other hand we may prefer to enforce an ordering by
	increasing memory address (unaligned matrices are stored rowwise).
*/

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
static inline vfpu_v4 vfpu_v4_load3(register const Asura_Vector_3& xV3)
{
	register vfpu_v4 afV;
	return vfpu_insert_float_into_v4xyz(afV,xV3.x,xV3.y,xV3.z);
}

static inline Asura_Vector_3 vfpu_v4_save3(register const vfpu_v4& afV,
	register Asura_Vector_3& xV3)
{
	xV3.x = vfpu_extract_float_from_v4x(afV);
	xV3.y = vfpu_extract_float_from_v4y(afV);
	xV3.z = vfpu_extract_float_from_v4z(afV);

	return xV3;
}

/*
	Unaligned 4-vectors load and save, a bit sad :-)
*/

static inline vfpu_v4 vfpu_v4_loadq(register const Asura_Quat& xQ)
{
#if defined ASURA_PSP_Q4_ALIGNED
	#error Aligned 'Quat' load not tested, and probably redundant.
	return xQ.m_afVec;
#else
	return vfpu_assign_float_to_v4xyzw(xQ.x,xQ.y,xQ.z,xQ.w);
#endif
}

static inline Asura_Quat vfpu_v4_saveq(register const vfpu_v4& afV,
	register Asura_Quat& xQ)
{
#if defined ASURA_PSP_Q4_ALIGNED
	#error Aligned 'Quat' save not tested, and probably redundant.
	xQ.m_afVec = afV;
#else
	xQ.x = vfpu_extract_float_from_v4x(afV);
	xQ.y = vfpu_extract_float_from_v4y(afV);
	xQ.z = vfpu_extract_float_from_v4z(afV);
	xQ.w = vfpu_extract_float_from_v4w(afV);
#endif
	return xQ;
}

static inline vfpu_m4x4 vfpu_m4x4_load3x3(register const Asura_Matrix_3x3& xM3)
{
	register vfpu_m4x4 aafM;

	{
		register vfpu_v4 afV;
		aafM = vfpu_insert_v4_into_m4x(aafM,
				vfpu_insert_float_into_v4xyz(afV,xM3.e11,xM3.e12,xM3.e13));
	}

	{
		register vfpu_v4 afV;
		aafM = vfpu_insert_v4_into_m4y(aafM,
				vfpu_insert_float_into_v4xyz(afV,xM3.e21,xM3.e22,xM3.e23));
	}

	{
		register vfpu_v4 afV;
		aafM = vfpu_insert_v4_into_m4z(aafM,
				vfpu_insert_float_into_v4xyz(afV,xM3.e31,xM3.e32,xM3.e33));
	}

	return aafM;
}

static inline Asura_Matrix_3x3 vfpu_m4x4_save3x3(
	register const vfpu_m4x4& aafM,register Asura_Matrix_3x3& xM3
)
{
	{
		register const vfpu_v4 afV = vfpu_extract_v4_from_m4x(aafM);
		xM3.e11 = vfpu_extract_float_from_v4x(afV);
		xM3.e12 = vfpu_extract_float_from_v4y(afV);
		xM3.e13 = vfpu_extract_float_from_v4z(afV);
	}

	{
		register const vfpu_v4 afV = vfpu_extract_v4_from_m4y(aafM);
		xM3.e21 = vfpu_extract_float_from_v4x(afV);
		xM3.e22 = vfpu_extract_float_from_v4y(afV);
		xM3.e23 = vfpu_extract_float_from_v4z(afV);
	}

	{
		register const vfpu_v4 afV = vfpu_extract_v4_from_m4z(aafM);
		xM3.e31 = vfpu_extract_float_from_v4x(afV);
		xM3.e32 = vfpu_extract_float_from_v4y(afV);
		xM3.e33 = vfpu_extract_float_from_v4z(afV);
	}

	return xM3;
}

/*
	We use this in 'Asura_PSP_VectorMaths.h' too, and we rely on
	it being included below and not above.
*/
static inline vfpu_v4 vfpu_v4s_inv(register const vfpu_v4 afS)
{
#if ASURA_PSP_GCC_RECIPROCAL
		return vfpu_vrcp_s(afS,0);
#else
		return vfpu_vdiv_s(vfpu_v4(1.0f),afS,0);
#endif
}
#endif

inline void Asura_Matrix_3x3::Transpose(void)
{
	/*
		Unaligned matrices are stored rowwise, so the order in memory is
		[11,12,13, 21,22,23, 31,32,33] and we need to swap [12,13,23]
		(above the diagonal) with [21,31,32] (below the diagonal).

		Values on the diagonal do not need to be moved, so we need to do
		only 3 swaps. Using the VFPU for unaligned transpose may be a bit
		overkill, but it may relieve some pressure on the CPU registers.
	*/
#if defined ASURA_PSP_ASM_USE_SN_SYNTAX
	SNScalarType t12,t13,t21;
	SNScalarType t23,t31,t32;

	_asm
	{	
		/* Fetch them in memory address order */
		lv.s t12, 4(this->Element)	// e12
		lv.s t13, 8(this->Element)	// e13
		lv.s t21, 12(this->Element)	// e21

		lv.s t23, 20(this->Element)	// e23
		lv.s t31, 24(this->Element)	// e31
		lv.s t32, 28(this->Element)	// e32

		/* Store them back in memory address order */
		sv.s t32, 20(this->Element)	// e23
		sv.s t13, 24(this->Element)	// e31
		sv.s t23, 28(this->Element)	// e32

		sv.s t21, 4(this->Element)	// e12
		sv.s t31, 8(this->Element)	// e13
		sv.s t12, 12(this->Element)	// e21
	}
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	/* Fetch them in memory address order */
	vfpu_v4 afT_12_13_21
		= vfpu_insert_float_into_v4xyz(afT_12_13_21,e12,e13,e21);
	vfpu_v4 afT_23_31_32
		= vfpu_insert_float_into_v4xyz(afT_23_31_32,e23,e31,e32);

	/* Store them back in memory address order */
	e23 = vfpu_extract_float_from_v4z(afT_23_31_32);
	e31 = vfpu_extract_float_from_v4y(afT_12_13_21);
	e32 = vfpu_extract_float_from_v4x(afT_23_31_32);

	e12 = vfpu_extract_float_from_v4z(afT_12_13_21);
	e13 = vfpu_extract_float_from_v4y(afT_23_31_32);
	e21 = vfpu_extract_float_from_v4x(afT_12_13_21);
#else
	float t;

	t = e12;
	e12 = e21;
	e21 = t;

	t = e13;
	e13 = e31;
	e31 = t;

	t = e23;
	e23 = e32;
	e32 = t;
#endif
}

inline Asura_Vector_3 Asura_Matrix_3x3::ApplyTranspose(const Asura_Vector_3& xVec) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	Asura_Vector_3 xResult;

	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load matrix elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"lv.s s710, 12+%1 \n"
		"lv.s s711, 16+%1 \n"
		"lv.s s712, 20+%1 \n"

		"lv.s s720, 24+%1 \n"
		"lv.s s721, 28+%1 \n"
		"lv.s s722, 32+%1 \n"

		"lv.s s730, 0+%2 \n" // Load vector elements
		"lv.s s731, 4+%2 \n"
		"lv.s s732, 8+%2 \n"

         // Apply matrix. Matrix read in horizontally, but we want the
         // transpose so apply reading vertically (e*** instead of m***)
		"vtfm3.t r703, e700, c730 \n"

        // grr, no simple way to store triples, save out one by one
		"sv.s s703, 0+%0 \n"
		"sv.s s713, 4+%0 \n"
		"sv.s s723, 8+%0 \n"

		: "=r" (&xResult)
		: "r" (this), "r" (&xVec)
	);

	return xResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register Asura_Vector_3 xResult;
	register const vfpu_m4x4 aafThis = vfpu_m4x4_load3x3(*this);
	register const vfpu_v4 afV = vfpu_v4_load3(xVec);
	register vfpu_v4 afR;

	/* No 3x3 intrinsics, so we can use only inline asm here */

	__asm__
	(
		 // Apply matrix. Matrix read in horizontally, but we want the
		 // transpose so apply reading vertically (e*** instead of m***)
		"vtfm3.t %[afR], %e[aafThis], %[afV]"

		: [afR] VFPU_AND_C (afR)
		: [aafThis] VFPU_C (aafThis), [afV] VFPU_C (afV)
	);

	return vfpu_v4_save3(afR,xResult);
#else
	return Asura_Vector_3(e11 * xVec.x + e21 * xVec.y + e31 * xVec.z,
                          e12 * xVec.x + e22 * xVec.y + e32 * xVec.z,
                          e13 * xVec.x + e23 * xVec.y + e33 * xVec.z);
#endif
}

inline void Asura_Matrix_3x3::Identity(void)
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	__asm__
	(
		"vmidt.t m700 \n" // Load identity 3*3 into memory

        // grr, no simple way to store triples, save out one by one
		"sv.s s700, 0+%0 \n"
		"sv.s s701, 4+%0 \n"
		"sv.s s702, 8+%0 \n"

		"sv.s s710, 12+%0 \n"
		"sv.s s711, 16+%0 \n"
		"sv.s s712, 20+%0 \n"

		"sv.s s720, 24+%0 \n"
		"sv.s s721, 28+%0 \n"
		"sv.s s722, 32+%0 \n"

		: "=r" (this)
	);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_m4x4 aafM = vfpu_vmidt_q(0);

	(void) vfpu_m4x4_save3x3(aafM,*this);
#else
	e11 = e22 = e33 = 1.0f;
	e12 = e13 = e21 = e23 = e31 = e32 = 0.0f;
#endif
}

inline Asura_Matrix_3x3 Asura_Matrix_3x3::operator *(const Asura_Matrix_3x3& xM) const
{
	class Asura_Matrix_3x3 xResult;

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load m elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"lv.s s710, 12+%1 \n"
		"lv.s s711, 16+%1 \n"
		"lv.s s712, 20+%1 \n"

		"lv.s s720, 24+%1 \n"
		"lv.s s721, 28+%1 \n"
		"lv.s s722, 32+%1 \n"

		"lv.s s600, 0+%2 \n" // Load *this elements
		"lv.s s601, 4+%2 \n"
		"lv.s s602, 8+%2 \n"

		"lv.s s610, 12+%2 \n"
		"lv.s s611, 16+%2 \n"
		"lv.s s612, 20+%2 \n"

		"lv.s s620, 24+%2 \n"
		"lv.s s621, 28+%2 \n"
		"lv.s s622, 32+%2 \n"

		"vmmul.t m500, m700, m600 \n" // Multiply *this * m

        // grr, no simple way to store triples, save out one by one
		"sv.s s500, 0+%0 \n"
		"sv.s s501, 4+%0 \n"
		"sv.s s502, 8+%0 \n"

		"sv.s s510, 12+%0 \n"
		"sv.s s511, 16+%0 \n"
		"sv.s s512, 20+%0 \n"

		"sv.s s520, 24+%0 \n"
		"sv.s s521, 28+%0 \n"
		"sv.s s522, 32+%0 \n"

		: "=r" (&xResult)
		: "r" (&xM), "r" (this)
	);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && !defined GCC4_BUGGED
	#if 0 && defined ASURA_PSP_GCC_BUILTINS
		#error Passes the unit tests but results in anomalies in some cases

		register const vfpu_m4x4 aafThis = vfpu_m4x4_load3x3(*this);
		register const vfpu_m4x4 aafM = vfpu_m4x4_load3x3(xM);
		register vfpu_m4x4 aafR;

		/* No 3x3 intrinsics, so we can use only inline asm here */

		__asm__
		(
			"vmmul.t %m[aafR], %m[aafThis], %m[aafM]"

			: [aafR] VFPU_AND_C (aafR)
			: [aafThis] VFPU_C (aafThis), [aafM] VFPU_C (aafM)
		);

		(void) vfpu_m4x4_save3x3(aafR,xResult);
	#else
		/* This is just a fallback for the intrinsics version above */

		__asm__
		(
			/* load 'xM' elementwise */
			"lv.s s700, 0+%[xM] \n\t"
			"lv.s s701, 4+%[xM] \n\t"
			"lv.s s702, 8+%[xM] \n\t"

			"lv.s s710, 12+%[xM] \n\t"
			"lv.s s711, 16+%[xM] \n\t"
			"lv.s s712, 20+%[xM] \n\t"

			"lv.s s720, 24+%[xM] \n\t"
			"lv.s s721, 28+%[xM] \n\t"
			"lv.s s722, 32+%[xM] \n\t"

			/* load '*this' elementwise */
			"lv.s s600, 0+%[xThis] \n\t"
			"lv.s s601, 4+%[xThis] \n\t"
			"lv.s s602, 8+%[xThis] \n\t"

			"lv.s s610, 12+%[xThis] \n\t"
			"lv.s s611, 16+%[xThis] \n\t"
			"lv.s s612, 20+%[xThis] \n\t"

			"lv.s s620, 24+%[xThis] \n\t"
			"lv.s s621, 28+%[xThis] \n\t"
			"lv.s s622, 32+%[xThis] \n\t"

			"vmmul.t m500, m700, m600 \n\t"

			/* save 'xResult' elementwise */
			"sv.s s500, 0+%[xResult] \n\t"
			"sv.s s501, 4+%[xResult] \n\t"
			"sv.s s502, 8+%[xResult] \n\t"

			"sv.s s510, 12+%[xResult] \n\t"
			"sv.s s511, 16+%[xResult] \n\t"
			"sv.s s512, 20+%[xResult] \n\t"

			"sv.s s520, 24+%[xResult] \n\t"
			"sv.s s521, 28+%[xResult] \n\t"
			"sv.s s522, 32+%[xResult]"

			: [xResult] "=o" (xResult)
			: [xM] "o" (xM), [xThis] "o" (*this)

			: "c500", "c510", "c520",
			  "c600", "c610", "c620",
			  "c700", "c710", "c720"
		);
	#endif
#else
	xResult.e11 = e11 * xM.e11 + e12 * xM.e21 + e13 * xM.e31;
	xResult.e21 = e21 * xM.e11 + e22 * xM.e21 + e23 * xM.e31;
	xResult.e31 = e31 * xM.e11 + e32 * xM.e21 + e33 * xM.e31;

	xResult.e12 = e11 * xM.e12 + e12 * xM.e22 + e13 * xM.e32;
	xResult.e22 = e21 * xM.e12 + e22 * xM.e22 + e23 * xM.e32;
	xResult.e32 = e31 * xM.e12 + e32 * xM.e22 + e33 * xM.e32;

	xResult.e13 = e11 * xM.e13 + e12 * xM.e23 + e13 * xM.e33;
	xResult.e23 = e21 * xM.e13 + e22 * xM.e23 + e23 * xM.e33;
	xResult.e33 = e31 * xM.e13 + e32 * xM.e23 + e33 * xM.e33;
#endif

	return xResult;
}

inline Asura_Matrix_3x3 Asura_Matrix_3x3::operator *(const float fS) const
{
	class Asura_Matrix_3x3 xResult;

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	__asm__
	(
		"mtv %1, s703 \n" // Load scalar

		"lv.s s600, 0+%2 \n" // Load matrix elements
		"lv.s s601, 4+%2 \n"
		"lv.s s602, 8+%2 \n"

		"lv.s s610, 12+%2 \n"
		"lv.s s611, 16+%2 \n"
		"lv.s s612, 20+%2 \n"

		"lv.s s620, 24+%2 \n"
		"lv.s s621, 28+%2 \n"
		"lv.s s622, 32+%2 \n"

		"vmscl.t m700, m600, s703 \n" // Scale matrix

		 // grr, no simple way to store triples, save out one by one
		"sv.s s700, 0+%0 \n"
		"sv.s s701, 4+%0 \n"
		"sv.s s702, 8+%0 \n"

		"sv.s s710, 12+%0 \n"
		"sv.s s711, 16+%0 \n"
		"sv.s s712, 20+%0 \n"

		"sv.s s720, 24+%0 \n"
		"sv.s s721, 28+%0 \n"
		"sv.s s722, 32+%0 \n"

		: "=r" (&xResult)
		: "r" (fS), "r" (this)
	);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_m4x4 aafThis = vfpu_m4x4_load3x3(*this);
	register const vfpu_v4 afS = vfpu_v4_s(fS);
	register vfpu_m4x4 aafR;

	/* No 3x3 intrinsics, so we can use only inline asm here */

	__asm__
	(
		"vmscl.t %m[aafR], %m[aafThis], %x[afS]"

		: [aafR] VFPU_AND_C (aafR)
		: [aafThis] VFPU_C (aafThis), [afS] VFPU_C (afS)
	);

	(void) vfpu_m4x4_save3x3(aafR,xResult);
#else
	xResult.e11 = e11 * fS;
	xResult.e21 = e21 * fS;
	xResult.e31 = e31 * fS;

	xResult.e12 = e12 * fS;
	xResult.e22 = e22 * fS;
	xResult.e32 = e32 * fS;

	xResult.e13 = e13 * fS;
	xResult.e23 = e23 * fS;
	xResult.e33 = e33 * fS;
#endif

	return xResult;
}

// A S U R A _ V E C T O R _ 2 ///////////////////////////////////////////////
inline Asura_Vector_2::Asura_Vector_2(const Asura_Vector_2& v)
{
	x = v.x;
	y = v.y;
}

inline Asura_Vector_2::Asura_Vector_2(float x0, float y0)
{
	x = x0;
	y = y0;
}

// binary ops

inline Asura_Vector_2 Asura_Vector_2::operator * ( float fScale ) const
{
	return Asura_Vector_2( this->x * fScale, this->y * fScale );
}

inline Asura_Vector_2 Asura_Vector_2::operator + ( const Asura_Vector_2& v) const
{
	return Asura_Vector_2(x + v.x, y + v.y);
}
inline void Asura_Vector_2::operator += ( const Asura_Vector_2& v)
{
	x += v.x;
	y += v.y;
}

inline Asura_Vector_2 Asura_Vector_2::operator - ( const Asura_Vector_2& v) const
{
	return Asura_Vector_2(x - v.x, y - v.y);
}
inline void Asura_Vector_2::operator -= ( const Asura_Vector_2& v)
{
	x -= v.x;
	y -= v.y;
}

inline void Asura_Vector_2::operator *= ( register float s )
{
	x *= s;
	y *= s;
}
inline void Asura_Vector_2::operator /= ( register float f)
{
	register const float fInv = 1.0f / f;
	x *= fInv;
	y *= fInv;
}

inline bool Asura_Vector_2::operator == ( const Asura_Vector_2& v) const
{
	return x == v.x && y == v.y;
}

inline bool Asura_Vector_2::operator != ( const Asura_Vector_2& v) const
{
	return x != v.x || y != v.y;
}

//dot product
inline float  Asura_Vector_2::operator * ( const Asura_Vector_2& v) const
{
	return (x*v.x + y*v.y);
}

//Pics a random vector in the plane, with random length [0..1] and
//random direction (with symmetrical distribution)
inline Asura_Vector_2& Asura_Vector_2::RandomVector(Asura_RandomGenerator* pxRNG)
{
	//Choose random elements for x and y, until the squared length is
	//within the unit circle.
	do
	{
		x = pxRNG->GetFloat( -1.0f, 1.0f );
		y = pxRNG->GetFloat( -1.0f, 1.0f );
	}
	while(x * x + y * y > 1.f);

	//Return the resulting vector
	return *this;
}


inline Asura_Vector_2& Asura_Vector_2::RandomDirection(Asura_RandomGenerator* pxRNG)
{
	float fLength2;

	//Choose random elements for x and y, until
	//the squared length is within the unit circle, but large
	//enough that the normalisation won't result in poor conditioning.
	do
	{
		x = pxRNG->GetFloat( -1.0f, 1.0f );
		y = pxRNG->GetFloat( -1.0f, 1.0f );

		fLength2 = x * x + y * y;
	}
	while(fLength2 > 1.f || fLength2 < 0.01f);

	fLength2 = 1.f / Asura_Maths::Sqrt(fLength2);

	x *= fLength2;
	y *= fLength2;

	//Return the resulting vector
	return *this;
}



inline float Asura_Vector_2::Normalise(void)
{
	const float m = Magnitude();

	if (m==0.0f)    return m;

	const float rm = 1.0f/m;
	x *= rm;
	y *= rm;

	return m;
}

inline float Asura_Vector_2::Magnitude( void ) const
{
	return Asura_Maths::Sqrt((x*x) + (y*y));
}

inline float Asura_Vector_2::MagnitudeSquared( void ) const
{
	return ((x*x) + (y*y));
}

inline void Asura_Vector_2::Zero(void)
{
	x=y=0.0f;
}


// A S U R A _ V E C T O R _ 3 ///////////////////////////////////////////////
inline Asura_Vector_3::Asura_Vector_3(const float * fPtr)
{
	x = fPtr[0];
	y = fPtr[1];
	z = fPtr[2];
}

inline Asura_Vector_3::Asura_Vector_3(const Asura_Vector_3& v)
{
	x = v.x;
	y = v.y;
	z = v.z;
}

inline Asura_Vector_3::Asura_Vector_3(float x0, float y0, float z0)
{
	x = x0;
	y = y0;
	z = z0;
}

// assignment operators
inline void Asura_Vector_3::operator = ( const Asura_Vector_3& v)
{
	x = v.x;
	y = v.y;
	z = v.z;
}

inline void Asura_Vector_3::operator += ( const Asura_Vector_3& v)
{
	x += v.x;
	y += v.y;
	z += v.z;
}

inline void Asura_Vector_3::operator -= ( const Asura_Vector_3& v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
}

inline void Asura_Vector_3::operator *= ( float f)
{
	x *= f;
	y *= f;
	z *= f;
}

inline void Asura_Vector_3::operator /= ( float f)
{
	float fInv = 1.0f / f;
	x *= fInv;
	y *= fInv;
	z *= fInv;
}

// unary operators
inline Asura_Vector_3 Asura_Vector_3::operator - () const
{
	return Asura_Vector_3(-x, -y, -z);
}

// binary operators

inline Asura_Vector_3 Asura_Vector_3::operator + ( const Asura_Vector_3& v) const
{
	return Asura_Vector_3(x + v.x, y + v.y, z + v.z);
}

inline Asura_Vector_3 Asura_Vector_3::operator - ( const Asura_Vector_3& v) const
{
	return Asura_Vector_3(x - v.x, y - v.y, z - v.z);
}

inline Asura_Vector_3 Asura_Vector_3::operator * ( float f) const
{
	return Asura_Vector_3(x * f, y * f, z * f);
}

inline Asura_Vector_3 Asura_Vector_3::operator / ( float f) const
{
	register const float fInv = 1.0f / f;
	return Asura_Vector_3(x * fInv, y * fInv, z * fInv);
}

inline Asura_Vector_3 Asura_Vector_3::operator ^ ( const Asura_Vector_3& xV) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	Asura_Vector_3 xResult;

	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load *this elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"lv.s s710, 0+%2 \n" // Load v elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"

		"vcrsp.t c720, c700, c710 \n" // Cross product *this ^ v

        // grr, no simple way to store triples, save out one by one
		"sv.s s720, 0+%0 \n"
		"sv.s s721, 4+%0 \n"
		"sv.s s722, 8+%0 \n"

		: "=r" (&xResult)
		: "r" (this), "r" (&xV)
	);

	return xResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register const vfpu_v4 afThis = vfpu_v4_load3(*this);
	register const vfpu_v4 afV = vfpu_v4_load3(xV);
	Asura_Vector_3 xResult;

	return vfpu_v4_save3(vfpu_vcrsp_t(afThis,afV,0),xResult);
#else
	return Asura_Vector_3(y*xV.z - z*xV.y, z*xV.x - x*xV.z, x*xV.y - y*xV.x);
#endif
}

inline float  Asura_Vector_3::operator *(register const Asura_Vector_3& xV) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	float fResult;

	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load xV elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"lv.s s710, 0+%2 \n" // Load *this elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"

		"vdot.t s720, c700, c710 \n" // Dot product

		"mfv %0, s720 \n" // Save result

		: "=r" (fResult)
		: "r" (&xV), "r" (this)
	);

	return fResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_v4 afThis = vfpu_v4_load3(*this);
	register const vfpu_v4 afV = vfpu_v4_load3(xV);

	return vfpu_extract_float_from_v4x(vfpu_vdot_t(afThis,afV,0));
#else
	return (x*xV.x + y*xV.y + z*xV.z);
#endif
}

#pragma inline

inline Asura_Vector_3 Asura_Vector_3::operator *(register const Asura_Matrix_3x3& xM) const
{
#if 0 && ASURA_PSP_ASM_USE_SNSYNTAX
	SNPokeVector xVector;

	_asm
	{
		lv.s s700, &xM.e11	// Load matrix elements
		lv.s s701, &xM.e12
		lv.s s702, &xM.e13

		lv.s s710, &xM.e21
		lv.s s711, &xM.e22
		lv.s s712, &xM.e23

		lv.s s720, &xM.e31
		lv.s s721, &xM.e32
		lv.s s722, &xM.e33

		lv.s s730, &x		// Load vector elements
		lv.s s731, &y
		lv.s s732, &z

		vtfm3.t* xVector, m700, c730 // Apply matrix
	};

	register const SNPokeVector* const pxData
		= reinterpret_cast<const SNPokeVector* const>(&xVector);

	return Asura_Vector_3(pxData->x, pxData->y, pxData->z);
#elif 0 && defined ASURA_PSP_USE_VFPU_ASSEMBLER_FOR_M3xV3 && defined __SNC__
	// Oddly, this one doesn't like outputting to an Asura vector, needs
	// more experimenting to work out why
	float x0, y0, z0;

	__asm__
	(
		"lv.s s700, 0+%3 \n" // Load matrix elements
		"lv.s s701, 4+%3 \n"
		"lv.s s702, 8+%3 \n"

		"lv.s s710, 12+%3 \n"
		"lv.s s711, 16+%3 \n"
		"lv.s s712, 20+%3 \n"

		"lv.s s720, 24+%3 \n"
		"lv.s s721, 28+%3 \n"
		"lv.s s722, 32+%3 \n"

		"lv.s s730, 0+%4 \n" // Load vector elements
		"lv.s s731, 4+%4 \n"
		"lv.s s732, 8+%4 \n"

		"vtfm3.t r703, m700, c730 \n" // Apply matrix

		"mfv %0, s703 \n" // Save vector elements
		"mfv %1, s713 \n"
		"mfv %2, s723 \n"

		: "=r" (x0), "=r" (y0), "=r" (z0)
		: "r" (&xM), "r" (this)
	);

	return Asura_Vector_3(x0, y0, z0);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register Asura_Vector_3 xResult;
	register const vfpu_v4 afThis = vfpu_v4_load3(*this);
	register const vfpu_m4x4 aafM = vfpu_m4x4_load3x3(xM);
	register vfpu_v4 afR;

	/* No 3x3 intrinsics, so we can use only inline asm here */

	__asm__
	(
		"vtfm3.t %[afR], %m[aafM], %[afThis]"

		: [afR] VFPU_AND_C (afR)
		: [aafM] VFPU_C (aafM), [afThis] VFPU_C (afThis)
	);

	return vfpu_v4_save3(afR,xResult);
#else
	Asura_Vector_3 xResult;

	xResult.x = xM.e11 * x + xM.e12 * y + xM.e13 * z;
	xResult.y = xM.e21 * x + xM.e22 * y + xM.e23 * z;
	xResult.z = xM.e31 * x + xM.e32 * y + xM.e33 * z;

	return xResult;
#endif
}

inline bool Asura_Vector_3::operator == ( const Asura_Vector_3& xV) const
{
	return x == xV.x && y == xV.y && z == xV.z;
}

inline bool Asura_Vector_3::ApproximatelyEquals(const Asura_Vector_3& xV, float epsilon /*= 1.0e-6f*/) const
{
	const Asura_Vector_3 delta(x-xV.x,y-xV.y,z-xV.z);

	return ((delta.x*delta.x+delta.y*delta.y+delta.z*delta.z)<epsilon);

}
inline bool Asura_Vector_3::operator != ( const Asura_Vector_3& xV) const
{
	return x != xV.x || y != xV.y || z != xV.z;
}

// useful operations

// Picks a random vector in the plane, with a length of 1 in a random
// direction. The distribution is symmetrical

inline Asura_Vector_3& Asura_Vector_3::RandomDirection(Asura_RandomGenerator* pxRNG)
{
	float fLength2;

	// Choose random elements for x and y, until the squared length is
	// within the unit sphere, but large enough that the normalisation
	// won't result in poor conditioning.
	do
	{
		x = pxRNG->GetFloat( -1.0f, 1.0f );
		y = pxRNG->GetFloat( -1.0f, 1.0f );
		z = pxRNG->GetFloat( -1.0f, 1.0f );

		fLength2 = x * x + y * y + z * z;
	}
	while(fLength2 > 1.f || fLength2 < 0.01f);

	fLength2 = 1.f / Asura_Maths::Sqrt(fLength2);

	x *= fLength2;
	y *= fLength2;
	z *= fLength2;

	//Return the resulting vector
	return *this;
}


// Picks a random vector in a sphere, with random length [0..1] and
// random direction (with symmetrical distribution)
inline Asura_Vector_3& Asura_Vector_3::RandomVector(Asura_RandomGenerator* pxRNG)
{
	// Choose random elements for x, y and z, until the squared length
	// is within the unit sphere.
	do
	{
		x = pxRNG->GetFloat( -1.0f, 1.0f );
		y = pxRNG->GetFloat( -1.0f, 1.0f );
		z = pxRNG->GetFloat( -1.0f, 1.0f );
	}
	while(x * x + y * y + z * z > 1.f);

	//Return the resulting vector
	return *this;
}

inline float Asura_Vector_3::MagnitudeSquared(void) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	float fResult;

	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load vector elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"vdot.t s720, c700, c700 \n" // Dot product with itself

		"mfv %0, s720 \n" // Save result

		: "=r" (fResult)
		: "r" (this)
	);

	return fResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_v4 afThis = vfpu_v4_load3(*this);

	return vfpu_extract_float_from_v4x(vfpu_vdot_t(afThis,afThis,0));
#else
	return ((x*x) + (y*y) + (z*z));
#endif
}

inline float Asura_Vector_3::Magnitude(void) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	float fResult;

	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load vector elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"vdot.t s720, c700, c700 \n" // Dot product with itself
		"vsqrt.s s720, s720 \n" // Squareroot

		"mfv %0, s720 \n" // Save result

		: "=r" (fResult)
		: "r" (this)
	);

	return fResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_v4 afThis = vfpu_v4_load3(*this);

	return vfpu_extract_float_from_v4x
		(vfpu_vsqrt_s(vfpu_vdot_t(afThis,afThis,0),0));
#else
	return Asura_Maths::Sqrt((x*x) + (y*y) + (z*z));
#endif
}

inline float Asura_Vector_3::Normalise(void)
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	float fResult;

	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load vector elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"vdot.t s720, c700, c700 \n" // Dot product with itself

		"vcmp.s EZ, s720 \n" // Compare result with 0
		"bvt 0, 0f \n" // Skip to return if comparison is true
		"vnop \n"

		"vsqrt.s s720, s720 \n" // Squareroot
        // Reciprocal (stored separately since we need to remember the
        // squareroot for the return value)
		"vrcp.s s710, s720 \n"
		"vscl.t c700, c700, s710 \n" // Scale vector by reciprocal of squareroot

         // Save vector. Grr, no simple way to store triples, save out
         // one by one
		"sv.s s700, 0+%1 \n"
		"sv.s s701, 4+%1 \n"
		"sv.s s702, 8+%1 \n"

	"0:\n" // Label for branching past main operations
         // Save magnitude (or 0 if mag squared was 0 and we branched
         // past most of the operations)
		"mfv $8, s720 \n"
		"mtc1 $8, %0 \n"

		: "=f" (fResult), "+r" (this)
		:
		: "$8"
	);

	return fResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register const vfpu_v4 afThis = vfpu_v4_load3(*this);
	register const vfpu_v4 afMagSq = vfpu_vdot_t(afThis,afThis,0);

	if (vfpu_extract_float_from_v4x(afMagSq) == 0.0f)
	{
		/*
			Note that for 'Asura_AVector3::Normalise' there is an
			'assert' instead.
		*/
		return 0.0f;
	}
	else
	{
		register const vfpu_v4 afMag = vfpu_vsqrt_s(afMagSq,0);

		(void) vfpu_v4_save3(vfpu_vscl_t(afThis,vfpu_v4s_inv(afMag),0),*this);

		return vfpu_extract_float_from_v4x(afMag);
	}
#else
	const float m = Magnitude();

	if (m == 0.0f)
    {
        return m;
    }


	const float rm = 1.0f/m;
	x *= rm;
	y *= rm;
	z *= rm;

	return m;
#endif
}

inline void Asura_Vector_3::Zero(void)
{
	x = y = z = 0.0f;
}

inline Asura_Vector_3 Asura_Vector_3::Vector_Multiply( const Asura_Vector_3& v1 ) const
{
	return Asura_Vector_3( v1.x * x, v1.y * y, v1.z * z );
}

// A S U R A _ E U L E R _ 3 /////////////////////////////////////////////////
inline Asura_Euler_3::Asura_Euler_3(float x0, float y0, float z0)
{
	x = x0;
	y = y0;
	z = z0;
}

// assignment operators
inline void Asura_Euler_3::operator += ( const Asura_Euler_3& xEuler)
{
	x += xEuler.x;
	y += xEuler.y;
	z += xEuler.z;
}

inline void Asura_Euler_3::operator -= ( const Asura_Euler_3& xEuler)
{
	x -= xEuler.x;
	y -= xEuler.y;
	z -= xEuler.z;
}

inline void Asura_Euler_3::operator *= ( float f)
{
	x *= f;
	y *= f;
	z *= f;
}

inline void Asura_Euler_3::operator /= ( float f)
{
	float fInv = 1.0f / f;
	x *= fInv;
	y *= fInv;
	z *= fInv;
}

// unary operators
inline Asura_Euler_3 Asura_Euler_3::operator - () const
{
	return Asura_Euler_3(-x, -y, -z);
}

// binary operators
inline Asura_Euler_3 Asura_Euler_3::operator + ( const Asura_Euler_3& xEuler) const
{
	return Asura_Euler_3(x + xEuler.x, y + xEuler.y, z + xEuler.z);
}

inline Asura_Euler_3 Asura_Euler_3::operator - ( const Asura_Euler_3& xEuler) const
{
	return Asura_Euler_3(x - xEuler.x, y - xEuler.y, z - xEuler.z);
}

inline Asura_Euler_3 Asura_Euler_3::operator * ( float f) const
{
	return Asura_Euler_3(x * f, y * f, z * f);
}

inline Asura_Euler_3 Asura_Euler_3::operator / ( float f) const
{
	float fInv = 1.0f / f;
	return Asura_Euler_3(x * fInv, y * fInv, z * fInv);
}

inline void Asura_Euler_3::ToMatrix(Asura_Matrix_3x3& m) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && !defined GCC4_BUGGED
	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load Euler elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

        // Store 2 / Pi *WARNING* This is slightly different to the Pi
        // approximation used in Asura normally, so if values are
        // looking too different we need to load in our own value.
		"vcst.s s703, VFPU_2_PI \n"

        // Cos, Sin use 0..1 to correspond to our original angle
        // 0..Pi/2, so scale our angles by 2/Pi
		"vscl.t c700, c700, s703 \n"

		"vcos.t c710, c700 \n" // Cosine each value (Cx, Cy, Cz)
		"vsin.t c720, c700 \n" // Sine each value (Sx, Sy, Sz)

        /* First row */

		"vmul.s s703, s720, s721 \n" // Sx * Sy
		"vmul.s s723, s703, s722 \n" // (Sx * Sy) * Sz
		"vmul.s s733, s711, s712 \n" // Cy * Cz
		"vfad.p s600, r723 \n" // ((Sx * Sy) * Sz) + (Cy * Cz) => e11

		"vmul.s s723, s703, s712 \n" // (Sx * Sy) * Cz
		"vmul.s s733, s711, s722 \n" // Cy * Sz
		"vmul.s s732, s721, s712 \n" // Sy * Cz
		"vneg.p c732, c732 \n" // -Sy * Cz , -Cy * Sz
		"vfad.p s601, r723 \n" // ((Sx * Sy) * Cz) + (-Cy * Sz) => e12

		"vmul.s s602, s710, s721 \n" // Cx * Sy => e13

        /* Second row */
		"vmul.s s610, s710, s722 \n" // Cx * Sz => e21
		"vmul.s s611, s710, s712 \n" // Cx * Cz => e22
		"vneg.s s612, s720 \n" // -Sx => e23

        /* Third row */
		"vmul.s s730, s720, s711 \n" // Sx * Cy
		"vmul.s s733, s730, s722 \n" // (Sx * Cy) * Sz
		"vfad.p s620, c732 \n" // ((Sx * Cy) * Sz) + (-Sy * Cz) => e31
		"vmul.s s733, s730, s712 \n" // (Sx * Cy) * Cz
		"vmul.s s732, s721, s722 \n" // Sy * Sz
		"vfad.p s621, c732 \n" // ((Sx * Cy) * Cz) + (Sy * Sz) => e32
		"vmul.s s622, s710, s711 \n" // Cx * Cy => e33

        // Save matrix. Grr, no simple way to store triples, save out
        // one by one
		"sv.s s600, 0+%0 \n"
		"sv.s s601, 4+%0 \n"
		"sv.s s602, 8+%0 \n"

		"sv.s s610, 12+%0 \n"
		"sv.s s611, 16+%0 \n"
		"sv.s s612, 20+%0 \n"

		"sv.s s620, 24+%0 \n"
		"sv.s s621, 28+%0 \n"
		"sv.s s622, 32+%0 \n"

	#if defined __SNC__
		: "=r" (&m)
		: "r" (this)
	#else
		: "=o" (m)
		: "o" (*this)
		: "c700", "c710", "c720", "c730",
		  "c600", "c610", "c620"
	#endif
	);
#else
	float c1 = Asura_Maths::Cosine(x);
	float s1 = Asura_Maths::Sine(x);

	float c2 = Asura_Maths::Cosine(y);
	float s2 = Asura_Maths::Sine(y);

	float c3 = Asura_Maths::Cosine(z);
	float s3 = Asura_Maths::Sine(z);

	m.e11 = c2*c3+s1*s2*s3;
	m.e12 = -c2*s3+s1*s2*c3;
	m.e13 = c1*s2;

	m.e21 = c1*s3;
	m.e22 = c1*c3;
	m.e23 = -s1;

	m.e31 = -s2*c3 + s1*c2*s3;
	m.e32 = s2*s3 + s1*c2*c3;
	m.e33 = c1*c2;
#endif
}

// Macro to avoid repeatedly entering the arctangent operations in
// to-euler conversion functions Returns angles still in the VFPU's
// bizarre range (0..1 corresponding to 0..Pi/2) Requires the function
// not to be using the fifth matrix of registers (x4**).

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER
    #define INSERT_ARCTANGENT(vd, vs, vt, label1, label2, label3)   \
		"vmov.s s400, " vs " \n" \
		"vmov.s s401, " vt " \n" \
		"vone.s s421 \n" /* Load 1 */ \
		"vsgn.p c410, c400 \n" /* Sign(x, y) */ \
		\
		"vcmp.p EZ, c400 \n" /* compare (x, y) with zero */ \
		"bvt 4, " label2 "f \n" /* branch to degenerate case if x==0 or y==0 */ \
		"vnop \n" \
		\
		"vabs.p c400, c400 \n" /* Abs the values for sorting, otherwise we can still get inaccurate results */ \
		"vslt.s s412, s401, s400 \n" /* store (y < x) for use if we swap numerator and denominator in arctan calculation */ \
		"vsrt1.q c400, c400 \n" /* Sort x and y (have the lesser first, to maintain accuracy in the trig operation. We have stored which was originally bigger, so we can add/subtract Pi/2 later if required) (Note - we can have nothing stored in the rest of the row if we use this, as sort only works on quadwords) */ \
		"vmul.p c400, c400, c410 \n" /* Restore signs of x and y (may have swapped which has which sign, but that shouldn't affect things at this point) */ \
		"vrcp.s s430, s401 \n" /* 1/y */ \
		"vmul.s s430, s430, s400 \n" /* x/y = w */ \
		"vmul.s s431, s430, s430 \n" /* sq(w) */ \
		"vadd.s s431, s431, s421 \n" /* 1 + sq(w) */ \
		"vrsq.s s431, s431 \n" /* 1 / sqrt(1 + sq(w)) */ \
		"vmul.s s431, s430, s431 \n" /* x / sqrt(1 + sq(w)) */ \
		"vasin.s s432, s431 \n" /* asin(x / sqrt(1 + sq(w))) = atan(w) */ \
		\
		"vcmp.s EZ, s412 \n" /* Comparison is true iff we did not swap x and y */ \
		"bvt 0, " label1 "f \n" /* Branch to quadrant adjustment if we didn't swap */ \
		"vnop \n" \
		\
		"vmul.s s413, s410, s411 \n" /* Get the combined sign of x and y (= sign(w)) */ \
		"vneg.s s432, s432 \n" /* We swapped x and y, so at the moment we actually have arctan(1/w). Conversion stage 1: negate */ \
		"vadd.s s432, s432, s413 \n" /* Conversion stage 2: Add sign(w) (which is equivalent to adding sign(w) * Pi/2 after conversion to radians). arctan(w) = (sign(w)*Pi/2) - arctan(1/w) where w!=0 */ \
		\
	label1 ":\n" /* Quadrant adjustment */ \
		"vpfxs -X,Y,Z,W \n" /* Prefix comparison with negation (want to find out if sign(y) is -1, so compare negation with 1) */ \
		\
		"vcmp.s EQ, s411, s421 \n" /* Compare -sign(y) with 1 */ \
		"bvf 0, " label3 "f \n" /* Branch to end if false */ \
		"vnop \n" \
		\
		"vadd.s s410, s410, s410 \n" /* Double sign(x) */ \
		"vsub.s s432, s432, s410 \n" /* Subtract 2 * sign(x) (equivalent to subracting sign(x) * Pi after conversion) */ \
		"b " label3 "f \n" /* branch to end */ \
		"vnop \n" \
		\
	label2 ":\n" /* Degenerate cases (x=0 or y=0) */ \
		"vmov.s s432, s410 \n" /* Move sign of x to position of unconverted result */ \
		"vpfxs -X,Y,Z,W \n" /* Prefix comparison with negation (want to find out if sign(y) is -1, so compare negation with 1) */ \
		\
		"vcmp.s EQ, s411, s421 \n" /* Compare -sign(y) with 1 */ \
		"bvf 0, " label3 "f \n" /* Branch to end if false */ \
		"vnop \n" \
		\
		"vpfxs 2,Y,Z,W \n" /* Prefix move with constant 2 */ \
		"vmov.s s432, s421 \n" /* x is 0 and y < 0, so the answer is Pi/-Pi (we'll use Pi). Put 2 in unconverted result. */ \
		\
	label3 ":\n" /* end */ \
		"vmov.s " vd ", s432 \n" /* Store at the destination */
#endif

// KJL - 18-May-01 16:22:35 - possibly the z angle
// should be chosen as zero for the non-unique case
inline void Asura_Euler_3::FromMatrix(const Asura_Matrix_3x3& m)
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && !defined GCC4_BUGGED
	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load matrix elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

		"lv.s s710, 12+%1 \n"
		"lv.s s711, 16+%1 \n"
		"lv.s s712, 20+%1 \n"

		"lv.s s720, 24+%1 \n"
		"lv.s s721, 28+%1 \n"
		"lv.s s722, 32+%1 \n"

        // Store Pi / 2 *WARNING* This is slightly different to the Pi
        // approximation used in Asura normally, so if values are
        // looking too different we need to load in our own value.
		"vcst.s s730, VFPU_PI_2 \n"

		"vone.s s731 \n" // Store 1

	#if 0
        // float sx = -m.e23. Can be ignored by using appropriate
        // prefixing on comparison
		"vneg.s s732, s712 \n"
	#else
		"vpfxs -X,Y,Z,W \n" // Prefix source with negation (to compare -m.e23 with 1)
	#endif
		"vcmp.s LT, s712, s731 \n" // (With prefix) comparison -m.e23 < 1
		"bvf 0, 1f \n" // Branch to section 1 if comparison above returns false
		"vnop \n"

		"vcmp.s LT, s712, s731 \n" // (Not prefixed) comparison m.e23 < 1 == -m.e23 > -1
		"bvf 0, 0f \n" // Branch to section 0 if comparison above returns false
		"vnop \n"

		"vpfxs -X,Y,Z,W \n" // Prefix source with negation (to arcsine -m.e23 instead of m.e23)
		"vasin.s s600, s712 \n" // x = ArcSin(-m.e23)
		INSERT_ARCTANGENT("s601", "s702", "s722", "3", "4", "5") // y = ArcTan(m.e13, m.e33)
		INSERT_ARCTANGENT("s602", "s710", "s711", "6", "7", "8") // z = ArcTan(m.e21, m.e22)
		"b 2f \n" // Skip to end
		"vnop \n"

	"0:\n" // sx <= -1.0f (non-unique case 1) (choose y as 0)
		"vneg.s s600, s731 \n" // x = -1
		"vzero.s s601 \n" // y = 0
		INSERT_ARCTANGENT("s602", "s720", "s700", "9", "10", "11") // z = ArcTan(m.e31, m.e11)
		"b 2f \n" // Skip to end
		"vnop \n"

	"1:\n" // sx >= 1.0f (non-unique case 2) (choose y as 0)
		"vmov.s s600, s731 \n" // x = 1
		"vzero.s s601 \n" // y = 0
		INSERT_ARCTANGENT("s602", "s701", "s721", "12", "13", "14") // z = ArcTan(m.e12, m.e32)

	"2:\n" // end
		"vscl.t c600, c600, s730 \n" // Scale our angles to the correct range (all should currently be 0..1 to correspond to 0..Pi/2)

		// Save euler. Grr, no simple way to store triples, save out one
		// by one
		"sv.s s600, 0+%0 \n"
		"sv.s s601, 4+%0 \n"
		"sv.s s602, 8+%0 \n"

	#if defined __SNC__
		: "=r" (this)
		: "r" (&m)
	#else
		: "=o" (*this)
		: "o" (m)
		: "c700", "c710", "c720", "c730", "c600",
		  "c400", "c410", "c420", "c430" // for 'INSERT_ARCTANGENT'
	#endif
	);
#else
	float sx = -m.e23;

	if (sx < 1.0f)
	{
		if (sx > -1.0f)
		{
			// cx is non-zero then
			x = Asura_Maths::ArcSine(sx);
			y = Asura_Maths::ArcTangent(m.e13,m.e33);
			z = Asura_Maths::ArcTangent(m.e21,m.e22);
		}
		else
		{
			// non-unique

			// s(y)c(z)+c(y)s(z) = s(y+z)
			// c(y)c(z)-s(y)s(z) = c(y+z))
			// y+z = atan(-e31/-e11)
			// if choose y to be zero

			x = -Asura_Maths::Pi*0.5f;
			y = 0;
			z = Asura_Maths::ArcTangent(m.e31,m.e11);
		}
	}
	else
	{
		// non-unique

		// s(y)c(z)-c(y)s(z) = s(y-z)
		// s(y)s(z)+c(y)c(z) = c(y-z)
		// y-z = atan(e12/e32)
		// if choose y to be zero

		x = Asura_Maths::Pi*0.5f;
		y = 0;
		z = -Asura_Maths::ArcTangent(m.e12,m.e32);
	}
#endif
}

inline void Asura_Euler_3::Zero(void)
{
	x = y = z = 0.0f;
}

// A S U R A _ Q U A T ///////////////////////////////////////////////////////

/*
	Some details on quaternion operations from the Matrix & Quaternion FAQ:
		http://WWW.J3D.org/matrix_faq/matrfaq_latest.html
*/

inline Asura_Quat::Asura_Quat(float x0, float y0, float z0, float w0)
{
	x = x0;
	y = y0;
	z = z0;
	w = w0;
}

inline void Asura_Quat::Identity( void )
{
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
    w = 1.0f;
}

inline void Asura_Quat::Invert( void )
{
    x = -x;
    y = -y;
    z = -z;
}

inline Asura_Quat Asura_Quat::operator * (register float fS) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	Asura_Quat xResult;

	__asm__
	(
		"mtv %1, s710 \n" // Load scalar

	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%2 \n" // Load quat aligned
	#else
		"lv.s s700, 0+%2 \n" // Load quat elements
		"lv.s s701, 4+%2 \n"
		"lv.s s702, 8+%2 \n"
		"lv.s s703, 12+%2 \n"
	#endif

		"vscl.q c700, c700, s710 \n" // Scale quat

	#if  defined ASURA_PSP_Q4_ALIGNED
		"sv.q c700, 0+%0 \n" // Save quat aligned
	#else
		"sv.s s700, 0+%0 \n" // Save quat elements
		"sv.s s701, 4+%0 \n"
		"sv.s s702, 8+%0 \n"
		"sv.s s703, 12+%0 \n"
	#endif

	#if defined __SNC__
		: "=r" (&xResult)
		: "r" (fS), "r" (this)
	#else
		: "=o" (xResult)
		: "r" (fS), "o" (*this)
		: "c700"
	#endif
	);

	return xResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register const vfpu_v4 afThis = vfpu_v4_loadq(*this);
    register const vfpu_v4 afT = vfpu_v4_q(fS);
	register Asura_Quat xResult;

	return vfpu_v4_saveq(vfpu_vmul_q(afThis,afT,0),xResult);
#else
	return Asura_Quat(x*fS, y*fS, z*fS, w*fS);
#endif
}

inline Asura_Quat Asura_Quat::operator - () const
{
	return Asura_Quat(-x, -y, -z, -w);
}

inline Asura_Quat Asura_Quat::operator + (const Asura_Quat& xQ) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	Asura_Quat xResult;

	__asm__ (

	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat 1 aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat 1 elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c710, 0+%2 \n" // Load quat 2 aligned
	#else
		"lv.s s710, 0+%2 \n" // Load quat 2 elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"
		"lv.s s713, 12+%2 \n"
	#endif

		"vadd.q c720, c700, c710 \n" // Add quat

	#if defined ASURA_PSP_Q4_ALIGNED
		"sv.q c720, 0+%0 \n" // Save quat aligned
	#else
		"sv.s s720, 0+%0 \n" // Save quat elements
		"sv.s s721, 4+%0 \n"
		"sv.s s722, 8+%0 \n"
		"sv.s s723, 12+%0 \n"
	#endif

	#if defined __SNC__
		: "=r" (&xResult)
		: "r" (&xQ), "r" (this)
	#else
		: "=o" (xResult)
		: "o" (xQ), "o" (*this)
		: "c700", "c710", "c720"
	#endif
	);

	return xResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register const vfpu_v4 afThis = vfpu_v4_loadq(*this);
    register const vfpu_v4 afQ =  vfpu_v4_loadq(xQ);
	register Asura_Quat xResult;

	return vfpu_v4_saveq(vfpu_vadd_q(afThis,afQ,0),xResult);
#else
	return Asura_Quat(x+xQ.x, y+xQ.y, z+xQ.z, w+xQ.w);
#endif
}

inline bool Asura_Quat::ApproximatelyEquals(const Asura_Quat& xQ,
	/*const*/ float fEpsilon /*= 1.0e-6f*/) const
{
#if defined ASURA_ASSERT
	{
		static const float fFuzz = 0.0001f;
		const float fQMag = xQ.MagnitudeSquared();
		const float fThisMag = this->MagnitudeSquared();
		Asura_Assertf(
			Asura_Maths::Abs(fThisMag - 1.0f) < fFuzz
			&& Asura_Maths::Abs(fQMag - 1.0f) < fFuzz,
			("Quats (thisMag %f, qMag %f) must be normalised",fThisMag,fQMag)
		);
	}
#endif

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_v4 afThis = vfpu_v4_loadq(*this);
	register const vfpu_v4 afQ = vfpu_v4_loadq(xQ);
	register const float fCosOmega
		= vfpu_extract_float_from_v4x(vfpu_vdot_q(afQ,afThis,0));
#else
	register const float fCosOmega = xQ.w*w + xQ.x*x + xQ.y*y + xQ.z*z;
#endif

#if 0
	sceKernelPrintf("Q: (%.8f,%.8f,%.8f,%.8f) ~ (%.8f,%.8f,%.8f,%.8f): %.8f\n",
		x,y,z,w,xQ.x,xQ.y,xQ.z,xQ.w,fCosOmega);
#endif

	return( Asura_Maths::Abs(fCosOmega) >= (1.0f - fEpsilon) );
}

inline bool Asura_Quat::operator == (const Asura_Quat& q) const
{
	return x == q.x && y == q.y && z == q.z && w == q.w;
}

inline float Asura_Quat::MagnitudeSquared(void) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	float fResult;

	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

		"vdot.q s710, c700, c700 \n" // Dot product with itself

		"mfv %0, s710 \n" // Save result

		: "=r" (fResult)
	#if defined __SNC__
		: "r" (this)
	#else
		: "o" (*this)
		: "c700", "c710"
	#endif
	);

	return fResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_v4 afThis = vfpu_v4_loadq(*this);

    return vfpu_extract_float_from_v4x(vfpu_vdot_q(afThis,afThis,0));
#else
	return x*x+y*y+z*z+w*w;
#endif
}

inline void Asura_Quat::ToMatrix(Asura_Matrix_3x3& m) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && !defined GCC4_BUGGED
	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

		"vone.s s720 \n" // Load 1

		"vadd.q c600, c700, c700 \n" // Double values (xs, ys, zs, ws) (ws isn't needed, but it makes no difference to the calculation time to include it)
		"vscl.q c500, c600, s700 \n" // calculate xx, xy, xz, wx (note for these operations that a 4-element scale operation is exactly as efficient as a single-element multiply operation)

		"vscl.q c510, c600, s701 \n" // calculate xy, yy, yz, wy
		"vscl.q c520, c600, s702 \n" // calculate xz, yz, zz, wz
		"vscl.q c530, c600, s703 \n" // calculate wx, wy, wz, ww

		"vsub.s s600, s720, s511 \n" // 1 - yy
		"vsub.s s622, s720, s500 \n" // 1 - xx

        /* First column */
		"vadd.s s610, s501, s523 \n" // xy + wz => e21
		"vsub.s s620, s502, s513 \n" // xz - wy => e31
		"vsub.s s600, s600, s522 \n" // 1 - yy - zz => e11

        /* Second column */
		"vsub.s s601, s501, s523 \n" // xy - wz => e12
		"vadd.s s621, s512, s503 \n" // yz + wx => e32
		"vsub.s s611, s622, s522 \n" // 1 - xx - zz => e22

        /* Third column */
		"vadd.s s602, s502, s513 \n" // xz + wy => e13
		"vsub.s s612, s512, s503 \n" // yz - wx => e23
		"vsub.s s622, s622, s511 \n" // 1 - xx - yy => e33

        // Save matrix. Grr, no simple way to store triples, save out
        // one by one
		"sv.s s600, 0+%0 \n"
		"sv.s s601, 4+%0 \n"
		"sv.s s602, 8+%0 \n"

		"sv.s s610, 12+%0 \n"
		"sv.s s611, 16+%0 \n"
		"sv.s s612, 20+%0 \n"

		"sv.s s620, 24+%0 \n"
		"sv.s s621, 28+%0 \n"
		"sv.s s622, 32+%0 \n"

	#if defined __SNC__
		: "=r" (&m)
		: "r" (this)
	#else
		: "=o" (m)
		: "o" (*this)
		: "c700", "c720",
		  "c500", "c510", "c520", "c530",
		  "c600", "c610", "c620"
	#endif
	);
#else
	/* Implementation inspired by 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.e11 = 1.0f - (yy+zz);
	m.e21 = xy + wz;
	m.e31 = xz - wy;

	m.e12 = xy - wz;
	m.e22 = 1.0f - (xx + zz);
	m.e32 = yz + wx;

	m.e13 = xz + wy;
	m.e23 = yz - wx;
	m.e33 = 1.0f - (xx+yy);
#endif
}

inline void Asura_Quat::ToEuler(Asura_Euler_3& xEuler) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER
	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

        // Store Pi / 2 *WARNING* This is slightly different to the Pi
        // approximation used in Asura normally, so if values are
        // looking too different we need to load in our own value.
		"vcst.s s730, VFPU_PI_2 \n"
		"vone.s s731 \n" // Store 1

		"vadd.q c600, c700, c700 \n" // Double values (xs, ys, zs, ws) (ws isn't needed, but it makes no difference to the calculation time to include it)

		"vscl.q c500, c600, s700 \n" // calculate xx, xy, xz, wx (note for these operations that a 4-element scale operation is exactly as efficient as a single-element multiply operation)
		"vscl.q c510, c600, s701 \n" // calculate xy, yy, yz, wy
		"vscl.q c520, c600, s702 \n" // calculate xz, yz, zz, wz
		"vscl.q c530, c600, s703 \n" // calculate wx, wy, wz, ww

		"vsub.s s732, s503, s512 \n" // wx - yz = sin(euler.x)
		"vsat1.s s732, s732 \n" // Saturate to range [-1,1] to give a definite solution.
		"vasin.s s600, s732 \n" // euler.x = ArcSin((wx - yz):[-1,1])

		"vsub.s s733, s731, s500 \n" // 1 - xx
		"vadd.s s720, s502, s513 \n" // xz + wy
		"vsub.s s721, s733, s511 \n" // 1 - (xx + yy)

		INSERT_ARCTANGENT("s601", "s720", "s721", "0", "1", "2") // euler.y = ArcTan(xz + wy, 1 - (xx + yy))

		"vadd.s s722, s501, s523 \n" // xy + wz
		"vsub.s s723, s733, s522 \n" // 1 - (xx + zz)

		INSERT_ARCTANGENT("s602", "s722", "s723", "3", "4", "5") // euler.z = ArcTan(xy + wz, 1 - (xx + zz))

		"vscl.t c600, c600, s730 \n" // Scale our angles to the correct range (all should currently be 0..1 to corresond to 0..Pi/2)

		// Save euler. Grr, no simple way to store triples, save out one
		// by one
		"sv.s s600, 0+%0 \n"
		"sv.s s601, 4+%0 \n"
		"sv.s s602, 8+%0 \n"

	#if defined __SNC__
		: "=r" (&xEuler)
		: "r" (this)
	#else
		: "=o" (xEuler)
		: "o" (*this)
		: "c700", "c730", "c600", "c720",
		  "c500", "c510", "c520", "c530",
		  "c400", "c410", "c420", "c430" // for 'INSERT_ARCTANGENT'
	#endif
	);
#else
	// KJL - 14/05/2003 13:02:02 - quat to euler. This should return
	// exactly the same results as quat->matrix->euler, except where we
	// have an ambiguity in the euler formulation and lose a degree of
	// freedom ('gimbal lock' due to 90 degree rotations etc)

	float fXX = x*x;
	float fYY = y*y;
	float fZZ = z*z;

	float fXY = x*y;
	float fXZ = x*z;
	float fYZ = y*z;
	float fWX = w*x;
	float fWY = w*y;
	float fWZ = w*z;

	// KJL - 14/05/2003 12:54:43 - inverse sine is only defined for
	// values in the range -1 to +1 (simply because -1 <= sine(x) <= 1
	// for all x). At the -1 & +1 points the euler notation breaks down
	// because there is an infinite number of solutions (gimbal lock
	// etc)

	float fSineX = 2.0f*(fWX-fYZ);

	if (fSineX <= -1.0f)
	{
		xEuler.x = -Asura_Maths::Pi*0.5f;
	}
	else if (fSineX >= +1.0f)
	{
		xEuler.x = Asura_Maths::Pi*0.5f;
	}
	else
	{
		xEuler.x = Asura_Maths::ArcSine(fSineX);
	}

	// KJL - 14/05/2003 12:59:49 - arc tangent is well defined, so we'll
	// let it look after itself

	xEuler.y = Asura_Maths::ArcTangent( 2.0F*(fXZ+fWY), 1.0F-2.0F*(fXX+fYY) );

	xEuler.z = Asura_Maths::ArcTangent( 2.0F*(fXY+fWZ), 1.0F-2.0F*(fXX+fZZ) );
#endif
}

inline void Asura_Quat::FromEuler(const Asura_Euler_3& e)
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER
	__asm__ (

		"lv.s s700, 0+%1 \n" // Load euler elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"

        // Load 1/Pi. The angles need to be in the range 0..1 to signify
        // 0..Pi/2, so need to be scaled by 2/Pi. Also, we need to scale
        // the angles themselves by 1/2 for the calculation, hence why
        // we're using 1/Pi.
		"vcst.s s703, VFPU_1_PI \n"

        // Scale euler by 1/Pi (the multiplication value location is
        // irrelevant as we've used a constant insertion prefix to
        // replace its value)
		"vscl.t c700, c700, s703 \n"

		"vcos.t c710, c700 \n" // Calculate Cx, Cy, Cz
		"vsin.t c720, c700 \n" // Calculate Sx, Sy, Sz

		"vmul.s s503, s711, s712 \n" // Cy * Cz
		"vmul.s s501, s721, s712 \n" // Sy * Cz
		"vmul.s s502, s711, s722 \n" // Cy * Sz
		"vmul.s s500, s720, s503 \n" // Sx * Cy * Cz
		"vscl.t c501, c501, s710 \n" // Cx * Sy * Cz, Cx * Cy * Sz, Cx * Cy * Cz
		"vmul.s s513, s721, s722 \n" // Sy * Sz
		"vmul.s s511, s711, s722 \n" // Cy * Sz
		"vmul.s s512, s721, s712 \n" // Sy * Cz
		"vmul.s s510, s710, s513 \n" // Cx * Sy * Sz

        // Prefix first and second element (relating to y and z of final
        // quat) with negation, so we can add everything in one go

		"vpfxs -X, -Y, Z, W \n"
		"vscl.t c511, c511, s720 \n" // -Sx * Cy * Sz, -Sx * Sy * Cz, Sx * Sy * Sz
		"vadd.q c600, c500, c510 \n" // Sx * Cy * Cz + Cx * Sy * Sz, Cx * Sy * Cz - Sx * Cy * Sz, Cx * Cy * Sz - Sx * Sy * Cz, Cx * Cy * Cz + Sx * Sy * Sz = x, y, z, w

	#if defined ASURA_PSP_Q4_ALIGNED
		"sv.q c600, 0+%0 \n" // Save quat aligned
	#else
		"sv.s s600, 0+%0 \n" // Save quat elements
		"sv.s s601, 4+%0 \n"
		"sv.s s602, 8+%0 \n"
		"sv.s s603, 12+%0 \n"
	#endif

    #if defined __SNC__
		: "=r" (this)
		: "r" (&e)
    #else
		: "=o" (*this)
		: "o" (e)
		: "c700", "c710", "c720",
		  "c500", "c510", "c600"
    #endif
		);
#else
	float fCx = Asura_Maths::Cosine(e.x/2.0f);
	float fCy = Asura_Maths::Cosine(e.y/2.0f);
	float fCz = Asura_Maths::Cosine(e.z/2.0f);

	// KJL - 14/05/2003 16:56:21 -  the sines below could be calculated from the cosines above
	// by using sin^2+cos^2 == 1. Probably be faster on the PS2 which has a fast square root,
	// may be faster on the PC also.
	float fSx = Asura_Maths::Sine(e.x/2.0f);
	float fSy = Asura_Maths::Sine(e.y/2.0f);
	float fSz = Asura_Maths::Sine(e.z/2.0f);

	// KJL - 14/05/2003 16:57:56 - you know your maths is right when the ocde makes pretty patterns
	w = fCx * fCy * fCz + fSx * fSy * fSz;
	x = fSx * fCy * fCz + fCx * fSy * fSz;
	y = fCx * fSy * fCz - fSx * fCy * fSz;
	z = fCx * fCy * fSz - fSx * fSy * fCz;
#endif //#else //#if defined ASURA_PSP_USE_VFPU_ASSEMBLER
}


// (MH - 15.07.2002) adapted this from 3dc tools chunk lib.
// See also, Game Programming Gems I p.202.
inline void Asura_Quat::FromMatrix(const Asura_Matrix_3x3 & m)
{
	const int X=0;
	const int Y=1;
	const int Z=2;
	const int W=3;

	int const nxt[3] =
	{
		Y,Z,X
	};

	float trace,s;

	trace = m[0][0] + m[1][1] + m[2][2];

	if (trace > 0.0f)
	{
		s = Asura_Maths::Sqrt(trace + 1.0f);
		w = s * 0.5f;
		s = 0.5f / s;

		x = (m[2][1] - m[1][2]) * s;
		y = (m[0][2] - m[2][0]) * s;
		z = (m[1][0] - m[0][1]) * s;
	}
	else
	{
		int i,j,k;
		float q[4];

		i = X;
		if (m[Y][Y] > m[X][X]) i = Y;
		if (m[Z][Z] > m[i][i]) i = Z;
		j = nxt[i];
		k = nxt[j];

		s = Asura_Maths::Sqrt((m[i][i] - (m[j][j]+m[k][k])) + 1.0f);

		q[i] = s * 0.5f;
		s = 0.5f / s;
		q[W] = (m[k][j] - m[j][k]) * s;
		q[j] = (m[j][i] + m[i][j]) * s;
		q[k] = (m[k][i] + m[i][k]) * s;

		x = q[0];
		y = q[1];
		z = q[2];
		w = q[3];
	}

}

// creates a quaternion from an angle and axis of rotation
inline void Asura_Quat::FromRotation(/*const*/ Asura_Angle fAngle,
	const Asura_Vector_3& xAxis)
{
#if defined ASURA_ASSERT
	{
		static const float fFuzz = 0.0001f;
		const float fMagSq = xAxis.MagnitudeSquared();
		Asura_Assertf(
			Asura_Maths::Abs(fMagSq - 1.0f) < 0.0001f,
			("Axis (MagSq %.8f) should be normalised",fMagSq)
		);
	}
#endif

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	__asm__
	(
		"mtv %1, s700 \n" // Load angle scalar

		"lv.s s710, 0+%2 \n" // Load vector elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"

        // Load 1/Pi. The angles need to be in the range 0..1 to signify
        // 0..Pi/2, so need to be scaled by 2/Pi. Also, we need to scale
        // the angle itself by 1/2 for the calculation, hence why we're
        // using 1/Pi.
		"vcst.s s701, VFPU_1_PI \n"

		"vmul.s s700, s700, s701 \n" // Scale angle by 1/Pi
		"vsin.s s701, s700 \n" // Calculate sin(angle)
		"vcos.s s723, s700 \n" // cos(angle) = w
		"vscl.t c720, c710, s701 \n" // sin(angle) * axis = x, y, z

	#if defined ASURA_PSP_Q4_ALIGNED
		"sv.q c720, 0+%0 \n" // Save quat aligned
	#else
		"sv.s s720, 0+%0 \n" // Save quat elements
		"sv.s s721, 4+%0 \n"
		"sv.s s722, 8+%0 \n"
		"sv.s s723, 12+%0 \n"
	#endif

	#if defined __SNC__
		: "=r" (this)
		: "r" (fAngle), "r" (&xAxis)
	#else
		: "=o" (*this)
		: "r" (fAngle), "o" (xAxis)
		: "c700", "c710", "c720"
	#endif
		);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	/* Divide by two, and the to convert to hectogradians multiply by 2/PI */
	register const vfpu_v4 afAxis = vfpu_v4_load3(xAxis);
	register const vfpu_v4 afHAngle
		= vfpu_vmul_s(vfpu_v4_s(fAngle),vfpu_vcst_s(VFPU_CONST_1_PI,0),0);
	register vfpu_v4 afResult
		 = vfpu_vscl_t(afAxis,vfpu_vsin_s(afHAngle,0),0);
	
	afResult = vfpu_vmov_s_partial
		(afResult,vfpu_vcos_s(afHAngle,0),VFPU_VD_W|VFPU_VS_X);

	(void) vfpu_v4_saveq(afResult,*this);
#else
	fAngle *= 0.5f;
	float s = Asura_Maths::Sine(fAngle);
	x = xAxis.x * s;
	y = xAxis.y * s;
	z = xAxis.z * s;
	w = Asura_Maths::Cosine(fAngle);
#endif
}

// 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_Vector_3 Asura_Quat::RotateVector(const Asura_Vector_3& p) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && !defined GCC4_BUGGED
	Asura_Vector_3 xResult;
	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat aligned
    #else
		"lv.s s700, 0+%1 \n" // Load quat elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
    #endif

		"lv.s s710, 0+%2 \n" // Load vector elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"

		"vcrsp.t c600, c700, c710 \n" // v^p
		"vscl.t c600, c600, s703 \n" //w(v^p)

        // Prefix target with 2 for next calculation (multiplying each
        // element by 2 is the same as scaling the vector by 2, and
        // takes exactly the same VFPU cycles. This is used because
        // prefixing the target on the scale operation is prohibited.)
		"vpfxt 2, 2, 2, W \n"
		"vmul.t c600, c600, c500 \n" // 2w(v^p) => section1
		"vdot.t s613, c700, c710 \n" // v.p
		"vscl.t c610, c700, s613 \n" // (v.p)v

        // Prefix target with 2 for next calculation (see prefixing
        // comment above)
		"vpfxt 2, 2, 2, W \n"
		"vmul.t c610, c610, c500 \n" // 2(v.p)v => section2
		"vmul.s s620, s703, s703 \n" // w*w
		"vdot.t s621, c700, c700 \n" // v.v = vMag*vMag
		"vsub.s s623, s620, s621 \n" // w*w - vMag*vMag
		"vscl.t c620, c710, s623 \n" // (w*w - vMag*vMag)p => section3

		"vadd.t c720, c600, c610 \n" // section1 + section2
		"vadd.t c720, c720, c620 \n" // (section1 + section2) + section3

		"sv.s s720, 0+%0 \n" // Save vector elements
		"sv.s s721, 4+%0 \n"
		"sv.s s722, 8+%0 \n"

	#if defined __SNC__
		: "=r" (&xResult)
		: "r" (this), "r" (&p)
	#else
		: "=o" (xRresult)
		: "o" (*this), "o" (p)
		: "c700", "c710", "c600", "c610", "c620", "c720"
	#endif
	);

	return xResult;
#else
	const Asura_Vector_3 v(x,y,z); // the vector part of q
	Asura_Vector_3 section1, section2, section3;

	float vMag = v.Magnitude();

	section1 = v^p;
	section1 *= (2*w);

	section2 = v;
	section2 *= (2*(v*p));

	section3 = p;
	section3 *= (w*w - vMag*vMag);

	return section1+section2+section3;
#endif
}

inline Asura_Quat Asura_Quat::operator *(const Asura_Quat& xQ) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	Asura_Quat xResult;

	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat 1 aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat 1 elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c710, 0+%2 \n" // Load quat 2 aligned
	#else
		"lv.s s710, 0+%2 \n" // Load quat 2 elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"
		"lv.s s713, 12+%2 \n"
	#endif

		"vqmul.q c720, c710, c700 \n" // Multiply quats

	#if defined ASURA_PSP_Q4_ALIGNED
		"sv.q c720, 0+%0 \n" // Save quat aligned
	#else
		"sv.s s720, 0+%0 \n" // Save quat elements
		"sv.s s721, 4+%0 \n"
		"sv.s s722, 8+%0 \n"
		"sv.s s723, 12+%0 \n"
	#endif

	#if defined __SNC__
		: "=r" (&xResult)
		: "r" (&xQ), "r" (this)
	#else
		: "=o" (xResult)
		: "o" (xQ), "o" (*this)
		: "c700", "c710", "c720"
	#endif
	);

	return xResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register const vfpu_v4 afThis = vfpu_v4_loadq(*this);
	register const vfpu_v4 afQ = vfpu_v4_loadq(xQ);
	Asura_Quat xResult;

	return vfpu_v4_saveq(vfpu_vqmul_q(afThis,afQ),xResult);
#else
	const Asura_Vector_3 v1(x,y,z);
	const Asura_Vector_3 v2(xQ.x, xQ.y, xQ.z);
	const Asura_Vector_3 temp = v1*xQ.w + v2*w + (v1^v2);

	return Asura_Quat(temp.x, temp.y, temp.z, w*xQ.w - v1*v2);
#endif
}

inline void Asura_Quat::Normalise(void)
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	float fResult;

	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

		"vdot.q s710, c700, c700 \n" // Dot product with itself
		"vcmp.s EZ, s710 \n" // Compare result with 0
		"bvt 0, 0f \n" // Skip to return if comparison is true
		"vnop \n"

		"vsqrt.s s710, s710 \n" // Squareroot (stored for return value)
		"vone.s s711 \n"
		"vdiv.s s711, s711, s710 \n" // Reciprocal
		"vscl.q c700, c700, s711 \n" // Scale vector by reciprocal of squareroot

	#if defined ASURA_PSP_Q4_ALIGNED
		"sv.q c700, 0+%1 \n" // Save quat
	#else
		"sv.s s700, 0+%1 \n" // Save quat elements
		"sv.s s701, 4+%1 \n"
		"sv.s s702, 8+%1 \n"
		"sv.s s703, 12+%1 \n"
	#endif

	"0:\n" // Label for branching past main operations
		"mfv $8, s710 \n" // Save magnitude (or 0 if mag squared was 0 and we branched past most of the operations)
		"mtc1 $8, %0 \n"

	#if defined __SNC__
		: "=f" (fResult), "+r" (this)
		:
		: "$8"
	#else
		: "=f" (fResult), "+o" (*this)
		:
		: "$8", "c700", "c710"
	#endif
	);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	register const vfpu_v4 afThis = vfpu_v4_loadq(*this);
	register const vfpu_v4 afMagSq = vfpu_vdot_q(afThis,afThis,0);

	if (vfpu_extract_float_from_v4x(afMagSq) == 0.0f)
	{
		/*
			Note that for 'Asura_AVector3::Normalise' there is an
			'assert' instead.
		*/
	}
	else
	{
		register const vfpu_v4 afMag = vfpu_vsqrt_s(afMagSq,0);

		(void) vfpu_v4_saveq(vfpu_vscl_q(afThis,vfpu_v4s_inv(afMag),0),*this);
	}
#else
	const float fMag = Asura_Maths::Sqrt((x*x)+(y*y)+(z*z)+(w*w));

	if (fMag > 0.0f)
	{
		const float fIMag = 1.0f/fMag;

		x *= fIMag;
		y *= fIMag;
		z *= fIMag;
		w *= fIMag;
	}
#endif
}

inline float Asura_Quat::Dot(const Asura_Quat& xQ) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	float fResult;

	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat 1 aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat 1 elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c710, 0+%2 \n" // Load quat 2 aligned
	#else
		"lv.s s710, 0+%2 \n" // Load quat 2 elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"
		"lv.s s713, 12+%2 \n"
	#endif

		"vdot.q s720, c700, c710 \n" // Dot product

		"mfv %0, s720 \n" // Save result

		: "=r" (fResult)
	#if defined __SNC__
		: "r" (&xQ), "r" (this)
	#else
		: "o" (xQ), "o" (*this)
		: "c700", "c710", "c720"
	#endif
	);

	return fResult;
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS
	register const vfpu_v4 afThis = vfpu_v4_loadq(*this);
	register const vfpu_v4 afQ = vfpu_v4_loadq(xQ);
	Asura_Quat xResult;

    return vfpu_extract_float_from_v4x(vfpu_vdot_q(afThis,afQ,0));
#else
	return x*xQ.x+y*xQ.y+z*xQ.z+w*xQ.w;
#endif
}

inline void Asura_Quat::Slerp(const Asura_Quat& input1, const Asura_Quat& input2, float l)
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && !defined GCC4_BUGGED
	static const float fComparison = 65500.0f/65536.0f; // The same value used in the non-VFPU version, for consistancy

	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat 1 aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat 1 elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c710, 0+%2 \n" // Load quat 2 aligned
	#else
		"lv.s s710, 0+%2 \n" // Load quat 2 elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"
		"lv.s s713, 12+%2 \n"
	#endif

		"mtv %3, s730 \n" // Load lerp float (into location we will use as sclq, as that's the base value for that)
		"mtv %4, s733 \n" // Load comparison constant

		"vdot.q s600, c700, c710 \n" // cosom = input1.input2
		"vzero.s s601 \n" // Load zero
		"vone.s s602 \n" // Load one
		"vsge.s s603, s601, s600 \n" // Store (0>=cosom) (so we know that we want to negate sclq later as cosom has been negated by the following call
		"vabs.s s600, s600 \n" // if (cosom <= 0.0f) cosom = -cosom
		"vsub.s s731, s602, s730 \n" // base value for sclp: 1 - l

		"vcmp.s LT, s600, s733 \n" // comparison (cosom < 65500.0f/65536.0f)
		"bvf 0, 0f \n" // Skip next section (using base values for sclp and sclq) if above comparison is false
		"vnop \n"

		"vasin.s s610, s600 \n" // asin(cosom)
        // Pi/2 - asin(cosom) = acos(cosom) = omega (note that the
        // VFPU's angle encoding of 0..1 for 0..Pi/2 means that this
        // subtraction of the asin value from one is the correct
        // calculation)
		"vsub.s s610, s602, s610 \n"
		"vsin.s s611, s610 \n" // sin(omega) = sinom
		"vscl.p c730, c730, s610 \n" // l*omega, (1-l)*omega
		"vrcp.s s612, s611 \n" // 1/sinom, for upcoming division
		"vsin.p c630, c730 \n" // sin(l*omega), sin((1-l)*omega)
		"vscl.p c730, c630, s612 \n" // sin(l*omega)/sinom, sin((1-l)*omega)/sinom = sclq, sclp

	"0:\n"
		"vcmp.s NZ, s603 \n" // Check if our earlier stored comparison was true
		"bvf 0, 1f \n" // Skip to end if it was false
		"vnop \n"

		"vneg.s s730, s730 \n" // cosom was <= 0, so negate sclq

	"1:\n"
		"vscl.q c500, c700, s731 \n" // input1 * sclp
		"vscl.q c510, c710, s730 \n" // input2 * sclq
		"vadd.q c720, c500, c510 \n" // (input1 * sclp) + (input2 * sclq) = unnormalised result
		"vdot.q s530, c720, c720 \n" // Normalise: Dot product with itself

		"vcmp.s EZ, s530 \n" // Compare result with 0
		"bvt 0, 2f \n" // Skip to return if comparison is true
		"vnop \n"

		"vrsq.s s530, s530 \n" // Reciprocal sqareroot of magnitude
		"vscl.q c720, c720, s530 \n" // Scale vector by reciprocal of squareroot

	"2:\n"

	#if defined ASURA_PSP_Q4_ALIGNED
		"sv.q c720, 0+%0 \n" // Save quat aligned
	#else
		"sv.s s720, 0+%0 \n" // Save quat elements
		"sv.s s721, 4+%0 \n"
		"sv.s s722, 8+%0 \n"
		"sv.s s723, 12+%0 \n"
	#endif


	#if defined __SNC__
		: "=r" (this)
		: "r" (&input1), "r" (&input2),
          "r" (l), "r" (fComparison)
	#else
		: "=o" (*this)
		: "o" (input1), "o" (input2),
          "r" (l), "r" (fComparison)
		: "c700", "c710", "c720", "c730",
		  "c600", "c610",
		  "c500", "c510", "c530"
	#endif
		);
#else
	float sclp,sclq;

	float cosom;
	float omega, sinom;
	bool bNegateInput2;

	cosom=input1.Dot(input2);

	if (cosom>0.0f)
	{
		bNegateInput2 = false;

	}
	else
	{
		bNegateInput2 = true;
		cosom =-cosom;
	}

	if(cosom<65500.0f/65536.0f)
	{
		omega=Asura_Maths::ArcCosine(cosom);
		sinom=Asura_Maths::Sine(omega);

		sclp= Asura_Maths::Sine((1.0f-l)*omega)/sinom;
		sclq= Asura_Maths::Sine((l)*omega)/sinom;

	}
	else
	{
		sclp=1.0f - l;
		sclq=l;
	}

	if(bNegateInput2) sclq = -sclq;

	*this = input1*sclp + input2*sclq;
	this->Normalise();

#endif
}

inline void Asura_Quat::NLerp(const Asura_Quat& xInput1, const Asura_Quat& xInput2, float fDist)
{
	//To be checked....
	//Need to make sure that the assembler version isn't bugged when fCosOmega is 0
	//In this case Sign(fCosOmega) is 0 , but it needs to be either 1 or -1.
	//RAR 28/11/2006

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && !defined GCC4_BUGGED
	__asm__
	(
		"lv.s s700, 0+%1 \n" // Load quat 1 elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"

		"lv.s s710, 0+%2 \n" // Load quat 2 elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"
		"lv.s s713, 12+%2 \n"

		"vdot.q s720, c700, c710 \n" // cosom = input1.input2
		"mtv %3, s730 \n" // s730 = fDist
		"vone.s s722 \n" // s722 = 1
		"vsgn.s s723, s720 \n" // s723 = sign(cosom)
		"vcmp.s EZ, s723 \n" // compare sign(cosom) to zero
		"vcmovt.s s723, s722, 0 \n" // if sign(cosom) == zero, set sign(cosom) = 1
		"vsub.s s731, s722, s730 \n" // s731 = 1 - fDist
		"vmul.s s730, s730, s723 \n" // s730 = fDist * Sign(cosom)

		"vscl.q c700, c700, s731 \n" // xInput1 * (1 - fDist)
		"vscl.q c710, c710, s730 \n" // xInput2 * (fDist * Sign(cosom))
		"vadd.q c700, c700, c710 \n" // (xInput1 * (1 - fDist)) + (xInput2 * (fDist * Sign(cosom))) = unnormalised result
		"vdot.q s710, c700, c700 \n" // Normalise: Magnitude = Dot product with itself
		"vrsq.s s710, s710 \n" // Reciprocal squareroot of magnitude
		"vscl.q c700, c700, s710 \n" // Scale vector by reciprocal of squareroot

		"svr.q c700, 0+%0 \n" // Save quat unaligned
		"svl.q c700, 12+%0 \n"

#if defined __SNC__
		: "=m" (*this)
		: "m" (xInput1), "m" (xInput2), "r" (fDist)
#else
		: "=o" (*this)
		: "o" (xInput1), "o" (xInput2), "r" (fDist)
		:  "c700", "c710", "c720", "c730"
#endif
	);
#else //#ifdef ASURA_PSP_USE_VFPU_ASSEMBLER
		// need cosine of angle between quats to determine shortest path. If cos is
		// negative, invert the second input.
		float fCosOmega = xInput1.Dot(xInput2);
		float fSign = fCosOmega >= 0.0f ? 1.0f : -1.0f;
		*this = xInput1 * (1.0f - fDist) + xInput2 * (fDist * fSign);
		this->Normalise();
#endif //#else //#ifdef ASURA_PSP_USE_VFPU_ASSEMBLER
}

/// ASURA_COMP_QUAT. /////////////////////////
// KOR - 17/06/2003 11:42:29
// Game Programming Gems 3, section 2.4, suggests this 32-bit quat format:
//
// 3 bits - signs of x, y, z components.
//   If w is negative, negate all the other signs, because rotating
//   through a negated value through a negated axis is the same as the
//   original rotation
// 9 bits - yaw
// 9 bits - pitch
//   Generate these from a normalised (abs(x), abs(y), abs(z))
//   Because we are taking the absolute values of x, y, z, we have 0 <=
//   yaw, pitch <= pi/2 and can rescale to 9 bits appropriately.
//   The other advantage to this range is that apparently order 5 Taylor
//   or Lagrange trig.  approximations are sufficient. I have not
//   implemented these yet; we shall have to see how performance is
//   affected.
// 11 bits - w - just rescale by 1024
//
// From these values, we can extract the normalised quat

#define PI_OVER_2 1.57080f
#define PI_OVER_512 0.0061359231f
#define ONE_OVER_1024 0.0009765625f

inline void Asura_Comp_Quat::ToQuat(Asura_Quat& xQuat) const
{
	float yaw	= (float)((m_uBits & 0x1FF00000) >> 20) * PI_OVER_512;
	if(yaw > PI_OVER_2) yaw = PI_OVER_2;
	float pitch	= (float)((m_uBits & 0x000FF800) >> 11) * PI_OVER_512;
	if(pitch > PI_OVER_2) pitch = PI_OVER_2;
	xQuat.w	= (float)((m_uBits & 0x000007FF)) * ONE_OVER_1024;
	if(xQuat.w >= 1.0f)
	{
		xQuat.x = 0.0f;
		xQuat.y = 0.0f;
		xQuat.z = 0.0f;
		xQuat.w = 1.0f;
	}
	else
	{
		float /*fSinPitch,*/ fCosPitch, fSinYaw, fCosYaw;
		Asura_Maths::SinCos2(pitch, yaw, xQuat.y/*fSinPitch*/, fCosPitch, fSinYaw, fCosYaw);
		xQuat.x = fCosPitch * fSinYaw;
//		xQuat.y = fSinPitch;
		xQuat.z = fCosPitch * fCosYaw;
		if(m_uBits & 0x80000000) xQuat.x = -xQuat.x;
		if(m_uBits & 0x40000000) xQuat.y = -xQuat.y;
		if(m_uBits & 0x20000000) xQuat.z = -xQuat.z;
		float length = Asura_Maths::Sqrt(1.0f - xQuat.w * xQuat.w);
		xQuat.x *= length;
		xQuat.y *= length;
		xQuat.z *= length;
	}
}

#define F512_OVER_PI 162.97466f

inline void Asura_Comp_Quat::FromQuat(const Asura_Quat& xQuat)
{
	Asura_Vector_3 v;
	v.x = xQuat.x;
	v.y = xQuat.y;
	v.z = xQuat.z;
	v.Normalise();
	// Signs
	if(xQuat.w > 0.0f)
	{
		m_uBits  = ((xQuat.x > 0.0f) ? 0 : 1) << 31;
		m_uBits |= ((xQuat.y > 0.0f) ? 0 : 1) << 30;
		m_uBits |= ((xQuat.z > 0.0f) ? 0 : 1) << 29;
	}
	else
	{
		m_uBits  = ((xQuat.x < 0.0f) ? 0 : 1) << 31;
		m_uBits |= ((xQuat.y < 0.0f) ? 0 : 1) << 30;
		m_uBits |= ((xQuat.z < 0.0f) ? 0 : 1) << 29;
	}
	// Yaw = atan(x, z)
	unsigned int yaw = Asura_Maths::FloatToInt_Near(F512_OVER_PI * Asura_Maths::ArcTangent(Asura_Maths::Abs(v.x), Asura_Maths::Abs(v.z)));
	m_uBits |= ((yaw << 20) & 0x1FF00000);
	// Pitch = asin(y)
	unsigned int pitch = Asura_Maths::FloatToInt_Near(F512_OVER_PI * Asura_Maths::ArcSine(Asura_Maths::Abs(v.y)));
	m_uBits |= ((pitch << 11) & 0x000FF800);
	// W
	unsigned int w = Asura_Maths::FloatToInt_Near(1024.0f * Asura_Maths::Abs(xQuat.w));
	m_uBits |= (w & 0x000007FF);
}

// KOR - 8/11/2005 - This is an implementation of the "smallest three" quaternion compression method
// detailed in Game Programming Gems 3. It appears to be just as accurate as the polar co-ordinate method,
// and much faster to decompress.
const float fSQRT_2 = (1.4142135623730950488016887242097f);
const float fONE_OVER_SQRT_2  = (1.f / fSQRT_2);

void Asura_Comp_Quat_Fast::ToQuat(Asura_Quat& xQuat) const
{
	// Bottom two bits house the offset to the largest element (0 = w, 1 = x, 2 = y, 3 = z).
	const u_int uOffset = m_uBits & 0x03;
	// Extract the three smallest values.
	const float fValue0 = static_cast<float>( (m_uBits & 0xFFC70000) >> 22) * (fONE_OVER_SQRT_2 * 2.f / 1022.f) - fONE_OVER_SQRT_2;
	const float fValue1 = static_cast<float>( (m_uBits & 0x003FF000) >> 12) * (fONE_OVER_SQRT_2 * 2.f / 1022.f) - fONE_OVER_SQRT_2;
	const float fValue2 = static_cast<float>( (m_uBits & 0x00000FFC) >>  2) * (fONE_OVER_SQRT_2 * 2.f / 1022.f) - fONE_OVER_SQRT_2;
	// Calculate the largest value using the knowledge that the quat is normalised.
	const float fValue3Squared = 1.f - (fValue0 * fValue0) - (fValue1 * fValue1) - (fValue2 * fValue2);
	const float fValue3 = fValue3Squared > 0.f ? Asura_Maths::Sqrt(fValue3Squared) : 0.f;
	// Put the values in the correct places.
	float* const pfArray = reinterpret_cast<float*>(&xQuat);
	pfArray[(0 + uOffset) & 0x03] = fValue0;
	pfArray[(1 + uOffset) & 0x03] = fValue1;
	pfArray[(2 + uOffset) & 0x03] = fValue2;
	pfArray[(3 + uOffset) & 0x03] = fValue3;
}

void Asura_Comp_Quat_Fast::FromQuat(const Asura_Quat& xQuat)
{
#if defined ASURA_ASSERT
	{
		static const float fFuzz = 0.0001f;
		const float fQMag = xQuat.MagnitudeSquared();
		Asura_Assertf(
			(1.f - fFuzz) < fQMag && fQMag < (1.f + fFuzz),
			("Quat (fQMag %f) must be normalised",fQMag)
		);
	}
#endif

	// Find the offset of the largest value (0 = w, 1 = x, 2 = y, 3 = z). Slightly clunky way of doing it.
	Asura_Quat xAbsQuat(Asura_Maths::Abs(xQuat.x), Asura_Maths::Abs(xQuat.y), Asura_Maths::Abs(xQuat.z), Asura_Maths::Abs(xQuat.w));
	const float fLargestValue = Asura_Maths::Max(Asura_Maths::Max(xAbsQuat.x, xAbsQuat.y), Asura_Maths::Max(xAbsQuat.z, xAbsQuat.w));
	if(fLargestValue == xAbsQuat.x)			m_uBits = 1;
	else if(fLargestValue == xAbsQuat.y)	m_uBits = 2;
	else if(fLargestValue == xAbsQuat.z)	m_uBits = 3;
	else if(fLargestValue == xAbsQuat.w)	m_uBits = 0;
	// We store 0, 1, 2, and calculate 3 (the largest)
	float fValue0, fValue1, fValue2, fValue3;
	const float* pfArray = reinterpret_cast<const float*>(&xQuat);
	fValue0 = pfArray[(0 + m_uBits) & 0x03];
	fValue1 = pfArray[(1 + m_uBits) & 0x03];
	fValue2 = pfArray[(2 + m_uBits) & 0x03];
	fValue3 = pfArray[(3 + m_uBits) & 0x03];
	// If the largest is negative, we negate all the other elements using the fact that q = -q
	if(fValue3 < 0.f)
	{
		fValue0 = -fValue0;
		fValue1 = -fValue1;
		fValue2 = -fValue2;
	}
	// Pack the three smallest values into the remaining 30 bits (10
	// bits each) Using a range of 1022 so that we can get exactly zero
	// while allowing the maximum and minimum values Regarding use of
	// FloatToInt_Near, I tried doing a RMS comparison of either side of
	// each float to int, but it made no discernable difference, so I
	// dropped it.
	m_uBits |= Asura_Maths::FloatToInt_Near( (fONE_OVER_SQRT_2 + fValue0) * (fSQRT_2 * 1022.f / 2.f) ) << 22;
	m_uBits |= Asura_Maths::FloatToInt_Near( (fONE_OVER_SQRT_2 + fValue1) * (fSQRT_2 * 1022.f / 2.f) ) << 12;
	m_uBits |= Asura_Maths::FloatToInt_Near( (fONE_OVER_SQRT_2 + fValue2) * (fSQRT_2 * 1022.f / 2.f) ) <<  2;
}

// A S U R A _ B O U N D I N G _ R E C T A N G L E . /////////////////////////
inline void Asura_Bounding_Rectangle::Reset(void)
{
	// set to exaggeratedly large values
	MinX = Asura_Maths::LargeFloat;
	MinY = Asura_Maths::LargeFloat;

	MaxX = -Asura_Maths::LargeFloat;
	MaxY = -Asura_Maths::LargeFloat;
}

inline void Asura_Bounding_Rectangle::Set( const Asura_Vector_2& v )
{
	if (v.x > MaxX) MaxX = v.x;
	if (v.y > MaxY) MaxY = v.y;

	if (v.x < MinX) MinX = v.x;
	if (v.y < MinY) MinY = v.y;
}

inline bool Asura_Bounding_Rectangle::Intersects(const Asura_Bounding_Rectangle& xRect) const
{
    if ( ((MaxX >= xRect.MinX) && (MinX <= xRect.MaxX))
      && ((MaxY >= xRect.MinY) && (MinY <= xRect.MaxY)) )
    {
        return true;
    }
    else return false;
}

inline bool Asura_Bounding_Rectangle::Contains( const Asura_Vector_2& point) const
{
	if ((point.x>=MinX && point.x<=MaxX)
	&&(point.y>=MinY && point.y<=MaxY))
	{
		return true;
	}
	return false;
}

inline Asura_Vector_2 Asura_Bounding_Rectangle::CalculateCentre() const
{
	return Asura_Vector_2( (MinX + MaxX) * 0.5f,
						(MinY + MaxY) * 0.5f );
}

inline void Asura_Bounding_Rectangle::Displace( const Asura_Vector_2& xDisp )
{
	MinX += xDisp.x; 
	MaxX += xDisp.x; 
	MinY += xDisp.y; 
	MaxY += xDisp.y;	
}


// A S U R A _ B O U N D I N G _ B O X ///////////////////////////////////////
inline void Asura_Bounding_Box::Reset(void)
{
	// set to exaggeratedly large values
	MinX = Asura_Maths::LargeFloat;
	MinY = Asura_Maths::LargeFloat;
	MinZ = Asura_Maths::LargeFloat;

	MaxX = -Asura_Maths::LargeFloat;
	MaxY = -Asura_Maths::LargeFloat;
	MaxZ = -Asura_Maths::LargeFloat;
}

inline void Asura_Bounding_Box::Set(const class Asura_Vector_3& v)
{
	if (v.x > MaxX) MaxX = v.x;
	if (v.y > MaxY) MaxY = v.y;
	if (v.z > MaxZ) MaxZ = v.z;

	if (v.x < MinX) MinX = v.x;
	if (v.y < MinY) MinY = v.y;
	if (v.z < MinZ) MinZ = v.z;
}

inline void Asura_Bounding_Box::Set(const Asura_Bounding_Box& bb)
{
	if (bb.MaxX > MaxX) MaxX = bb.MaxX;
	if (bb.MaxY > MaxY) MaxY = bb.MaxY;
	if (bb.MaxZ > MaxZ) MaxZ = bb.MaxZ;

	if (bb.MinX < MinX) MinX = bb.MinX;
	if (bb.MinY < MinY) MinY = bb.MinY;
	if (bb.MinZ < MinZ) MinZ = bb.MinZ;
}

inline Asura_Vector_3 Asura_Bounding_Box::CalculateCentre(void) const
{
	return Asura_Vector_3( (MinX + MaxX) * 0.5f,
						(MinY + MaxY) * 0.5f,
						(MinZ + MaxZ) * 0.5f );
}

inline Asura_Vector_3 Asura_Bounding_Box::CalculateExtents( void ) const
{
	return Asura_Vector_3( MaxX - MinX, MaxY - MinY, MaxZ - MinZ );
}

inline float Asura_Bounding_Box::GetVolume(void) const
{
	float x = MaxX-MinX;
	float y = MaxY-MinY;
	float z = MaxZ-MinZ;
	return x*y*z;
}

inline bool Asura_Bounding_Box::Intersects(const Asura_Bounding_Box& b) const
{
	if ( ((MaxX >= b.MinX) && (MinX <= b.MaxX))
	&& ((MaxY >= b.MinY) && (MinY <= b.MaxY))
	&& ((MaxZ >= b.MinZ) && (MinZ <= b.MaxZ)) )
	{
		return true;
	}
	else return false;
}


inline bool Asura_Bounding_Box::Intersects(const Asura_Bounding_Box& b, const Asura_Vector_3 & offset) const
{
	if ( ((MaxX >= b.MinX+offset.x) && (MinX <= b.MaxX+offset.x))
	&& ((MaxY >= b.MinY+offset.y) && (MinY <= b.MaxY+offset.y))
	&& ((MaxZ >= b.MinZ+offset.z) && (MinZ <= b.MaxZ+offset.z)) )
	{
		return true;
	}
	else return false;
}

inline bool Asura_Bounding_Box::IntersectsSphere(const Asura_Vector_3& centre, const float radius) const
{
	if ( ((MaxX>=centre.x-radius) && (MinX <= centre.x+radius))
	&& ((MaxY>=centre.y-radius) && (MinY <= centre.y+radius))
	&& ((MaxZ>=centre.z-radius) && (MinZ <= centre.z+radius)) )
	{
		return true;
	}
	else return false;
}

inline bool Asura_Bounding_Box::Contains(const Asura_Vector_3& point) const
{
	if ((point.x>=MinX && point.x<=MaxX)
	&&(point.y>=MinY && point.y<=MaxY)
	&&(point.z>=MinZ && point.z<=MaxZ))
		return true;

	return false;

}

inline bool Asura_Bounding_Box::Contains(const Asura_Bounding_Box& b) const
{
	if ((b.MinX>=MinX && b.MaxX<=MaxX)
	&&(b.MinY>=MinY && b.MaxY<=MaxY)
	&&(b.MinZ>=MinZ && b.MaxZ<=MaxZ))
	{
		return true;
	}

	return false;
}

inline void Asura_Bounding_Box::Displace(const Asura_Vector_3& displacement)
{
	MaxX += displacement.x;
	MinX += displacement.x;
	MaxY += displacement.y;
	MinY += displacement.y;
	MaxZ += displacement.z;
	MinZ += displacement.z;
}

//
// Asura_RandomGenerator.
//

inline float Asura_RandomGenerator::GetFloat( void )
{
	return( static_cast<int>(GetIntU()) / static_cast<float>(INT_MAX) );
}

inline float Asura_RandomGenerator::GetFloat( float fMin, float fMax )
{
	return( fMin + GetFloat() * (fMax - fMin) );
}

inline int Asura_RandomGenerator::GetInt( int iMin, int iMax )
{
	// Doing something like (iMin + GetFloat() * (iMax - iMin)) would
	// actually be faster but leads to gaps in the output with really
	// large ranges. While this probably won't ever be an issue, nor
	// should the performance, and so the more robust implementation is
	// probably a better choice.
	return( iMin + GetIntU() % (iMax - iMin + 1) );
}

inline u_int Asura_RandomGenerator::GetIntU( u_int uMin, u_int uMax )
{
	// This is ok since unsigned range only goes up to INT_MAX as well.
	return GetInt( uMin, uMax );
}


//
// Asura_Complex.
//

inline Asura_Complex& Asura_Complex::operator*=( const Asura_Complex& xOther )
{
	// (a + ib) * (c + id)
	float fAC = m_fR * xOther.m_fR;
	float fAD = m_fR * xOther.m_fI;
	float fBC = m_fI * xOther.m_fR;
	float fBD = m_fI * xOther.m_fI;
	m_fR = fAC - fBD;
	m_fI = fAD + fBC;
	return *this;
}

inline Asura_Complex& Asura_Complex::operator+=( const Asura_Complex& xOther )
{
	m_fR += xOther.m_fR;
	m_fI += xOther.m_fI;
	return *this;
}

inline Asura_Complex& Asura_Complex::operator-=( const Asura_Complex& xOther )
{
	m_fR -= xOther.m_fR;
	m_fI -= xOther.m_fI;
	return *this;
}

inline Asura_Complex Asura_Complex::operator*( const Asura_Complex& xOther ) const
{
	return Asura_Complex( m_fR * xOther.m_fR - m_fI * xOther.m_fI, m_fR * xOther.m_fI + m_fI * xOther.m_fR );
}

inline Asura_Complex Asura_Complex::operator+( const Asura_Complex& xOther ) const
{
	return Asura_Complex( m_fR + xOther.m_fR, m_fI + xOther.m_fI );
}

inline Asura_Complex Asura_Complex::operator-( const Asura_Complex& xOther ) const
{
	return Asura_Complex( m_fR - xOther.m_fR, m_fI - xOther.m_fI );
}

#if defined ASURA_USE_PLATFORM_VECTORS
#include "Core/Asura_PSP_VectorMaths.h"
#endif

#if defined ASURA_USE_PLATFORM_VECTORS
inline void Asura_Quat::ToMatrix(Asura_AMatrix3x3& m) const
{
#if defined ASURA_PSP_USE_VFPU_ASSEMBLER
	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

		"vone.s s720 \n" // Load 1

		"vadd.q c600, c700, c700 \n" // Double values (xs, ys, zs, ws) (ws isn't needed, but it makes no difference to the calculation time to include it)

		"vscl.q c500, c600, s700 \n" // calculate xx, xy, xz, wx (note for these operations that a 4-element scale operation is exactly as efficient as a single-element multiply operation)
		"vscl.q c510, c600, s701 \n" // calculate xy, yy, yz, wy
		"vscl.q c520, c600, s702 \n" // calculate xz, yz, zz, wz
		"vscl.q c530, c600, s703 \n" // calculate wx, wy, wz, ww

		"vsub.s s600, s720, s511 \n" // 1 - yy
		"vsub.s s622, s720, s500 \n" // 1 - xx
		"vadd.s s610, s501, s523 \n" // xy + wz = e21
		"vsub.s s620, s502, s513 \n" // xz - wy = e31
		"vsub.s s600, s600, s522 \n" // 1 - yy - zz = e11

		"vsub.s s601, s501, s523 \n" // xy - wz = e12
		"vadd.s s621, s512, s503 \n" // yz + wx = e32
		"vsub.s s611, s622, s522 \n" // 1 - xx - zz = e22

		"vadd.s s602, s502, s513 \n" // xz + wy = e13
		"vsub.s s612, s512, s503 \n" // yz - wx = e23
		"vsub.s s622, s622, s511 \n" // 1 - xx - yy = e33

		"sv.q r600, 0+%0 \n" // Save matrix aligned
		"sv.q r601, 16+%0 \n"
		"sv.q r602, 32+%0 \n"

	#if defined __SNC__
		: "=r" (&m)
		: "r" (this)
	#else
		: "=o" (m)
		: "o" (*this)
		: "c700", "c720",
		  "c500", "c510", "c520", "c530",
		  "c600", "c610", "c620"
	#endif
	);
#else
	/* 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) ));
#endif
}

inline void Asura_Quat::FromMatrix(const Asura_AMatrix3x3 & xM4)
{
	Asura_Matrix_3x3 xM3;
	xM4.ToMat3(xM3);
	FromMatrix(xM3);
}

inline Asura_AVector3 Asura_Quat::RotateVector(const Asura_AVector3& p) const
{
	//-----------------------------------------------------
	// 
	// 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
	//
	//--------------------------------------------------------
#ifdef ASURA_PSP_USE_VFPU_ASSEMBLER
	Asura_AVector3 xResult;

	__asm__
	(
	#if defined ASURA_PSP_Q4_ALIGNED
		"lv.q c700, 0+%1 \n" // Load quat aligned
	#else
		"lv.s s700, 0+%1 \n" // Load quat elements
		"lv.s s701, 4+%1 \n"
		"lv.s s702, 8+%1 \n"
		"lv.s s703, 12+%1 \n"
	#endif

		"lv.q c710, 0+%2 \n" // Load avector aligned

		"vcrsp.t c600, c700, c710 \n" // v^p
		"vscl.t c600, c600, s703 \n" //w(v^p)
		"vpfxt 2, 2, 2, W \n" // Prefix target with 2 for next calculation (multiplying each element by 2 is the same as scaling the vector by 2, and takes exactly the same VFPU cycles. This is used because prefixing the target on the scale operation is prohibited.)
		"vmul.t c600, c600, c500 \n" // 2w(v^p) = section1
		"vdot.t s613, c700, c710 \n" // v.p
		"vscl.t c610, c700, s613 \n" // (v.p)v
		"vpfxt 2, 2, 2, W \n" // Prefix target with 2 for next calculation (see prefixing comment above)
		"vmul.t c610, c610, c500 \n" // 2(v.p)v = section2
		"vmul.s s620, s703, s703 \n" // w*w
		"vdot.t s621, c700, c700 \n" // v.v = vMag*vMag
		"vsub.s s623, s620, s621 \n" // w*w - vMag*vMag
		"vscl.t c620, c710, s623 \n" // (w*w - vMag*vMag)p = section3
		"vadd.t c720, c600, c610 \n" // section1 + section2
		"vadd.t c720, c720, c620 \n" // (section1 + section2) + section3

		"sv.q c720, 0+%0 \n" // Save avector aligned

	#if defined __SNC__
		: "=r" (&xResult)
		: "r" (this), "r" (&p)
	#else
		: "=o" (xResult)
		: "o" (*this), "o" (p)
		: "c700", "c710", "c600", "c610", "c620", "c720"
	#endif
	);
	return xResult;
#else //#ifdef ASURA_PSP_USE_VFPU_ASSEMBLER

	const Asura_AVector3 v(x,y,z); // the vector part of q
//  Asura_Vector_3 result;
	Asura_AVector3 section1, section2, section3;

	float vMag = v.Magnitude();

	section1 = v^p;
	section1 *= (2*w);

	section2 = v;
	section2 *= (2*(v*p));

	section3 = p;
	section3 *= (w*w - vMag*vMag);

	return section1+section2+section3;

//  p = result;

#endif //#else //#ifdef ASURA_PSP_USE_VFPU_ASSEMBLER
}

// creates a quaternion from an angle and axis of rotation
// TODO: optimise using alignedness of vectors
inline void Asura_Quat::FromRotation(/*const*/ Asura_Angle fAngle,
	const Asura_AVector3& xAxis)
{
#if defined ASURA_ASSERT
	{
		static const float fFuzz = 0.0001f;
		const float fMagSq = xAxis.MagnitudeSquared();
		Asura_Assertf(
			Asura_Maths::Abs(fMagSq - 1.0f) < 0.0001f,
			("Axis (MagSq %f) should be normalised",fMagSq)
		);
	}
#endif

#if defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined __SNC__
	__asm__
	(
		"mtv %1, s700 \n" // Load angle scalar

	#if defined ASURA_PSP_AV3_ALIGNED
		"lv.q c710, 0+%2 \n" // Load avector aligned
	#else
		"lv.s s710, 0+%2 \n" // Load avector elements
		"lv.s s711, 4+%2 \n"
		"lv.s s712, 8+%2 \n"
	#endif

		// Load 1/Pi. The angles need to be in the range 0..1 to signify
		// 0..Pi/2, so need to be scaled by 2/Pi. Also, we need to scale
		// the angle itself by 1/2 for the calculation, hence why we're
		// using 1/Pi.
		"vcst.s s701, VFPU_1_PI \n"
		"vmul.s s700, s700, s701 \n" // Scale angle by 1/Pi
		"vsin.s s701, s700 \n" // Calculate sin(angle)
		"vcos.s s723, s700 \n" // cos(angle) = w

		"vscl.t c720, c710, s701 \n" // sin(angle) * axis = x, y, z

	#if defined ASURA_PSP_Q4_ALIGNED
		"sv.q c720, 0+%0 \n" // Save quat aligned
	#else
		"sv.s s720, 0+%0 \n" // Save quat elements
		"sv.s s721, 4+%0 \n"
		"sv.s s722, 8+%0 \n"
		"sv.s s723, 12+%0 \n"
	#endif

		: "=m" (*this)
		: "r" (fAngle), "m" (xAxis)
		: "c700", "c710", "c720"
	);
#elif defined ASURA_PSP_USE_VFPU_ASSEMBLER && defined ASURA_PSP_GCC_BUILTINS && !defined GCC4_BUGGED
	/* Divide by two, and the to convert to hectogradians multiply by 2/PI */
	register const vfpu_v4 afHAngle
		= vfpu_vmul_s(vfpu_v4_s(fAngle),vfpu_vcst_s(VFPU_CONST_1_PI,0),0);
	register vfpu_v4 afResult
		 = vfpu_vscl_t(xAxis.m_afVec,vfpu_vsin_s(afHAngle,0),0);
	
	afResult = vfpu_vmov_s_partial
		(afResult,vfpu_vcos_s(afHAngle,0),VFPU_VD_W|VFPU_VS_X);

	(void) vfpu_v4_saveq(afResult,*this);
#else //#ifdef ASURA_PSP_USE_VFPU_ASSEMBLER
	fAngle *= 0.5f;
	float s;
	Asura_Maths::SinCos(fAngle, s, w);
	x = xAxis.GetX() * s;
	y = xAxis.GetY() * s;
	z = xAxis.GetZ() * s;
	//w = Asura_Maths::Cosine(fAngle);
#endif //#else //#ifdef ASURA_PSP_USE_VFPU_ASSEMBLER
}

#endif // #ifdef ASURA_USE_PLATFORM_VECTORS

// E N D   F I L E   W R A P P E R ///////////////////////////////////////////

#endif//__ASURA_PLATFORMMATHS_H__
