From bd95bd2d24d0ab2f11cb4f1b420208ac1db703ba Mon Sep 17 00:00:00 2001 From: Sandesh Venkatesh Date: Sun, 28 Jan 2024 16:14:05 +0530 Subject: [PATCH] few functions converted to fixed point in ivas_orient_trk.c Quaternion Product, angle, conjugate, Dotproduct converted. --- lib_com/common_api_types.h | 12 ++ lib_com/ivas_cnst.h | 2 + lib_rend/ivas_orient_trk.c | 280 ++++++++++++++++++++++++++++++++++++- lib_rend/ivas_stat_rend.h | 18 +++ 4 files changed, 308 insertions(+), 4 deletions(-) diff --git a/lib_com/common_api_types.h b/lib_com/common_api_types.h index c4454fd62..3ea977cf4 100644 --- a/lib_com/common_api_types.h +++ b/lib_com/common_api_types.h @@ -39,6 +39,10 @@ #include #include #include "ivas_error.h" +#ifdef IVAS_FLOAT_FIXED +#include "typedef.h" +#include "basop32.h" +#endif /*----------------------------------------------------------------------------------* * Common API constants @@ -131,6 +135,14 @@ typedef struct } IVAS_QUATERNION; +#ifdef IVAS_FLOAT_FIXED +typedef struct +{ + Word32 w_fx, x_fx, y_fx, z_fx; + Word16 w_qfact, x_qfact, y_qfact, z_qfact; +} IVAS_QUATERNION_FX; +#endif + typedef struct { float x, y, z; diff --git a/lib_com/ivas_cnst.h b/lib_com/ivas_cnst.h index a2fd290dd..6a70a92e6 100644 --- a/lib_com/ivas_cnst.h +++ b/lib_com/ivas_cnst.h @@ -48,6 +48,8 @@ #define _180_OVER_PI ( 180.0f / EVS_PI ) #ifdef IVAS_FLOAT_FIXED #define _180_OVER_PI_Q25 1922527233 +#define PI_OVER_4_Q29 421657440 +#define PI_OVER_Q29 1686629760 #endif #define SQRT2 1.414213562373095f diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c index c540c9911..c6f5a4ba7 100644 --- a/lib_rend/ivas_orient_trk.c +++ b/lib_rend/ivas_orient_trk.c @@ -39,7 +39,10 @@ #include #include #include "wmc_auto.h" - +#ifdef IVAS_FLOAT_FIXED +#include "prot_fx1.h" +#include "prot_fx2.h" +#endif /*------------------------------------------------------------------------------------------* * Local constants @@ -92,6 +95,56 @@ void QuaternionProduct( return; } +#ifdef IVAS_FLOAT_FIXED +void QuaternionProduct_fx( + const IVAS_QUATERNION_FX q1, + const IVAS_QUATERNION_FX q2, + IVAS_QUATERNION_FX *const r ) +{ + IVAS_QUATERNION_FX tmp; + Word32 mul_1, mul_2, mul_3, mul_4 = 0; + mul_1 = Mpy_32_32( q1.w_fx, q2.w_fx ); + mul_2 = Mpy_32_32( q1.x_fx, q2.x_fx ); + mul_3 = Mpy_32_32( q1.y_fx, q2.y_fx ); + mul_4 = Mpy_32_32( q1.z_fx, q2.z_fx ); + mul_3 = L_add( mul_3, mul_4 ); + mul_2 = L_add( mul_2, mul_3 ); + tmp.w_fx = L_sub( mul_1, mul_2 ); + tmp.w_qfact = q1.w_qfact + q2.w_qfact - 31; + + mul_1 = Mpy_32_32( q1.w_fx, q2.x_fx ); + mul_2 = Mpy_32_32( q1.x_fx, q2.w_fx ); + mul_3 = Mpy_32_32( q1.y_fx, q2.z_fx ); + mul_4 = Mpy_32_32( q1.z_fx, q2.y_fx ); + mul_2 = L_add( mul_1, mul_2 ); + mul_3 = L_add( mul_2, mul_3 ); + tmp.x_fx = L_sub( mul_3, mul_4 ); + tmp.x_qfact = q1.w_qfact + q2.x_qfact - 31; + + mul_1 = Mpy_32_32( q1.w_fx, q2.y_fx ); + mul_2 = Mpy_32_32( q1.x_fx, q2.z_fx ); + mul_3 = Mpy_32_32( q1.y_fx, q2.w_fx ); + mul_4 = Mpy_32_32( q1.z_fx, q2.x_fx ); + mul_3 = L_add( mul_1, mul_3 ); + mul_4 = L_add( mul_3, mul_4 ); + tmp.y_fx = L_sub( mul_4, mul_2 ); + tmp.y_qfact = q1.w_qfact + q2.y_qfact - 31; + + mul_1 = Mpy_32_32( q1.w_fx, q2.z_fx ); + mul_2 = Mpy_32_32( q1.x_fx, q2.y_fx ); + mul_3 = Mpy_32_32( q1.y_fx, q2.x_fx ); + mul_4 = Mpy_32_32( q1.z_fx, q2.w_fx ); + mul_2 = L_add( mul_1, mul_2 ); + mul_4 = L_add( mul_2, mul_4 ); + tmp.z_fx = L_sub( mul_4, mul_3 ); + tmp.z_qfact = q1.w_qfact + q2.z_qfact - 31; + + *r = tmp; + + return; +} +#endif + /*------------------------------------------------------------------------------------------* * QuaternionDotProduct() * @@ -105,6 +158,28 @@ static float QuaternionDotProduct( return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; } +#ifdef IVAS_FLOAT_FIXED +static Word32 QuaternionDotProduct_fx( + const IVAS_QUATERNION_FX q1, + const IVAS_QUATERNION_FX q2 ) +{ + Word32 a[4], b[4] = { 0 }; + Word32 result = 0; + a[0] = q1.w_fx; + a[1] = q1.x_fx; + a[2] = q1.y_fx; + a[3] = q1.z_fx; + b[0] = q2.w_fx; + b[1] = q2.x_fx; + b[2] = q2.y_fx; + b[3] = q2.z_fx; + for ( int i = 0; i < 4; i++ ) + { + result = Madd_32_32( result, a[i], b[i] ); + } + return ( result ); +} +#endif /*------------------------------------------------------------------------------------------* * QuaternionDivision() @@ -136,8 +211,22 @@ static void QuaternionNormalize( const IVAS_QUATERNION q, IVAS_QUATERNION *const r ) { +#ifdef IVAS_FLOAT_FIXED + IVAS_QUATERNION_FX q_fx; + Word16 q_dot = 0; + q_fx.w_qfact = q_fx.x_qfact = q_fx.y_qfact = q_fx.z_qfact = Q29; + q_fx.w_fx = float_to_fix( q.w, q_fx.w_qfact ); + q_fx.x_fx = float_to_fix( q.x, q_fx.x_qfact ); + q_fx.y_fx = float_to_fix( q.y, q_fx.y_qfact ); + q_fx.z_fx = float_to_fix( q.z, q_fx.z_qfact ); + Word32 dot_prod_fx = QuaternionDotProduct_fx( q_fx, q_fx ); + q_dot = q_fx.w_qfact + q_fx.w_qfact - 31; + float dot_prod = fix_to_float( dot_prod_fx, q_dot ); + float sqrt = sqrtf( dot_prod ); + QuaternionDivision( q, sqrt, r ); +#else QuaternionDivision( q, sqrtf( QuaternionDotProduct( q, q ) ), r ); - +#endif return; } @@ -155,8 +244,25 @@ void QuaternionSlerp( IVAS_QUATERNION *const r ) { float angle, denom, s, s2; - +#ifdef IVAS_FLOAT_FIXED + IVAS_QUATERNION_FX q1_fx, q2_fx; + Word16 q_dot = 0; + q1_fx.w_qfact = q1_fx.x_qfact = q1_fx.y_qfact = q1_fx.z_qfact = Q29; + q2_fx.w_qfact = q2_fx.x_qfact = q2_fx.y_qfact = q2_fx.z_qfact = Q29; + q1_fx.w_fx = float_to_fix( q1.w, q1_fx.w_qfact); + q1_fx.x_fx = float_to_fix( q1.x, q1_fx.x_qfact); + q1_fx.y_fx = float_to_fix( q1.y, q1_fx.y_qfact); + q1_fx.z_fx = float_to_fix( q1.z, q1_fx.z_qfact); + q2_fx.w_fx = float_to_fix( q2.w, q2_fx.w_qfact); + q2_fx.x_fx = float_to_fix( q2.x, q2_fx.x_qfact); + q2_fx.y_fx = float_to_fix( q2.y, q2_fx.y_qfact); + q2_fx.z_fx = float_to_fix( q2.z, q2_fx.z_qfact); + Word32 s_fx = QuaternionDotProduct_fx( q1_fx, q2_fx ); + q_dot = q1_fx.w_qfact + q2_fx.w_qfact - 31; + s = fix_to_float( s_fx, q_dot ); +#else s = QuaternionDotProduct( q1, q2 ); +#endif if ( fabsf( s ) >= 1.0f ) { @@ -198,6 +304,22 @@ static void QuaternionConjugate( return; } +#ifdef IVAS_FLOAT_FIXED +static void QuaternionConjugate_fx( + const IVAS_QUATERNION_FX q, + IVAS_QUATERNION_FX *const r ) +{ + r->w_fx = q.w_fx; + r->x_fx = L_negate( q.x_fx ); + r->y_fx = L_negate( q.y_fx ); + r->z_fx = L_negate( q.z_fx ); + r->w_qfact = q.w_qfact; + r->x_qfact = q.x_qfact; + r->y_qfact = q.y_qfact; + r->z_qfact = q.z_qfact; + return; +} +#endif /*------------------------------------------------------------------------------------------* * QuaternionAngle() @@ -219,6 +341,69 @@ static float QuaternionAngle( return angle; } +#ifdef IVAS_FLOAT_FIXED +static Word32 QuaternionAngle_fx( + const IVAS_QUATERNION_FX q1, + const IVAS_QUATERNION_FX q2 ) +{ + IVAS_QUATERNION_FX q12; + Word32 angle = 0; + + QuaternionConjugate_fx( q1, &q12 ); + QuaternionProduct_fx( q12, q2, &q12 ); // q12:Q25, q2:Q29, q1: Q27// + Word16 sign_bit = ( L_shr( q12.w_fx, 31 ) ) & ( 1 ); + IF( NE_16( sign_bit, 1 ) ) + { + Word32 temp = 0; + Word16 q_dot, result_e = 0; + temp = q12.w_fx; + q12.w_fx = 0; + Word32 result = 0; + q12.x_fx = L_shl( q12.x_fx, ( 31 - q12.x_qfact ) ); // Q31 + q12.y_fx = L_shl( q12.y_fx, ( 31 - q12.y_qfact ) ); + q12.z_fx = L_shl( q12.z_fx, ( 31 - q12.z_qfact ) ); + result = QuaternionDotProduct_fx( q12, q12 ); + q_dot = Q31; + q12.x_fx = L_shr( q12.x_fx, ( 31 - q12.x_qfact ) ); // original Q + q12.y_fx = L_shr( q12.y_fx, ( 31 - q12.y_qfact ) ); + q12.z_fx = L_shr( q12.z_fx, ( 31 - q12.z_qfact ) ); + result = Sqrt32( result, &result_e ); + q12.w_fx = temp; + // Converting numerator to same Q as denominator// + IF( GT_32( 0, result_e ) ) + { + result = L_shr( L_shr( result, -1 * ( result_e ) ), ( 31 - q12.x_qfact ) ); // Q25 + } + ELSE + { + IF( GT_32( result_e, ( 31 - q12.x_qfact ) ) ) + { + result = L_shl( result, ( result_e - ( 31 - q12.x_qfact ) ) ); + } + ELSE + { + result = L_shr( result, ( ( 31 - q12.x_qfact ) - result_e ) ); + } + } + IF( GT_32( q12.w_fx, result ) ) + { + Word16 tan_result = BASOP_util_atan2( result, q12.w_fx, 0 ); + result = L_deposit_h( tan_result ); // Q29 + return result; + } + ELSE + { + return PI_OVER_Q29; + } + } + ELSE + { + angle = PI_OVER_Q29; + } + + return angle; +} +#endif /*------------------------------------------------------------------------------------------* * QuaternionInverse() @@ -231,9 +416,30 @@ void QuaternionInverse( IVAS_QUATERNION *const r ) { float dot_product; - +#ifdef IVAS_FLOAT_FIXED + IVAS_QUATERNION_FX q_fx; + Word16 q_dot = 0; + q_fx.w_qfact = q_fx.x_qfact = q_fx.y_qfact = q_fx.z_qfact = Q29; + q_fx.w_fx = float_to_fix( q.w, q_fx.w_qfact ); + q_fx.x_fx = float_to_fix( q.x, q_fx.x_qfact ); + q_fx.y_fx = float_to_fix( q.y, q_fx.y_qfact ); + q_fx.z_fx = float_to_fix( q.z, q_fx.z_qfact ); + Word32 dot_product_fx = QuaternionDotProduct_fx( q_fx, q_fx ); + q_dot = q_fx.w_qfact + q_fx.w_qfact - 31; + dot_product = fix_to_float( dot_product_fx, q_dot ); + + IVAS_QUATERNION_FX *r_fx = malloc( sizeof( IVAS_QUATERNION_FX ) ); + QuaternionConjugate_fx( q_fx, r_fx ); + r->w = fix_to_float( r_fx->w_fx, r_fx->w_qfact ); + r->x = fix_to_float( r_fx->x_fx, r_fx->x_qfact ); + r->y = fix_to_float( r_fx->y_fx, r_fx->y_qfact ); + r->z = fix_to_float( r_fx->z_fx, r_fx->z_qfact ); + + free( r_fx ); +#else dot_product = QuaternionDotProduct( q, q ); QuaternionConjugate( q, r ); +#endif QuaternionDivision( *r, dot_product, r ); return; @@ -618,7 +824,32 @@ ivas_error ivas_orient_trk_Process( /* Compute relative orientation = (absolute orientation) - (reference orientation) */ QuaternionInverse( pOTR->refRot, &pOTR->trkRot ); +#ifdef IVAS_FLOAT_FIXED + IVAS_QUATERNION_FX absRot_fx = { 0 }; + ivas_orient_trk_state_t_fx *pOTR_fx = malloc( sizeof( ivas_orient_trk_state_t_fx ) ); + pOTR_fx->trkRot_fx.w_qfact = pOTR_fx->trkRot_fx.x_qfact = pOTR_fx->trkRot_fx.y_qfact = pOTR_fx->trkRot_fx.z_qfact = Q29; + absRot_fx.w_qfact = absRot_fx.x_qfact = absRot_fx.y_qfact = absRot_fx.z_qfact = Q29; + pOTR_fx->trkRot_fx.w_fx = float_to_fix( pOTR->trkRot.w, pOTR_fx->trkRot_fx.w_qfact ); + pOTR_fx->trkRot_fx.x_fx = float_to_fix( pOTR->trkRot.x, pOTR_fx->trkRot_fx.x_qfact ); + pOTR_fx->trkRot_fx.y_fx = float_to_fix( pOTR->trkRot.y, pOTR_fx->trkRot_fx.y_qfact ); + pOTR_fx->trkRot_fx.z_fx = float_to_fix( pOTR->trkRot.z, pOTR_fx->trkRot_fx.z_qfact ); + absRot_fx.w_fx = float_to_fix( absRot.w, absRot_fx.w_qfact ); + absRot_fx.x_fx = float_to_fix( absRot.x, absRot_fx.x_qfact ); + absRot_fx.y_fx = float_to_fix( absRot.y, absRot_fx.y_qfact ); + absRot_fx.z_fx = float_to_fix( absRot.z, absRot_fx.z_qfact ); + QuaternionProduct_fx( pOTR_fx->trkRot_fx, absRot_fx, &pOTR_fx->trkRot_fx ); + pOTR->trkRot.w = fix_to_float( pOTR_fx->trkRot_fx.w_fx, pOTR_fx->trkRot_fx.w_qfact); + pOTR->trkRot.x = fix_to_float( pOTR_fx->trkRot_fx.x_fx, pOTR_fx->trkRot_fx.x_qfact); + pOTR->trkRot.y = fix_to_float( pOTR_fx->trkRot_fx.y_fx, pOTR_fx->trkRot_fx.y_qfact); + pOTR->trkRot.z = fix_to_float( pOTR_fx->trkRot_fx.z_fx, pOTR_fx->trkRot_fx.z_qfact); + absRot.w = fix_to_float( absRot_fx.w_fx, absRot_fx.w_qfact); + absRot.x = fix_to_float( absRot_fx.x_fx, absRot_fx.x_qfact); + absRot.y = fix_to_float( absRot_fx.y_fx, absRot_fx.y_qfact); + absRot.z = fix_to_float( absRot_fx.z_fx, absRot_fx.z_qfact); + free( pOTR_fx ); +#else QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot ); +#endif break; case IVAS_HEAD_ORIENT_TRK_AVG: @@ -627,6 +858,46 @@ ivas_error ivas_orient_trk_Process( /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */ QuaternionInverse( pOTR->absAvgRot, &pOTR->trkRot ); +#ifdef IVAS_FLOAT_FIXED + IVAS_QUATERNION_FX absRot1_fx = { 0 }; + ivas_orient_trk_state_t_fx *pOTR1_fx = malloc( sizeof( ivas_orient_trk_state_t_fx ) ); + Word32 angle_fx, relativeOrientationRate_fx = 0; + pOTR1_fx->trkRot_fx.w_qfact = pOTR1_fx->trkRot_fx.x_qfact = pOTR1_fx->trkRot_fx.y_qfact = pOTR1_fx->trkRot_fx.z_qfact = Q29; + absRot1_fx.w_qfact = absRot1_fx.x_qfact = absRot1_fx.y_qfact = absRot1_fx.z_qfact = Q29; + pOTR1_fx->trkRot_fx.w_fx = float_to_fix( pOTR->trkRot.w, pOTR1_fx->trkRot_fx.w_qfact ); + pOTR1_fx->trkRot_fx.x_fx = float_to_fix( pOTR->trkRot.x, pOTR1_fx->trkRot_fx.x_qfact ); + pOTR1_fx->trkRot_fx.y_fx = float_to_fix( pOTR->trkRot.y, pOTR1_fx->trkRot_fx.y_qfact ); + pOTR1_fx->trkRot_fx.z_fx = float_to_fix( pOTR->trkRot.z, pOTR1_fx->trkRot_fx.z_qfact ); + absRot1_fx.w_fx = float_to_fix( absRot.w, absRot1_fx.w_qfact ); + absRot1_fx.x_fx = float_to_fix( absRot.x, absRot1_fx.x_qfact ); + absRot1_fx.y_fx = float_to_fix( absRot.y, absRot1_fx.y_qfact ); + absRot1_fx.z_fx = float_to_fix( absRot.z, absRot1_fx.z_qfact ); + QuaternionProduct_fx( pOTR1_fx->trkRot_fx, absRot1_fx, &pOTR1_fx->trkRot_fx ); + angle_fx = QuaternionAngle_fx( absRot1_fx, pOTR1_fx->trkRot_fx ); // Q29 + Word16 result_e = 0; + Word16 temp_result = BASOP_Util_Divide3232_Scale( angle_fx, PI_OVER_4_Q29, &result_e ); + relativeOrientationRate_fx = L_deposit_h( temp_result ); + Word32 one_fx; + Word16 temp = result_e; + f2me( 1.0, &one_fx, &temp); + IF( GT_32( relativeOrientationRate_fx, one_fx ) ) + { + relativeOrientationRate = 1.0f; + } + ELSE + { + relativeOrientationRate = me2f( relativeOrientationRate_fx, result_e ); + } + pOTR->trkRot.w = fix_to_float( pOTR1_fx->trkRot_fx.w_fx, pOTR1_fx->trkRot_fx.w_qfact ); + pOTR->trkRot.x = fix_to_float( pOTR1_fx->trkRot_fx.x_fx, pOTR1_fx->trkRot_fx.x_qfact ); + pOTR->trkRot.y = fix_to_float( pOTR1_fx->trkRot_fx.y_fx, pOTR1_fx->trkRot_fx.y_qfact ); + pOTR->trkRot.z = fix_to_float( pOTR1_fx->trkRot_fx.z_fx, pOTR1_fx->trkRot_fx.z_qfact ); + absRot.w = fix_to_float( absRot1_fx.w_fx, absRot1_fx.w_qfact ); + absRot.x = fix_to_float( absRot1_fx.x_fx, absRot1_fx.x_qfact ); + absRot.y = fix_to_float( absRot1_fx.y_fx, absRot1_fx.y_qfact ); + absRot.z = fix_to_float( absRot1_fx.z_fx, absRot1_fx.z_qfact ); + free( pOTR1_fx ); +#else QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot ); /* Adapt LPF constant based on orientation excursion relative to current mean: @@ -642,6 +913,7 @@ ivas_error ivas_orient_trk_Process( { relativeOrientationRate = 1.0f; } +#endif /* Compute range of the adaptation rate between center = lower rate and off-center = higher rate */ rateRange = pOTR->offCenterAdaptationRate - pOTR->centerAdaptationRate; diff --git a/lib_rend/ivas_stat_rend.h b/lib_rend/ivas_stat_rend.h index 71b008e52..b8c5ee81e 100644 --- a/lib_rend/ivas_stat_rend.h +++ b/lib_rend/ivas_stat_rend.h @@ -742,6 +742,24 @@ typedef struct ivas_orient_trk_state_t } ivas_orient_trk_state_t; +#ifdef IVAS_FLOAT_FIXED + +typedef struct ivas_orient_trk_state_t_fx +{ + IVAS_HEAD_ORIENT_TRK_T orientation_tracking_fx; + Word16 centerAdaptationRate_fx; + Word16 offCenterAdaptationRate_fx; + Word16 adaptationAngle_fx; + + Word16 alpha_fx; + IVAS_QUATERNION_FX absAvgRot_fx; /* average absolute orientation */ + IVAS_QUATERNION_FX refRot_fx; /* reference orientation */ + IVAS_QUATERNION_FX trkRot_fx; /* tracked rotation */ + +} ivas_orient_trk_state_t_fx; + +#endif + /*----------------------------------------------------------------------------------* * Head rotation data structure *----------------------------------------------------------------------------------*/ -- GitLab