Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : #include "ivas_cnst.h"
34 : #include <assert.h>
35 : #include <stdint.h>
36 : #include "options.h"
37 : #include <math.h>
38 : #include "cnst.h"
39 : #include "prot_fx.h"
40 : #include "ivas_prot_fx.h"
41 : #ifdef DEBUGGING
42 : #include "debug.h"
43 : #endif
44 : #include "wmc_auto.h"
45 : #include "basop_util.h"
46 : #include "enh64.h"
47 : /*-------------------------------------------------------------------------
48 : * Euler2Quat()
49 : *
50 : * Calculate corresponding Quaternion from Euler angles in radians
51 : *------------------------------------------------------------------------*/
52 0 : void Euler2Quat_fx(
53 : const Word32 yaw, /* i : yaw (x) Q22 */
54 : const Word32 pitch, /* i : pitch (y) Q22 */
55 : const Word32 roll, /* i : roll (z) Q22 */
56 : IVAS_QUATERNION *quat /* o : quaternion describing the rotation Q22*/
57 : )
58 : {
59 0 : Word16 cr = getCosWord16( extract_l( L_shr_r( roll, 10 ) ) ); // Q14
60 0 : Word16 sr = shr( getSinWord16( extract_l( L_shr_r( roll, 10 ) ) ), 1 ); // Q14
61 0 : Word16 cp = getCosWord16( extract_l( L_shr_r( pitch, 10 ) ) );
62 0 : Word16 sp = shr( getSinWord16( extract_l( L_shr_r( pitch, 10 ) ) ), 1 ); // Q14
63 0 : Word16 cy = getCosWord16( extract_l( L_shr_r( yaw, 10 ) ) );
64 0 : Word16 sy = shr( getSinWord16( extract_l( L_shr_r( yaw, 10 ) ) ), 1 ); // Q14
65 0 : quat->w_fx = L_shr_r( L_add( Mpy_32_16_1( L_mult0( cr, cp ), cy ), Mpy_32_16_1( L_mult0( sr, sp ), sy ) ), 5 ); // Q22
66 0 : move32();
67 0 : quat->x_fx = L_shr_r( L_sub( Mpy_32_16_1( L_mult0( sr, cp ), cy ), Mpy_32_16_1( L_mult0( cr, sp ), sy ) ), 5 ); // Q22
68 0 : move32();
69 0 : quat->y_fx = L_shr_r( L_add( Mpy_32_16_1( L_mult0( sr, cp ), sy ), Mpy_32_16_1( L_mult0( cr, sp ), cy ) ), 5 ); // Q22
70 0 : move32();
71 0 : quat->z_fx = L_shr_r( L_sub( Mpy_32_16_1( L_mult0( cr, cp ), sy ), Mpy_32_16_1( L_mult0( sr, sp ), cy ) ), 5 ); // Q22
72 0 : move32();
73 :
74 0 : quat->q_fact = Q22;
75 0 : move16();
76 :
77 0 : return;
78 : }
79 :
80 :
81 : /*-------------------------------------------------------------------------
82 : * Copy_Quat_fx()
83 : *
84 : * Quaternion q factor modification
85 : *------------------------------------------------------------------------*/
86 0 : void Copy_Quat_fx(
87 : const IVAS_QUATERNION *in_quat, /* i : quaternion describing the rotation */
88 : IVAS_QUATERNION *out_quat /* i : quaternion describing the rotation */
89 : )
90 : {
91 0 : out_quat->q_fact = in_quat->q_fact;
92 0 : out_quat->w_fx = in_quat->w_fx;
93 0 : out_quat->x_fx = in_quat->x_fx;
94 0 : out_quat->y_fx = in_quat->y_fx;
95 0 : out_quat->z_fx = in_quat->z_fx;
96 0 : move16();
97 0 : move32();
98 0 : move32();
99 0 : move32();
100 0 : move32();
101 :
102 0 : return;
103 : }
104 :
105 : /*-------------------------------------------------------------------------
106 : * Scale_Quat_fx()
107 : *
108 : * Quaternion q factor modification
109 : *------------------------------------------------------------------------*/
110 0 : void modify_Quat_q_fx(
111 : const IVAS_QUATERNION *in_quat, /* i : quaternion describing the rotation */
112 : IVAS_QUATERNION *out_quat, /* i : quaternion describing the rotation */
113 : Word16 q_new /* i : quaternion describing the rotation */
114 : )
115 : {
116 0 : out_quat->w_fx = L_shl_sat( in_quat->w_fx, sub( q_new, in_quat->q_fact ) ); // q_new
117 0 : out_quat->x_fx = L_shl_sat( in_quat->x_fx, sub( q_new, in_quat->q_fact ) ); // q_new
118 0 : out_quat->y_fx = L_shl_sat( in_quat->y_fx, sub( q_new, in_quat->q_fact ) ); // q_new
119 0 : out_quat->z_fx = L_shl_sat( in_quat->z_fx, sub( q_new, in_quat->q_fact ) ); // q_new
120 0 : out_quat->q_fact = q_new;
121 0 : return;
122 : }
123 :
124 : /*-------------------------------------------------------------------------
125 : * modify_Rmat_q_fx()
126 : *
127 : * Rotation matrix q factor modification
128 : *------------------------------------------------------------------------*/
129 0 : void modify_Rmat_q_fx(
130 : Word32 Rmat_in[3][3], /* i : real-space rotation matrix for this rotation */
131 : Word32 Rmat_out[3][3], /* o : real-space rotation matrix for this rotation*/
132 : Word16 q_cur, /* i : current q factor for rotation matrix */
133 : Word16 q_new /* i : target q factor for rotation matrix */
134 : )
135 : {
136 : Word16 j, k;
137 :
138 0 : FOR( j = 0; j < 3; j++ )
139 : {
140 0 : FOR( k = 0; k < 3; k++ )
141 : {
142 0 : Rmat_out[j][k] = L_shl( Rmat_in[j][k], sub( q_new, q_cur ) );
143 0 : move32();
144 : }
145 : }
146 0 : return;
147 : }
148 :
149 : /*-------------------------------------------------------------------------
150 : * Quat2EulerDegree()
151 : *
152 : * Quaternion handling: calculate corresponding Euler angles in degrees
153 : *------------------------------------------------------------------------*/
154 0 : void Quat2EulerDegree_fx(
155 : const IVAS_QUATERNION quat, /* i : quaternion describing the rotation */
156 : Word32 *yaw_fx, /* o : yaw */
157 : Word32 *pitch_fx, /* o : pitch */
158 : Word32 *roll_fx /* o : roll */
159 : )
160 : {
161 0 : IF( NE_32( quat.w_fx, L_negate( 12582912 ) /*3.0f in Q22*/ ) )
162 : {
163 0 : Word32 tmp1 = W_extract_l( W_shr( W_mult0_32_32( quat.w_fx, quat.x_fx ), Q22 ) ); // Q22
164 0 : Word32 tmp2 = W_extract_l( W_shr( W_mult0_32_32( quat.y_fx, quat.z_fx ), Q22 ) ); // Q22
165 0 : Word32 tmp3 = L_shl( L_add( tmp1, tmp2 ), 1 ); // Q22
166 :
167 0 : Word32 tmp4 = W_extract_l( W_shr( W_mult0_32_32( quat.x_fx, quat.x_fx ), Q22 ) ); // Q22
168 0 : Word32 tmp5 = W_extract_l( W_shr( W_mult0_32_32( quat.y_fx, quat.y_fx ), Q22 ) ); // Q22
169 0 : Word32 tmp6 = L_shl( L_add( tmp4, tmp5 ), 1 );
170 0 : Word32 tmp7 = L_sub( ONE_IN_Q22, tmp6 );
171 :
172 0 : IF( tmp3 >= -2 && tmp3 <= 2 )
173 : {
174 0 : tmp3 = 0;
175 : }
176 0 : IF( tmp7 >= -2 && tmp7 <= 2 )
177 : {
178 0 : tmp7 = 0;
179 : }
180 :
181 0 : Word16 yaw_fx_16 = BASOP_util_atan2( tmp3, tmp7, 0 ); // Q13
182 0 : *yaw_fx = Mpy_32_16_1( 961263669 /*_180_OVER_PI in Q24*/, yaw_fx_16 );
183 :
184 : Word32 p_fx;
185 0 : Word32 tmp8 = W_extract_l( W_shr( W_mult0_32_32( quat.w_fx, quat.y_fx ), Q22 ) ); // Q22
186 0 : Word32 tmp9 = W_extract_l( W_shr( W_mult0_32_32( quat.z_fx, quat.x_fx ), Q22 ) ); // Q22
187 0 : p_fx = L_shl( L_sub( tmp8, tmp9 ), 1 );
188 0 : p_fx = max( L_negate( ONE_IN_Q22 ), min( ONE_IN_Q22, p_fx ) ); // Q22
189 :
190 0 : Word32 p_fx_sq = W_extract_l( W_shr( W_mult0_32_32( p_fx, p_fx ), Q22 ) );
191 0 : Word16 res_exp = 0;
192 0 : Word32 one_minus_p_fx_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( p_fx_sq ), 31 - Q22, &res_exp );
193 0 : Word32 sqrt_one_minus_p_fx_sq = Sqrt32( one_minus_p_fx_sq, &res_exp );
194 :
195 0 : Word16 pitch_fx_16 = BASOP_util_atan2( p_fx, sqrt_one_minus_p_fx_sq, ( 31 - Q22 ) - res_exp );
196 :
197 0 : *pitch_fx = Mpy_32_16_1( 961263669 /*_180_OVER_PI in Q24*/, pitch_fx_16 );
198 :
199 0 : Word32 tmp10 = W_extract_l( W_shr( W_mult0_32_32( quat.w_fx, quat.z_fx ), Q22 ) ); // Q22
200 0 : Word32 tmp11 = W_extract_l( W_shr( W_mult0_32_32( quat.x_fx, quat.y_fx ), Q22 ) ); // Q22
201 0 : Word32 tmp12 = L_shl( L_add( tmp10, tmp11 ), 1 ); // Q22
202 :
203 0 : Word32 tmp13 = W_extract_l( W_shr( W_mult0_32_32( quat.y_fx, quat.y_fx ), Q22 ) ); // Q22
204 0 : Word32 tmp14 = W_extract_l( W_shr( W_mult0_32_32( quat.z_fx, quat.z_fx ), Q22 ) ); // Q22
205 0 : Word32 tmp15 = L_shl( L_add( tmp13, tmp14 ), 1 ); // Q22
206 0 : Word32 tmp16 = L_sub( ONE_IN_Q22, tmp15 ); // Q22
207 :
208 0 : IF( tmp12 >= -2 && tmp12 <= 2 )
209 : {
210 0 : tmp12 = 0;
211 : }
212 0 : IF( tmp16 >= -2 && tmp16 <= 2 )
213 : {
214 0 : tmp16 = 0;
215 : }
216 0 : Word16 roll_fx_16 = BASOP_util_atan2( tmp12, tmp16, 0 ); // Q13
217 0 : *roll_fx = Mpy_32_16_1( 961263669 /*_180_OVER_PI in Q24*/, roll_fx_16 );
218 : }
219 : ELSE
220 : {
221 : /* Euler angles in R_X(roll)*R_Y(pitch)*R_Z(yaw) convention
222 : *
223 : * yaw: rotate scene counter-clockwise in the horizontal plane
224 : * pitch: rotate scene in the median plane, increase elevation with positive values
225 : * roll: rotate scene from the right ear to the top
226 : */
227 0 : *yaw_fx = quat.z_fx;
228 0 : *pitch_fx = quat.y_fx;
229 0 : *roll_fx = quat.x_fx;
230 : }
231 :
232 0 : return;
233 : }
234 :
235 :
236 : /*-------------------------------------------------------------------------
237 : * deg2rad()
238 : *
239 : * Converts degrees to normalized radians
240 : *------------------------------------------------------------------------*/
241 456 : Word32 deg2rad_fx(
242 : Word32 degrees // Q22
243 : )
244 : {
245 456 : WHILE( GE_32( degrees, DEGREE_180 ) )
246 : {
247 0 : degrees = L_sub( degrees, DEGREE_360 );
248 : }
249 456 : WHILE( LE_32( degrees, -DEGREE_180 ) )
250 : {
251 0 : degrees = L_add( degrees, DEGREE_360 );
252 : }
253 :
254 456 : return Mpy_32_32( PI_OVER_180_FX, degrees ); // Q22
255 : }
256 :
257 0 : float deg2rad(
258 : float degrees )
259 : {
260 0 : while ( degrees >= 180.0f )
261 : {
262 0 : degrees = degrees - 360.0f;
263 : }
264 0 : while ( degrees <= -180.0f )
265 : {
266 0 : degrees = degrees + 360.0f;
267 : }
268 :
269 0 : return PI_OVER_180 * degrees;
270 : }
|
