From dd2208a442420e3aee37365911f16cac4c85f9c8 Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Tue, 10 Mar 2026 11:32:53 +0100 Subject: [PATCH 1/9] add precission upgrade for VectorLength_fx --- lib_com/options.h | 2 + lib_rend/ivas_orient_trk_fx.c | 92 ++++++++++++++++++++++++++++++++++- 2 files changed, 93 insertions(+), 1 deletion(-) diff --git a/lib_com/options.h b/lib_com/options.h index 8db1debfc..7d23ff139 100644 --- a/lib_com/options.h +++ b/lib_com/options.h @@ -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 ######################### */ diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index 00d7f7da8..d5555e247 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -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; -- GitLab From bfabdcea4168dc1f1b84bda7d4d2ae7ad245b021 Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Tue, 10 Mar 2026 11:33:36 +0100 Subject: [PATCH 2/9] delete float reference for VectorLength_fx --- lib_rend/ivas_orient_trk_fx.c | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index d5555e247..fecff02c9 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -527,18 +527,6 @@ 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; -- GitLab From 05e1cfa496fe334241015fdc7e3fbfb067cbb16c Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Tue, 10 Mar 2026 11:39:36 +0100 Subject: [PATCH 3/9] cleanup Sqrt32_NewtonRaphson --- lib_rend/ivas_orient_trk_fx.c | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index fecff02c9..0b965ce96 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -71,8 +71,8 @@ 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 result_fx_0; + Word16 result_fx_e, result_fx_0_e; Word32 tmp; Word16 scale; @@ -85,11 +85,8 @@ static Word32 Sqrt32_NewtonRaphson( Word32 mantissa, Word16 * e, Word16 n ) { 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; + result_fx_0 = tmp; + result_fx_0_e = sub( scale, 1 ); } *e = result_fx_0_e; return result_fx_0; -- GitLab From a79a56a9dee19c88d0c84480b9fc4030d74ae6ff Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Tue, 10 Mar 2026 16:09:05 +0100 Subject: [PATCH 4/9] merged VectorNormalize_fx and VectorCrossProduct_fx resp VectorDotProduct_fx --- lib_rend/ivas_orient_trk_fx.c | 104 ++++++++++++++++++++++++++++++++++ 1 file changed, 104 insertions(+) diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index 0b965ce96..e7a944973 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -614,6 +614,10 @@ static IVAS_VECTOR3 VectorNormalize_fx( * 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 + static void VectorRotationToQuaternion_fx( const IVAS_VECTOR3 p1, const IVAS_VECTOR3 p2, @@ -623,10 +627,110 @@ static void VectorRotationToQuaternion_fx( 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, &scale ); + move32(); + result1_e_x = add( scale, sub( q_len, p1.q_fact ) ); // e+(e1-e2)// + + result1_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p1.y_fx, length_fx, &scale ); + move32(); + result1_e_y = add( scale, sub( q_len, p1.q_fact ) ); + + result1_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p1.z_fx, length_fx, &scale ); + move32(); + result1_e_z = add( scale, sub( q_len, p1.q_fact ) ); + +#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, &scale ); + move32(); + result2_e_x = add( scale, sub( q_len, p2.q_fact ) ); // e+(e1-e2)// + + result2_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p2.y_fx, length_fx, &scale ); + move32(); + result2_e_y = add( scale, sub( q_len, p2.q_fact ) ); + + result2_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p2.z_fx, length_fx, &scale ); + move32(); + result2_e_z = add( scale, sub( q_len, p2.q_fact ) ); + +#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 ) ); + move32(); + cross_product_fx.z_fx = L_shr( cross_product_fx.z_fx, sub( scale, cross_product_e_z ) ); + 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 ); + + 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 -- GitLab From 23c9384c55ba762e019ec4d8a15863dbd001fab0 Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Thu, 12 Mar 2026 08:16:59 +0100 Subject: [PATCH 5/9] Added some more macros to enable more precission tweaks --- lib_rend/ivas_orient_trk_fx.c | 252 ++++++++++++++++++++++++++++++---- 1 file changed, 226 insertions(+), 26 deletions(-) diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index e7a944973..b168459e6 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -40,6 +40,28 @@ #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 @@ -61,12 +83,7 @@ #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 ) { @@ -185,21 +202,41 @@ static void QuaternionDivision_fx( 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// @@ -232,7 +269,11 @@ static void QuaternionNormalize_fx( 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; } @@ -577,6 +618,27 @@ static Word32 VectorLength_fx( 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; @@ -614,15 +676,129 @@ static IVAS_VECTOR3 VectorNormalize_fx( * 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; @@ -637,18 +813,30 @@ static void VectorRotationToQuaternion_fx( 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(); - result1_e_x = add( scale, sub( q_len, p1.q_fact ) ); // e+(e1-e2)// - - result1_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p1.y_fx, length_fx, &scale ); + 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_y = add( scale, sub( q_len, p1.q_fact ) ); + 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.z_fx = BASOP_Util_Divide3232_Scale_newton( p1.z_fx, length_fx, &scale ); + result1_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p1.y_fx, length_fx, &result1_e_y ); move32(); - result1_e_z = 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, &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 @@ -663,17 +851,29 @@ static void VectorRotationToQuaternion_fx( 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 ); @@ -697,11 +897,11 @@ static void VectorRotationToQuaternion_fx( 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 ); @@ -718,7 +918,7 @@ static void VectorRotationToQuaternion_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 ); + 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 ); } -- GitLab From ea35469edef7bb018319d50f0824e6f2d76dfbfe Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Thu, 12 Mar 2026 08:29:34 +0100 Subject: [PATCH 6/9] erase unnecessary code - correct instrumentation of Sqrt32_NewtonRaphson() --- lib_com/options.h | 2 +- lib_rend/ivas_orient_trk_fx.c | 341 +--------------------------------- 2 files changed, 11 insertions(+), 332 deletions(-) diff --git a/lib_com/options.h b/lib_com/options.h index 7d23ff139..eae3c2048 100644 --- a/lib_com/options.h +++ b/lib_com/options.h @@ -103,7 +103,7 @@ #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 ########################### */ diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index b168459e6..6766967af 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -41,25 +41,8 @@ #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 @@ -78,13 +61,12 @@ /*------------------------------------------------------------------------------------------* * 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*/ @@ -94,18 +76,23 @@ static Word32 Sqrt32_NewtonRaphson( Word32 mantissa, Word16 * e, Word16 n ) 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 @@ -202,41 +189,21 @@ static void QuaternionDivision_fx( 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// @@ -568,41 +535,14 @@ static Word32 VectorLength_fx( 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(); @@ -618,27 +558,6 @@ static Word32 VectorLength_fx( 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; @@ -676,264 +595,24 @@ static IVAS_VECTOR3 VectorNormalize_fx( * 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(); -- GitLab From 0f1db61722bca284b0300ad55801bf728a25b2e5 Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Thu, 12 Mar 2026 08:02:35 +0000 Subject: [PATCH 7/9] minimize diffs --- lib_rend/ivas_orient_trk_fx.c | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index 6766967af..0846d9708 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -40,11 +40,6 @@ #include "wmc_auto.h" #include "prot_fx.h" -#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING -#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_QuaternionNormalize_fx_USE_Sqrt32_NewtonRaphson /*use refinement of Sqrt32 within QuaternionNormalize_fx() function*/ -#endif - /*------------------------------------------------------------------------------------------* * Local constants @@ -236,7 +231,7 @@ static void QuaternionNormalize_fx( 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 +#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING sqrt_fx = Sqrt32_NewtonRaphson( dot_prod_fx, &sqrt_e, 1 ); #else sqrt_fx = Sqrt32( dot_prod_fx, &sqrt_e ); @@ -521,7 +516,6 @@ static Word32 VectorDotProduct_fx( return result_fx; } - /*------------------------------------------------------------------------------------------* * VectorLength() * @@ -532,18 +526,15 @@ static Word32 VectorLength_fx( IVAS_VECTOR3 p, Word16 *q_fact ) { - Word16 sqrt_e; Word32 result_fx; 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 */ -#ifndef FIX_2398_PRECISSION_ORIENTATION_TRACKING_VectorLength_fx_USE_Sqrt32_NewtonRaphson +#ifndef FIX_2398_PRECISSION_ORIENTATION_TRACKING result_fx = Sqrt32( result_fx, &sqrt_e ); #else result_fx = Sqrt32_NewtonRaphson( result_fx, &sqrt_e, 1 ); #endif - - *q_fact = sub( 31, sqrt_e ); /* back to Q again */ move16(); return result_fx; @@ -606,13 +597,11 @@ static void VectorRotationToQuaternion_fx( 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 ); - // dot & cross product are same q// - Word32 comp_fx = -2147481472; + Word32 comp_fx = -2147481472; //-0.999999f in Q31 move32(); Word16 comp_e = 0, check_flag; move16(); -- GitLab From 4c34ba7515fa804cdaf975772ccf7f89644697d3 Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Thu, 12 Mar 2026 08:28:03 +0000 Subject: [PATCH 8/9] correction of macro location #ifdef --- lib_rend/ivas_orient_trk_fx.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index 0846d9708..c1b3d4a64 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -52,7 +52,7 @@ * Local functions *------------------------------------------------------------------------------------------*/ - +#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING /*------------------------------------------------------------------------------------------* * Sqrt32 with Newton-Raphson Correction * @@ -60,8 +60,6 @@ * 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*/ -- GitLab From 1fec2d0799e2c2e5a61c0c9a032fd67c252edb8e Mon Sep 17 00:00:00 2001 From: Fabian Bauer Date: Thu, 12 Mar 2026 11:42:28 +0100 Subject: [PATCH 9/9] clang format patch --- lib_rend/ivas_orient_trk_fx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib_rend/ivas_orient_trk_fx.c b/lib_rend/ivas_orient_trk_fx.c index c1b3d4a64..8208488e1 100644 --- a/lib_rend/ivas_orient_trk_fx.c +++ b/lib_rend/ivas_orient_trk_fx.c @@ -60,7 +60,7 @@ * As final step. The number of Newton-Raphson iterations can be set by 3rd parameter n *------------------------------------------------------------------------------------------*/ -static Word32 Sqrt32_NewtonRaphson( Word32 mantissa, Word16 * e, Word16 n ) +static Word32 Sqrt32_NewtonRaphson( Word32 mantissa, Word16 *e, Word16 n ) { /*higher precission by Newton-Raphson iterations*/ Word32 result_fx_0; -- GitLab