Commit dd2208a4 authored by Fabian Bauer's avatar Fabian Bauer
Browse files

add precission upgrade for VectorLength_fx

parent 5b8cdb5a

lib_com/options.h

+2 −0
Original line number Diff line number Diff line
@@ -103,6 +103,8 @@ 
#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



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



/* ################## End MAINTENANCE switches ######################### */

lib_rend/ivas_orient_trk_fx.c

+91 −1
Original line number Diff line number Diff line
@@ -52,6 +52,50 @@ 
 * Local functions

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





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

 * Sqrt32 with Newton-Raphson Correction

 *

 *

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





#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 )

{

    /*higher precission by Newton-Raphson iterations*/

    Word32 result_fx_0, result_fx_1;

    Word16 result_fx_e, result_fx_0_e, result_fx_1_e;

    Word32 tmp;

    Word16 scale;



    result_fx_e = *e;

    result_fx_0 = Sqrt32( mantissa, &result_fx_e );

    result_fx_0_e = result_fx_e;

    result_fx_e = *e;



    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_1 = tmp;

        result_fx_1_e = sub( scale, 1 );



        result_fx_0 = result_fx_1;

        result_fx_0_e = result_fx_1_e;

    }

    *e = result_fx_0_e;

    return result_fx_0;

}

#endif



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

 * IdentityQuaternion()

 *
@@ -472,6 +516,7 @@ static Word32 VectorDotProduct_fx( 
    return result_fx;

}





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

 * VectorLength()

 *
@@ -482,12 +527,57 @@ static Word32 VectorLength_fx( 
    IVAS_VECTOR3 p,

    Word16 *q_fact )

{

#if 0

  double x, y, z;

  double len;

  Word32 len_fx;

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

  *q_fact = p.q_fact;

  len_fx = (Word32) ( len * pow( 2, p.q_fact ) );

  return len_fx;

#endif



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

    return result_fx;