From 7fd11a77e2fa2b4ac2380b122a148e8be46e0afc Mon Sep 17 00:00:00 2001 From: Sandesh Venkatesh Date: Wed, 31 Jan 2024 09:50:14 +0530 Subject: [PATCH] Ported vector operation functions in Ivas_orient_trk.c [x] Vector cross & Dot Product,length,Normalise,subtract are converted to fixed point. --- lib_com/common_api_types.h | 9 ++ lib_rend/ivas_orient_trk.c | 195 +++++++++++++++++++++++++++++++++++++ 2 files changed, 204 insertions(+) diff --git a/lib_com/common_api_types.h b/lib_com/common_api_types.h index 3ea977cf4..0127d0555 100644 --- a/lib_com/common_api_types.h +++ b/lib_com/common_api_types.h @@ -149,6 +149,15 @@ typedef struct } IVAS_VECTOR3; +#ifdef IVAS_FLOAT_FIXED +typedef struct +{ + Word32 x_fx, y_fx, z_fx; + Word16 x_qfact, y_qfact, z_qfact; + +} IVAS_VECTOR3_FX; +#endif + typedef enum { IVAS_HEAD_ORIENT_TRK_NONE, diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c index c6f5a4ba7..d355bc5a7 100644 --- a/lib_rend/ivas_orient_trk.c +++ b/lib_rend/ivas_orient_trk.c @@ -465,6 +465,24 @@ static IVAS_VECTOR3 VectorSubtract( return result; } +#ifdef IVAS_FLOAT_FIXED +static IVAS_VECTOR3_FX VectorSubtract_fx( + const IVAS_VECTOR3_FX p1, + const IVAS_VECTOR3_FX p2 ) +{ + IVAS_VECTOR3_FX result; + + result.x_fx = L_sub( p1.x_fx, p2.x_fx ); + result.y_fx = L_sub( p1.y_fx, p2.y_fx ); + result.z_fx = L_sub( p1.z_fx, p2.z_fx ); + result.x_qfact = p1.x_qfact; + result.y_qfact = p1.y_qfact; + result.y_qfact = p1.y_qfact; + + return result; +} +#endif + /*------------------------------------------------------------------------------------------* * VectorCrossProduct() @@ -484,6 +502,66 @@ static IVAS_VECTOR3 VectorCrossProduct( return result; } +#ifdef IVAS_FLOAT_FIXED +static IVAS_VECTOR3_FX VectorCrossProduct_fx( + const IVAS_VECTOR3_FX p1, + const IVAS_VECTOR3_FX p2 ) +{ + IVAS_VECTOR3_FX result_fx; + Word32 mul1, mul2; + Word16 diff, q_mul1, q_mul2; + mul1 = Mpy_32_32( p1.y_fx, p2.z_fx ); + mul2 = Mpy_32_32( p1.z_fx, p2.y_fx ); + q_mul1 = p1.y_qfact + p2.z_qfact - 31; + q_mul2 = p1.z_qfact + p2.x_qfact - 31; + IF( GT_16( q_mul1, q_mul2 ) ) + { + diff = q_mul1 - q_mul2; + result_fx.x_fx = L_sub( ( L_shr( mul1, diff ) ), mul2 ); + result_fx.x_qfact = q_mul2; + } + ELSE + { + diff = q_mul2 - q_mul1; + result_fx.x_fx = L_sub( mul1, ( L_shr( mul2, diff ) ) ); + result_fx.x_qfact = q_mul1; + } + mul1 = Mpy_32_32( p1.z_fx, p2.x_fx ); + mul2 = Mpy_32_32( p1.x_fx, p2.z_fx ); + q_mul1 = p1.z_qfact + p2.x_qfact - 31; + q_mul2 = p1.x_qfact + p2.z_qfact - 31; + IF( GT_16( q_mul1, q_mul2 ) ) + { + diff = q_mul1 - q_mul2; + result_fx.y_fx = L_sub( ( L_shr( mul1, diff ) ), mul2 ); + result_fx.y_qfact = q_mul2; + } + ELSE + { + diff = q_mul2 - q_mul1; + result_fx.y_fx = L_sub( mul1, ( L_shr( mul2, diff ) ) ); + result_fx.y_qfact = q_mul1; + } + mul1 = Mpy_32_32( p1.x_fx, p2.y_fx ); + mul2 = Mpy_32_32( p1.y_fx, p2.x_fx ); + q_mul1 = p1.x_qfact + p2.y_qfact - 31; + q_mul2 = p1.y_qfact + p2.x_qfact - 31; + IF( GT_16( q_mul1, q_mul2 ) ) + { + diff = q_mul1 - q_mul2; + result_fx.z_fx = L_sub( ( L_shr( mul1, diff ) ), mul2 ); + result_fx.z_qfact = q_mul2; + } + ELSE + { + diff = q_mul2 - q_mul1; + result_fx.z_fx = L_sub( mul1, ( L_shr( mul2, diff ) ) ); + result_fx.z_qfact = q_mul1; + } + + return result_fx; +} +#endif /*------------------------------------------------------------------------------------------* * VectorDotProduct( @@ -498,6 +576,25 @@ static float VectorDotProduct( return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z; } +#ifdef IVAS_FLOAT_FIXED +static Word32 VectorDotProduct_fx( + const IVAS_VECTOR3_FX p1, + const IVAS_VECTOR3_FX p2, + Word16 *q_fact ) +{ + Word32 mul_1, mul_2, mul_3 = 0; + Word32 result_fx = 0; + + mul_1 = Mpy_32_32( p1.x_fx, p2.x_fx ); + mul_2 = Mpy_32_32( p1.y_fx, p2.y_fx ); + mul_3 = Mpy_32_32( p1.z_fx, p2.z_fx ); + result_fx = L_add( mul_1, mul_2 ); + result_fx = L_add( result_fx, mul_3 ); + *q_fact = p1.x_qfact + p2.x_qfact - 31; + + return result_fx; +} +#endif /*------------------------------------------------------------------------------------------* * VectorLength() @@ -511,6 +608,25 @@ static float VectorLength( return sqrtf( p.x * p.x + p.y * p.y + p.z * p.z ); } +#ifdef IVAS_FLOAT_FIXED +static Word32 VectorLength_fx( + const IVAS_VECTOR3_FX p, + Word16 *q_fact ) +{ + Word32 mul_1, mul_2, mul_3 = 0; + Word32 result_fx = 0; + Word16 result_e = 0; + mul_1 = Mpy_32_32( p.x_fx, p.x_fx ); + mul_2 = Mpy_32_32( p.y_fx, p.y_fx ); + mul_3 = Mpy_32_32( p.z_fx, p.z_fx ); + result_fx = L_add( mul_1, mul_2 ); + result_fx = L_add( result_fx, mul_3 ); + result_e = 31 - (2 * p.x_qfact - 31); + result_fx = Sqrt32( result_fx, &result_e ); + *q_fact = 31 - result_e; + return result_fx; +} +#endif /*------------------------------------------------------------------------------------------* * VectorNormalize() @@ -532,6 +648,29 @@ static IVAS_VECTOR3 VectorNormalize( return result; } +#ifdef IVAS_FLOAT_FIXED +static IVAS_VECTOR3_FX VectorNormalize_fx( + const IVAS_VECTOR3_FX p ) +{ + IVAS_VECTOR3_FX result_fx; + Word32 length_fx; + Word16 q_len, scale = 0; + length_fx = VectorLength_fx( p, &q_len ); + Word16 div; + + div = BASOP_Util_Divide3232_Scale( p.x_fx, length_fx, &scale ); + result_fx.x_fx = L_deposit_h( div ); + result_fx.x_qfact = ( 31 - ( scale + ( q_len - p.x_qfact ) ) ); // e+(e1-e2)// + div = BASOP_Util_Divide3232_Scale( p.y_fx, length_fx, &scale ); + result_fx.y_fx = L_deposit_h( div ); + result_fx.y_qfact = ( 31 - ( scale + ( q_len - p.y_qfact ) ) ); + div = BASOP_Util_Divide3232_Scale( p.z_fx, length_fx, &scale ); + result_fx.z_fx = L_deposit_h( div ); + result_fx.z_qfact = ( 31 - ( scale + ( q_len - p.z_qfact ) ) ); + + return result_fx; +} +#endif /*------------------------------------------------------------------------------------------* * VectorRotationToQuaternion() @@ -547,10 +686,33 @@ static void VectorRotationToQuaternion( float dot_product; IVAS_VECTOR3 cross_product, p1_normalized, p2_normalized; +#ifdef IVAS_FLOAT_FIXED + IVAS_VECTOR3_FX cross_product_fx, p1_normalized_fx, p2_normalized_fx; + IVAS_VECTOR3_FX p1_fx, p2_fx; + Word32 dot_product_fx; + Word16 q_dot; + p1_fx.x_qfact = p1_fx.y_qfact = p1_fx.z_qfact = Q29; + p2_fx.x_qfact = p2_fx.y_qfact = p2_fx.z_qfact = Q29; + p1_fx.x_fx = float_to_fix(p1.x, p1_fx.x_qfact); + p1_fx.y_fx = float_to_fix(p1.y, p1_fx.y_qfact); + p1_fx.z_fx = float_to_fix(p1.z, p1_fx.z_qfact); + p2_fx.x_fx = float_to_fix(p2.x, p2_fx.x_qfact); + p2_fx.y_fx = float_to_fix(p2.y, p2_fx.y_qfact); + p2_fx.z_fx = float_to_fix(p2.z, p2_fx.z_qfact); + p1_normalized_fx = VectorNormalize_fx( p1_fx ); + p2_normalized_fx = VectorNormalize_fx( p2_fx ); + 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_product = me2f(dot_product_fx, (31 - q_dot)); + cross_product.x = me2f(cross_product_fx.x_fx, (31 - cross_product_fx.x_qfact)); + cross_product.y = me2f(cross_product_fx.y_fx, (31 - cross_product_fx.y_qfact)); + cross_product.z = me2f(cross_product_fx.z_fx, (31 - cross_product_fx.z_qfact)); +#else p1_normalized = VectorNormalize( p1 ); p2_normalized = VectorNormalize( p2 ); cross_product = VectorCrossProduct( p1_normalized, p2_normalized ); dot_product = VectorDotProduct( p1_normalized, p2_normalized ); +#endif if ( dot_product < -0.999999 ) { @@ -736,6 +898,9 @@ ivas_error ivas_orient_trk_SetReferenceVector( IVAS_VECTOR3 acousticFrontVector, ivasForwardVector; IVAS_VECTOR3 listenerPosLevel, refPosLevel; float acousticFrontVectorLength; +#ifdef IVAS_FLOAT_FIXED + IVAS_VECTOR3_FX acousticFrontVector_fx, listenerPos_fx, refPos_fx, listenerPosLevel_fx, refPosLevel_fx; +#endif if ( pOTR == NULL ) { @@ -748,7 +913,22 @@ ivas_error ivas_orient_trk_SetReferenceVector( case IVAS_HEAD_ORIENT_TRK_REF: case IVAS_HEAD_ORIENT_TRK_AVG: case IVAS_HEAD_ORIENT_TRK_REF_VEC: +#ifdef IVAS_FLOAT_FIXED + listenerPos_fx.x_qfact = listenerPos_fx.y_qfact = listenerPos_fx.z_qfact = Q29; + refPos_fx.x_qfact = refPos_fx.y_qfact = refPos_fx.z_qfact = Q29; + listenerPos_fx.x_fx = float_to_fix( listenerPos.x, listenerPos_fx.x_qfact ); + listenerPos_fx.y_fx = float_to_fix( listenerPos.y, listenerPos_fx.y_qfact ); + listenerPos_fx.z_fx = float_to_fix( listenerPos.z, listenerPos_fx.z_qfact ); + refPos_fx.x_fx = float_to_fix( refPos.x, refPos_fx.x_qfact ); + refPos_fx.y_fx = float_to_fix( refPos.y, refPos_fx.y_qfact ); + refPos_fx.z_fx = float_to_fix( refPos.z, refPos_fx.z_qfact ); + acousticFrontVector_fx = VectorSubtract_fx( listenerPos_fx, refPos_fx ); + acousticFrontVector.x = me2f( acousticFrontVector_fx.x_fx, 31 - acousticFrontVector_fx.x_qfact ); + acousticFrontVector.y = me2f( acousticFrontVector_fx.y_fx, 31 - acousticFrontVector_fx.y_qfact ); + acousticFrontVector.z = me2f( acousticFrontVector_fx.z_fx, 31 - acousticFrontVector_fx.z_qfact ); +#else acousticFrontVector = VectorSubtract( listenerPos, refPos ); +#endif break; case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV: /* ignore the height difference between listener position and reference position */ @@ -757,7 +937,22 @@ ivas_error ivas_orient_trk_SetReferenceVector( listenerPosLevel.y = listenerPos.y; refPosLevel.x = refPos.x; refPosLevel.y = refPos.y; +#ifdef IVAS_FLOAT_FIXED + listenerPosLevel_fx.x_qfact = listenerPosLevel_fx.y_qfact = listenerPosLevel_fx.z_qfact = Q29; + refPosLevel_fx.x_qfact = refPosLevel_fx.y_qfact = refPosLevel_fx.z_qfact = Q29; + listenerPosLevel_fx.x_fx = float_to_fix( listenerPosLevel.x, listenerPosLevel_fx.x_qfact ); + listenerPosLevel_fx.y_fx = float_to_fix( listenerPosLevel.y, listenerPosLevel_fx.y_qfact ); + listenerPosLevel_fx.z_fx = float_to_fix( listenerPosLevel.z, listenerPosLevel_fx.z_qfact ); + refPosLevel_fx.x_fx = float_to_fix(refPosLevel.x, refPosLevel_fx.x_qfact ); + refPosLevel_fx.y_fx = float_to_fix(refPosLevel.y, refPosLevel_fx.y_qfact ); + refPosLevel_fx.z_fx = float_to_fix(refPosLevel.z, refPosLevel_fx.z_qfact ); + acousticFrontVector_fx = VectorSubtract_fx( listenerPosLevel_fx, refPosLevel_fx ); + acousticFrontVector.x = me2f( acousticFrontVector_fx.x_fx, 31 - acousticFrontVector_fx.x_qfact ); + acousticFrontVector.y = me2f( acousticFrontVector_fx.y_fx, 31 - acousticFrontVector_fx.y_qfact ); + acousticFrontVector.z = me2f( acousticFrontVector_fx.z_fx, 31 - acousticFrontVector_fx.z_qfact ); +#else acousticFrontVector = VectorSubtract( listenerPosLevel, refPosLevel ); +#endif break; default: return IVAS_ERR_WRONG_PARAMS; -- GitLab