Merge branch '2199_ivas_rotation_instrumentation' into 'main' (02b92de6) · Commits · SA4 / Audio / IVAS BASOP

lib_rend/ivas_rotation_fx.c

+152 −13

Original line number	Diff line number	Diff line
		@@ -109,7 +109,8 @@ static Word16 Q_factor_L_32(
		{
		Word16 Q = 31;

		if ( x >= 1 \|\| x <= -1 )
		test();
		IF( GE_32( x, 1 ) \|\| LE_32( x, -1 ) )
		{
		Q = norm_l( L_abs( (Word32) x ) );
		}
		@@ -220,7 +221,7 @@ void QuatToRotMat_fx(
		// Euler2Quat_fx( deg2rad( quat.x_fx ), deg2rad( quat.y_fx ), deg2rad( quat.z_fx ), &quat_local );
		// QuatToRotMat_fx( quat_local, Rmat );
		}
		else
		ELSE
		{
		Word32 w = quat.w_fx; // Qx
		move32();
		@@ -1402,7 +1403,17 @@ ivas_error ivas_combined_orientation_open_fx(
		( *hCombinedOrientationData )->isInterpolationOngoing = FALSE;
		move16();
		( *hCombinedOrientationData )->Quaternions_ext_interpolation_start = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		( *hCombinedOrientationData )->Quaternions_ext_interpolation_target = identity;
		move32();
		move32();
		move32();
		move32();
		move16();

		/* Initialise orientations to identity */
		FOR( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
		@@ -1410,7 +1421,17 @@ ivas_error ivas_combined_orientation_open_fx(
		( *hCombinedOrientationData )->enableCombinedOrientation[i] = 0;
		move16();
		( *hCombinedOrientationData )->Quaternions[i] = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		( *hCombinedOrientationData )->listenerPos[i] = origo;
		move32();
		move32();
		move32();
		move32();
		move16();

		FOR( j = 0; j < 3; j++ )
		{
		@@ -1434,10 +1455,24 @@ ivas_error ivas_combined_orientation_open_fx(
		move32();
		move16();

		( *hCombinedOrientationData )
		->Quaternion_prev_extOrientation = identity;
		( *hCombinedOrientationData )->Quaternion_prev_extOrientation = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		( *hCombinedOrientationData )->Quaternion_frozen_ext = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		( *hCombinedOrientationData )->Quaternion_frozen_head = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		set_zero_fx( ( *hCombinedOrientationData )->chEneIIR_fx[0], MASA_FREQUENCY_BANDS );
		set_zero_fx( ( *hCombinedOrientationData )->chEneIIR_fx[1], MASA_FREQUENCY_BANDS );
		set_zero_fx( ( *hCombinedOrientationData )->procChEneIIR_fx[0], MASA_FREQUENCY_BANDS );
		@@ -1516,10 +1551,12 @@ ivas_error combine_external_and_head_orientations_dec(
		pHeadRotQuaternion = hHeadTrackData->Quaternions;
		listenerPos = hHeadTrackData->Pos;
		sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
		move32();
		}
		ELSE
		{
		sr_pose_pred_axis = DEFAULT_AXIS;
		move32();
		}

		return combine_external_and_head_orientations( pHeadRotQuaternion, listenerPos, sr_pose_pred_axis, hExtOrientationData, hCombinedOrientationData );
		@@ -1544,12 +1581,14 @@ ivas_error combine_external_and_head_orientations_rend(
		Word16 i;

		sr_pose_pred_axis = DEFAULT_AXIS;
		move32();

		IF( hHeadTrackData->headRotEnabled )
		{
		headRotQuaternions = hHeadTrackData->headPositions;
		listenerPos = hHeadTrackData->Pos;
		sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
		move32();
		}
		ELSE IF( hExtOrientationData != NULL )
		{
		@@ -1631,13 +1670,32 @@ ivas_error combine_external_and_head_orientations(
		hCombinedOrientationData->interpolationIncrement_fx = ONE_IN_Q30;
		move32();
		hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		hCombinedOrientationData->Quaternions_ext_interpolation_target = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
		{
		hCombinedOrientationData->enableCombinedOrientation[i] = 0;
		move16();
		hCombinedOrientationData->Quaternions[i] = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		hCombinedOrientationData->listenerPos[i] = origo;
		move32();
		move32();
		move32();
		move16();

		FOR( j = 0; j < 3; j++ )
		{
		@@ -1701,6 +1759,11 @@ ivas_error combine_external_and_head_orientations(
		IF( NE_16( hCombinedOrientationData->isExtOrientationFrozen, 1 ) )
		{
		hCombinedOrientationData->Quaternion_frozen_ext = hExtOrientationData->Quaternions[i];
		move32();
		move32();
		move32();
		move32();
		move16();
		hCombinedOrientationData->isExtOrientationFrozen = 1;
		move16();
		}
		@@ -1708,11 +1771,16 @@ ivas_error combine_external_and_head_orientations(
		ELSE
		{
		hCombinedOrientationData->Quaternion_frozen_ext = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		hCombinedOrientationData->isExtOrientationFrozen = 0;
		move16();
		}
		test();
		IF( EQ_16( hExtOrientationData->enableRotationInterpolation[i], 1 ) && hExtOrientationData->enableExternalOrientation[i] > 0 )
		IF( EQ_16( hExtOrientationData->enableRotationInterpolation[i], 1 ) && GT_16( hExtOrientationData->enableExternalOrientation[i], 0 ) )
		{
		test();
		test();
		@@ -1778,11 +1846,21 @@ ivas_error combine_external_and_head_orientations(
		IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 1 ) )
		{
		hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		/* Use the freezed external orientation */
		ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
		{
		hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_ext;
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		}
		}
		@@ -1800,6 +1878,11 @@ ivas_error combine_external_and_head_orientations(
		IF( NE_16( hCombinedOrientationData->isHeadRotationFrozen, 1 ) )
		{
		hCombinedOrientationData->Quaternion_frozen_head = headRotQuaternions[i];
		move32();
		move32();
		move32();
		move32();
		move16();
		hCombinedOrientationData->isHeadRotationFrozen = 1;
		move16();
		}
		@@ -1807,12 +1890,22 @@ ivas_error combine_external_and_head_orientations(
		ELSE
		{
		hCombinedOrientationData->Quaternion_frozen_head = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		hCombinedOrientationData->isHeadRotationFrozen = 0;
		move16();
		}

		#ifdef RTP_S4_251135_CR26253_0016_REV1
		/* Disable head rotation if diegetic PI data indicating non-diegetic audio is received */
		test();
		test();
		test();
		test();
		test();
		IF( hCombinedOrientationData->isDiegeticInputPISet && !hCombinedOrientationData->isDiegeticInputPI[0] && !hCombinedOrientationData->isDiegeticInputPI[1] && !hCombinedOrientationData->isDiegeticInputPI[2] && !hCombinedOrientationData->isDiegeticInputPI[3] && !hCombinedOrientationData->isDiegeticInputPI[4] )
		{
		CONTINUE;
		@@ -1882,9 +1975,14 @@ ivas_error combine_external_and_head_orientations(
		#endif
		/* Reset the combined orientations to identity */
		test();
		if ( hExtOrientationData->enableHeadRotation[i] == 0 && hExtOrientationData->enableExternalOrientation[i] == 0 )
		IF( hExtOrientationData->enableHeadRotation[i] == 0 && hExtOrientationData->enableExternalOrientation[i] == 0 )
		{
		hCombinedOrientationData->Quaternions[i] = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		}
		}
		@@ -1902,13 +2000,23 @@ ivas_error combine_external_and_head_orientations(
		/* Save the current orientations */
		IF( hExtOrientationData != NULL )
		{
		IF( hExtOrientationData->enableExternalOrientation[hExtOrientationData->num_subframes - 1] > 0 )
		IF( GT_16( hExtOrientationData->enableExternalOrientation[hExtOrientationData->num_subframes - 1], 0 ) )
		{
		hCombinedOrientationData->Quaternion_prev_extOrientation = hExtOrientationData->Quaternions[hExtOrientationData->num_subframes - 1];
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		ELSE
		{
		hCombinedOrientationData->Quaternion_prev_extOrientation = identity;
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		}

		@@ -1917,6 +2025,10 @@ ivas_error combine_external_and_head_orientations(
		FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
		{
		hCombinedOrientationData->listenerPos[i] = listenerPos[i];
		move32();
		move32();
		move32();
		move16();
		}
		}

		@@ -2012,6 +2124,7 @@ ivas_error combine_external_and_head_orientations(
		}

		hCombinedOrientationData->sr_pose_pred_axis = sr_pose_pred_axis;
		move32();
		#ifdef IVAS_RTPDUMP

		/* Reset external orientations */
		@@ -2058,42 +2171,68 @@ static void external_target_interpolation_fx(

		/* Set the received orientation as the target */
		hCombinedOrientationData->Quaternions_ext_interpolation_target = hExtOrientationData->Quaternions[i];
		move32();
		move32();
		move32();
		move32();
		move16();

		/* Use the most recent external orientation as the starting orientation */
		IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 1 ) )
		{
		IF( i > 0 )
		IF( GT_16( i, 0 ) )
		{
		IF( hExtOrientationData->enableExternalOrientation[i - 1] == 0 )
		{
		IVAS_QUATERNION identity;
		identity.w_fx = ONE_IN_Q31;
		/* Set to identity */
		hCombinedOrientationData->Quaternions_ext_interpolation_start.w_fx = ONE_IN_Q31;
		hCombinedOrientationData->Quaternions_ext_interpolation_start.x_fx = 0;
		hCombinedOrientationData->Quaternions_ext_interpolation_start.y_fx = 0;
		hCombinedOrientationData->Quaternions_ext_interpolation_start.z_fx = 0;
		hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact = 31;
		move32();
		identity.x_fx = identity.y_fx = identity.z_fx = 0;
		move32();
		move32();
		move32();
		identity.q_fact = 31;
		move16();
		hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
		}
		ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i - 1], 2 ) )
		{
		hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		ELSE
		{
		hCombinedOrientationData->Quaternions_ext_interpolation_start = hExtOrientationData->Quaternions[i - 1];
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		}
		ELSE
		{
		hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_prev_extOrientation;
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		}
		ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
		{
		hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
		move32();
		move32();
		move32();
		move32();
		move16();
		}
		Word16 tmp_e = 0;
		move16();