From 8fdad991b4b0c53b78407a8ac8daba4dae08f982 Mon Sep 17 00:00:00 2001 From: Sandesh Venkatesh Date: Tue, 7 May 2024 11:17:06 +0530 Subject: [PATCH] Quaternion and vector functions simplified fixed implementation --- lib_com/common_api_types.h | 4 +- lib_dec/ivas_objectRenderer_internal.c | 40 +-- lib_dec/lib_dec.h | 5 + lib_dec/lib_dec_fx.c | 56 +-- lib_rend/ivas_objectRenderer.c | 13 +- lib_rend/ivas_orient_trk.c | 477 +++++++++---------------- lib_rend/ivas_rotation.c | 63 ++-- lib_rend/lib_rend.c | 20 +- 8 files changed, 250 insertions(+), 428 deletions(-) diff --git a/lib_com/common_api_types.h b/lib_com/common_api_types.h index 9de8aae05..d61457a11 100644 --- a/lib_com/common_api_types.h +++ b/lib_com/common_api_types.h @@ -134,7 +134,7 @@ typedef struct float w, x, y, z; #ifdef IVAS_FLOAT_FIXED Word32 w_fx, x_fx, y_fx, z_fx; - Word16 w_qfact, x_qfact, y_qfact, z_qfact; + Word16 q_fact; #endif } IVAS_QUATERNION; @@ -145,7 +145,7 @@ typedef struct float x, y, z; #ifdef IVAS_FLOAT_FIXED Word32 x_fx, y_fx, z_fx; - Word16 x_qfact, y_qfact, z_qfact; + Word16 q_fact; #endif } IVAS_VECTOR3; diff --git a/lib_dec/ivas_objectRenderer_internal.c b/lib_dec/ivas_objectRenderer_internal.c index 1c9ec0308..ce8e3c6f3 100644 --- a/lib_dec/ivas_objectRenderer_internal.c +++ b/lib_dec/ivas_objectRenderer_internal.c @@ -269,45 +269,13 @@ ivas_error ivas_td_binaural_renderer_sf_fx( tmp_vector_fx = &st_ivas->hCombinedOrientationData->listenerPos[st_ivas->hCombinedOrientationData->subframe_idx]; enableCombinedOrientation = st_ivas->hCombinedOrientationData->enableCombinedOrientation[st_ivas->hCombinedOrientationData->subframe_idx]; - /* Shifting w_fx, x_fx, y_fx, z_fx to a common Q-factor if they are not having the same Q-factor */ - Word16 min_q; - IF( !( EQ_16( tmp_Quaternion_fx->w_qfact, tmp_Quaternion_fx->x_qfact ) && EQ_16( tmp_Quaternion_fx->x_qfact, tmp_Quaternion_fx->y_qfact ) && EQ_16( tmp_Quaternion_fx->y_qfact, tmp_Quaternion_fx->z_qfact ) ) ) - { - min_q = MAX16B; - move16(); - min_q = s_min( min_q, tmp_Quaternion_fx->w_qfact ); - min_q = s_min( min_q, tmp_Quaternion_fx->x_qfact ); - min_q = s_min( min_q, tmp_Quaternion_fx->y_qfact ); - min_q = s_min( min_q, tmp_Quaternion_fx->z_qfact ); - tmp_Quaternion_fx->w_fx = L_shr( tmp_Quaternion_fx->w_fx, sub( tmp_Quaternion_fx->w_qfact, min_q ) ); - tmp_Quaternion_fx->x_fx = L_shr( tmp_Quaternion_fx->x_fx, sub( tmp_Quaternion_fx->x_qfact, min_q ) ); - tmp_Quaternion_fx->y_fx = L_shr( tmp_Quaternion_fx->y_fx, sub( tmp_Quaternion_fx->y_qfact, min_q ) ); - tmp_Quaternion_fx->z_fx = L_shr( tmp_Quaternion_fx->z_fx, sub( tmp_Quaternion_fx->z_qfact, min_q ) ); - tmp_Quaternion_fx->w_qfact = min_q; - move16(); - tmp_Quaternion_fx->x_qfact = min_q; - move16(); - tmp_Quaternion_fx->y_qfact = min_q; - move16(); - tmp_Quaternion_fx->z_qfact = min_q; - move16(); - } - /* Shifting x_fx, y_fx, z_fx to the same Q-factor as Listener_p->Pos_q (usually Q25) */ Word16 pos_q = st_ivas->hBinRendererTd->Listener_p->Pos_q; move16(); - IF( NE_16( tmp_vector_fx->x_qfact, pos_q ) || NE_16( tmp_vector_fx->z_qfact, pos_q ) || NE_16( tmp_vector_fx->y_qfact, pos_q ) ) - { - tmp_vector_fx->x_fx = L_shr( tmp_vector_fx->x_fx, sub( tmp_vector_fx->x_qfact, pos_q ) ); - tmp_vector_fx->y_fx = L_shr( tmp_vector_fx->y_fx, sub( tmp_vector_fx->y_qfact, pos_q ) ); - tmp_vector_fx->z_fx = L_shr( tmp_vector_fx->z_fx, sub( tmp_vector_fx->z_qfact, pos_q ) ); - tmp_vector_fx->x_qfact = pos_q; - move16(); - tmp_vector_fx->y_qfact = pos_q; - move16(); - tmp_vector_fx->z_qfact = pos_q; - move16(); - } + tmp_vector_fx->x_fx = L_shr( tmp_vector_fx->x_fx, sub( tmp_vector_fx->q_fact, pos_q ) ); + tmp_vector_fx->y_fx = L_shr( tmp_vector_fx->y_fx, sub( tmp_vector_fx->q_fact, pos_q ) ); + tmp_vector_fx->z_fx = L_shr( tmp_vector_fx->z_fx, sub( tmp_vector_fx->q_fact, pos_q ) ); + tmp_vector_fx->q_fact = pos_q; } ELSE { diff --git a/lib_dec/lib_dec.h b/lib_dec/lib_dec.h index a8c740841..02aedd958 100644 --- a/lib_dec/lib_dec.h +++ b/lib_dec/lib_dec.h @@ -496,8 +496,13 @@ ivas_error IVAS_DEC_FeedRefRotData( /*! r: error code */ ivas_error IVAS_DEC_FeedRefVectorData( IVAS_DEC_HANDLE hIvasDec, /* i/o: IVAS decoder handle */ +#ifndef IVAS_FLOAT_FIXED const IVAS_VECTOR3 listenerPos, /* i : Listener position */ const IVAS_VECTOR3 refPos /* i : Reference position */ +#else + IVAS_VECTOR3 listenerPos, /* i : Listener position */ + IVAS_VECTOR3 refPos /* i : Reference position */ +#endif ); /*! r: error code */ diff --git a/lib_dec/lib_dec_fx.c b/lib_dec/lib_dec_fx.c index 92d4479b2..e5e73a07e 100644 --- a/lib_dec/lib_dec_fx.c +++ b/lib_dec/lib_dec_fx.c @@ -1472,9 +1472,7 @@ ivas_error IVAS_DEC_FeedHeadTrackData( hHeadTrackData->Pos[subframe_idx].x_fx = Pos.x_fx; hHeadTrackData->Pos[subframe_idx].y_fx = Pos.y_fx; hHeadTrackData->Pos[subframe_idx].z_fx = Pos.z_fx; - hHeadTrackData->Pos[subframe_idx].x_qfact = Pos.x_qfact; - hHeadTrackData->Pos[subframe_idx].y_qfact = Pos.y_qfact; - hHeadTrackData->Pos[subframe_idx].z_qfact = Pos.z_qfact; + hHeadTrackData->Pos[subframe_idx].q_fact = Pos.q_fact; #endif hIvasDec->updateOrientation = true; @@ -1523,9 +1521,14 @@ ivas_error IVAS_DEC_FeedRefRotData( *---------------------------------------------------------------------*/ ivas_error IVAS_DEC_FeedRefVectorData( - IVAS_DEC_HANDLE hIvasDec, /* i/o: IVAS decoder handle */ + IVAS_DEC_HANDLE hIvasDec, /* i/o: IVAS decoder handle */ +#ifndef IVAS_FLOAT_FIXED const IVAS_VECTOR3 listenerPos, /* i : Listener position */ const IVAS_VECTOR3 refPos /* i : Reference position */ +#else + IVAS_VECTOR3 listenerPos, /* i : Listener position */ + IVAS_VECTOR3 refPos /* i : Reference position */ +#endif ) { ivas_orient_trk_state_t *pOtr; @@ -1540,20 +1543,19 @@ ivas_error IVAS_DEC_FeedRefVectorData( hIvasDec->updateOrientation = true; #ifdef IVAS_FLOAT_FIXED - IVAS_VECTOR3 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 ); - ivas_error error_fx = ivas_orient_trk_SetReferenceVector_fx(pOtr, listenerPos_fx, refPos_fx ); - pOtr->refRot.w = me2f(pOtr->refRot.w_fx, 31- pOtr->refRot.w_qfact ); - pOtr->refRot.x = me2f(pOtr->refRot.x_fx, 31 - pOtr->refRot.x_qfact ); - pOtr->refRot.y = me2f(pOtr->refRot.y_fx, 31- pOtr->refRot.y_qfact ); - pOtr->refRot.z = me2f(pOtr->refRot.z_fx, 31 - pOtr->refRot.z_qfact ); + listenerPos.q_fact = Q27; + refPos.q_fact = Q27; + listenerPos.x_fx = float_to_fix( listenerPos.x, listenerPos.q_fact ); + listenerPos.y_fx = float_to_fix( listenerPos.y, listenerPos.q_fact ); + listenerPos.z_fx = float_to_fix( listenerPos.z, listenerPos.q_fact ); + refPos.x_fx = float_to_fix( refPos.x, refPos.q_fact ); + refPos.y_fx = float_to_fix( refPos.y, refPos.q_fact ); + refPos.z_fx = float_to_fix( refPos.z, refPos.q_fact ); + ivas_error error_fx = ivas_orient_trk_SetReferenceVector_fx( pOtr, listenerPos, refPos ); + pOtr->refRot.w = me2f( pOtr->refRot.w_fx, 31 - pOtr->refRot.q_fact ); + pOtr->refRot.x = me2f( pOtr->refRot.x_fx, 31 - pOtr->refRot.q_fact ); + pOtr->refRot.y = me2f( pOtr->refRot.y_fx, 31 - pOtr->refRot.q_fact ); + pOtr->refRot.z = me2f( pOtr->refRot.z_fx, 31 - pOtr->refRot.q_fact ); return error_fx; #else return ivas_orient_trk_SetReferenceVector( pOtr, listenerPos, refPos ); @@ -1593,21 +1595,21 @@ ivas_error IVAS_DEC_FeedExternalOrientationData( #ifdef IVAS_FLOAT_FIXED IVAS_QUATERNION 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); + orientation_fx.q_fact = Q29; + orientation_fx.w_fx = float_to_fix(orientation.w, orientation_fx.q_fact); + orientation_fx.x_fx = float_to_fix(orientation.x, orientation_fx.q_fact); + orientation_fx.y_fx = float_to_fix(orientation.y, orientation_fx.q_fact); + orientation_fx.z_fx = float_to_fix(orientation.z, orientation_fx.q_fact); /* Move external orientation data to the decoder handle (invert orientations) */ QuaternionInverse_fx(orientation_fx, &hExternalOrientationData->Quaternions[subframe_idx]); hExternalOrientationData->Quaternions[subframe_idx].w = me2f(hExternalOrientationData->Quaternions[subframe_idx].w_fx, - 31 - hExternalOrientationData->Quaternions[subframe_idx].w_qfact); + 31 - hExternalOrientationData->Quaternions[subframe_idx].q_fact); hExternalOrientationData->Quaternions[subframe_idx].x = me2f(hExternalOrientationData->Quaternions[subframe_idx].x_fx, - 31 - hExternalOrientationData->Quaternions[subframe_idx].x_qfact); + 31 - hExternalOrientationData->Quaternions[subframe_idx].q_fact); hExternalOrientationData->Quaternions[subframe_idx].y = me2f(hExternalOrientationData->Quaternions[subframe_idx].y_fx, - 31 - hExternalOrientationData->Quaternions[subframe_idx].y_qfact); + 31 - hExternalOrientationData->Quaternions[subframe_idx].q_fact); hExternalOrientationData->Quaternions[subframe_idx].z = me2f(hExternalOrientationData->Quaternions[subframe_idx].z_fx, - 31 - hExternalOrientationData->Quaternions[subframe_idx].z_qfact); + 31 - hExternalOrientationData->Quaternions[subframe_idx].q_fact); #else /* Move external orientation data to the decoder handle (invert orientations) */ QuaternionInverse( orientation, &hExternalOrientationData->Quaternions[subframe_idx] ); diff --git a/lib_rend/ivas_objectRenderer.c b/lib_rend/ivas_objectRenderer.c index 9ee54f329..f8e2a9a17 100644 --- a/lib_rend/ivas_objectRenderer.c +++ b/lib_rend/ivas_objectRenderer.c @@ -649,13 +649,13 @@ ivas_error ivas_td_binaural_renderer_unwrap( tmp_Quaternions->x_fx = float_to_fix(tmp_Quaternions->x, quat_q); tmp_Quaternions->y_fx = float_to_fix(tmp_Quaternions->y, quat_q); tmp_Quaternions->z_fx = float_to_fix(tmp_Quaternions->z, quat_q); - tmp_Quaternions->w_qfact = tmp_Quaternions->x_qfact = tmp_Quaternions->y_qfact = tmp_Quaternions->z_qfact = quat_q; + tmp_Quaternions->q_fact = quat_q; Word16 pos_q = Q25; tmp_Pos->x_fx = (Word32)float_to_fix(tmp_Pos->x, pos_q); tmp_Pos->y_fx = (Word32)float_to_fix(tmp_Pos->y, pos_q); tmp_Pos->z_fx = (Word32)float_to_fix(tmp_Pos->z, pos_q); - tmp_Pos->x_qfact = tmp_Pos->y_qfact = tmp_Pos->z_qfact = pos_q; + tmp_Pos->q_fact= pos_q; for (int i = 0; i < 3; i++) { hBinRendererTd->Listener_p->Front_fx[i] = float_to_fix(hBinRendererTd->Listener_p->Front[i], Q30); @@ -1218,10 +1218,7 @@ ivas_error TDREND_Update_listener_orientation_fx( { Word16 Rmat_q; - assert(headPosition_fx->w_qfact == headPosition_fx->x_qfact - && headPosition_fx->x_qfact == headPosition_fx->y_qfact - && headPosition_fx->y_qfact == headPosition_fx->z_qfact); - headPosition_q = headPosition_fx->w_qfact; // Assuming Q is same for w, x, y and z + headPosition_q = headPosition_fx->q_fact; move16(); /* Obtain head rotation matrix */ @@ -1242,10 +1239,6 @@ ivas_error TDREND_Update_listener_orientation_fx( IF ( Pos_fx != NULL ) { - // Assuming Q is same for x, y and z - assert((*Pos_fx).x_qfact == (*Pos_fx).y_qfact && (*Pos_fx).y_qfact == (*Pos_fx).z_qfact ); - assert((*Pos_fx).x_qfact == hBinRendererTd->Listener_p->Pos_q ); - /* Input position */ Pos_p_fx[0] = ( *Pos_fx ).x_fx; Pos_p_fx[1] = ( *Pos_fx ).y_fx; diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c index 21873782f..b570c8216 100644 --- a/lib_rend/ivas_orient_trk.c +++ b/lib_rend/ivas_orient_trk.c @@ -84,7 +84,7 @@ static IVAS_QUATERNION IdentityQuaternion_fx( q.w_fx = ONE_IN_Q31; q.x_fx = q.y_fx = q.z_fx = 0; - q.w_qfact = q.x_qfact = q.y_qfact = q.z_qfact = Q31; + q.q_fact = Q31; return q; } @@ -119,90 +119,15 @@ void QuaternionProduct_fx( IVAS_QUATERNION *const r ) { IVAS_QUATERNION tmp; - 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; + //once verify// + tmp.w_fx = L_sub( ( L_sub( Mpy_32_32( q1.w_fx, q2.w_fx ), Mpy_32_32( q1.x_fx, q2.x_fx ) ) ), ( L_add( Mpy_32_32( q1.y_fx, q2.y_fx ), Mpy_32_32( q1.z_fx, q2.z_fx ) ) ) ); + tmp.x_fx = L_add( ( L_add( Mpy_32_32( q1.w_fx, q2.x_fx ), Mpy_32_32( q1.x_fx, q2.w_fx ) ) ), ( L_sub( Mpy_32_32( q1.y_fx, q2.z_fx ), Mpy_32_32( q1.z_fx, q2.y_fx ) ) ) ); + tmp.y_fx = L_add( ( L_sub( Mpy_32_32( q1.w_fx, q2.y_fx ), Mpy_32_32( q1.x_fx, q2.z_fx ) ) ), ( L_add( Mpy_32_32( q1.y_fx, q2.w_fx ), Mpy_32_32( q1.z_fx, q2.x_fx ) ) ) ); + tmp.z_fx = L_sub( ( L_add( Mpy_32_32( q1.w_fx, q2.z_fx ), Mpy_32_32( q1.x_fx, q2.y_fx ) ) ), ( L_sub( Mpy_32_32( q1.y_fx, q2.x_fx ), Mpy_32_32( q1.z_fx, q2.w_fx ) ) ) ); + tmp.q_fact = sub(add(q1.q_fact, q2.q_fact), 31); + *r = tmp; return; } #endif @@ -226,51 +151,12 @@ static Word32 QuaternionDotProduct_fx( const IVAS_QUATERNION 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++ ) - { - 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]; - } - 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; + + result = L_add( ( L_add( Mpy_32_32( q1.x_fx, q2.x_fx ), Mpy_32_32( q1.y_fx, q2.y_fx ) ) ), ( L_add( Mpy_32_32( q1.z_fx, q2.z_fx ), Mpy_32_32( q1.w_fx, q2.w_fx ) ) ) ); + + *q_fact = sub( add( q1.q_fact, q2.q_fact ), 31 ); return result; } @@ -302,21 +188,31 @@ static void QuaternionDivision_fx( IVAS_QUATERNION *const r, Word16 den_e ) { - Word16 scale_e, result_e = 0; - r->w_fx = L_deposit_h(BASOP_Util_Divide3232_Scale((q.w_fx), d, &scale_e)); - result_e = scale_e + ((31 - q.w_qfact) - den_e); // e+e1-e2// - r->w_qfact = 31 - result_e; - r->x_fx = L_deposit_h(BASOP_Util_Divide3232_Scale((q.x_fx), d, &scale_e)); - result_e = scale_e + ((31 - q.x_qfact) - den_e); - r->x_qfact = 31 - result_e; - r->y_fx = L_deposit_h(BASOP_Util_Divide3232_Scale((q.y_fx), d, &scale_e)); - result_e = scale_e + ((31 - q.y_qfact) - den_e); - r->y_qfact = 31 - result_e; - r->z_fx = L_deposit_h(BASOP_Util_Divide3232_Scale((q.z_fx), d, &scale_e)); - result_e = scale_e + ((31 - q.z_qfact) - den_e); - r->z_qfact = 31 - result_e; + Word16 scale_e, result_e = 0, w_q, x_q, y_q, z_q, result_q; + + r->w_fx = BASOP_Util_Divide3232_Scale_cadence( ( q.w_fx ), d, &scale_e ); + result_e = scale_e + ( ( 31 - q.q_fact ) - den_e ); // e+e1-e2// + w_q = 31 - result_e; + + r->x_fx = BASOP_Util_Divide3232_Scale_cadence( ( q.x_fx ), d, &scale_e ); + result_e = scale_e + ( ( 31 - q.q_fact ) - den_e ); + x_q = 31 - result_e; + + r->y_fx = BASOP_Util_Divide3232_Scale_cadence( ( q.y_fx ), d, &scale_e ); + result_e = scale_e + ( ( 31 - q.q_fact ) - den_e ); + y_q = 31 - result_e; + r->z_fx = BASOP_Util_Divide3232_Scale_cadence( ( q.z_fx ), d, &scale_e ); + result_e = scale_e + ( ( 31 - q.q_fact ) - den_e ); + z_q = 31 - result_e; + result_q = sub(s_min( s_min( w_q, x_q ), s_min( y_q, z_q ) ), 1); //gaurdbits// + + r->w_fx = L_shr( r->w_fx, sub( w_q, result_q ) ); + r->x_fx = L_shr( r->x_fx, sub( x_q, result_q ) ); + r->y_fx = L_shr( r->y_fx, sub( y_q, result_q ) ); + r->z_fx = L_shr( r->z_fx, sub( z_q, result_q ) ); + r->q_fact = result_q; } #endif @@ -433,20 +329,19 @@ void QuaternionSlerp_fx( QuaternionNormalize_fx( q1_fx, &r1 ); QuaternionNormalize_fx( q2_fx, &r2 ); - Word16 q_dot = 0; + Word16 w_qfact, x_qfact, y_qfact, z_qfact, q_result, q_dot = 0; cosPhi = QuaternionDotProduct_fx( r1, r2, &q_dot ); - q_min = s_min( s_min( r1.w_qfact, r1.x_qfact ), s_min( r1.y_qfact, r1.z_qfact ) ); - q_min = s_min( q_min, s_min( s_min( r1.w_qfact, r1.x_qfact ), s_min( r1.y_qfact, r1.z_qfact ) ) ); - r1.w_fx = L_shr( r1.w_fx, sub( r1.w_qfact, q_min ) ); - r1.x_fx = L_shr( r1.x_fx, sub( r1.x_qfact, q_min ) ); - r1.y_fx = L_shr( r1.y_fx, sub( r1.y_qfact, q_min ) ); - r1.z_fx = L_shr( r1.z_fx, sub( r1.z_qfact, q_min ) ); - r2.w_fx = L_shr( r2.w_fx, sub( r2.w_qfact, q_min ) ); - r2.x_fx = L_shr( r2.x_fx, sub( r2.x_qfact, q_min ) ); - r2.y_fx = L_shr( r2.y_fx, sub( r2.y_qfact, q_min ) ); - r2.z_fx = L_shr( r2.z_fx, sub( r2.z_qfact, q_min ) ); - r1.w_qfact = r1.x_qfact = r1.y_qfact = r1.z_qfact = r2.w_qfact = r2.x_qfact = r2.y_qfact = r2.z_qfact = q_min; + q_min = s_min( r1.q_fact, r2.q_fact ); + r1.w_fx = L_shr( r1.w_fx, sub( r1.q_fact, q_min ) ); + r1.x_fx = L_shr( r1.x_fx, sub( r1.q_fact, q_min ) ); + r1.y_fx = L_shr( r1.y_fx, sub( r1.q_fact, q_min ) ); + r1.z_fx = L_shr( r1.z_fx, sub( r1.q_fact, q_min ) ); + r2.w_fx = L_shr( r2.w_fx, sub( r2.q_fact, q_min ) ); + r2.x_fx = L_shr( r2.x_fx, sub( r2.q_fact, q_min ) ); + r2.y_fx = L_shr( r2.y_fx, sub( r2.q_fact, q_min ) ); + r2.z_fx = L_shr( r2.z_fx, sub( r2.q_fact, q_min ) ); + r1.q_fact = r2.q_fact = q_min; IF( LT_32( cosPhi, 0 ) ) { @@ -466,7 +361,7 @@ void QuaternionSlerp_fx( r_fx->y_fx = L_add( L_shr( r1.y_fx, 1 ), Mpy_32_32( L_sub( r2.y_fx, r1.y_fx ), t_fx ) ); // q_min-1 r_fx->z_fx = L_add( L_shr( r1.z_fx, 1 ), Mpy_32_32( L_sub( r2.z_fx, r1.z_fx ), t_fx ) ); // q_min-1 - r2.w_qfact = r2.x_qfact = r2.y_qfact = r2.z_qfact = sub( q_min, 1 ); + r_fx->q_fact = sub( q_min, 1 ); } ELSE { @@ -476,39 +371,45 @@ void QuaternionSlerp_fx( phi = BASOP_util_atan2( sinPhi, cosPhi, sub( sin_e, sub( 31, q_dot ) ) ); // Q13 - temp_32 = L_shl(Mpy_32_16_1( L_sub( ONE_IN_Q30, t_fx ), phi ),1); // Q29 + temp_32 = L_shl( Mpy_32_16_1( L_sub( ONE_IN_Q30, t_fx ), phi ), 1 ); // Q29 temp_16 = extract_h( temp_32 ); // Q13 s1 = getSineWord16R2( mult( temp_16, 20860 ) ); // Q15 - temp_32 = L_shl(Mpy_32_16_1( t_fx, phi ),1); // Q29 + temp_32 = L_shl( Mpy_32_16_1( t_fx, phi ), 1 ); // Q29 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 = L_deposit_h(BASOP_Util_Divide3232_Scale( temp_32, sinPhi, &e_div )); + r_fx->w_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); e_div = e_div + ( 31 - q_min - sin_e ); - r_fx->w_qfact = 31 - e_div; + 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 = L_deposit_h(BASOP_Util_Divide3232_Scale( temp_32, sinPhi, &e_div )); + r_fx->x_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); e_div = e_div + ( 31 - q_min - sin_e ); - r_fx->x_qfact = 31 - e_div; + 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 = L_deposit_h(BASOP_Util_Divide3232_Scale( temp_32, sinPhi, &e_div )); + r_fx->y_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); e_div = e_div + ( 31 - q_min - sin_e ); - r_fx->y_qfact = 31 - e_div; + 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 = L_deposit_h(BASOP_Util_Divide3232_Scale( temp_32, sinPhi, &e_div )); + r_fx->z_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div ); e_div = e_div + ( 31 - q_min - sin_e ); - r_fx->z_qfact = 31 - e_div; + 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; } - QuaternionNormalize_fx(*r_fx, r_fx); + QuaternionNormalize_fx( *r_fx, r_fx ); return; - } #endif @@ -539,10 +440,8 @@ static void QuaternionConjugate_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; + r->q_fact = q.q_fact; + return; } #endif @@ -577,38 +476,33 @@ static Word32 QuaternionAngle_fx( 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 ) ) + + IF( LT_32( q12.w_fx, 0 ) ) { 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 ); - 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_e = sub( 31, q_dot ); 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 + result = L_shr( L_shr( result, -1 * ( result_e ) ), sub( 31, q12.q_fact ) ); // Q25 } ELSE { - IF( GT_32( result_e, ( 31 - q12.x_qfact ) ) ) + IF( GT_32( result_e, ( 31 - q12.q_fact ) ) ) { - result = L_shl( result, ( result_e - ( 31 - q12.x_qfact ) ) ); + result = L_shl( result, ( result_e - ( 31 - q12.q_fact ) ) ); } ELSE { - result = L_shr( result, ( ( 31 - q12.x_qfact ) - result_e ) ); + result = L_shr( result, ( ( 31 - q12.q_fact ) - result_e ) ); } } IF( GT_32( q12.w_fx, result ) ) @@ -691,15 +585,21 @@ static IVAS_VECTOR3 VectorSubtract_fx( const IVAS_VECTOR3 p2 ) { IVAS_VECTOR3 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; + Word16 e_result = 0, x_qfact, y_qfact, z_qfact, q_result; + result.x_fx = BASOP_Util_Add_Mant32Exp( p1.x_fx, ( 31 - p1.q_fact ), ( L_negate( p2.x_fx ) ), ( 31 - p2.q_fact ), &e_result ); + x_qfact = sub( 31, e_result ); + + result.y_fx = BASOP_Util_Add_Mant32Exp( p1.y_fx, ( 31 - p1.q_fact ), ( L_negate( p2.y_fx ) ), ( 31 - p2.q_fact ), &e_result ); + y_qfact = sub( 31, e_result ); - 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.q_fact ), ( L_negate( p2.z_fx ) ), ( 31 - p2.q_fact ), &e_result ); + z_qfact = sub( 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; + q_result = sub(s_min( s_min( x_qfact, y_qfact ), z_qfact ), 1); //guardbit// + result.x_fx = L_shr( result.x_fx, sub( x_qfact, q_result ) ); + result.y_fx = L_shr( result.y_fx, sub( y_qfact, q_result ) ); + result.z_fx = L_shr( result.z_fx, sub( z_qfact, q_result ) ); + result.q_fact = q_result; return result; } @@ -730,30 +630,11 @@ static IVAS_VECTOR3 VectorCrossProduct_fx( const IVAS_VECTOR3 p2 ) { IVAS_VECTOR3 result_fx; - Word32 mul1, 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.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; - 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; - 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; + + result_fx.x_fx = L_sub( Mpy_32_32( p1.y_fx, p2.z_fx ), Mpy_32_32( p1.z_fx, p2.y_fx ) ); + result_fx.y_fx = L_sub( Mpy_32_32( p1.z_fx, p2.x_fx ), Mpy_32_32( p1.x_fx, p2.z_fx ) ); + result_fx.z_fx = L_sub( Mpy_32_32( p1.x_fx, p2.y_fx ), Mpy_32_32( p1.y_fx, p2.x_fx ) ); + result_fx.q_fact = sub(add(p1.q_fact, p2.q_fact), 31); return result_fx; } @@ -778,25 +659,10 @@ static Word32 VectorDotProduct_fx( const IVAS_VECTOR3 p2, Word16 *q_fact ) { - Word32 mul[3] = { 0 }; Word32 result_fx = 0; - 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; + + result_fx = L_add( L_add( Mpy_32_32( p1.x_fx, p2.x_fx ), Mpy_32_32( p1.y_fx, p2.y_fx ) ), Mpy_32_32( p1.z_fx, p2.z_fx ) ); + *q_fact = sub(add(p1.q_fact, p2.q_fact), 31); return result_fx; } @@ -819,21 +685,10 @@ static Word32 VectorLength_fx( IVAS_VECTOR3 p, Word16 *q_fact ) { - Word32 mul[3] = { 0 }; Word32 result_fx = 0; - 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; + result_fx = L_add(L_add(Mpy_32_32(p.x_fx, p.x_fx), Mpy_32_32(p.y_fx, p.y_fx)), Mpy_32_32(p.z_fx, p.z_fx)); + + *q_fact = sub(add(p.q_fact, p.q_fact), 31); return result_fx; } #endif @@ -864,19 +719,23 @@ static IVAS_VECTOR3 VectorNormalize_fx( { IVAS_VECTOR3 result_fx; Word32 length_fx; - Word16 q_len, scale = 0; + Word16 q_len, scale = 0, x_qfact, y_qfact, z_qfact, q_result; 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 ) ) ); + + result_fx.x_fx = BASOP_Util_Divide3232_Scale_cadence( p.x_fx, length_fx, &scale ); + x_qfact = ( 31 - ( scale + ( q_len - p.q_fact) ) ); // e+(e1-e2)// + + result_fx.y_fx = BASOP_Util_Divide3232_Scale_cadence( p.y_fx, length_fx, &scale ); + y_qfact = ( 31 - ( scale + ( q_len - p.q_fact) ) ); + + result_fx.z_fx = BASOP_Util_Divide3232_Scale_cadence( p.z_fx, length_fx, &scale ); + z_qfact = ( 31 - ( scale + ( q_len - p.q_fact) ) ); + + q_result = s_min(s_min(x_qfact, y_qfact), z_qfact); + result_fx.x_fx = L_shr(result_fx.x_fx, sub(x_qfact, q_result)); + result_fx.y_fx = L_shr(result_fx.y_fx, sub(y_qfact, q_result)); + result_fx.z_fx = L_shr(result_fx.z_fx, sub(z_qfact, q_result)); + result_fx.q_fact = q_result; return result_fx; } @@ -931,15 +790,16 @@ static void VectorRotationToQuaternion_fx( { IVAS_VECTOR3 cross_product_fx, p1_normalized_fx = { 0 }, p2_normalized_fx = { 0 }; Word32 dot_product_fx; - Word16 q_dot; + Word16 q_dot, e_add, q_result; 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 ); + // dot & cross product are same q// Word32 comp_fx = 0; - Word16 comp_e, check_flag, result_e = 0; + Word16 comp_e, check_flag; f2me( -0.999999f, &comp_fx, &comp_e ); IF( GT_32( dot_product_fx, 0 ) ) { @@ -947,7 +807,7 @@ static void VectorRotationToQuaternion_fx( } ELSE { - check_flag = BASOP_Util_Cmp_Mant32Exp( comp_fx, comp_e, dot_product_fx, 31 - q_dot ); + check_flag = BASOP_Util_Cmp_Mant32Exp( comp_fx, comp_e, dot_product_fx, sub( 31, q_dot ) ); } IF( EQ_16( check_flag, 1 ) ) { @@ -955,19 +815,22 @@ static void VectorRotationToQuaternion_fx( 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; + r->q_fact = 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; + r->w_fx = BASOP_Util_Add_Mant32Exp( dot_product_fx, sub( 31, q_dot ), ONE_IN_Q31, 0, &e_add ); + q_result = sub(s_min( sub( 31, e_add ), q_dot ),1);//gaurd bits// + r->x_fx = L_shr( r->x_fx, sub( q_dot, q_result ) ); + r->y_fx = L_shr( r->y_fx, sub( q_dot, q_result ) ); + r->z_fx = L_shr( r->z_fx, sub( q_dot, q_result ) ); + r->w_fx = L_shr( r->w_fx, sub( sub( 31, e_add ), q_result ) ); + + r->q_fact = q_result; } QuaternionNormalize_fx( *r, r ); @@ -1025,10 +888,9 @@ ivas_error ivas_orient_trk_Init_fx( 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; + identity_fx.w_fx = ONE_IN_Q31; + identity_fx.x_fx = identity_fx.y_fx = identity_fx.z_fx = 0; + identity_fx.q_fact = Q31; /* configuration parameters */ pOTR->centerAdaptationRate_fx = C_ADP_RATE_Q31; @@ -1050,10 +912,10 @@ ivas_error ivas_orient_trk_Init_fx( /* set safe default tracking mode */ pOTR->orientation_tracking = IVAS_HEAD_ORIENT_TRK_NONE; pOTR->alpha = me2f(pOTR->alpha_fx, 0 ); - pOTR->refRot.w = pOTR->absAvgRot.w = pOTR->trkRot.w = fix_to_float(pOTR->trkRot.w_fx, pOTR->trkRot.w_qfact ); - pOTR->refRot.x = pOTR->absAvgRot.x = pOTR->trkRot.x = fix_to_float(pOTR->trkRot.x_fx, pOTR->trkRot.x_qfact ); - pOTR->refRot.y = pOTR->absAvgRot.y = pOTR->trkRot.y = fix_to_float(pOTR->trkRot.y_fx, pOTR->trkRot.y_qfact ); - pOTR->refRot.z = pOTR->absAvgRot.z = pOTR->trkRot.z = fix_to_float(pOTR->trkRot.z_fx, pOTR->trkRot.z_qfact ); + pOTR->refRot.w = pOTR->absAvgRot.w = pOTR->trkRot.w = fix_to_float(pOTR->trkRot.w_fx, pOTR->trkRot.q_fact); + pOTR->refRot.x = pOTR->absAvgRot.x = pOTR->trkRot.x = fix_to_float(pOTR->trkRot.x_fx, pOTR->trkRot.q_fact); + pOTR->refRot.y = pOTR->absAvgRot.y = pOTR->trkRot.y = fix_to_float(pOTR->trkRot.y_fx, pOTR->trkRot.q_fact); + pOTR->refRot.z = pOTR->absAvgRot.z = pOTR->trkRot.z = fix_to_float(pOTR->trkRot.z_fx, pOTR->trkRot.q_fact); return IVAS_ERR_OK; } @@ -1322,12 +1184,12 @@ 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 ) + SWITCH( pOTR->orientation_tracking ) { case IVAS_HEAD_ORIENT_TRK_NONE: case IVAS_HEAD_ORIENT_TRK_REF: @@ -1338,22 +1200,25 @@ ivas_error ivas_orient_trk_SetReferenceVector_fx( 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; + listenerPosLevel.q_fact = listenerPos.q_fact; + refPosLevel.x_fx = refPos.x_fx; - refPosLevel.x_qfact = refPos.x_qfact; refPosLevel.y_fx = refPos.y_fx; - refPosLevel.y_qfact = refPos.y_qfact; + Word16 q_min = s_min( listenerPos.q_fact, refPos.q_fact ); + refPosLevel.x_fx = L_shr( refPosLevel.x_fx, sub( refPos.q_fact, q_min ) ); + refPosLevel.y_fx = L_shr( refPosLevel.y_fx, sub( refPos.q_fact, q_min ) ); + refPosLevel.z_fx = L_shr( refPosLevel.z_fx, sub( listenerPos.q_fact, q_min ) ); + refPosLevel.q_fact = q_min; + acousticFrontVector = VectorSubtract_fx( listenerPosLevel, refPosLevel ); BREAK; default: return IVAS_ERR_WRONG_PARAMS; } - acousticFrontVectorLength = VectorLength_fx( acousticFrontVector, &acousticFrontVector_q ); + acousticFrontVectorLength = VectorLength_fx( acousticFrontVector, &acousticFrontVector.q_fact ); /* if the length is zero, the user has entered insensible listener and reference positions */ IF( LE_32( acousticFrontVectorLength, 0 ) ) { @@ -1363,7 +1228,7 @@ ivas_error ivas_orient_trk_SetReferenceVector_fx( 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; + ivasForwardVector.q_fact = Q31; VectorRotationToQuaternion_fx( ivasForwardVector, acousticFrontVector, &pOTR->refRot ); return IVAS_ERR_OK; @@ -1483,22 +1348,22 @@ ivas_error ivas_orient_trk_Process_fx( Word32 cutoffFrequency_fx, cutoff_prod; Word16 q_cutoff_prod = 0; Word32 alpha_fx = float_to_fix( alpha, Q30 ); - pOTR->refRot.w_qfact = pOTR->refRot.x_qfact = pOTR->refRot.y_qfact = pOTR->refRot.z_qfact = Q29; - absRot.w_qfact = absRot.x_qfact = absRot.y_qfact = absRot.z_qfact = Q29; - pOTR->absAvgRot.w_qfact = pOTR->absAvgRot.x_qfact = pOTR->absAvgRot.y_qfact = pOTR->absAvgRot.z_qfact = Q29; + pOTR->refRot.q_fact = pOTR->refRot.q_fact = pOTR->refRot.q_fact = pOTR->refRot.q_fact = Q29; + absRot.q_fact = absRot.q_fact = absRot.q_fact = absRot.q_fact = Q29; + pOTR->absAvgRot.q_fact = pOTR->absAvgRot.q_fact = pOTR->absAvgRot.q_fact = pOTR->absAvgRot.q_fact = Q29; updateRate_fx = float_to_fix( updateRate, Q23 ); // value is 200// - absRot.w_fx = float_to_fix( absRot.w, absRot.w_qfact ); - absRot.x_fx = float_to_fix( absRot.x, absRot.x_qfact ); - absRot.y_fx = float_to_fix( absRot.y, absRot.y_qfact ); - absRot.z_fx = float_to_fix( absRot.z, absRot.z_qfact ); - pOTR->refRot.w_fx = float_to_fix( pOTR->refRot.w, pOTR->refRot.w_qfact ); - pOTR->refRot.x_fx = float_to_fix( pOTR->refRot.x, pOTR->refRot.x_qfact ); - pOTR->refRot.y_fx = float_to_fix( pOTR->refRot.y, pOTR->refRot.y_qfact ); - pOTR->refRot.z_fx = float_to_fix( pOTR->refRot.z, pOTR->refRot.z_qfact ); - pOTR->absAvgRot.w_fx = float_to_fix( pOTR->absAvgRot.w, pOTR->absAvgRot.w_qfact ); - pOTR->absAvgRot.x_fx = float_to_fix( pOTR->absAvgRot.x, pOTR->absAvgRot.x_qfact ); - pOTR->absAvgRot.y_fx = float_to_fix( pOTR->absAvgRot.y, pOTR->absAvgRot.y_qfact ); - pOTR->absAvgRot.z_fx = float_to_fix( pOTR->absAvgRot.z, pOTR->absAvgRot.z_qfact ); + absRot.w_fx = float_to_fix( absRot.w, absRot.q_fact); + absRot.x_fx = float_to_fix( absRot.x, absRot.q_fact); + absRot.y_fx = float_to_fix( absRot.y, absRot.q_fact); + absRot.z_fx = float_to_fix( absRot.z, absRot.q_fact); + pOTR->refRot.w_fx = float_to_fix( pOTR->refRot.w, pOTR->refRot.q_fact); + pOTR->refRot.x_fx = float_to_fix( pOTR->refRot.x, pOTR->refRot.q_fact); + pOTR->refRot.y_fx = float_to_fix( pOTR->refRot.y, pOTR->refRot.q_fact); + pOTR->refRot.z_fx = float_to_fix( pOTR->refRot.z, pOTR->refRot.q_fact); + pOTR->absAvgRot.w_fx = float_to_fix( pOTR->absAvgRot.w, pOTR->absAvgRot.q_fact); + pOTR->absAvgRot.x_fx = float_to_fix( pOTR->absAvgRot.x, pOTR->absAvgRot.q_fact); + pOTR->absAvgRot.y_fx = float_to_fix( pOTR->absAvgRot.y, pOTR->absAvgRot.q_fact); + pOTR->absAvgRot.z_fx = float_to_fix( pOTR->absAvgRot.z, pOTR->absAvgRot.q_fact); IF ( pOTR == NULL || pTrkRot == NULL ) { @@ -1587,14 +1452,14 @@ ivas_error ivas_orient_trk_Process_fx( IF ( result == IVAS_ERR_OK ) { - pOTR->trkRot.w = me2f( pOTR->trkRot.w_fx, 31 - pOTR->trkRot.w_qfact ); - pOTR->trkRot.x = me2f( pOTR->trkRot.x_fx, 31 - pOTR->trkRot.x_qfact ); - pOTR->trkRot.y = me2f( pOTR->trkRot.y_fx, 31 - pOTR->trkRot.y_qfact ); - pOTR->trkRot.z = me2f( pOTR->trkRot.z_fx, 31 - pOTR->trkRot.z_qfact ); - pOTR->absAvgRot.w = me2f( pOTR->absAvgRot.w_fx, 31 - pOTR->absAvgRot.w_qfact ); - pOTR->absAvgRot.x = me2f( pOTR->absAvgRot.x_fx, 31 - pOTR->absAvgRot.x_qfact ); - pOTR->absAvgRot.y = me2f( pOTR->absAvgRot.y_fx, 31 - pOTR->absAvgRot.y_qfact ); - pOTR->absAvgRot.z = me2f( pOTR->absAvgRot.z_fx, 31 - pOTR->absAvgRot.z_qfact ); + pOTR->trkRot.w = me2f( pOTR->trkRot.w_fx, 31 - pOTR->trkRot.q_fact ); + pOTR->trkRot.x = me2f( pOTR->trkRot.x_fx, 31 - pOTR->trkRot.q_fact); + pOTR->trkRot.y = me2f( pOTR->trkRot.y_fx, 31 - pOTR->trkRot.q_fact); + pOTR->trkRot.z = me2f( pOTR->trkRot.z_fx, 31 - pOTR->trkRot.q_fact); + pOTR->absAvgRot.w = me2f( pOTR->absAvgRot.w_fx, 31 - pOTR->absAvgRot.q_fact); + pOTR->absAvgRot.x = me2f( pOTR->absAvgRot.x_fx, 31 - pOTR->absAvgRot.q_fact); + pOTR->absAvgRot.y = me2f( pOTR->absAvgRot.y_fx, 31 - pOTR->absAvgRot.q_fact); + pOTR->absAvgRot.z = me2f( pOTR->absAvgRot.z_fx, 31 - pOTR->absAvgRot.q_fact); *pTrkRot = pOTR->trkRot; } diff --git a/lib_rend/ivas_rotation.c b/lib_rend/ivas_rotation.c index 7a9db2c79..0073b9b85 100644 --- a/lib_rend/ivas_rotation.c +++ b/lib_rend/ivas_rotation.c @@ -1712,20 +1712,15 @@ ivas_error ivas_combined_orientation_open( origo.x = origo.y = origo.z = 0.0f; #ifdef IVAS_FLOAT_FIXED identity.w_fx = ONE_IN_Q31; - identity.w_qfact = 31; identity.x_fx = 0; - identity.x_qfact = 31; identity.y_fx = 0; - identity.y_qfact = 31; identity.z_fx = 0; - identity.z_qfact = 31; + identity.q_fact = 31; origo.x_fx = 0; - origo.x_qfact = 31; origo.y_fx = 0; - origo.y_qfact = 31; origo.z_fx = 0; - origo.z_qfact = 31; + origo.q_fact = 31; #endif /* Allocate handle */ @@ -2014,15 +2009,15 @@ ivas_error combine_external_and_head_orientations( hCombinedOrientationData->Quaternions_ext_interpolation_target.y_fx = float_to_fix(hCombinedOrientationData->Quaternions_ext_interpolation_target.y, Q29); hCombinedOrientationData->Quaternions_ext_interpolation_target.z_fx = float_to_fix(hCombinedOrientationData->Quaternions_ext_interpolation_target.z, Q29); - hCombinedOrientationData->Quaternions_ext_interpolation_start.w_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_start.x_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_start.y_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_start.z_qfact = Q29; - hCombinedOrientationData->Quaternions_ext_interpolation_target.w_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_target.x_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_target.y_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_target.z_qfact = Q29; + hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact = Q29; + hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact = Q29; - QuaternionSlerp_fx(hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i]); + QuaternionSlerp_fx(hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient_fx, &hCombinedOrientationData->Quaternions[i]); - hCombinedOrientationData->Quaternions[i].w = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].w_fx, hCombinedOrientationData->Quaternions[i].w_qfact); - hCombinedOrientationData->Quaternions[i].x = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].x_fx, hCombinedOrientationData->Quaternions[i].x_qfact); - hCombinedOrientationData->Quaternions[i].y = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].y_fx, hCombinedOrientationData->Quaternions[i].y_qfact); - hCombinedOrientationData->Quaternions[i].z = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].z_fx, hCombinedOrientationData->Quaternions[i].z_qfact); + hCombinedOrientationData->Quaternions[i].w = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].w_fx, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].x = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].x_fx, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].y = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].y_fx, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].z = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].z_fx, hCombinedOrientationData->Quaternions[i].q_fact); #else QuaternionSlerp(hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i]); @@ -2199,14 +2194,11 @@ ivas_error combine_external_and_head_orientations( for (i = 0; i < hCombinedOrientationData->num_subframes; i++) { - hCombinedOrientationData->Quaternions[i].w_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].w); - hCombinedOrientationData->Quaternions[i].w_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].w, hCombinedOrientationData->Quaternions[i].w_qfact); - hCombinedOrientationData->Quaternions[i].x_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].x); - hCombinedOrientationData->Quaternions[i].x_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].x, hCombinedOrientationData->Quaternions[i].x_qfact); - hCombinedOrientationData->Quaternions[i].y_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].y); - hCombinedOrientationData->Quaternions[i].y_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].y, hCombinedOrientationData->Quaternions[i].y_qfact); - hCombinedOrientationData->Quaternions[i].z_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].z); - hCombinedOrientationData->Quaternions[i].z_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].z, hCombinedOrientationData->Quaternions[i].z_qfact); + hCombinedOrientationData->Quaternions[i].q_fact = s_min(s_min(Q_factor_L(hCombinedOrientationData->Quaternions[i].w), Q_factor_L(hCombinedOrientationData->Quaternions[i].x)), s_min(Q_factor_L(hCombinedOrientationData->Quaternions[i].y), Q_factor_L(hCombinedOrientationData->Quaternions[i].z))); + hCombinedOrientationData->Quaternions[i].w_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].w, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].x_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].x, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].y_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].y, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].z_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].z, hCombinedOrientationData->Quaternions[i].q_fact); } #endif return IVAS_ERR_OK; @@ -2289,15 +2281,15 @@ static void external_target_interpolation( hCombinedOrientationData->Quaternions_ext_interpolation_target.y_fx = float_to_fix(hCombinedOrientationData->Quaternions_ext_interpolation_target.y, Q29); hCombinedOrientationData->Quaternions_ext_interpolation_target.z_fx = float_to_fix(hCombinedOrientationData->Quaternions_ext_interpolation_target.z, Q29); - hCombinedOrientationData->Quaternions_ext_interpolation_start.w_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_start.x_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_start.y_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_start.z_qfact = Q29; - hCombinedOrientationData->Quaternions_ext_interpolation_target.w_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_target.x_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_target.y_qfact = hCombinedOrientationData->Quaternions_ext_interpolation_target.z_qfact = Q29; + hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact = Q29; + hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact = Q29; - QuaternionSlerp_fx(hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i]); + QuaternionSlerp_fx(hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient_fx, &hCombinedOrientationData->Quaternions[i]); - hCombinedOrientationData->Quaternions[i].w = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].w_fx, hCombinedOrientationData->Quaternions[i].w_qfact); - hCombinedOrientationData->Quaternions[i].x = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].x_fx, hCombinedOrientationData->Quaternions[i].x_qfact); - hCombinedOrientationData->Quaternions[i].y = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].y_fx, hCombinedOrientationData->Quaternions[i].y_qfact); - hCombinedOrientationData->Quaternions[i].z = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].z_fx, hCombinedOrientationData->Quaternions[i].z_qfact); + hCombinedOrientationData->Quaternions[i].w = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].w_fx, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].x = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].x_fx, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].y = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].y_fx, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].z = fixedToFloat_32(hCombinedOrientationData->Quaternions[i].z_fx, hCombinedOrientationData->Quaternions[i].q_fact); #else QuaternionSlerp(hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i]); @@ -2314,14 +2306,11 @@ static void external_target_interpolation( } #ifdef IVAS_FLOAT_FIXED /* Updating the fixed point values which will be used later */ - hCombinedOrientationData->Quaternions[i].w_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].w); - hCombinedOrientationData->Quaternions[i].w_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].w, hCombinedOrientationData->Quaternions[i].w_qfact); - hCombinedOrientationData->Quaternions[i].x_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].x); - hCombinedOrientationData->Quaternions[i].x_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].x, hCombinedOrientationData->Quaternions[i].x_qfact); - hCombinedOrientationData->Quaternions[i].y_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].y); - hCombinedOrientationData->Quaternions[i].y_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].y, hCombinedOrientationData->Quaternions[i].y_qfact); - hCombinedOrientationData->Quaternions[i].z_qfact = Q_factor_L(hCombinedOrientationData->Quaternions[i].z); - hCombinedOrientationData->Quaternions[i].z_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].z, hCombinedOrientationData->Quaternions[i].z_qfact); + hCombinedOrientationData->Quaternions[i].q_fact = s_min(s_min(Q_factor_L(hCombinedOrientationData->Quaternions[i].w), Q_factor_L(hCombinedOrientationData->Quaternions[i].x)), s_min(Q_factor_L(hCombinedOrientationData->Quaternions[i].y), Q_factor_L(hCombinedOrientationData->Quaternions[i].z))); + hCombinedOrientationData->Quaternions[i].w_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].w, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].x_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].x, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].y_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].y, hCombinedOrientationData->Quaternions[i].q_fact); + hCombinedOrientationData->Quaternions[i].z_fx = float_to_fix(hCombinedOrientationData->Quaternions[i].z, hCombinedOrientationData->Quaternions[i].q_fact); #endif return; diff --git a/lib_rend/lib_rend.c b/lib_rend/lib_rend.c index 98730d630..c49d837e5 100644 --- a/lib_rend/lib_rend.c +++ b/lib_rend/lib_rend.c @@ -476,7 +476,7 @@ static IVAS_QUATERNION quaternionInit_fx( move32(); move32(); - q.w_qfact = q.x_qfact = q.y_qfact = q.z_qfact = Q29; + q.q_fact = Q29; move16(); move16(); move16(); @@ -7141,16 +7141,16 @@ ivas_error IVAS_REND_SetReferenceRotation( #ifdef IVAS_FLOAT_FIXED IVAS_QUATERNION 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 ); + refRot_fx.q_fact = Q29; + refRot_fx.w_fx = (Word32) float_to_fix( refRot.w, refRot_fx.q_fact); + refRot_fx.x_fx = (Word32) float_to_fix( refRot.x, refRot_fx.q_fact); + refRot_fx.y_fx = (Word32) float_to_fix( refRot.y, refRot_fx.q_fact); + refRot_fx.z_fx = (Word32) float_to_fix( refRot.z, refRot_fx.q_fact); error = ivas_orient_trk_SetReferenceRotation_fx( hIvasRend->headRotData.hOrientationTracker, refRot_fx ); - hIvasRend->headRotData.hOrientationTracker->refRot.w = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.w_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.w_qfact ); - hIvasRend->headRotData.hOrientationTracker->refRot.x = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.x_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.x_qfact ); - hIvasRend->headRotData.hOrientationTracker->refRot.y = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.y_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.y_qfact ); - hIvasRend->headRotData.hOrientationTracker->refRot.z = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.z_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.z_qfact ); + hIvasRend->headRotData.hOrientationTracker->refRot.w = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.w_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.q_fact); + hIvasRend->headRotData.hOrientationTracker->refRot.x = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.x_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.q_fact); + hIvasRend->headRotData.hOrientationTracker->refRot.y = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.y_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.q_fact); + hIvasRend->headRotData.hOrientationTracker->refRot.z = me2f( hIvasRend->headRotData.hOrientationTracker->refRot.z_fx, 31 - hIvasRend->headRotData.hOrientationTracker->refRot.q_fact); if ( error != IVAS_ERR_OK ) { -- GitLab