//////////////////////////////////////////////////////////////////////////////
//
// Asura_Platform_Maths.cpp
//
// Copyright (c) 2004 Rebellion. All rights reserved.
//
//////////////////////////////////////////////////////////////////////////////

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

#include "Core/Asura.h"

// Engine independent.
#include "Core/Asura_Maths.h"
#include "Debugging/Asura_Assert.h"

// Engine platform-specific.
//#include "Core/Asura_PlatformBasics.h"
#include "Core/Asura_PS2_VU0.h"

// M A C R O S ///////////////////////////////////////////////////////////////

#ifdef ASURA_PS2_IN_VC

#define Asura_PS2_LoadVectorToVu0(pfVectorAddress, VU_Reg)
#define Asura_PS2_StoreVectorFromVu0(pfVectorAddress, VU_Reg)
#define Asura_PS2_LoadMatrixToVu0(pfVectorAddress, VU_Reg)
#define Asura_PS2_StoreMatrixFromVu0(pfVectorAddress, VU_Reg)

#else // not ASURA_PS2_IN_VC

#define Asura_PS2_LoadVectorToVu0(xVectorIn, VU_Reg)\
{\
    u_long128 xTemp1;\
    u_long128 xTemp2;\
    u_long128 xVectorTemp;\
    asm __volatile__( "mtsab %0, 0" : : "r"(&xVectorIn) );\
    asm __volatile__( "lq %0,  0(%1)" : "=r"(xTemp1) : "r"(&xVectorIn) );\
    asm __volatile__( "lq %0, 16(%1)" : "=r"(xTemp2) : "r"(&xVectorIn) );\
    asm __volatile__( "qfsrv %0, %1, %2" : "=r"(xVectorTemp) : "r"(xTemp2), "r"(xTemp1) );\
    asm __volatile__( "qmtc2.i %0, "VU_Reg : : "r"(xVectorTemp));\
}

#define Asura_PS2_StoreVectorFromVu0(xVectorIn, VU_Reg)\
{\
    u_long128 xVectorTemp;\
    u_long128 xTemp;\
    asm __volatile__( "qmfc2.i %0, "VU_Reg :"=r"(xVectorTemp));\
    asm __volatile__( "sdl %1, 7(%0)" : "=m"(xVectorIn) : "r"(xVectorTemp) );\
    asm __volatile__( "pcpyud %0, %1, %2" : "=r"(xTemp) : "r"(xVectorTemp), "r"(xVectorTemp) );\
    asm __volatile__( "sdr %1, 0(%0)" : "=m"(xVectorIn) : "r"(xVectorTemp) );\
    asm __volatile__( "sw %1, 8(%0)" : "=m"(xVectorIn) : "r"(xTemp) );\
}

#define Asura_PS2_LoadMatrixToVu0(xMatrix, VU_Reg0, VU_Reg1, VU_Reg2)\
{\
    register u_long128 xTemp1;\
    register u_long128 xTemp2;\
    register u_long128 xVectorTemp;\
    register const float* pfVectorAddress = &xMatrix[0][0];\
    asm __volatile__( "nop");\
    asm __volatile__( "mtsab %0, 0" : : "r"(pfVectorAddress) );\
    asm __volatile__( "lq %0,  0(%1)" : "=r"(xTemp1) : "r"(pfVectorAddress) );\
    asm __volatile__( "lq %0, 16(%1)" : "=r"(xTemp2) : "r"(pfVectorAddress) );\
    asm __volatile__( "qfsrv %0, %1, %2" : "=r"(xVectorTemp) : "r"(xTemp2), "r"(xTemp1) );\
    asm __volatile__( "qmtc2.i %0, "VU_Reg0 : : "r"(xVectorTemp));\
    pfVectorAddress += 3;\
    asm __volatile__( "nop");\
    asm __volatile__( "mtsab %0, 0" : : "r"(pfVectorAddress) );\
    asm __volatile__( "lq %0,  0(%1)" : "=r"(xTemp1) : "r"(pfVectorAddress) );\
    asm __volatile__( "lq %0, 16(%1)" : "=r"(xTemp2) : "r"(pfVectorAddress) );\
    asm __volatile__( "qfsrv %0, %1, %2" : "=r"(xVectorTemp) : "r"(xTemp2), "r"(xTemp1) );\
    asm __volatile__( "qmtc2.i %0, "VU_Reg1 : : "r"(xVectorTemp));\
    pfVectorAddress += 3;\
    asm __volatile__( "nop");\
    asm __volatile__( "mtsab %0, 0" : : "r"(pfVectorAddress) );\
    asm __volatile__( "lq %0,  0(%1)" : "=r"(xTemp1) : "r"(pfVectorAddress) );\
    asm __volatile__( "lq %0, 16(%1)" : "=r"(xTemp2) : "r"(pfVectorAddress) );\
    asm __volatile__( "qfsrv %0, %1, %2" : "=r"(xVectorTemp) : "r"(xTemp2), "r"(xTemp1) );\
    asm __volatile__( "qmtc2.i %0, "VU_Reg2 : : "r"(xVectorTemp));\
}

#define Asura_PS2_StoreMatrixFromVu0(xMatrix, VU_Reg0, VU_Reg1, VU_Reg2)\
{\
    register float* const pfVec0 = &xMatrix[0][0]; \
    register float* const pfVec1 = pfVec0 + 3; \
    register float* const pfVec2 = pfVec0 + 6; \
    register u_long128 xVectorTemp;\
    register u_long128 xTemp;\
    asm __volatile__( "qmfc2.i %0, "VU_Reg0 :"=r"(xVectorTemp));\
    asm __volatile__( "sdl %1, 7(%2)" : "=m"(*pfVec0) : "r"(xVectorTemp), "r"(pfVec0) );\
    asm __volatile__( "pcpyud %0, %1, %2" : "=r"(xTemp) : "r"(xVectorTemp), "r"(xVectorTemp) );\
    asm __volatile__( "sdr %1, %0" : "=m"(*pfVec0) : "r"(xVectorTemp) );\
    asm __volatile__( "sw %1, 8(%2)" : "=m"(*pfVec0) : "r"(xTemp), "r"(pfVec0) );\
    asm __volatile__( "qmfc2.i %0, "VU_Reg1 :"=r"(xVectorTemp));\
    asm __volatile__( "sdl %1, 7(%2)" : "=m"(*pfVec1) : "r"(xVectorTemp), "r"(pfVec1) );\
    asm __volatile__( "pcpyud %0, %1, %2" : "=r"(xTemp) : "r"(xVectorTemp), "r"(xVectorTemp) );\
    asm __volatile__( "sdr %1, %0" : "=m"(*pfVec1) : "r"(xVectorTemp) );\
    asm __volatile__( "sw %1, 8(%2)" : "=m"(*pfVec1) : "r"(xTemp), "r"(pfVec1) );\
    asm __volatile__( "qmfc2.i %0, "VU_Reg2 :"=r"(xVectorTemp));\
    asm __volatile__( "sdl %1, 7(%2)" : "=m"(*pfVec2) : "r"(xVectorTemp), "r"(pfVec2) );\
    asm __volatile__( "pcpyud %0, %1, %2" : "=r"(xTemp) : "r"(xVectorTemp), "r"(xVectorTemp) );\
    asm __volatile__( "sdr %1, %0" : "=m"(*pfVec2) : "r"(xVectorTemp) );\
    asm __volatile__( "sw %1, 8(%2)" : "=m"(*pfVec2) : "r"(xTemp), "r"(pfVec2) );\
}

#endif // else not ASURA_PS2_IN_VC

// G L O B A L S /////////////////////////////////////////////////////////////

const Asura_Vector_2   Asura_Maths::ZeroVector2(0.0f, 0.0f);

const Asura_Vector_3   Asura_Maths::ZeroVector3(0.0f, 0.0f, 0.0f);

const Asura_Matrix_3x3 Asura_Maths::IdentityMatrix3x3( 1.0f, 0.0f, 0.0f,
                                                       0.0f, 1.0f, 0.0f,
                                                       0.0f, 0.0f, 1.0f );

const Asura_Quat       Asura_Maths::IdentityQuat( 0.0f, 0.0f, 0.0f, 1.0f );

// C L A S S   I M P L E M E N T A T I O N S /////////////////////////////////

//
// Asura_Bounding_Box.
//

//////////////////////////////////////////////////////////////////////////////
//
// FUNCTION     Asura_Bounding_Box::GetSqdMinDistFromBB().
// PARAMETERS   xOtherBB - The BB to be tested. Both BBs are assumed to be in
//                         the same space.
// RETURNS      Minimum distance between the two BBs.
// DESCRIPTION  See RETURNS.
// AUTHOR       MP - 27.11.2002.
//              Based on GetSqdMinDistFromPoint() - see below.
//
//////////////////////////////////////////////////////////////////////////////
float Asura_Bounding_Box::GetSqdMinDistFromBB( const Asura_Bounding_Box& xOtherBB ) const
{
    float fSqdDistance = 0.0f;
    float fDelta;

    if( xOtherBB.MaxX < MinX )
    {
        fDelta = xOtherBB.MaxX - MinX;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xOtherBB.MinX > MaxX )
    {
        fDelta = xOtherBB.MinX - MaxX;
        fSqdDistance += fDelta * fDelta;
    }

    if( xOtherBB.MaxY < MinY )
    {
        fDelta = xOtherBB.MaxY - MinY;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xOtherBB.MinY > MaxY )
    {
        fDelta = xOtherBB.MinY - MaxY;
        fSqdDistance += fDelta * fDelta;
    }

    if( xOtherBB.MaxZ < MinZ )
    {
        fDelta = xOtherBB.MaxZ - MinZ;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xOtherBB.MinZ > MaxZ )
    {
        fDelta = xOtherBB.MinZ - MaxZ;
        fSqdDistance += fDelta * fDelta;
    }

    return fSqdDistance;
}
// End function Asura_Bounding_Box::GetSqdMinDistFromBB().

//////////////////////////////////////////////////////////////////////////////
//
// FUNCTION     Asura_Bounding_Box::GetSqdMinDistFromPoint().
// PARAMETERS   xPoint - Point to be tested. This is assumed to be in the
//                       same space as the BB.
// RETURNS      Minimum distance from the point to the BB.
// DESCRIPTION  See RETURNS.
// AUTHOR       Original code: Magic Software. (Note that we're allowed to
//                             use it under the terms of their license.)
//              Asura port   : MP - 27.11.2002.
//
//////////////////////////////////////////////////////////////////////////////
float Asura_Bounding_Box::GetSqdMinDistFromPoint( const Asura_Vector_3& xPoint ) const
{
    float fSqdDistance = 0.0f;
    float fDelta;

    if( xPoint.x < MinX )
    {
        fDelta = xPoint.x - MinX;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xPoint.x > MaxX )
    {
        fDelta = xPoint.x - MaxX;
        fSqdDistance += fDelta * fDelta;
    }

    if( xPoint.y < MinY )
    {
        fDelta = xPoint.y - MinY;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xPoint.y > MaxY )
    {
        fDelta = xPoint.y - MaxY;
        fSqdDistance += fDelta * fDelta;
    }

    if( xPoint.z < MinZ )
    {
        fDelta = xPoint.z - MinZ;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xPoint.z > MaxZ )
    {
        fDelta = xPoint.z - MaxZ;
        fSqdDistance += fDelta * fDelta;
    }

    return fSqdDistance;
}
// End function Asura_Bounding_Box::GetSqdMinDistFromPoint().


// essentially the same calculation as GetSqdMinDistFromPoint, but this returns the result as an unsquared vector
void Asura_Bounding_Box::CalcMinVectorIntoBox( const Asura_Vector_3& xPoint, Asura_Vector_3& xVector ) const
{
    xVector.x = xVector.y = xVector.z = 0.0f;

    if ( xPoint.x < MinX )
    {
        xVector.x = MinX - xPoint.x;
    }
    else if ( xPoint.x > MaxX )
    {
        xVector.x = MaxX - xPoint.x;
    }

    if ( xPoint.y < MinY )
    {
        xVector.y = MinY - xPoint.y;
    }
    else if ( xPoint.y > MaxY )
    {
        xVector.y = MaxY - xPoint.y;
    }

    if ( xPoint.z < MinZ )
    {
        xVector.z = MinZ - xPoint.z;
    }
    else if ( xPoint.z > MaxZ )
    {
        xVector.z = MaxZ - xPoint.z;
    }
}


void Asura_Bounding_Box::FromOBB( const Asura_Bounding_Box& xOBB, const Asura_Matrix_3x3& xOri, bool bRotateAboutMidpoint /*=false*/ )
{
    const Asura_Vector_3 xCentre  = xOBB.CalculateCentre();
    const Asura_Vector_3 xExtents = xOBB.CalculateExtents();

    float fHalfAABBExtentX = 0.5f * (   Asura_Maths::Abs(xOri.e11 * xExtents.x)
                                      + Asura_Maths::Abs(xOri.e12 * xExtents.y)
                                      + Asura_Maths::Abs(xOri.e13 * xExtents.z) );

    float fHalfAABBExtentY = 0.5f * (   Asura_Maths::Abs(xOri.e21 * xExtents.x)
                                      + Asura_Maths::Abs(xOri.e22 * xExtents.y)
                                      + Asura_Maths::Abs(xOri.e23 * xExtents.z) );

    float fHalfAABBExtentZ = 0.5f * (   Asura_Maths::Abs(xOri.e31 * xExtents.x)
                                      + Asura_Maths::Abs(xOri.e32 * xExtents.y)
                                      + Asura_Maths::Abs(xOri.e33 * xExtents.z) );

    MinX = -fHalfAABBExtentX;
    MaxX =  fHalfAABBExtentX;
    MinY = -fHalfAABBExtentY;
    MaxY =  fHalfAABBExtentY;
    MinZ = -fHalfAABBExtentZ;
    MaxZ =  fHalfAABBExtentZ;

	if(bRotateAboutMidpoint)
	{
		Displace( xCentre );
	}
	else
	{
		Displace( xCentre*xOri );
	}
}

//
// Asura_PS2_Maths.
//

float Asura_PS2_Maths::Sine( Asura_Angle fAngle )
{
    Asura_Assert(Asura_PS2_VU0::IsInitialised(), "VU0 microprog not loaded.");

    const u_int uSubroutineAddress = reinterpret_cast<u_int>(VU0_SinCos) >> 3;
    asm __volatile__( "CTC2 %0, $vi27" : : "r"(uSubroutineAddress) );

    Asura_PS2_QW xTemp;
    xTemp.f32s[0] = fAngle;
    asm __volatile__ ("QMTC2 %0, $vf01" : : "r"(xTemp.u128));

    // activate microprogram
    asm __volatile__( "VCALLMSR $vi27" );

    // fetch results. specifying .I to interlock means we stall until the program is completed.
    asm __volatile__ ("QMFC2.I %0, $vf02" : "=r" (xTemp.u128) );

    return xTemp.f32s[0];
}

float Asura_PS2_Maths::Cosine(Asura_Angle fAngle)
{
    Asura_Assert(Asura_PS2_VU0::IsInitialised(), "VU0 microprog not loaded.");

    const u_int uSubroutineAddress = reinterpret_cast<u_int>(VU0_SinCos) >> 3;
    asm __volatile__( "CTC2 %0, $vi27" : : "r"(uSubroutineAddress) );

    Asura_PS2_QW xTemp;
    xTemp.f32s[0] = fAngle;
    asm __volatile__( "QMTC2 %0, $vf01" : : "r"(xTemp.u128) );

    // activate microprogram
    asm __volatile__( "VCALLMSR $vi27" );

    // fetch results. specifying .I to interlock means we stall until the program is completed.
    asm __volatile__( "QMFC2.I %0, $vf02" : "=r" (xTemp.u128) );

    return xTemp.f32s[1];
}

void Asura_PS2_Maths::SinCos(Asura_Angle fAngle, float& fOutputSine, float& fOutputCos)
{
    Asura_Assert(Asura_PS2_VU0::IsInitialised(), "VU0 microprog not loaded.");

    const u_int uSubroutineAddress = reinterpret_cast<u_int>(VU0_SinCos) >> 3;
    asm __volatile__( "CTC2 %0, $vi27" : : "r"(uSubroutineAddress) );

    Asura_PS2_QW xTemp;
    xTemp.f32s[0] = fAngle;
    asm __volatile__( "QMTC2 %0, $vf01" : : "r"(xTemp.u128) );

    // activate microprogram
    asm __volatile__( "VCALLMSR $vi27" );

    // fetch results. specifying .I to interlock means we stall until the program is completed.
    asm __volatile__( "QMFC2.I %0, $vf02" : "=r" (xTemp.u128) );

    fOutputSine = xTemp.f32s[0];
    fOutputCos = xTemp.f32s[1];
}

void Asura_PS2_Maths::SinCos2(Asura_Angle fAngle1, Asura_Angle fAngle2, float& fOutputSine1, 
                                float& fOutputCos1, float& fOutputSine2, float& fOutputCos2)
{
    Asura_Assert(Asura_PS2_VU0::IsInitialised(), "VU0 microprog not loaded.");
    
    const u_int uSubroutineAddress = reinterpret_cast<u_int>(VU0_SinCos) >> 3;
    asm __volatile__( "CTC2 %0, $vi27" : : "r"(uSubroutineAddress) );

    Asura_PS2_QW xTemp;
    xTemp.f32s[0] = fAngle1;
    xTemp.f32s[2] = fAngle2;
    asm __volatile__( "QMTC2 %0, $vf01" : : "r"(xTemp.u128) );

    // activate microprogram
    asm __volatile__( "VCALLMSR $vi27" );

    // fetch results. specifying .I to interlock means we stall until the program is completed.
    asm __volatile__( "QMFC2.I %0, $vf02" : "=r" (xTemp.u128) );

    fOutputSine1 = xTemp.f32s[0];
    fOutputCos1 = xTemp.f32s[1];
    fOutputSine2 = xTemp.f32s[2];
    fOutputCos2 = xTemp.f32s[3];
}


float Asura_PS2_Maths::ArcSine( float fSine )
{
    Asura_Assert(Asura_PS2_VU0::IsInitialised(), "VU0 microprog not loaded.");

    const u_int uSubroutineAddress = reinterpret_cast<u_int>(VU0_ArcSin) >> 3;
    asm __volatile__( "CTC2 %0, $vi27" : : "r"(uSubroutineAddress) );
 
    Asura_PS2_QW xTemp;
    xTemp.f32s[0] = fSine;
    asm __volatile__( "QMTC2 %0, $vf01" : : "r"(xTemp.u128) );

    // activate microprogram
    asm __volatile__( "VCALLMSR $vi27" );

    // fetch results. specifying .I to interlock means we stall until the program is completed.
    asm __volatile__( "QMFC2.I %0, $vf02" : "=r" (xTemp.u128) );

    return xTemp.f32s[0];
}


float Asura_PS2_Maths::ArcCosine( float fCosine )
{
    Asura_Assert(Asura_PS2_VU0::IsInitialised(), "VU0 microprog not loaded.");

    const u_int uSubroutineAddress = reinterpret_cast<u_int>(VU0_ArcCos) >> 3;
    asm __volatile__( "CTC2 %0, $vi27" : : "r"(uSubroutineAddress) );
 
    Asura_PS2_QW xTemp;
    xTemp.f32s[0] = fCosine;
    asm __volatile__( "QMTC2 %0, $vf01" : : "r"(xTemp.u128) );

    // activate microprogram
    asm __volatile__( "VCALLMSR $vi27" );

    // fetch results. specifying .I to interlock means we stall until the program is completed.
    asm __volatile__( "QMFC2.I %0, $vf02" : "=r" (xTemp.u128) );

    return xTemp.f32s[0];
}


void Asura_PS2_Maths::Quat_Slerp(Asura_Quat& xOutput, const Asura_Quat& xInput1, const Asura_Quat& xInput2, float l)
{
    float sclp,sclq;
    
    float cosom;
    float omega, sinom;

    cosom=xInput1.Dot(xInput2);

    bool bNegateInput2;
    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);

        float fIgnore;
        Asura_Maths::SinCos2( (1.0f - l) * omega, l * omega, sclp, fIgnore, sclq, fIgnore );

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

    xOutput = xInput1 * sclp + xInput2 * (bNegateInput2 ? -sclq : sclq);
    xOutput.Normalise();
}


//DEBUGVAR bool dbg_bTestMatMul = false;
void Asura_PS2_Maths::MatrixMultiply(Asura_Matrix_3x3& xOutput, const Asura_Matrix_3x3& xInput1, const Asura_Matrix_3x3& xInput2)
{
    Asura_Assert(Asura_PS2_VU0::IsInitialised(), "VU0 microprog not loaded.");
    
    const u_int uSubroutineAddress = reinterpret_cast<u_int>(VU0_Asura_Matrix3x3_Multiply) >> 3;
    asm __volatile__( "CTC2 %0, $vi27" : : "r"(uSubroutineAddress) );

    Asura_PS2_LoadMatrixToVu0( xInput1, "$vf01", "$vf02", "$vf03" );
    Asura_PS2_LoadMatrixToVu0( xInput2, "$vf04", "$vf05", "$vf06" );
    asm __volatile__( "VCALLMSR $vi27" );
    Asura_PS2_StoreMatrixFromVu0( xOutput, "$vf07", "$vf08", "$vf09" );
}


// 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

void Asura_PS2_Maths::CompQuat_ToQuat(const Asura_Comp_Quat* pxCompQuat, Asura_Quat& xQuat)
{
    const u_int uBits = pxCompQuat->GetBits();
	int iTemp = static_cast<int>(uBits & 0x000007FF);
    xQuat.w   = static_cast<float>(iTemp) * 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
    {
		iTemp = static_cast<int>(uBits & 0x1FF00000);
		float fYaw   = Asura_Maths::Min(static_cast<float>(iTemp >> 20) * PI_OVER_512, PI_OVER_2);
		iTemp = static_cast<int>(uBits & 0x000FF800);
		float fPitch = Asura_Maths::Min(static_cast<float>(iTemp >> 11) * PI_OVER_512, PI_OVER_2);
        float fSinPitch, fCosPitch, fSinYaw, fCosYaw;
        Asura_Maths::SinCos2(fPitch, fYaw, fSinPitch, fCosPitch, fSinYaw, fCosYaw);
        float fQuatX = fCosPitch * fSinYaw;
        float fQuatY = fSinPitch;
        float fQuatZ = fCosPitch * fCosYaw;
        if(uBits & 0x80000000) fQuatX = -fQuatX;
        if(uBits & 0x40000000) fQuatY = -fQuatY;
        if(uBits & 0x20000000) fQuatZ = -fQuatZ;
        float fLength = Asura_Maths::Sqrt(1.0f - xQuat.w * xQuat.w);
        xQuat.x = fLength * fQuatX;
        xQuat.y = fLength * fQuatY;
        xQuat.z = fLength * fQuatZ;
    }
}

#define F512_OVER_PI 162.97466f

void Asura_PS2_Maths::CompQuat_FromQuat(Asura_Comp_Quat* pxCompQuat, const Asura_Quat& xQuat)
{
    register u_int bits;

    Asura_Vector_3 v;
    v.x = xQuat.x;
    v.y = xQuat.y;
    v.z = xQuat.z;
    v.Normalise();
    // Signs
    if(xQuat.w > 0.0f)
    {
        bits  = ((xQuat.x > 0.0f) ? 0 : 1) << 31;
        bits |= ((xQuat.y > 0.0f) ? 0 : 1) << 30;
        bits |= ((xQuat.z > 0.0f) ? 0 : 1) << 29;
    }
    else
    {
        bits  = ((xQuat.x < 0.0f) ? 0 : 1) << 31;
        bits |= ((xQuat.y < 0.0f) ? 0 : 1) << 30;
        bits |= ((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)));
    bits |= ((yaw << 20) & 0x1FF00000);
    // Pitch = asin(y)
    // AW - 24/07/03 - Abs(v.y) can be slightly more than 1.0f due to floating point inaccuracies. Allow for that here.
    float fArcSine = Asura_Maths::Abs(v.y) >= 1.0f ? Asura_Maths::Pi * 0.5f : Asura_Maths::ArcSine(Asura_Maths::Abs(v.y));
    unsigned int pitch = Asura_Maths::FloatToInt_Near(F512_OVER_PI * fArcSine);
    bits |= ((pitch << 11) & 0x000FF800);
    // W
    unsigned int w = Asura_Maths::FloatToInt_Near(1024.0f * Asura_Maths::Abs(xQuat.w));
    bits |= (w & 0x000007FF);

    pxCompQuat->SetBits(bits);
}

//
// Asura_Random.
//

float Asura_RandomGenerator::GetNormalFloat( void )
{
    // We generate two at a time so cache the other one for the next call.
    // The maths below is pretty expensive so this is worth the effort.
    static bool bGotSpare = false;
    static float fSpare;

    if( bGotSpare )
    {
        bGotSpare = false;
        return fSpare;
    }

    // We use a Box-Muller transformation to get two normal distribution
    // random numbers from two uniform distribution ones. Given the latter,
    // u0 and u1, our output values are:
    //
    //     f0 = sqrt(-2 ln u0) * cos(2Pi * u1)
    //     f1 = sqrt(-2 ln u0) * sin(2Pi * u1)
    //
    // However, we can avoid the trigonometry by picking a point inside the
    // 2D unit circle, at the potential expense of having to generate extra
    // input uniform random numbers. See "Numerical Recipes In C" (page 289
    // in 2nd edition) for more details.

    float f0, f1, fMagSqd;
    do
    {
        f0 = 2.0f * GetFloat() - 1.0f;
        f1 = 2.0f * GetFloat() - 1.0f;
        fMagSqd = f0 * f0 + f1 * f1;
    }
    while( !fMagSqd || fMagSqd >= 1.0f );

    float fScale = Asura_Maths::Sqrt( -2.0f * logf(fMagSqd) / fMagSqd );
    
    fSpare = f1 * fScale;
    bGotSpare = true;

    return f0 * fScale;
}

//
// Asura_Vector_2.
//

//Choose a random direction within an angular deviation of this vector.
Asura_Vector_2 Asura_Vector_2::ChooseRandomDirectionInSegment(const float fMaxTheta, const float fMinTheta) const
{
    float fCosTheta, fSinTheta;

    if(x*x + y*y < 0.000001f)
    {
        Asura_Assert(false, "ChooseRandomDirectionInSegment() used with zero starting vector");
        return *this;
    }

    //Work out a random theta in the range
    // 0 <= theta <= fMaxTheta, and take its cosine.
    fCosTheta = Asura_Maths::Cosine((fMaxTheta-fMinTheta)*Asura_Random::GetFloat() + fMinTheta);

    //Randomly decide on which side of the vector the point should lie, and
    //derive the sine from the cosine, using a square root.
    if(Asura_Random::GetIntU()&0x40000000)
    {
        fSinTheta = Asura_Maths::Sqrt(1.f - fCosTheta*fCosTheta);
    }
    else
    {
        fSinTheta = -Asura_Maths::Sqrt(1.f - fCosTheta*fCosTheta);
    }

    //Rotate this through the angle specified
    Asura_Vector_2 xRet;

    xRet.x = fCosTheta * x - fSinTheta * y;
    xRet.y = fSinTheta * x + fCosTheta * y;

    //Choose a random length for the new vector, in the range of 0..1
    xRet.Normalise();

    return xRet;
}

//Choose a vector of random length within a specified angular devation of
//this vector, and with a length in the specified range.
Asura_Vector_2 Asura_Vector_2::ChooseRandomVectorInSegment(const float fMaxTheta, const float fMinTheta, const float fMaxLength, const float fMinLength) const
{
    return ChooseRandomDirectionInSegment(fMaxTheta, fMinTheta) 
            * ((fMaxLength - fMinLength) * Asura_Random::GetFloat() + fMinLength);
}

//
// Asura_Vector_3.
//

//Choose a unit vector in a cone, with an angle subtended to this vector in the specified range.
Asura_Vector_3 Asura_Vector_3::ChooseRandomDirectionInCone(const float fMaxTheta, const float fMinTheta) const
{
    Asura_Vector_3 xDirection = *this;
    xDirection.Normalise();

    if(fMinTheta > fMaxTheta) return xDirection;

    //Now, we construct an arbitrary orthogonal basis about the axis.
    float fAbsX = Asura_Maths::Abs(xDirection.x);
    float fAbsY = Asura_Maths::Abs(xDirection.y);
    float fAbsZ = Asura_Maths::Abs(xDirection.z);

    //Asura_Vector_3 xContructionAxis(1.f, 0.f, 0.f);
    Asura_Vector_3 xNormal;

    //Work out the largest component of xDirection, and construct an appropriate normal,
    //using the unrolled cross product with that axis.
    if(fAbsX > fAbsY)
    {
        if(fAbsY > fAbsZ)
        {   
            //Cross with z-axis.
            xNormal.x = xDirection.y;
            xNormal.y = -xDirection.x;
            xNormal.z = 0.f;
        }
        else
        {
            //Cross with y-axis.
            xNormal.x = -xDirection.z;
            xNormal.y = 0.f;
            xNormal.z = xDirection.x;
        }
    }
    else if(fAbsX > fAbsZ)
    {
        //Cross with z-axis.
        xNormal.x = xDirection.y;
        xNormal.y = -xDirection.x;
        xNormal.z = 0.f;
    }
    else
    {
        //Cross with x-axis.
        xNormal.x = 0.f;
        xNormal.y = xDirection.z;
        xNormal.z = -xDirection.y;
    }

    xNormal.Normalise();

    //Now that we have the first normal, we can construct the second.
    Asura_Vector_3 xNormal2 = xDirection^xNormal;

    //Now, we choose a random 2-vector.
    Asura_Vector_2 xRandom;
    xRandom.RandomDirection();

    //Now we choose a random number between the cosines of the two angles.
    float fV1 = Asura_Maths::Cosine(fMinTheta);
    float fV2 = Asura_Maths::Cosine(fMaxTheta);

    float fRandomFloat = Asura_Random::GetFloat() * (fV1-fV2) + fV2;

    //Just check the normalisation, to be sure.
    if(fRandomFloat > 1.f) fRandomFloat = 1.f;
    else if(fRandomFloat < -1.f) fRandomFloat = -1.f;

    //Scale the primary component to the chosen length.
    xDirection *= fRandomFloat;

    //Work out the correct normalisation factor for the second component...
    //...and add the second component, scaled by that factor.
    xDirection += (xNormal * xRandom.x + xNormal2 * xRandom.y) * Asura_Maths::Sqrt(1.f - fRandomFloat*fRandomFloat);

    return xDirection;
}

//Choose a vector of length within the specified range, and with an angular deviation
//from this vector in the specified range.
Asura_Vector_3 Asura_Vector_3::ChooseRandomVectorInCone(const float fMaxTheta, const float fMinTheta, const float fMaxLength, const float fMinLength) const
{
    float fLength = Asura_Random::GetFloat()*(fMaxLength - fMinLength) + fMinLength;

    return ChooseRandomDirectionInCone(fMaxTheta, fMinTheta)*fLength;
}

// F U N C T I O N   I M P L E M E N T A T I O N S ///////////////////////////

void Asura_Maths::OrientationFromDirection( Asura_Matrix_3x3 &Orientation, const Asura_Vector_3& d )
{
    Asura_Vector_3 XVector;
    Asura_Vector_3 YVector;
    Asura_Vector_3 ZVector = d;

    ZVector.Normalise();

    if (ZVector.z==0.0f && ZVector.x==0.0f)
    {
        XVector.x = -ZVector.y;
        XVector.y = 0.0f;
        XVector.z = 0.0f;

// (MP - 13.05.2002) We know this vector will be normalised already.
//      XVector.Normalise();
    }
    else
    {
        XVector.x = ZVector.z;
        XVector.y = 0;
        XVector.z = -ZVector.x;

        XVector.Normalise();
    }

//  YVector = -XVector^ZVector;
    YVector = ZVector ^ XVector;

    Orientation.e11 = XVector.x;
    Orientation.e21 = XVector.y;
    Orientation.e31 = XVector.z;

    Orientation.e12 = YVector.x;
    Orientation.e22 = YVector.y;
    Orientation.e32 = YVector.z;

    Orientation.e13 = ZVector.x;
    Orientation.e23 = ZVector.y;
    Orientation.e33 = ZVector.z;
}
