Added some more macros to enable more precission tweaks

#include "wmc_auto.h"

#include "prot_fx.h"

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING

/*Changes done in fx code*/

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_VectorLength_fx_USE_Sqrt32_NewtonRaphson /*use refinement of Sqrt32          within VectorLength_fx() function*/

//#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_VectorLength_fx                          /*additionally, increase precission within VectorLength_fx() function*/ /*almost No diff in precission*/

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_QuaternionNormalize_fx_USE_Sqrt32_NewtonRaphson /*use refinement of Sqrt32   within QuaternionNormalize_fx() function*/

//#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_QuaternionDivision_fx_USE_NORMALIZATION  /*use normalization after division  within QuaternionDivision_fx() function*/ /*almost No diff in precission*/

//#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_NormalizeVectorCrossProduct_VectorDotProduct /*merges some vectorfunctions within VectorRotationToQuaternion_fx*/ /*Very minor diff in precission*/

//-----------------------------------------

/*Float-substitutes FOR DEVELOPMENT - overriding fx precission fixes*/ 

//#define FLSUBST_VectorNormalize_fx            //substitute VectorNormalize_fx   

//#define FLSUBST_VectorRotationToQuaternion_fx //substitute VectorRotationToQuaternion_fx

#if 1

#include "math.h"

#endif

//-----------------------------------------

#endif

/*------------------------------------------------------------------------------------------*

 * Local constants

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_VectorLength_fx_USE_Sqrt32_NewtonRaphson /*use refinement of Sqrt32*/

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_VectorLength_fx                          /*additionally, increase precission*/

#if 1

#include "math.h"

#endif

static Word32 Sqrt32_NewtonRaphson( Word32 mantissa, Word16 * e, Word16 n )

{

    r->w_fx = BASOP_Util_Divide3232_Scale_newton( ( q.w_fx ), d, &scale_e );

    move32();

    result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) ); // e+e1-e2//

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_QuaternionDivision_fx_USE_NORMALIZATION

    scale_e = norm_l( r->w_fx );

    result_e = sub( result_e, scale_e );

    r->w_fx = L_shl( r->w_fx, scale_e );

#endif

    w_q = sub( Q31, result_e );

    r->x_fx = BASOP_Util_Divide3232_Scale_newton( ( q.x_fx ), d, &scale_e );

    move32();

    result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) );

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_QuaternionDivision_fx_USE_NORMALIZATION

    scale_e = norm_l( r->x_fx );

    result_e = sub( result_e, scale_e );

    r->x_fx = L_shl( r->x_fx, scale_e );

#endif

    x_q = sub( Q31, result_e );

    r->y_fx = BASOP_Util_Divide3232_Scale_newton( ( q.y_fx ), d, &scale_e );

    move32();

    result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) );

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_QuaternionDivision_fx_USE_NORMALIZATION

    scale_e = norm_l( r->y_fx );

    result_e = sub( result_e, scale_e );

    r->y_fx = L_shl( r->y_fx, scale_e );

#endif

    y_q = sub( Q31, result_e );

    r->z_fx = BASOP_Util_Divide3232_Scale_newton( ( q.z_fx ), d, &scale_e );

    move32();

    result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) );

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_QuaternionDivision_fx_USE_NORMALIZATION

    scale_e = norm_l( r->z_fx );

    result_e = sub( result_e, scale_e );

    r->z_fx = L_shl( r->z_fx, scale_e );

#endif

    z_q = sub( Q31, result_e );

    result_q = sub( s_min( s_min( w_q, x_q ), s_min( y_q, z_q ) ), 1 ); // gaurdbits//

    Word32 sqrt_fx;

    Word32 dot_prod_fx = QuaternionDotProduct_fx( q_fx, q_fx, &q_dot );

    sqrt_e = sub( Q31, q_dot );

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_QuaternionNormalize_fx_USE_Sqrt32_NewtonRaphson

    sqrt_fx = Sqrt32_NewtonRaphson( dot_prod_fx, &sqrt_e, 1 );

#else

    sqrt_fx = Sqrt32( dot_prod_fx, &sqrt_e );

#endif

    QuaternionDivision_fx( q_fx, sqrt_fx, r_fx, sqrt_e );

    return;

}

static IVAS_VECTOR3 VectorNormalize_fx(

    const IVAS_VECTOR3 p )

{

#ifdef FLSUBST_VectorNormalize_fx

    double x, y, z;

    double len;

    IVAS_VECTOR3 result;

    x = p.x_fx / pow( 2, p.q_fact );

    y = p.y_fx / pow( 2, p.q_fact );

    z = p.z_fx / pow( 2, p.q_fact );

    len = sqrt( x * x + y * y + z * z );

    result.x = x / len;

    result.y = y / len;

    result.z = z / len;

    result.x_fx = result.x * pow( 2, p.q_fact );

    result.y_fx = result.y * pow( 2, p.q_fact );

    result.z_fx = result.z * pow( 2, p.q_fact );

    result.q_fact = p.q_fact;

    return result;

#endif

    IVAS_VECTOR3 result_fx;

    Word32 length_fx;

    Word16 q_len, scale = 0, x_qfact, y_qfact, z_qfact, q_result;

 * Computes a quaternion representing the rotation from vector p1 to vector p2

 *------------------------------------------------------------------------------------------*/

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_NormalizeVectorCrossProduct_VectorDotProduct

#endif

 #ifdef FLSUBST_VectorRotationToQuaternion_fx

typedef struct

{

    float w, x, y, z;

} IVAS_QUATERNION_FL;

static IVAS_VECTOR3 VectorCrossProduct(

    const IVAS_VECTOR3 p1,

    const IVAS_VECTOR3 p2 ) 

{

    IVAS_VECTOR3 result;

    result.x = p1.y * p2.z - p1.z * p2.y;

    result.y = p1.z * p2.x - p1.x * p2.z;

    result.z = p1.x * p2.y - p1.y * p2.x;

    return result;

}

static float VectorDotProduct(

    const IVAS_VECTOR3 p1,

    const IVAS_VECTOR3 p2 ) 

{

    return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;

}

static float VectorLength(

    const IVAS_VECTOR3 p ) 

{

    return sqrtf( p.x * p.x + p.y * p.y + p.z * p.z );

}

static IVAS_VECTOR3 VectorNormalize(

    const IVAS_VECTOR3 p )

{

    IVAS_VECTOR3 result;

    const float length = VectorLength( p );

    result.x = p.x / length;

    result.y = p.y / length;

    result.z = p.z / length;

    return result;

}

static float QuaternionDotProduct(

    const IVAS_QUATERNION_FL q1,

    const IVAS_QUATERNION_FL q2 ) 

{

    return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;

}

/*------------------------------------------------------------------------------------------*

 * QuaternionDivision()

 *

 * Divides a quaternion by a scalar

 *------------------------------------------------------------------------------------------*/

static void QuaternionDivision(

    const IVAS_QUATERNION_FL q,

    const float d,

    IVAS_QUATERNION_FL *const r )

{

    r->w = q.w / d;

    r->x = q.x / d;

    r->y = q.y / d;

    r->z = q.z / d;

    return;

}

static void QuaternionNormalize(

    const IVAS_QUATERNION_FL q,

    IVAS_QUATERNION_FL *const r ) 

{

    QuaternionDivision( q, sqrtf( QuaternionDotProduct( q, q ) ), r );

    return;

}

#endif /*FLSUBST_VectorRotationToQuaternion_fx*/

static void VectorRotationToQuaternion_fx(

    const IVAS_VECTOR3 p1,

    const IVAS_VECTOR3 p2,

    IVAS_QUATERNION *const r )

{

#ifdef FLSUBST_VectorRotationToQuaternion_fx

    float dot_product;

    IVAS_QUATERNION_FL r_fl;

    IVAS_VECTOR3 p1_fl, p2_fl;

    IVAS_VECTOR3 cross_product, p1_normalized, p2_normalized;

    p1_fl.x = p1.x_fx / powf( 2, p1.q_fact );

    p1_fl.y = p1.y_fx / powf( 2, p1.q_fact );

    p1_fl.z = p1.z_fx / powf( 2, p1.q_fact );

    p2_fl.x = p2.x_fx / powf( 2, p2.q_fact );

    p2_fl.y = p2.y_fx / powf( 2, p2.q_fact );

    p2_fl.z = p2.z_fx / powf( 2, p2.q_fact );

    p1_normalized = VectorNormalize( p1_fl );

    p2_normalized = VectorNormalize( p2_fl );

    cross_product = VectorCrossProduct( p1_normalized, p2_normalized );

    /*  x = 0

        y = -0.712638557

        z = 0.0220356304

        */

    dot_product = VectorDotProduct( p1_normalized, p2_normalized );

    /*0.701185286*/

    if ( dot_product < -0.999999 )

    {

        /* happens when the p1 vector is parallel to p2, but direction is flipped */

        r_fl.w = 0.0f;

        r_fl.x = 0.0f;

        r_fl.y = 0.0f;

        r_fl.z = 1.0f;

    }

    else

    {

        /* all regular cases */

        r_fl.x = cross_product.x;

        r_fl.y = cross_product.y;

        r_fl.z = cross_product.z;

        r_fl.w = 1.0f + dot_product;

    }

    QuaternionNormalize( r_fl, &r_fl );

    r->w_fx = r_fl.w * powf( 2, r->q_fact );  //  +1980572160

    r->x_fx = r_fl.x * powf( 2, r->q_fact );  //  +0

    r->y_fx = r_fl.y * powf( 2, r->q_fact );  //  -829675712

    r->z_fx = r_fl.z * powf( 2, r->q_fact );  //  +25654558

    return;

#endif

    IVAS_VECTOR3 cross_product_fx, p1_normalized_fx = { 0 }, p2_normalized_fx = { 0 };

    Word32 dot_product_fx;

    Word16 q_dot, e_add, q_result;

        move16();

        length_fx = VectorLength_fx( p1, &q_len );

        result1_fx.x_fx = BASOP_Util_Divide3232_Scale_newton( p1.x_fx, length_fx, &scale );

        result1_fx.x_fx = BASOP_Util_Divide3232_Scale_newton( p1.x_fx, length_fx, &result1_e_x );

        move32();

        scale = norm_l( result1_fx.x_fx );

        result1_fx.x_fx = L_shl( result1_fx.x_fx, scale );                              /*This step maybe is not necessary*/

        move32();

        result1_e_x = add( scale, sub( q_len, p1.q_fact ) ); // e+(e1-e2)//

        result1_e_x = sub( add( result1_e_x, sub( q_len, p1.q_fact ) ), scale );

        float result1_fl_x = (float)result1_fx.x_fx / powf( 2, 31-result1_e_x );

        result1_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p1.y_fx, length_fx, &scale );

        result1_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p1.y_fx, length_fx, &result1_e_y );

        move32();

        result1_e_y = add( scale, sub( q_len, p1.q_fact ) );

        scale = norm_l( result1_fx.y_fx );

        result1_fx.y_fx = L_shl( result1_fx.y_fx, scale );

        move32();

        result1_e_y = sub( add( result1_e_y, sub( q_len, p1.q_fact ) ), scale );

        float result1_fl_y = (float) result1_fx.y_fx / powf( 2, 31-result1_e_y );

        result1_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p1.z_fx, length_fx, &scale );

        result1_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p1.z_fx, length_fx, &result1_e_z );

        move32();

        scale = norm_l( result1_fx.z_fx );

        result1_fx.z_fx = L_shl( result1_fx.z_fx, scale );

        move32();

        result1_e_z = add( scale, sub( q_len, p1.q_fact ) );

        result1_e_z = sub( add( result1_e_z, sub( q_len, p1.q_fact ) ), scale );

        float result1_fl_z = (float) result1_fx.z_fx / powf( 2, 31-result1_e_z );

#if 0

            q_result = s_min( s_min( x_qfact, y_qfact ), z_qfact );

        length_fx = VectorLength_fx( p2, &q_len );

        result2_fx.x_fx = BASOP_Util_Divide3232_Scale_newton( p2.x_fx, length_fx, &scale );

        result2_fx.x_fx = BASOP_Util_Divide3232_Scale_newton( p2.x_fx, length_fx, &result2_e_x );

        move32();

        scale = norm_l( result2_fx.x_fx );

        result2_fx.x_fx = L_shl( result2_fx.x_fx, scale );

        move32();

        result2_e_x = add( scale, sub( q_len, p2.q_fact ) ); // e+(e1-e2)//

        result2_e_x = sub( add( result2_e_x, sub( q_len, p2.q_fact ) ), scale );

        float result2_fl_x = (float) result2_fx.x_fx / powf( 2, 31-result2_e_x );

        result2_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p2.y_fx, length_fx, &scale );

        result2_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p2.y_fx, length_fx, &result2_e_y );

        move32();

        result2_e_y = add( scale, sub( q_len, p2.q_fact ) );

        scale = norm_l( result2_fx.y_fx );

        result2_fx.y_fx = L_shl( result2_fx.y_fx, scale );

        move32();

        result2_e_y = sub( add( result2_e_y, sub( q_len, p2.q_fact ) ), scale );

        float result2_fl_y = (float) result2_fx.y_fx / powf( 2, 31-result2_e_y );

        result2_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p2.z_fx, length_fx, &scale );

        result2_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p2.z_fx, length_fx, &result2_e_z );

        move32();

        scale = norm_l( result2_fx.z_fx );

        result2_fx.z_fx = L_shl( result2_fx.z_fx, scale );

        move32();

        result2_e_z = add( scale, sub( q_len, p2.q_fact ) );

        result2_e_z = sub( add( result2_e_z, sub( q_len, p2.q_fact ) ), scale );

        float result2_fl_z = (float) result2_fx.z_fx / powf( 2, 31-result2_e_z );

#if 0

            q_result = s_min( s_min( x_qfact, y_qfact ), z_qfact );

        move32();

        scale = s_max( cross_product_e_x, ( s_max( cross_product_e_y, cross_product_e_z ) ) );

        cross_product_fx.x_fx = L_shr( cross_product_fx.x_fx, sub( scale, cross_product_e_x ) );

        cross_product_fx.x_fx = L_shr( cross_product_fx.x_fx, sub( scale, cross_product_e_x ) );  //

        move32();

        cross_product_fx.y_fx = L_shr( cross_product_fx.y_fx, sub( scale, cross_product_e_y ) );

        cross_product_fx.y_fx = L_shr( cross_product_fx.y_fx, sub( scale, cross_product_e_y ) );  // - 0,712638527154922

        move32();

        cross_product_fx.z_fx = L_shr( cross_product_fx.z_fx, sub( scale, cross_product_e_z ) );

        cross_product_fx.z_fx = L_shr( cross_product_fx.z_fx, sub( scale, cross_product_e_z ) );  //   0,022035629022866

        move32();

        cross_product_fx.q_fact = sub( 31, scale );

            zz = Mpy_32_32( result1_fx.z_fx, result2_fx.z_fx );

            dot0 = BASOP_Util_Add_Mant32Exp( xx, add( result1_e_x, result2_e_x ), yy, add( result1_e_y, result2_e_y ), &scale );

            dot_product_fx = BASOP_Util_Add_Mant32Exp( dot0, scale, zz, add( result1_e_z, result2_e_z ), &q_dot );

            dot_product_fx = BASOP_Util_Add_Mant32Exp( dot0, scale, zz, add( result1_e_z, result2_e_z ), &q_dot ); //0,7011852525174617767333984375

            q_dot = sub( 31, q_dot );

        }