From f6dc3dd3c72362c41b4cc5df4bfad6f8a700b8dc Mon Sep 17 00:00:00 2001 From: Sandesh Venkatesh Date: Sat, 17 Feb 2024 12:31:13 +0530 Subject: [PATCH 1/2] Remaining functions in ivas_orient_trk.c converted to fxd --- lib_com/ivas_cnst.h | 7 + lib_dec/lib_dec_fx.c | 10 +- lib_rend/ivas_orient_trk.c | 435 +++++++++++++++++++++++++------------ lib_rend/ivas_prot_rend.h | 23 ++ lib_rend/ivas_rotation.c | 8 +- lib_rend/lib_rend.c | 35 ++- 6 files changed, 372 insertions(+), 146 deletions(-) diff --git a/lib_com/ivas_cnst.h b/lib_com/ivas_cnst.h index 88901949a..215e61e5a 100644 --- a/lib_com/ivas_cnst.h +++ b/lib_com/ivas_cnst.h @@ -53,6 +53,13 @@ #define Q31_0_99 2126008811 #define Q31_0_01 21474836 #endif +#define PI_OVER_4_Q29 421657440 +#define PI_OVER_Q29 1686629760 +#define PI2_C_ADP_RATE_Q31 449767936 //(2.0f * EVS_PI * pOTR->centerAdaptationRate)// +#define PI2_C_Q28 1686629760 +#define OFF_C_ADP_RATE_Q31 268435456 +#define C_ADP_RATE_Q31 71582792 + #define SQRT2 1.414213562373095f #define SQRT2_FIXED 1518500250 // Q30 diff --git a/lib_dec/lib_dec_fx.c b/lib_dec/lib_dec_fx.c index 9d080ebb4..77d6d921e 100644 --- a/lib_dec/lib_dec_fx.c +++ b/lib_dec/lib_dec_fx.c @@ -1441,10 +1441,17 @@ ivas_error IVAS_DEC_FeedHeadTrackData( Euler2Quat( deg2rad( orientation.x ), deg2rad( orientation.y ), deg2rad( orientation.z ), &orientation ); } +#ifdef IVAS_FLOAT_FIXED + IF((error = ivas_orient_trk_Process_fx(hHeadTrackData->OrientationTracker, orientation, FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES, &hHeadTrackData->Quaternions[subframe_idx])) != IVAS_ERR_OK) + { + return error; + } +#else IF( ( error = ivas_orient_trk_Process( hHeadTrackData->OrientationTracker, orientation, FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES, &hHeadTrackData->Quaternions[subframe_idx] ) ) != IVAS_ERR_OK ) { return error; } +#endif hHeadTrackData->Pos[subframe_idx].x = Pos.x; hHeadTrackData->Pos[subframe_idx].y = Pos.y; @@ -1524,12 +1531,13 @@ ivas_error IVAS_DEC_FeedRefVectorData( 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 ); pOTR_fx->orientation_tracking_fx = pOtr->orientation_tracking; - ivas_orient_trk_SetReferenceVector_fx( pOTR_fx, listenerPos_fx, refPos_fx ); + ivas_error error_fx = ivas_orient_trk_SetReferenceVector_fx( pOTR_fx, listenerPos_fx, refPos_fx ); pOtr->refRot.w = me2f( pOTR_fx->refRot_fx.w_fx, 31- pOTR_fx->refRot_fx.w_qfact ); pOtr->refRot.x = me2f( pOTR_fx->refRot_fx.x_fx, 31 -pOTR_fx->refRot_fx.x_qfact ); pOtr->refRot.y = me2f( pOTR_fx->refRot_fx.y_fx, 31- pOTR_fx->refRot_fx.y_qfact ); pOtr->refRot.z = me2f( pOTR_fx->refRot_fx.z_fx, 31 -pOTR_fx->refRot_fx.z_qfact ); free(pOTR_fx); + return error_fx; #else return ivas_orient_trk_SetReferenceVector( pOtr, listenerPos, refPos ); #endif diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c index 2cc670272..4358a059c 100644 --- a/lib_rend/ivas_orient_trk.c +++ b/lib_rend/ivas_orient_trk.c @@ -219,7 +219,8 @@ static float QuaternionDotProduct( #ifdef IVAS_FLOAT_FIXED static Word32 QuaternionDotProduct_fx( const IVAS_QUATERNION_FX q1, - const IVAS_QUATERNION_FX q2, Word16 *q_fact) + const IVAS_QUATERNION_FX q2, + Word16 *q_fact ) { Word32 a[4], b[4] = { 0 }; Word64 mult_res[4] = { 0 }; @@ -255,10 +256,10 @@ static Word32 QuaternionDotProduct_fx( { mult_res[j] = W_shr( mult_res[j], ( mult_res_q[j] - mult_res_q[j + 1] ) ); result_q = mult_res_q[j + 1]; - } + } ELSE { - mult_res[j + 1] = W_shr( mult_res[j + 1], ( mult_res_q[j+1] - mult_res_q[j] ) ); + mult_res[j + 1] = W_shr( mult_res[j + 1], ( mult_res_q[j + 1] - mult_res_q[j] ) ); result_q = mult_res_q[j]; } mult_res[j + 1] = W_add( mult_res[j], mult_res[j + 1] ); @@ -361,27 +362,12 @@ 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); - s = me2f( s_fx, 31 - q_dot ); -#else + s = QuaternionDotProduct( q1, q2 ); -#endif if ( fabsf( s ) >= 1.0f ) { + *r = q2; return; } @@ -396,27 +382,61 @@ void QuaternionSlerp( r->z = ( q1.z * s + q2.z * s2 ) / denom; r->w = ( q1.w * s + q2.w * s2 ) / denom; + QuaternionNormalize( *r, r ); + + return; +} + #ifdef IVAS_FLOAT_FIXED - IVAS_QUATERNION_FX *r_fx = malloc( sizeof( IVAS_QUATERNION_FX ) ); +void QuaternionSlerp_fx( + const IVAS_QUATERNION_FX q1_fx, + const IVAS_QUATERNION_FX q2_fx, + const Word32 t_fx, + IVAS_QUATERNION_FX *const r_fx ) +{ + float angle, denom, s, s2; + float t = me2f( t_fx, 1 ); + IVAS_QUATERNION q1, q2; + IVAS_QUATERNION *r = malloc( sizeof( IVAS_QUATERNION ) ); + Word16 q_dot = 0; + Word32 s_fx = QuaternionDotProduct_fx( q1_fx, q2_fx, &q_dot ); + s = me2f( s_fx, 31 - q_dot ); + + + if ( fabsf( s ) >= 1.0f ) + { + *r_fx = q2_fx; + return; + } + + angle = acosf( s ); + denom = sinf( angle ); + + s = sinf( ( 1 - t ) * angle ); + s2 = sinf( t * angle ); + q1.w = me2f( q1_fx.w_fx, 31 - q1_fx.w_qfact ); + q1.x = me2f( q1_fx.x_fx, 31 - q1_fx.x_qfact ); + q1.y = me2f( q1_fx.y_fx, 31 - q1_fx.y_qfact ); + q1.z = me2f( q1_fx.z_fx, 31 - q1_fx.z_qfact ); + q2.w = me2f( q2_fx.w_fx, 31 - q2_fx.w_qfact ); + q2.x = me2f( q2_fx.x_fx, 31 - q2_fx.x_qfact ); + q2.y = me2f( q2_fx.y_fx, 31 - q2_fx.y_qfact ); + q2.z = me2f( q2_fx.z_fx, 31 - q2_fx.z_qfact ); + r->x = ( q1.x * s + q2.x * s2 ) / denom; + r->y = ( q1.y * s + q2.y * s2 ) / denom; + r->z = ( q1.z * s + q2.z * s2 ) / denom; + r->w = ( q1.w * s + q2.w * s2 ) / denom; + + r_fx->w_fx = float_to_fix( r->w, Q29 ); + r_fx->x_fx = float_to_fix( r->x, Q29 ); + r_fx->y_fx = float_to_fix( r->y, Q29 ); + r_fx->z_fx = float_to_fix( r->z, Q29 ); r_fx->w_qfact = r_fx->x_qfact = r_fx->y_qfact = r_fx->z_qfact = Q29; - r_fx->w_fx = float_to_fix( r->w, r_fx->w_qfact ); - r_fx->x_fx = float_to_fix( r->x, r_fx->x_qfact ); - r_fx->y_fx = float_to_fix( r->y, r_fx->y_qfact ); - r_fx->z_fx = float_to_fix( r->z, r_fx->z_qfact ); QuaternionNormalize_fx( *r_fx, r_fx ); - r->w = me2f( r_fx->w_fx, 31 - r_fx->w_qfact ); - r->x = me2f( r_fx->x_fx, 31 - r_fx->x_qfact ); - r->y = me2f( r_fx->y_fx, 31 - r_fx->y_qfact ); - r->z = me2f( r_fx->z_fx, 31 - r_fx->z_qfact ); - free(r_fx); -#else - QuaternionNormalize( *r, r ); -#endif - return; } - +#endif /*------------------------------------------------------------------------------------------* * QuaternionConjugate() @@ -494,7 +514,7 @@ static Word32 QuaternionAngle_fx( 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); + result = QuaternionDotProduct_fx( q12, q12, &q_dot ); 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 ) ); @@ -694,11 +714,11 @@ static Word32 VectorDotProduct_fx( mul[2] = Mpy_32_32( p1.z_fx, p2.z_fx ); mul_q[2] = p1.z_qfact + p2.z_qfact - 31; q_min = mul_q[0]; - for ( int i = 0; i < 3; i++ ) + FOR ( int i = 0; i < 3; i++ ) { q_min = s_min( q_min, mul_q[i] ); } - for ( int j = 0; j < 3; j++ ) + FOR ( int j = 0; j < 3; j++ ) { result_fx = L_add( result_fx, L_shr( mul[j], ( sub( mul_q[j], q_min ) ) ) ); } @@ -706,7 +726,7 @@ static Word32 VectorDotProduct_fx( return result_fx; } -#endif +#endif /*------------------------------------------------------------------------------------------* * VectorLength() @@ -734,7 +754,7 @@ static Word32 VectorLength_fx( q[1] = 2 * p.y_qfact - 31; mul[2] = Mpy_32_32( p.z_fx, p.z_fx ); q[2] = 2 * p.z_qfact - 31; - for ( int j = 0; j < 3; j++ ) + FOR ( int j = 0; j < 3; j++ ) { result_fx = BASOP_Util_Add_Mant32Exp( result_fx, result_e, mul[j], 31 - q[j], &result_e ); } @@ -786,7 +806,7 @@ static IVAS_VECTOR3_FX VectorNormalize_fx( return result_fx; } -#endif +#endif /*------------------------------------------------------------------------------------------* * VectorRotationToQuaternion() @@ -835,12 +855,12 @@ static void VectorRotationToQuaternion_fx( const IVAS_VECTOR3_FX p2, IVAS_QUATERNION_FX *const r ) { - IVAS_VECTOR3_FX cross_product_fx, p1_normalized_fx = { 0 }, p2_normalized_fx = { 0 }; + IVAS_VECTOR3_FX cross_product_fx, p1_normalized_fx = { 0 }, p2_normalized_fx = { 0 }; Word32 dot_product_fx; Word16 q_dot; - p1_normalized_fx = VectorNormalize_fx(p1); - p2_normalized_fx = VectorNormalize_fx(p2); + 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 ); @@ -879,7 +899,7 @@ static void VectorRotationToQuaternion_fx( return; } -#endif +#endif /*-------------------------------------------------------------------* * ivas_orient_trk_Init() @@ -919,6 +939,49 @@ ivas_error ivas_orient_trk_Init( return IVAS_ERR_OK; } +#ifdef IVAS_FLOAT_FIXED +ivas_error ivas_orient_trk_Init_fx( + ivas_orient_trk_state_t *pOTR ) /* i/o : orientation tracker handle */ +{ + IVAS_QUATERNION_FX identity_fx; + ivas_orient_trk_state_t_fx *pOTR_fx = malloc( sizeof( ivas_orient_trk_state_t_fx ) ); + + //===================Remove float code once caller function is converted to fixed=================// + IF ( pOTR == NULL ) + { + return IVAS_ERR_UNEXPECTED_NULL_POINTER; + } + + identity_fx.w_fx = float_to_fix( 1.0, Q30 ); + identity_fx.w_qfact = Q30; + identity_fx.x_fx = identity_fx.y_fx = identity_fx.z_fx = float_to_fix( 0.0, Q31 ); + identity_fx.x_qfact = identity_fx.y_qfact = identity_fx.z_qfact = Q31; + + /* configuration parameters */ + pOTR_fx->centerAdaptationRate_fx = C_ADP_RATE_Q31; + pOTR_fx->offCenterAdaptationRate_fx = OFF_C_ADP_RATE_Q31; + pOTR_fx->adaptationAngle_fx = PI_OVER_4_Q29; /* Excursion angle relative to center at which maximum adaptation rate shall be applied */ + + /* initial adaptivity filter coefficient, will be auto-adapted */ + // pOTR->alpha = sinf( PI2 * pOTR->offCenterAdaptationRate / OTR_UPDATE_RATE ); /* start adaptation at off-center rate = fastest rate */ + pOTR_fx->alpha_fx = 33731208; // Q31 + pOTR_fx->trkRot_fx = identity_fx; + pOTR_fx->absAvgRot_fx = identity_fx; + /* Use frontal and horiontal orientation as reference orientation, unless/until overridden */ + pOTR_fx->refRot_fx = identity_fx; + + // this part still float// + /* set safe default tracking mode */ + pOTR->orientation_tracking = IVAS_HEAD_ORIENT_TRK_NONE; + pOTR->alpha = me2f( pOTR_fx->alpha_fx, 0 ); + pOTR->refRot.w = pOTR->absAvgRot.w = pOTR->trkRot.w = fix_to_float( pOTR_fx->trkRot_fx.w_fx, pOTR_fx->trkRot_fx.w_qfact ); + pOTR->refRot.x = pOTR->absAvgRot.x = pOTR->trkRot.x = fix_to_float( pOTR_fx->trkRot_fx.x_fx, pOTR_fx->trkRot_fx.x_qfact ); + pOTR->refRot.y = pOTR->absAvgRot.y = pOTR->trkRot.y = fix_to_float( pOTR_fx->trkRot_fx.y_fx, pOTR_fx->trkRot_fx.y_qfact ); + pOTR->refRot.z = pOTR->absAvgRot.z = pOTR->trkRot.z = fix_to_float( pOTR_fx->trkRot_fx.z_fx, pOTR_fx->trkRot_fx.z_qfact ); + + return IVAS_ERR_OK; +} +#endif /*-------------------------------------------------------------------* * ivas_orient_trk_SetTrackingType() @@ -943,11 +1006,11 @@ ivas_error ivas_orient_trk_SetTrackingType( #ifdef IVAS_FLOAT_FIXED ivas_error ivas_orient_trk_SetTrackingType_fx( - ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ + ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ const IVAS_HEAD_ORIENT_TRK_T orientation_tracking /* i : orientation tracking type */ ) { - if ( pOTR == NULL ) + IF ( pOTR == NULL ) { return IVAS_ERR_UNEXPECTED_NULL_POINTER; } @@ -964,6 +1027,29 @@ ivas_error ivas_orient_trk_SetTrackingType_fx( * *-------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +ivas_error ivas_orient_trk_SetReferenceRotation_fx( + ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ + const IVAS_QUATERNION_FX refRot /* i : reference rotation */ +) +{ + IF ( pOTR == NULL ) + { + return IVAS_ERR_UNEXPECTED_NULL_POINTER; + } + + /* check for Euler angle signaling */ + // This part is not covered in code coverage for test streams// + /* if ( refRot.w == -3.0f ) + { + Euler2Quat( deg2rad( refRot.x ), deg2rad( refRot.y ), deg2rad( refRot.z ), &pOTR->refRot ); + }*/ + pOTR->refRot_fx = refRot; + + return IVAS_ERR_OK; +} +#endif + ivas_error ivas_orient_trk_SetReferenceRotation( ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */ const IVAS_QUATERNION refRot /* i : reference rotation */ @@ -1028,24 +1114,24 @@ ivas_error ivas_orient_trk_GetMainOrientation_fx( IVAS_QUATERNION_FX *pOrientation /* i/o: average/reference orientation */ ) { - if ( pOTR == NULL || pOrientation == NULL ) + IF ( pOTR == NULL || pOrientation == NULL ) { return IVAS_ERR_UNEXPECTED_NULL_POINTER; } - switch ( pOTR->orientation_tracking_fx ) + SWITCH ( pOTR->orientation_tracking_fx ) { case IVAS_HEAD_ORIENT_TRK_NONE: *pOrientation = IdentityQuaternion_fx(); - break; + BREAK; case IVAS_HEAD_ORIENT_TRK_REF_VEC: case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV: case IVAS_HEAD_ORIENT_TRK_REF: *pOrientation = pOTR->refRot_fx; - break; + BREAK; case IVAS_HEAD_ORIENT_TRK_AVG: *pOrientation = pOTR->absAvgRot_fx; - break; + BREAK; } return IVAS_ERR_OK; @@ -1075,18 +1161,18 @@ ivas_error ivas_orient_trk_GetTrackedRotation( #ifdef IVAS_FLOAT_FIXED ivas_error ivas_orient_trk_GetTrackedRotation_fx( - ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ - IVAS_QUATERNION_FX *pRotation /* i/o: processed rotation */ + ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ + IVAS_QUATERNION_FX *pRotation /* i/o: processed rotation */ ) { - if (pOTR == NULL || pRotation == NULL) - { - return IVAS_ERR_UNEXPECTED_NULL_POINTER; - } + IF ( pOTR == NULL || pRotation == NULL ) + { + return IVAS_ERR_UNEXPECTED_NULL_POINTER; + } - *pRotation = pOTR->trkRot_fx; + *pRotation = pOTR->trkRot_fx; - return IVAS_ERR_OK; + return IVAS_ERR_OK; } #endif @@ -1160,19 +1246,19 @@ ivas_error ivas_orient_trk_SetReferenceVector_fx( Word32 acousticFrontVectorLength; Word16 acousticFrontVector_q; - if ( pOTR == NULL ) + IF ( pOTR == NULL ) { return IVAS_ERR_UNEXPECTED_NULL_POINTER; } - switch ( pOTR->orientation_tracking_fx ) + SWITCH ( pOTR->orientation_tracking_fx ) { case IVAS_HEAD_ORIENT_TRK_NONE: case IVAS_HEAD_ORIENT_TRK_REF: case IVAS_HEAD_ORIENT_TRK_AVG: case IVAS_HEAD_ORIENT_TRK_REF_VEC: acousticFrontVector = VectorSubtract_fx( listenerPos, refPos ); - break; + BREAK; case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV: /* ignore the height difference between listener position and reference position */ listenerPosLevel.z_fx = refPosLevel.z_fx = listenerPos.z_fx; @@ -1186,7 +1272,7 @@ ivas_error ivas_orient_trk_SetReferenceVector_fx( refPosLevel.y_fx = refPos.y_fx; refPosLevel.y_qfact = refPos.y_qfact; acousticFrontVector = VectorSubtract_fx( listenerPosLevel, refPosLevel ); - break; + BREAK; default: return IVAS_ERR_WRONG_PARAMS; } @@ -1195,10 +1281,10 @@ ivas_error ivas_orient_trk_SetReferenceVector_fx( /* if the length is zero, the user has entered insensible listener and reference positions */ IF( LE_32( acousticFrontVectorLength, 0 ) ) { - return IVAS_ERR_WRONG_PARAMS; + return IVAS_ERR_WRONG_PARAMS; } - ivasForwardVector.x_fx = L_negate(ONE_IN_Q31); + ivasForwardVector.x_fx = L_negate( ONE_IN_Q31 ); ivasForwardVector.y_fx = 0; ivasForwardVector.z_fx = 0; ivasForwardVector.x_qfact = ivasForwardVector.y_qfact = ivasForwardVector.z_qfact = Q31; @@ -1251,91 +1337,17 @@ ivas_error ivas_orient_trk_Process( /* Reset adaptation filter - start adaptation at center rate */ pOTR->alpha = sinf( 2.0f * EVS_PI * pOTR->centerAdaptationRate / updateRate ); -#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->refRot_fx.w_qfact = pOTR_fx->refRot_fx.x_qfact = pOTR_fx->refRot_fx.y_qfact = pOTR_fx->refRot_fx.z_qfact = Q29; - absRot_fx.w_qfact = absRot_fx.x_qfact = absRot_fx.y_qfact = absRot_fx.z_qfact = Q29; - pOTR_fx->refRot_fx.w_fx = float_to_fix(pOTR->refRot.w, pOTR_fx->refRot_fx.w_qfact); - pOTR_fx->refRot_fx.x_fx = float_to_fix(pOTR->refRot.x, pOTR_fx->refRot_fx.x_qfact); - pOTR_fx->refRot_fx.y_fx = float_to_fix(pOTR->refRot.y, pOTR_fx->refRot_fx.y_qfact); - pOTR_fx->refRot_fx.z_fx = float_to_fix(pOTR->refRot.z, pOTR_fx->refRot_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 ); - - /* Compute relative orientation = (absolute orientation) - (reference orientation) */ - QuaternionInverse_fx( pOTR_fx->refRot_fx, &pOTR_fx->trkRot_fx ); - - 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 /* Compute relative orientation = (absolute orientation) - (reference orientation) */ - QuaternionInverse(pOTR->refRot, &pOTR->trkRot); + QuaternionInverse( pOTR->refRot, &pOTR->trkRot ); QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot ); -#endif break; case IVAS_HEAD_ORIENT_TRK_AVG: /* Compute average (low-pass filtered) absolute orientation */ QuaternionSlerp( pOTR->absAvgRot, absRot, alpha, &pOTR->absAvgRot ); -#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->absAvgRot_fx.w_qfact = pOTR1_fx->absAvgRot_fx.x_qfact = pOTR1_fx->absAvgRot_fx.y_qfact = pOTR1_fx->absAvgRot_fx.z_qfact = Q29; - absRot1_fx.w_qfact = absRot1_fx.x_qfact = absRot1_fx.y_qfact = absRot1_fx.z_qfact = Q29; - pOTR1_fx->absAvgRot_fx.w_fx = float_to_fix(pOTR->absAvgRot.w, pOTR1_fx->absAvgRot_fx.w_qfact); - pOTR1_fx->absAvgRot_fx.x_fx = float_to_fix(pOTR->absAvgRot.x, pOTR1_fx->absAvgRot_fx.x_qfact); - pOTR1_fx->absAvgRot_fx.y_fx = float_to_fix(pOTR->absAvgRot.y, pOTR1_fx->absAvgRot_fx.y_qfact); - pOTR1_fx->absAvgRot_fx.z_fx = float_to_fix(pOTR->absAvgRot.z, pOTR1_fx->absAvgRot_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 ); - - /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */ - QuaternionInverse_fx(pOTR1_fx->absAvgRot_fx, &pOTR1_fx->trkRot_fx ); - - 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 - /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */ - QuaternionInverse(pOTR->absAvgRot, &pOTR->trkRot); + QuaternionInverse( pOTR->absAvgRot, &pOTR->trkRot ); QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot ); /* Adapt LPF constant based on orientation excursion relative to current mean: @@ -1351,7 +1363,6 @@ 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; @@ -1379,3 +1390,141 @@ ivas_error ivas_orient_trk_Process( return result; } + +#ifdef IVAS_FLOAT_FIXED +ivas_error ivas_orient_trk_Process_fx( + ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */ + IVAS_QUATERNION absRot, /* i : absolute head rotation */ + float updateRate, /* i : rotation update rate [Hz] */ + IVAS_QUATERNION *pTrkRot /* o : tracked rotation */ +) +{ + //===================Remove float code once caller function is converted to fixed=================// + float alpha = pOTR->alpha; + ivas_error result; + IVAS_QUATERNION_FX absRot_fx = { 0 }; + Word32 updateRate_fx; + Word32 rateRange_fx; + Word32 cutoffFrequency_fx, cutoff_prod; + Word16 q_cutoff_prod = 0; + Word32 alpha_fx = float_to_fix( alpha, Q30 ); + ivas_orient_trk_state_t_fx *pOTR_fx = malloc( sizeof( ivas_orient_trk_state_t_fx ) ); + pOTR_fx->refRot_fx.w_qfact = pOTR_fx->refRot_fx.x_qfact = pOTR_fx->refRot_fx.y_qfact = pOTR_fx->refRot_fx.z_qfact = Q29; + absRot_fx.w_qfact = absRot_fx.x_qfact = absRot_fx.y_qfact = absRot_fx.z_qfact = Q29; + pOTR_fx->absAvgRot_fx.w_qfact = pOTR_fx->absAvgRot_fx.x_qfact = pOTR_fx->absAvgRot_fx.y_qfact = pOTR_fx->absAvgRot_fx.z_qfact = Q29; + updateRate_fx = float_to_fix( updateRate, Q23 ); // value is 200// + 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 ); + pOTR_fx->refRot_fx.w_fx = float_to_fix( pOTR->refRot.w, pOTR_fx->refRot_fx.w_qfact ); + pOTR_fx->refRot_fx.x_fx = float_to_fix( pOTR->refRot.x, pOTR_fx->refRot_fx.x_qfact ); + pOTR_fx->refRot_fx.y_fx = float_to_fix( pOTR->refRot.y, pOTR_fx->refRot_fx.y_qfact ); + pOTR_fx->refRot_fx.z_fx = float_to_fix( pOTR->refRot.z, pOTR_fx->refRot_fx.z_qfact ); + pOTR_fx->absAvgRot_fx.w_fx = float_to_fix( pOTR->absAvgRot.w, pOTR_fx->absAvgRot_fx.w_qfact ); + pOTR_fx->absAvgRot_fx.x_fx = float_to_fix( pOTR->absAvgRot.x, pOTR_fx->absAvgRot_fx.x_qfact ); + pOTR_fx->absAvgRot_fx.y_fx = float_to_fix( pOTR->absAvgRot.y, pOTR_fx->absAvgRot_fx.y_qfact ); + pOTR_fx->absAvgRot_fx.z_fx = float_to_fix( pOTR->absAvgRot.z, pOTR_fx->absAvgRot_fx.z_qfact ); + + IF ( pOTR == NULL || pTrkRot == NULL ) + { + return IVAS_ERR_UNEXPECTED_NULL_POINTER; + } + + result = IVAS_ERR_OK; + + SWITCH ( pOTR->orientation_tracking ) + { + case IVAS_HEAD_ORIENT_TRK_NONE: + pOTR_fx->trkRot_fx = absRot_fx; + BREAK; + case IVAS_HEAD_ORIENT_TRK_REF: + case IVAS_HEAD_ORIENT_TRK_REF_VEC: + case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV: + /* Reset average orientation */ + pOTR_fx->absAvgRot_fx = absRot_fx; + + Word16 scale_e; + Word32 div; + div = L_deposit_h( BASOP_Util_Divide3232_Scale( PI2_C_ADP_RATE_Q31, updateRate_fx, &scale_e ) ); + + scale_e = scale_e - 8; // e+e1-e2// + // here div value is less so we can use sandwitch rule of sine// + pOTR_fx->alpha_fx = div; + /* Compute relative orientation = (absolute orientation) - (reference orientation) */ + QuaternionInverse_fx( pOTR_fx->refRot_fx, &pOTR_fx->trkRot_fx ); + QuaternionProduct_fx( pOTR_fx->trkRot_fx, absRot_fx, &pOTR_fx->trkRot_fx ); + ; + BREAK; + + case IVAS_HEAD_ORIENT_TRK_AVG: + /* Compute average (low-pass filtered) absolute orientation */ + QuaternionSlerp_fx( pOTR_fx->absAvgRot_fx, absRot_fx, alpha_fx, &pOTR_fx->absAvgRot_fx ); + + /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */ + QuaternionInverse_fx( pOTR_fx->absAvgRot_fx, &pOTR_fx->trkRot_fx ); + Word32 angle_fx, relativeOrientationRate_fx = 0; + QuaternionProduct_fx( pOTR_fx->trkRot_fx, absRot_fx, &pOTR_fx->trkRot_fx ); + angle_fx = QuaternionAngle_fx( absRot_fx, pOTR_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_fx = 1; + } + + /* Compute range of the adaptation rate between center = lower rate and off-center = higher rate */ + // rateRange_fx = L_sub(pOTR1_fx->offCenterAdaptationRate_fx, pOTR1_fx->centerAdaptationRate_fx); + rateRange_fx = L_sub( OFF_C_ADP_RATE_Q31, C_ADP_RATE_Q31 ); // repalce this with above line once calling functions are converted// + /* 'if' assumed to perform comparison to 0 */ + IF( GT_32( 0, rateRange_fx ) ) + { + rateRange_fx = 0; + } + IF( relativeOrientationRate_fx == 1 ) + { + cutoff_prod = rateRange_fx; + q_cutoff_prod = Q31; + } + ELSE + { + cutoff_prod = Mpy_32_32( relativeOrientationRate_fx, rateRange_fx ); + q_cutoff_prod = Q31 + ( 31 - result_e ) - 31; + } + Word16 temp_diff; + temp_diff = 31 - q_cutoff_prod; + cutoff_prod = L_shl( cutoff_prod, temp_diff ); + /* Compute adaptivity cutoff frequency: interpolate between minimum (center) and maximum (off-center) values */ + cutoffFrequency_fx = L_add( C_ADP_RATE_Q31, cutoff_prod ); + cutoff_prod = Mpy_32_32( cutoffFrequency_fx, PI2_C_Q28 ); + q_cutoff_prod = 31 + 28 - 31; + temp_result = BASOP_Util_Divide3232_Scale( cutoff_prod, updateRate_fx, &result_e ); + result_e = result_e + ( 23 - q_cutoff_prod ); + pOTR_fx->alpha_fx = L_deposit_h( temp_result ); + pOTR->alpha = me2f( pOTR_fx->alpha_fx, result_e ); // Remove this floating code// + BREAK; + default: + result = IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "Non-supported orientation tracking adaptation type" ); + BREAK; + } + + IF ( result == IVAS_ERR_OK ) + { + pOTR->trkRot.w = me2f( pOTR_fx->trkRot_fx.w_fx, 31 - pOTR_fx->trkRot_fx.w_qfact ); + pOTR->trkRot.x = me2f( pOTR_fx->trkRot_fx.x_fx, 31 - pOTR_fx->trkRot_fx.x_qfact ); + pOTR->trkRot.y = me2f( pOTR_fx->trkRot_fx.y_fx, 31 - pOTR_fx->trkRot_fx.y_qfact ); + pOTR->trkRot.z = me2f( pOTR_fx->trkRot_fx.z_fx, 31 - pOTR_fx->trkRot_fx.z_qfact ); + pOTR->absAvgRot.w = me2f( pOTR_fx->absAvgRot_fx.w_fx, 31 - pOTR_fx->absAvgRot_fx.w_qfact ); + pOTR->absAvgRot.x = me2f( pOTR_fx->absAvgRot_fx.x_fx, 31 - pOTR_fx->absAvgRot_fx.x_qfact ); + pOTR->absAvgRot.y = me2f( pOTR_fx->absAvgRot_fx.y_fx, 31 - pOTR_fx->absAvgRot_fx.y_qfact ); + pOTR->absAvgRot.z = me2f( pOTR_fx->absAvgRot_fx.z_fx, 31 - pOTR_fx->absAvgRot_fx.z_qfact ); + *pTrkRot = pOTR->trkRot; + } + + return result; +} +#endif diff --git a/lib_rend/ivas_prot_rend.h b/lib_rend/ivas_prot_rend.h index ebf5f3d4a..6d19d88a3 100644 --- a/lib_rend/ivas_prot_rend.h +++ b/lib_rend/ivas_prot_rend.h @@ -1791,6 +1791,13 @@ void QuaternionInverse_fx( const IVAS_QUATERNION_FX q, IVAS_QUATERNION_FX *const r ); + +void QuaternionSlerp_fx( + const IVAS_QUATERNION_FX q1, + const IVAS_QUATERNION_FX q2, + const Word32 t, + IVAS_QUATERNION_FX *const r +); #endif /*----------------------------------------------------------------------------------* @@ -1836,6 +1843,15 @@ ivas_error ivas_orient_trk_Process( #ifdef IVAS_FLOAT_FIXED +ivas_error ivas_orient_trk_Init_fx( + ivas_orient_trk_state_t *pOTR /* i/o: orientation tracker handle */ +); + +ivas_error ivas_orient_trk_SetReferenceRotation_fx( + ivas_orient_trk_state_t_fx *pOTR_fx, /* i/o: orientatoin trakcer handle */ + const IVAS_QUATERNION_FX refRot /* i : reference rotation */ +); + ivas_error ivas_orient_trk_SetReferenceVector_fx( ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ const IVAS_VECTOR3_FX listenerPos, /* i : Listener position */ @@ -1856,6 +1872,13 @@ ivas_error ivas_orient_trk_GetTrackedRotation_fx( ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ IVAS_QUATERNION_FX *pRotation /* i/o: processed rotation */ ); + +ivas_error ivas_orient_trk_Process_fx( + ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */ + IVAS_QUATERNION absRot, /* i : absolute head rotation */ + float updateRate, /* i : rotation update rate [Hz] */ + IVAS_QUATERNION *pTrkRot /* o : tracked rotation */ +); #endif /*----------------------------------------------------------------------------------* diff --git a/lib_rend/ivas_rotation.c b/lib_rend/ivas_rotation.c index 930afc334..8d48b7964 100644 --- a/lib_rend/ivas_rotation.c +++ b/lib_rend/ivas_rotation.c @@ -136,12 +136,18 @@ ivas_error ivas_headTrack_open( { return IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for Orientation tracking"); } -#endif + if ((error = ivas_orient_trk_Init_fx((*hHeadTrackData)->OrientationTracker)) != IVAS_ERR_OK) + { + return error; + } + +#else if ( ( error = ivas_orient_trk_Init( ( *hHeadTrackData )->OrientationTracker ) ) != IVAS_ERR_OK ) { return error; } +#endif /* Initialise Rmat_prev to I, Rmat will be computed later */ for ( i = 0; i < 3; i++ ) diff --git a/lib_rend/lib_rend.c b/lib_rend/lib_rend.c index 7e7972e75..3221284f4 100644 --- a/lib_rend/lib_rend.c +++ b/lib_rend/lib_rend.c @@ -1391,12 +1391,17 @@ static ivas_error initHeadRotation( { return IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for Orientation tracking"); } -#endif + IF((error = ivas_orient_trk_Init_fx(hIvasRend->headRotData.hOrientationTracker)) != IVAS_ERR_OK) + { + return error; + } +#else IF( ( error = ivas_orient_trk_Init( hIvasRend->headRotData.hOrientationTracker ) ) != IVAS_ERR_OK ) { return error; } +#endif return IVAS_ERR_OK; } @@ -4647,10 +4652,17 @@ ivas_error IVAS_REND_SetHeadRotation( rotQuat = headRot; } +#ifdef IVAS_FLOAT_FIXED + if ((error = ivas_orient_trk_Process_fx(hIvasRend->headRotData.hOrientationTracker, rotQuat, FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES, &hIvasRend->headRotData.headPositions[sf_idx])) != IVAS_ERR_OK) + { + return error; + } +#else if ( ( error = ivas_orient_trk_Process( hIvasRend->headRotData.hOrientationTracker, rotQuat, FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES, &hIvasRend->headRotData.headPositions[sf_idx] ) ) != IVAS_ERR_OK ) { return error; } +#endif hIvasRend->headRotData.Pos[sf_idx] = Pos; @@ -4741,10 +4753,31 @@ ivas_error IVAS_REND_SetReferenceRotation( return IVAS_ERR_UNEXPECTED_NULL_POINTER; } +#ifdef IVAS_FLOAT_FIXED + + IVAS_QUATERNION_FX refRot_fx; + refRot_fx.w_qfact = refRot_fx.x_qfact = refRot_fx.y_qfact = refRot_fx.z_qfact = Q29; + refRot_fx.w_fx = (Word32)float_to_fix( refRot.w, refRot_fx.w_qfact ); + refRot_fx.x_fx = (Word32)float_to_fix( refRot.x, refRot_fx.x_qfact ); + refRot_fx.y_fx = (Word32)float_to_fix( refRot.y, refRot_fx.y_qfact ); + refRot_fx.z_fx = (Word32)float_to_fix( refRot.z, refRot_fx.z_qfact ); + error = ivas_orient_trk_SetReferenceRotation_fx( hIvasRend->headRotData.hOrientationTracker_fx, refRot_fx ); + hIvasRend->headRotData.hOrientationTracker->refRot.w = me2f( hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.w_fx, 31 - hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.w_qfact ); + hIvasRend->headRotData.hOrientationTracker->refRot.x = me2f( hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.x_fx, 31 - hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.x_qfact ); + hIvasRend->headRotData.hOrientationTracker->refRot.y = me2f( hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.y_fx, 31 - hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.y_qfact ); + hIvasRend->headRotData.hOrientationTracker->refRot.z = me2f( hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.z_fx, 31 - hIvasRend->headRotData.hOrientationTracker_fx->refRot_fx.z_qfact ); + + if (error != IVAS_ERR_OK) + { + + return error; + } +#else if ( ( error = ivas_orient_trk_SetReferenceRotation( hIvasRend->headRotData.hOrientationTracker, refRot ) ) != IVAS_ERR_OK ) { return error; } +#endif return IVAS_ERR_OK; } -- GitLab From acd7138fff304bc2698f4205c1f242a05b77acf9 Mon Sep 17 00:00:00 2001 From: Sandesh Venkatesh Date: Sat, 17 Feb 2024 12:35:50 +0530 Subject: [PATCH 2/2] Free added for mallocs in ivas_orient_trk.c --- lib_rend/ivas_orient_trk.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c index 4358a059c..6b8101d9a 100644 --- a/lib_rend/ivas_orient_trk.c +++ b/lib_rend/ivas_orient_trk.c @@ -433,6 +433,7 @@ void QuaternionSlerp_fx( r_fx->z_fx = float_to_fix( r->z, Q29 ); r_fx->w_qfact = r_fx->x_qfact = r_fx->y_qfact = r_fx->z_qfact = Q29; QuaternionNormalize_fx( *r_fx, r_fx ); + free(r); return; } @@ -978,6 +979,7 @@ ivas_error ivas_orient_trk_Init_fx( pOTR->refRot.x = pOTR->absAvgRot.x = pOTR->trkRot.x = fix_to_float( pOTR_fx->trkRot_fx.x_fx, pOTR_fx->trkRot_fx.x_qfact ); pOTR->refRot.y = pOTR->absAvgRot.y = pOTR->trkRot.y = fix_to_float( pOTR_fx->trkRot_fx.y_fx, pOTR_fx->trkRot_fx.y_qfact ); pOTR->refRot.z = pOTR->absAvgRot.z = pOTR->trkRot.z = fix_to_float( pOTR_fx->trkRot_fx.z_fx, pOTR_fx->trkRot_fx.z_qfact ); + free(pOTR_fx); return IVAS_ERR_OK; } @@ -1523,6 +1525,7 @@ ivas_error ivas_orient_trk_Process_fx( pOTR->absAvgRot.y = me2f( pOTR_fx->absAvgRot_fx.y_fx, 31 - pOTR_fx->absAvgRot_fx.y_qfact ); pOTR->absAvgRot.z = me2f( pOTR_fx->absAvgRot_fx.z_fx, 31 - pOTR_fx->absAvgRot_fx.z_qfact ); *pTrkRot = pOTR->trkRot; + free(pOTR_fx); } return result; -- GitLab