diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c index 0a6cdbe4d41c6b0c9d05ea4612aeead0daf4db5f..316a4a674f89afb5cc0cb8e66cfef4f54771d00b 100644 --- a/lib_rend/ivas_orient_trk.c +++ b/lib_rend/ivas_orient_trk.c @@ -322,14 +322,14 @@ void QuaternionSlerp_fx( const Word32 t_fx, IVAS_QUATERNION *const r_fx ) { - IVAS_QUATERNION r1, r2; + IVAS_QUATERNION r1, r2, tmp_quat; Word32 sinPhi, cosPhi, temp_32; - Word16 q_min, sin_e, phi, s1, s2, temp_16, e_div; + Word16 q_min, sin_e, phi, s1, s2, temp_16; QuaternionNormalize_fx( q1_fx, &r1 ); QuaternionNormalize_fx( q2_fx, &r2 ); - Word16 w_qfact, x_qfact, y_qfact, z_qfact, q_result, q_dot = 0; + Word16 q_dot = 0; cosPhi = QuaternionDotProduct_fx( r1, r2, &q_dot ); q_min = s_min( r1.q_fact, r2.q_fact ); @@ -380,32 +380,13 @@ void QuaternionSlerp_fx( temp_16 = extract_h( temp_32 ); // Q13 s2 = getSineWord16R2( mult( temp_16, 20860 ) ); // Q15 - temp_32 = L_add( Mpy_32_16_1( r1.w_fx, s1 ), Mpy_32_16_1( r2.w_fx, s2 ) ); // q_min - r_fx->w_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); - e_div = e_div + ( 31 - q_min - sin_e ); - w_qfact = 31 - e_div; - - temp_32 = L_add( Mpy_32_16_1( r1.x_fx, s1 ), Mpy_32_16_1( r2.x_fx, s2 ) ); // q_min - r_fx->x_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); - e_div = e_div + ( 31 - q_min - sin_e ); - x_qfact = 31 - e_div; - - temp_32 = L_add( Mpy_32_16_1( r1.y_fx, s1 ), Mpy_32_16_1( r2.y_fx, s2 ) ); // q_min - r_fx->y_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); - e_div = e_div + ( 31 - q_min - sin_e ); - y_qfact = 31 - e_div; - - temp_32 = L_add( Mpy_32_16_1( r1.z_fx, s1 ), Mpy_32_16_1( r2.z_fx, s2 ) ); // q_min - r_fx->z_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); - e_div = e_div + ( 31 - q_min - sin_e ); - z_qfact = 31 - e_div; - - q_result = s_min( s_min( w_qfact, x_qfact ), s_min( y_qfact, z_qfact ) ); - r_fx->w_fx = L_shr( r_fx->w_fx, sub( w_qfact, q_result ) ); - r_fx->x_fx = L_shr( r_fx->x_fx, sub( x_qfact, q_result ) ); - r_fx->y_fx = L_shr( r_fx->y_fx, sub( y_qfact, q_result ) ); - r_fx->z_fx = L_shr( r_fx->z_fx, sub( z_qfact, q_result ) ); - r_fx->q_fact = q_result; + tmp_quat.w_fx = L_add( Mpy_32_16_1( r1.w_fx, s1 ), Mpy_32_16_1( r2.w_fx, s2 ) ); // q_min + tmp_quat.x_fx = L_add( Mpy_32_16_1( r1.x_fx, s1 ), Mpy_32_16_1( r2.x_fx, s2 ) ); // q_min + tmp_quat.y_fx = L_add( Mpy_32_16_1( r1.y_fx, s1 ), Mpy_32_16_1( r2.y_fx, s2 ) ); // q_min + tmp_quat.z_fx = L_add( Mpy_32_16_1( r1.z_fx, s1 ), Mpy_32_16_1( r2.z_fx, s2 ) ); // q_min + tmp_quat.q_fact = q_min; + + QuaternionDivision_fx( tmp_quat, sinPhi, r_fx, sin_e ); } QuaternionNormalize_fx( *r_fx, r_fx );