erase unnecessary code - correct instrumentation of Sqrt32_NewtonRaphson()

#define FIX_BASOP_2262_OLAP_BUFFER_SYNTH_SWITCHING      /* FhG: basop issue 2262: correct buffer update for FD-CNG buffer in case of BR switching */

#define FIX_2440_AGC_PRESCALING                         /* FhG: basop issue 2440: Fix loop bounds when scaling p_output_fx before ivas_spar_dec_agc_pca_fx() */

#define FIX_2471_REMOVE_POSSIBLE_OVRF                   /* VA: basop issue 2471: correcting undesired overflow */

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING        /* FhG: use refinement of Sqrt32 within certain functions*/

/* ##################### End NON-BE switches ########################### */

#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

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

 * Sqrt32 with Newton-Raphson Correction

 *

 *

 * This is simply a wrapper for Sqrt32() which performs Newton-Raphson correction

 * As final step. The number of Newton-Raphson iterations can be set by 3rd parameter n

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

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING

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

{

    /*higher precission by Newton-Raphson iterations*/

    Word16 scale;

    result_fx_e = *e;

    move32();

    result_fx_0 = Sqrt32( mantissa, &result_fx_e );

    result_fx_0_e = result_fx_e;

    move32();

    result_fx_e = *e;

    move32();

    FOR( int NRi = 0; NRi < n; NRi++ )

    {

        tmp = BASOP_Util_Divide3232_Scale_newton( mantissa, result_fx_0, &scale );

        tmp = BASOP_Util_Add_Mant32Exp( tmp, add( sub( result_fx_e, result_fx_0_e ), scale ), result_fx_0, result_fx_0_e, &scale );

        result_fx_0 = tmp;

        move32();

        result_fx_0_e = sub( scale, 1 );

    }

    *e = result_fx_0_e;

    move32();

    return result_fx_0;

}

#endif

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

    Word16 sqrt_e;

    Word32 result_fx;

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_VectorLength_fx

    Word16 scale_x = norm_l( p.x_fx );

    Word16 scale_y = norm_l( p.y_fx );

    Word16 scale_z = norm_l( p.z_fx );

    Word64 result_fx64;

    Word16 exp = sub( 31, p.q_fact );

    Word32 x_fx, y_fx, z_fx;

    Word16 scale;

    scale = s_min( s_min( scale_x, scale_y ), scale_z );

    x_fx = L_shl( p.x_fx, scale );

    y_fx = L_shl( p.y_fx, scale );

    z_fx = L_shl( p.z_fx, scale );

    exp = sub( exp, scale );

    result_fx64 = W_mac0_32_32( W_mac0_32_32( W_mult0_32_32( x_fx, x_fx ), y_fx, y_fx ), z_fx, z_fx );

    exp = add( shl( exp, 1 ), 1 );

    result_fx = W_extract_h( result_fx64 );

    sqrt_e = exp;

    move16();

    result_fx = Sqrt32_NewtonRaphson( result_fx, &sqrt_e, 1 );

#else

    result_fx = Madd_32_32( Madd_32_32( Mpy_32_32( p.x_fx, p.x_fx ), p.y_fx, p.y_fx ), p.z_fx, p.z_fx );

    sqrt_e = shl( sub( 31, p.q_fact ), 1 ); /* convert Q to E */

    /*536870867 e2 */

#ifndef FIX_2398_PRECISSION_ORIENTATION_TRACKING_VectorLength_fx_USE_Sqrt32_NewtonRaphson

    result_fx = Sqrt32( result_fx, &sqrt_e );

#else

    result_fx = Sqrt32_NewtonRaphson( result_fx, &sqrt_e, 1 );

#endif

    /*default 2 147 458 624 e0 Q31*/

    /*        2 147 458 624 e0 Q31*/

#endif

    *q_fact = sub( 31, sqrt_e ); /* back to Q again */

    move16();

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

#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_NormalizeVectorCrossProduct_VectorDotProduct

    {

        IVAS_VECTOR3 result1_fx, result2_fx;

        Word32 length_fx;

        Word16 q_len, scale = 0, result1_e_x, result2_e_x, result1_e_y, result2_e_y, result1_e_z, result2_e_z, q_result;

        Word16 cross_product_e_x, cross_product_e_y, cross_product_e_z;

        move16();

        length_fx = VectorLength_fx( p1, &q_len );

        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 = 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, &result1_e_y );

        move32();

        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, &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 = 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 );

            result1_fx.x_fx = L_shr( result1_fx.x_fx, sub( x_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.y_fx = L_shr( result1_fx.y_fx, sub( y_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.z_fx = L_shr( result1_fx.z_fx, sub( z_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.q_fact = q_result;

            move16();

#endif

        length_fx = VectorLength_fx( p2, &q_len );

        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 = 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, &result2_e_y );

        move32();

        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, &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 = 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 );

            result1_fx.x_fx = L_shr( result1_fx.x_fx, sub( x_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.y_fx = L_shr( result1_fx.y_fx, sub( y_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.z_fx = L_shr( result1_fx.z_fx, sub( z_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.q_fact = q_result;

            move16();

#endif

        // static IVAS_VECTOR3 VectorCrossProduct_fx(                const IVAS_VECTOR3 p1,                const IVAS_VECTOR3 p2 )

        cross_product_fx.x_fx = BASOP_Util_Add_Mant32Exp( Mpy_32_32( result1_fx.y_fx, result2_fx.z_fx ), add( result1_e_y, result2_e_z ), L_negate( Mpy_32_32( result1_fx.z_fx, result2_fx.y_fx ) ), add( result1_e_z, result2_e_y ), &cross_product_e_x );

        move32();

        cross_product_fx.y_fx = BASOP_Util_Add_Mant32Exp( Mpy_32_32( result1_fx.z_fx, result2_fx.x_fx ), add( result1_e_z, result2_e_x ), L_negate( Mpy_32_32( result1_fx.x_fx, result2_fx.z_fx ) ), add( result1_e_x, result2_e_z ), &cross_product_e_y );

        move32();

        cross_product_fx.z_fx = BASOP_Util_Add_Mant32Exp( Mpy_32_32( result1_fx.x_fx, result2_fx.y_fx ), add( result1_e_x, result2_e_y ), L_negate( Mpy_32_32( result1_fx.y_fx, result2_fx.x_fx ) ), add( result1_e_y, result2_e_x ), &cross_product_e_z );

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

        move32();

        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 ) );  //   0,022035629022866

        move32();

        cross_product_fx.q_fact = sub( 31, scale );

        move16();

        // return result_fx;

        //dot_product_fx = VectorDotProduct_fx( p1_normalized_fx, p2_normalized_fx, &q_dot );

        //static Word32 VectorDotProduct_fx(            const IVAS_VECTOR3 p1,            const IVAS_VECTOR3 p2,            Word16 *q_fact )

        {

            Word32 xx, yy, zz, dot0;

            xx = Mpy_32_32( result1_fx.x_fx, result2_fx.x_fx );

            yy = Mpy_32_32( result1_fx.y_fx, result2_fx.y_fx );

            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 ); //0,7011852525174617767333984375

            q_dot = sub( 31, q_dot );

        }

    }

#else

    p1_normalized_fx = VectorNormalize_fx( p1 );

    p2_normalized_fx = VectorNormalize_fx( p2 );

    cross_product_fx = VectorCrossProduct_fx( p1_normalized_fx, p2_normalized_fx );

    dot_product_fx = VectorDotProduct_fx( p1_normalized_fx, p2_normalized_fx, &q_dot );

#endif

    // dot & cross product are same q//

    Word32 comp_fx = -2147481472; //-0.999999f in Q31

    Word32 comp_fx = -2147481472;

    move32();

    Word16 comp_e = 0, check_flag;

    move16();