diff --git a/lib_dec/lib_dec_fx.c b/lib_dec/lib_dec_fx.c index 24d311a105ee5544f8899f6e69d700e3a5f8fb0b..9d080ebb422b2d6f54ef4102ad606151afb0ce08 100644 --- a/lib_dec/lib_dec_fx.c +++ b/lib_dec/lib_dec_fx.c @@ -1512,7 +1512,27 @@ ivas_error IVAS_DEC_FeedRefVectorData( hIvasDec->updateOrientation = true; +#ifdef IVAS_FLOAT_FIXED + ivas_orient_trk_state_t_fx *pOTR_fx = malloc(sizeof(ivas_orient_trk_state_t_fx)); + IVAS_VECTOR3_FX listenerPos_fx, refPos_fx; + listenerPos_fx.x_qfact = listenerPos_fx.y_qfact = listenerPos_fx.z_qfact = Q27; + refPos_fx.x_qfact = refPos_fx.y_qfact = refPos_fx.z_qfact = Q27; + 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 ); + pOTR_fx->orientation_tracking_fx = pOtr->orientation_tracking; + 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); +#else return ivas_orient_trk_SetReferenceVector( pOtr, listenerPos, refPos ); +#endif } @@ -1546,8 +1566,27 @@ ivas_error IVAS_DEC_FeedExternalOrientationData( return IVAS_ERR_UNEXPECTED_NULL_POINTER; } +#ifdef IVAS_FLOAT_FIXED + IVAS_QUATERNION_FX orientation_fx = { 0 }; + orientation_fx.w_qfact = orientation_fx.x_qfact = orientation_fx.y_qfact = orientation_fx.z_qfact = Q29; + orientation_fx.w_fx = float_to_fix(orientation.w, orientation_fx.w_qfact); + orientation_fx.x_fx = float_to_fix(orientation.x, orientation_fx.x_qfact); + orientation_fx.y_fx = float_to_fix(orientation.y, orientation_fx.y_qfact); + orientation_fx.z_fx = float_to_fix(orientation.z, orientation_fx.z_qfact); + /* Move external orientation data to the decoder handle (invert orientations) */ + QuaternionInverse_fx(orientation_fx, &hExternalOrientationData->Quaternions_fx[subframe_idx]); + hExternalOrientationData->Quaternions[subframe_idx].w = me2f(hExternalOrientationData->Quaternions_fx[subframe_idx].w_fx, + 31 - hExternalOrientationData->Quaternions_fx[subframe_idx].w_qfact); + hExternalOrientationData->Quaternions[subframe_idx].x = me2f(hExternalOrientationData->Quaternions_fx[subframe_idx].x_fx, + 31 - hExternalOrientationData->Quaternions_fx[subframe_idx].x_qfact); + hExternalOrientationData->Quaternions[subframe_idx].y = me2f(hExternalOrientationData->Quaternions_fx[subframe_idx].y_fx, + 31 - hExternalOrientationData->Quaternions_fx[subframe_idx].y_qfact); + hExternalOrientationData->Quaternions[subframe_idx].z = me2f(hExternalOrientationData->Quaternions_fx[subframe_idx].z_fx, + 31 - hExternalOrientationData->Quaternions_fx[subframe_idx].z_qfact); +#else /* Move external orientation data to the decoder handle (invert orientations) */ QuaternionInverse( orientation, &hExternalOrientationData->Quaternions[subframe_idx] ); +#endif hExternalOrientationData->enableHeadRotation[subframe_idx] = enableHeadRotation; hExternalOrientationData->enableExternalOrientation[subframe_idx] = enableExternalOrientation; diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c index f761fa83ca4f1cb484179dbfa0d0ab4db6738ee5..2cc67027292cf14e3b0ac51422983369f34c8378 100644 --- a/lib_rend/ivas_orient_trk.c +++ b/lib_rend/ivas_orient_trk.c @@ -115,42 +115,87 @@ void QuaternionProduct_fx( 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; + Word64 mul[4] = { 0 }; + Word16 q_min, q_fact[4] = { 0 }; + mul[0] = W_mult0_32_32( q1.w_fx, q2.w_fx ); + q_fact[0] = q1.w_qfact + q2.w_qfact; + mul[1] = W_mult0_32_32( q1.x_fx, q2.x_fx ); + q_fact[1] = q1.x_qfact + q2.x_qfact; + mul[2] = W_mult0_32_32( q1.y_fx, q2.y_fx ); + q_fact[2] = q1.y_qfact + q2.y_qfact; + mul[3] = W_mult0_32_32( q1.z_fx, q2.z_fx ); + q_fact[3] = q1.z_qfact + q2.z_qfact; + q_min = q_fact[0]; + for ( int i = 0; i < 4; i++ ) + { + q_min = s_min( q_min, q_fact[i] ); + } + mul[2] = W_add( ( W_shr( mul[2], ( sub( q_fact[2], q_min ) ) ) ), ( W_shr( mul[3], ( sub( q_fact[3], q_min ) ) ) ) ); + q_fact[2] = q_min; + mul[1] = W_add( ( W_shr( mul[1], ( sub( q_fact[1], q_min ) ) ) ), ( W_shr( mul[2], ( sub( q_fact[2], q_min ) ) ) ) ); + q_fact[1] = q_min; + tmp.w_fx = W_extract_h( W_sub( ( W_shr( mul[0], ( sub( q_fact[0], q_min ) ) ) ), ( W_shr( mul[1], ( sub( q_fact[1], q_min ) ) ) ) ) ); + tmp.w_qfact = q_min - 32; + + mul[0] = W_mult0_32_32( q1.w_fx, q2.x_fx ); + q_fact[0] = q1.w_qfact + q2.x_qfact; + mul[1] = W_mult0_32_32( q1.x_fx, q2.w_fx ); + q_fact[1] = q1.x_qfact + q2.w_qfact; + mul[2] = W_mult0_32_32( q1.y_fx, q2.z_fx ); + q_fact[2] = q1.y_qfact + q2.z_qfact; + mul[3] = W_mult0_32_32( q1.z_fx, q2.y_fx ); + q_fact[3] = q1.z_qfact + q2.y_qfact; + q_min = q_fact[0]; + for ( int i = 0; i < 4; i++ ) + { + q_min = s_min( q_min, q_fact[i] ); + } + mul[1] = W_add( ( W_shr( mul[0], ( sub( q_fact[0], q_min ) ) ) ), ( W_shr( mul[1], ( sub( q_fact[1], q_min ) ) ) ) ); + q_fact[1] = q_min; + mul[2] = W_add( ( W_shr( mul[1], ( sub( q_fact[1], q_min ) ) ) ), ( W_shr( mul[2], ( sub( q_fact[2], q_min ) ) ) ) ); + q_fact[2] = q_min; + tmp.x_fx = W_extract_h( W_sub( ( W_shr( mul[2], ( sub( q_fact[2], q_min ) ) ) ), ( W_shr( mul[3], ( sub( q_fact[3], q_min ) ) ) ) ) ); + tmp.x_qfact = q_min - 32; + + mul[0] = W_mult0_32_32( q1.w_fx, q2.y_fx ); + q_fact[0] = q1.w_qfact + q2.y_qfact; + mul[1] = W_mult0_32_32( q1.x_fx, q2.z_fx ); + q_fact[1] = q1.x_qfact + q2.z_qfact; + mul[2] = W_mult0_32_32( q1.y_fx, q2.w_fx ); + q_fact[2] = q1.y_qfact + q2.w_qfact; + mul[3] = W_mult0_32_32( q1.z_fx, q2.x_fx ); + q_fact[3] = q1.z_qfact + q2.x_qfact; + q_min = q_fact[0]; + for ( int i = 0; i < 4; i++ ) + { + q_min = s_min( q_min, q_fact[i] ); + } + mul[2] = W_add( ( W_shr( mul[0], ( sub( q_fact[0], q_min ) ) ) ), ( W_shr( mul[2], ( sub( q_fact[2], q_min ) ) ) ) ); + q_fact[2] = q_min; + mul[3] = W_add( ( W_shr( mul[2], ( sub( q_fact[2], q_min ) ) ) ), ( W_shr( mul[3], ( sub( q_fact[3], q_min ) ) ) ) ); + q_fact[3] = q_min; + tmp.y_fx = W_extract_h( W_sub( ( W_shr( mul[3], ( sub( q_fact[3], q_min ) ) ) ), ( W_shr( mul[1], ( sub( q_fact[1], q_min ) ) ) ) ) ); + tmp.y_qfact = q_min - 32; + + mul[0] = W_mult0_32_32( q1.w_fx, q2.z_fx ); + q_fact[0] = q1.w_qfact + q2.z_qfact; + mul[1] = W_mult0_32_32( q1.x_fx, q2.y_fx ); + q_fact[1] = q1.x_qfact + q2.y_qfact; + mul[2] = W_mult0_32_32( q1.y_fx, q2.x_fx ); + q_fact[2] = q1.y_qfact + q2.x_qfact; + mul[3] = W_mult0_32_32( q1.z_fx, q2.w_fx ); + q_fact[3] = q1.z_qfact + q2.w_qfact; + q_min = q_fact[0]; + for ( int i = 0; i < 4; i++ ) + { + q_min = s_min( q_min, q_fact[i] ); + } + mul[1] = W_add( ( W_shr( mul[0], ( sub( q_fact[0], q_min ) ) ) ), ( W_shr( mul[1], ( sub( q_fact[1], q_min ) ) ) ) ); + q_fact[1] = q_min; + mul[3] = W_add( ( W_shr( mul[1], ( sub( q_fact[1], q_min ) ) ) ), ( W_shr( mul[3], ( sub( q_fact[3], q_min ) ) ) ) ); + q_fact[3] = q_min; + tmp.z_fx = W_extract_h( W_sub( ( W_shr( mul[3], ( sub( q_fact[3], q_min ) ) ) ), ( W_shr( mul[2], ( sub( q_fact[2], q_min ) ) ) ) ) ); + tmp.z_qfact = q_min - 32; *r = tmp; @@ -174,23 +219,55 @@ static float QuaternionDotProduct( #ifdef IVAS_FLOAT_FIXED static Word32 QuaternionDotProduct_fx( const IVAS_QUATERNION_FX q1, - const IVAS_QUATERNION_FX q2 ) + const IVAS_QUATERNION_FX q2, Word16 *q_fact) { Word32 a[4], b[4] = { 0 }; + Word64 mult_res[4] = { 0 }; + Word16 a_q[4], b_q[4], mult_res_q[4] = { 0 }; Word32 result = 0; + Word16 result_q = 0; a[0] = q1.w_fx; + a_q[0] = q1.w_qfact; a[1] = q1.x_fx; + a_q[1] = q1.x_qfact; a[2] = q1.y_fx; + a_q[2] = q1.y_qfact; a[3] = q1.z_fx; + a_q[3] = q1.z_qfact; b[0] = q2.w_fx; + b_q[0] = q2.w_qfact; b[1] = q2.x_fx; + b_q[1] = q2.x_qfact; b[2] = q2.y_fx; + b_q[2] = q2.y_qfact; b[3] = q2.z_fx; + b_q[3] = q2.z_qfact; + for ( int i = 0; i < 4; i++ ) { - result = Madd_32_32( result, a[i], b[i] ); + mult_res[i] = W_mult0_32_32( a[i], b[i] ); + // mult_res_e[i] = ( 31 - ( a_q[i] + b_q[i] - 31 ) ); + mult_res_q[i] = a_q[i] + b_q[i]; + } + for ( int j = 0; j < 3; j++ ) + { + IF( GT_16( mult_res_q[j], mult_res_q[j + 1] ) ) + { + mult_res[j] = W_shr( mult_res[j], ( mult_res_q[j] - mult_res_q[j + 1] ) ); + result_q = mult_res_q[j + 1]; } - return ( result ); + ELSE + { + 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] ); + mult_res_q[j + 1] = result_q; + } + result = W_extract_h( mult_res[3] ); + *q_fact = result_q - 32; + + return result; } #endif @@ -213,6 +290,31 @@ static void QuaternionDivision( return; } +#ifdef IVAS_FLOAT_FIXED +static void QuaternionDivision_fx( + const IVAS_QUATERNION_FX q, + const Word32 d, + IVAS_QUATERNION_FX *const r, + Word16 den_e ) +{ + float den = me2f( d, den_e ); + IVAS_QUATERNION q_flt; + Word16 e_temp; + q_flt.w = me2f( q.w_fx, 31 - q.w_qfact ); + q_flt.x = me2f( q.x_fx, 31 - q.x_qfact ); + q_flt.y = me2f( q.y_fx, 31 - q.y_qfact ); + q_flt.z = me2f( q.z_fx, 31 - q.z_qfact ); + f2me( ( q_flt.w / den ), &r->w_fx, &e_temp ); + r->w_qfact = 31 - e_temp; + f2me( ( q_flt.x / den ), &r->x_fx, &e_temp ); + r->x_qfact = 31 - e_temp; + f2me( ( q_flt.y / den ), &r->y_fx, &e_temp ); + r->y_qfact = 31 - e_temp; + f2me( ( q_flt.z / den ), &r->z_fx, &e_temp ); + r->z_qfact = 31 - e_temp; +} +#endif + /*------------------------------------------------------------------------------------------* * QuaternionNormalize() @@ -224,24 +326,26 @@ 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; +} + +#ifdef IVAS_FLOAT_FIXED +static void QuaternionNormalize_fx( + const IVAS_QUATERNION_FX q_fx, + IVAS_QUATERNION_FX *const r_fx ) +{ + Word16 q_dot, sqrt_e = 0; + Word32 sqrt_fx; + Word32 dot_prod_fx = QuaternionDotProduct_fx( q_fx, q_fx, &q_dot ); + sqrt_e = 31 - q_dot; + sqrt_fx = Sqrt32( dot_prod_fx, &sqrt_e ); + QuaternionDivision_fx( q_fx, sqrt_fx, r_fx, sqrt_e ); return; } +#endif /*------------------------------------------------------------------------------------------* @@ -270,9 +374,8 @@ void QuaternionSlerp( 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 ); + Word32 s_fx = QuaternionDotProduct_fx( q1_fx, q2_fx, &q_dot); + s = me2f( s_fx, 31 - q_dot ); #else s = QuaternionDotProduct( q1, q2 ); #endif @@ -293,7 +396,23 @@ void QuaternionSlerp( r->z = ( q1.z * s + q2.z * s2 ) / denom; r->w = ( q1.w * s + q2.w * s2 ) / denom; +#ifdef IVAS_FLOAT_FIXED + IVAS_QUATERNION_FX *r_fx = malloc( sizeof( IVAS_QUATERNION_FX ) ); + 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; } @@ -375,7 +494,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 ); + 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 ) ); @@ -429,35 +548,29 @@ 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; } +#ifdef IVAS_FLOAT_FIXED +void QuaternionInverse_fx( + const IVAS_QUATERNION_FX q, + IVAS_QUATERNION_FX *const r ) +{ + Word32 dot_product; + Word16 dot_q = 0; + dot_product = QuaternionDotProduct_fx( q, q, &dot_q ); + QuaternionConjugate_fx( q, r ); + QuaternionDivision_fx( *r, dot_product, r, ( 31 - dot_q ) ); + + return; +} +#endif + /*------------------------------------------------------------------------------------------* * VectorSubtract() @@ -484,13 +597,15 @@ static IVAS_VECTOR3_FX VectorSubtract_fx( const IVAS_VECTOR3_FX p2 ) { IVAS_VECTOR3_FX result; + Word16 e_result = 0; + result.x_fx = BASOP_Util_Add_Mant32Exp( p1.x_fx, ( 31 - p1.x_qfact ), ( L_negate( p2.x_fx ) ), ( 31 - p2.x_qfact ), &e_result ); + result.x_qfact = 31 - e_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.z_qfact = p1.z_qfact; + result.y_fx = BASOP_Util_Add_Mant32Exp( p1.y_fx, ( 31 - p1.y_qfact ), ( L_negate( p2.y_fx ) ), ( 31 - p2.y_qfact ), &e_result ); + result.y_qfact = 31 - e_result; + + result.z_fx = BASOP_Util_Add_Mant32Exp( p1.z_fx, ( 31 - p1.z_qfact ), ( L_negate( p2.z_fx ) ), ( 31 - p2.z_qfact ), &e_result ); + result.z_qfact = 31 - e_result; return result; } @@ -522,55 +637,29 @@ static IVAS_VECTOR3_FX VectorCrossProduct_fx( { IVAS_VECTOR3_FX result_fx; Word32 mul1, mul2; - Word16 diff, q_mul1, q_mul2; + Word16 e_result = 0, 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; - } + q_mul2 = p1.z_qfact + p2.y_qfact - 31; + + result_fx.x_fx = BASOP_Util_Add_Mant32Exp( mul1, ( 31 - q_mul1 ), ( L_negate( mul2 ) ), ( 31 - q_mul2 ), &e_result ); + result_fx.x_qfact = 31 - e_result; + 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; - } + result_fx.y_fx = BASOP_Util_Add_Mant32Exp( mul1, ( 31 - q_mul1 ), ( L_negate( mul2 ) ), ( 31 - q_mul2 ), &e_result ); + result_fx.y_qfact = 31 - e_result; + + 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; - } + result_fx.z_fx = BASOP_Util_Add_Mant32Exp( mul1, ( 31 - q_mul1 ), ( L_negate( mul2 ) ), ( 31 - q_mul2 ), &e_result ); + result_fx.z_qfact = 31 - e_result; return result_fx; } @@ -595,15 +684,25 @@ static Word32 VectorDotProduct_fx( const IVAS_VECTOR3_FX p2, Word16 *q_fact ) { - Word32 mul_1, mul_2, mul_3 = 0; + Word32 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; + Word16 mul_q[3], q_min = 0; + mul[0] = Mpy_32_32( p1.x_fx, p2.x_fx ); + mul_q[0] = p1.x_qfact + p2.x_qfact - 31; + mul[1] = Mpy_32_32( p1.y_fx, p2.y_fx ); + mul_q[1] = p1.y_qfact + p2.y_qfact - 31; + 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++ ) + { + q_min = s_min( q_min, mul_q[i] ); + } + for ( int j = 0; j < 3; j++ ) + { + result_fx = L_add( result_fx, L_shr( mul[j], ( sub( mul_q[j], q_min ) ) ) ); + } + *q_fact = q_min; return result_fx; } @@ -623,18 +722,22 @@ static float VectorLength( #ifdef IVAS_FLOAT_FIXED static Word32 VectorLength_fx( - const IVAS_VECTOR3_FX p, + IVAS_VECTOR3_FX p, Word16 *q_fact ) { - Word32 mul_1, mul_2, mul_3 = 0; + Word32 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); + Word16 result_e = 0, q[3] = { 0 }; + mul[0] = Mpy_32_32( p.x_fx, p.x_fx ); + q[0] = 2 * p.x_qfact - 31; + mul[1] = Mpy_32_32( p.y_fx, p.y_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++ ) + { + result_fx = BASOP_Util_Add_Mant32Exp( result_fx, result_e, mul[j], 31 - q[j], &result_e ); + } result_fx = Sqrt32( result_fx, &result_e ); *q_fact = 31 - result_e; return result_fx; @@ -699,33 +802,10 @@ 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 ) { @@ -749,6 +829,58 @@ static void VectorRotationToQuaternion( return; } +#ifdef IVAS_FLOAT_FIXED +static void VectorRotationToQuaternion_fx( + const IVAS_VECTOR3_FX p1, + const IVAS_VECTOR3_FX p2, + IVAS_QUATERNION_FX *const r ) +{ + 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); + cross_product_fx = VectorCrossProduct_fx( p1_normalized_fx, p2_normalized_fx ); + dot_product_fx = VectorDotProduct_fx( p1_normalized_fx, p2_normalized_fx, &q_dot ); + + Word32 comp_fx = 0; + Word16 comp_e, check_flag, result_e = 0; + f2me( -0.999999, &comp_fx, &comp_e ); + IF( GT_32( dot_product_fx, 0 ) ) + { + check_flag = 0; + } + ELSE + { + check_flag = BASOP_Util_Cmp_Mant32Exp( comp_fx, comp_e, dot_product_fx, 31 - q_dot ); + } + IF( EQ_16( check_flag, 1 ) ) + { + r->w_fx = 0; + r->x_fx = 0; + r->y_fx = 0; + r->z_fx = ONE_IN_Q31; + r->w_qfact = r->x_qfact = r->y_qfact = r->z_qfact = Q31; + } + ELSE + { + /* all regular cases */ + r->x_fx = cross_product_fx.x_fx; + r->x_qfact = cross_product_fx.x_qfact; + r->y_fx = cross_product_fx.y_fx; + r->y_qfact = cross_product_fx.y_qfact; + r->z_fx = cross_product_fx.z_fx; + r->z_qfact = cross_product_fx.z_qfact; + r->w_fx = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, dot_product_fx, 31 - q_dot, &result_e ); + r->w_qfact = 31 - result_e; + } + QuaternionNormalize_fx( *r, r ); + + return; +} +#endif + /*-------------------------------------------------------------------* * ivas_orient_trk_Init() * @@ -973,9 +1105,6 @@ 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 ) { @@ -988,22 +1117,7 @@ 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 = Q27; - refPos_fx.x_qfact = refPos_fx.y_qfact = refPos_fx.z_qfact = Q27; - 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 */ @@ -1012,22 +1126,7 @@ 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; @@ -1049,6 +1148,66 @@ ivas_error ivas_orient_trk_SetReferenceVector( return IVAS_ERR_OK; } +#ifdef IVAS_FLOAT_FIXED +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 */ + const IVAS_VECTOR3_FX refPos /* i : Reference position */ +) +{ + IVAS_VECTOR3_FX acousticFrontVector, ivasForwardVector; + IVAS_VECTOR3_FX listenerPosLevel, refPosLevel; + Word32 acousticFrontVectorLength; + Word16 acousticFrontVector_q; + + if ( pOTR == NULL ) + { + return IVAS_ERR_UNEXPECTED_NULL_POINTER; + } + + 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; + 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; + listenerPosLevel.z_qfact = refPosLevel.z_qfact = listenerPos.z_qfact; + listenerPosLevel.x_fx = listenerPos.x_fx; + listenerPosLevel.x_qfact = listenerPos.x_qfact; + listenerPosLevel.y_fx = listenerPos.y_fx; + listenerPosLevel.y_qfact = listenerPos.y_qfact; + refPosLevel.x_fx = refPos.x_fx; + refPosLevel.x_qfact = refPos.x_qfact; + refPosLevel.y_fx = refPos.y_fx; + refPosLevel.y_qfact = refPos.y_qfact; + acousticFrontVector = VectorSubtract_fx( listenerPosLevel, refPosLevel ); + break; + default: + return IVAS_ERR_WRONG_PARAMS; + } + + acousticFrontVectorLength = VectorLength_fx( acousticFrontVector, &acousticFrontVector_q ); + /* if the length is zero, the user has entered insensible listener and reference positions */ + IF( LE_32( acousticFrontVectorLength, 0 ) ) + { + return IVAS_ERR_WRONG_PARAMS; + } + + 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; + VectorRotationToQuaternion_fx( ivasForwardVector, acousticFrontVector, &pOTR->refRot_fx ); + + return IVAS_ERR_OK; +} +#endif + /*-------------------------------------------------------------------* * ivas_orient_trk_Process() @@ -1092,21 +1251,23 @@ ivas_error ivas_orient_trk_Process( /* Reset adaptation filter - start adaptation at center rate */ pOTR->alpha = sinf( 2.0f * EVS_PI * pOTR->centerAdaptationRate / updateRate ); - /* 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; + 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->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 ); + 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); @@ -1118,6 +1279,8 @@ ivas_error ivas_orient_trk_Process( 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); QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot ); #endif break; @@ -1126,22 +1289,24 @@ ivas_error ivas_orient_trk_Process( /* Compute average (low-pass filtered) absolute orientation */ QuaternionSlerp( pOTR->absAvgRot, absRot, alpha, &pOTR->absAvgRot ); - /* 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; + 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->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 ); + 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; @@ -1168,6 +1333,9 @@ ivas_error ivas_orient_trk_Process( 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); QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot ); /* Adapt LPF constant based on orientation excursion relative to current mean: diff --git a/lib_rend/ivas_prot_rend.h b/lib_rend/ivas_prot_rend.h index 79eafa71409bb064411782a0de27a91ed7c9aa4e..89653c36e6f525602f03530585d170cd827ee1ad 100644 --- a/lib_rend/ivas_prot_rend.h +++ b/lib_rend/ivas_prot_rend.h @@ -1763,6 +1763,12 @@ void QuaternionSlerp( IVAS_QUATERNION *const r ); +#ifdef IVAS_FLOAT_FIXED +void QuaternionInverse_fx( + const IVAS_QUATERNION_FX q, + IVAS_QUATERNION_FX *const r +); +#endif /*----------------------------------------------------------------------------------* * Orientation tracking @@ -1807,6 +1813,12 @@ ivas_error ivas_orient_trk_Process( #ifdef IVAS_FLOAT_FIXED +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 */ + const IVAS_VECTOR3_FX refPos /* i : Reference position */ +); + ivas_error ivas_orient_trk_SetTrackingType_fx( ivas_orient_trk_state_t_fx *pOTR, /* i/o: orientation tracker handle */ const IVAS_HEAD_ORIENT_TRK_T orientation_tracking /* i : orientation tracking type */