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 :
49 : /*-------------------------------------------------------------------------
50 : * Euler2Quat()
51 : *
52 : * Calculate corresponding Quaternion from Euler angles in radians
53 : *------------------------------------------------------------------------*/
54 0 : void Euler2Quat_fx(
55 : const Word32 yaw, /* i : yaw (x) Q22 */
56 : const Word32 pitch, /* i : pitch (y) Q22 */
57 : const Word32 roll, /* i : roll (z) Q22 */
58 : IVAS_QUATERNION *quat /* o : quaternion describing the rotation Q22*/
59 : )
60 : {
61 0 : Word16 cr = getCosWord16( extract_l( L_shr_r( roll, 10 ) ) ); // Q14
62 0 : Word16 sr = shr( getSinWord16( extract_l( L_shr_r( roll, 10 ) ) ), 1 ); // Q14
63 0 : Word16 cp = getCosWord16( extract_l( L_shr_r( pitch, 10 ) ) );
64 0 : Word16 sp = shr( getSinWord16( extract_l( L_shr_r( pitch, 10 ) ) ), 1 ); // Q14
65 0 : Word16 cy = getCosWord16( extract_l( L_shr_r( yaw, 10 ) ) );
66 0 : Word16 sy = shr( getSinWord16( extract_l( L_shr_r( yaw, 10 ) ) ), 1 ); // Q14
67 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
68 0 : move32();
69 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
70 0 : move32();
71 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
72 0 : move32();
73 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
74 0 : move32();
75 :
76 0 : quat->q_fact = Q22;
77 0 : move16();
78 :
79 0 : return;
80 : }
81 :
82 :
83 : /*-------------------------------------------------------------------------
84 : * Copy_Quat_fx()
85 : *
86 : * Quaternion q factor modification
87 : *------------------------------------------------------------------------*/
88 0 : void Copy_Quat_fx(
89 : const IVAS_QUATERNION *in_quat, /* i : quaternion describing the rotation */
90 : IVAS_QUATERNION *out_quat /* i : quaternion describing the rotation */
91 : )
92 : {
93 0 : out_quat->q_fact = in_quat->q_fact;
94 0 : out_quat->w_fx = in_quat->w_fx;
95 0 : out_quat->x_fx = in_quat->x_fx;
96 0 : out_quat->y_fx = in_quat->y_fx;
97 0 : out_quat->z_fx = in_quat->z_fx;
98 0 : move16();
99 0 : move32();
100 0 : move32();
101 0 : move32();
102 0 : move32();
103 :
104 0 : return;
105 : }
106 :
107 : /*-------------------------------------------------------------------------
108 : * Scale_Quat_fx()
109 : *
110 : * Quaternion q factor modification
111 : *------------------------------------------------------------------------*/
112 0 : void modify_Quat_q_fx(
113 : const IVAS_QUATERNION *in_quat, /* i : quaternion describing the rotation */
114 : IVAS_QUATERNION *out_quat, /* i : quaternion describing the rotation */
115 : Word16 q_new /* i : quaternion describing the rotation */
116 : )
117 : {
118 0 : out_quat->w_fx = L_shl_sat( in_quat->w_fx, sub( q_new, in_quat->q_fact ) ); // q_new
119 0 : out_quat->x_fx = L_shl_sat( in_quat->x_fx, sub( q_new, in_quat->q_fact ) ); // q_new
120 0 : out_quat->y_fx = L_shl_sat( in_quat->y_fx, sub( q_new, in_quat->q_fact ) ); // q_new
121 0 : out_quat->z_fx = L_shl_sat( in_quat->z_fx, sub( q_new, in_quat->q_fact ) ); // q_new
122 0 : out_quat->q_fact = q_new;
123 0 : return;
124 : }
125 :
126 : /*-------------------------------------------------------------------------
127 : * modify_Rmat_q_fx()
128 : *
129 : * Rotation matrix q factor modification
130 : *------------------------------------------------------------------------*/
131 0 : void modify_Rmat_q_fx(
132 : Word32 Rmat_in[3][3], /* i : real-space rotation matrix for this rotation */
133 : Word32 Rmat_out[3][3], /* o : real-space rotation matrix for this rotation*/
134 : Word16 q_cur, /* i : current q factor for rotation matrix */
135 : Word16 q_new /* i : target q factor for rotation matrix */
136 : )
137 : {
138 : Word16 j, k;
139 :
140 0 : FOR( j = 0; j < 3; j++ )
141 : {
142 0 : FOR( k = 0; k < 3; k++ )
143 : {
144 0 : Rmat_out[j][k] = L_shl( Rmat_in[j][k], sub( q_new, q_cur ) );
145 0 : move32();
146 : }
147 : }
148 0 : return;
149 : }
150 :
151 : /*-------------------------------------------------------------------------
152 : * Quat2EulerDegree()
153 : *
154 : * Quaternion handling: calculate corresponding Euler angles in degrees
155 : *------------------------------------------------------------------------*/
156 0 : void Quat2EulerDegree_fx(
157 : const IVAS_QUATERNION quat, /* i : quaternion describing the rotation */
158 : Word32 *yaw_fx, /* o : yaw */
159 : Word32 *pitch_fx, /* o : pitch */
160 : Word32 *roll_fx /* o : roll */
161 : )
162 : {
163 0 : IF( NE_32( quat.w_fx, L_negate( 12582912 ) /*3.0f in Q22*/ ) )
164 : {
165 0 : Word32 tmp1 = W_extract_l( W_shr( W_mult0_32_32( quat.w_fx, quat.x_fx ), Q22 ) ); // Q22
166 0 : Word32 tmp2 = W_extract_l( W_shr( W_mult0_32_32( quat.y_fx, quat.z_fx ), Q22 ) ); // Q22
167 0 : Word32 tmp3 = L_shl( L_add( tmp1, tmp2 ), 1 ); // Q22
168 :
169 0 : Word32 tmp4 = W_extract_l( W_shr( W_mult0_32_32( quat.x_fx, quat.x_fx ), Q22 ) ); // Q22
170 0 : Word32 tmp5 = W_extract_l( W_shr( W_mult0_32_32( quat.y_fx, quat.y_fx ), Q22 ) ); // Q22
171 0 : Word32 tmp6 = L_shl( L_add( tmp4, tmp5 ), 1 );
172 0 : Word32 tmp7 = L_sub( ONE_IN_Q22, tmp6 );
173 :
174 0 : IF( tmp3 >= -2 && tmp3 <= 2 )
175 : {
176 0 : tmp3 = 0;
177 : }
178 0 : IF( tmp7 >= -2 && tmp7 <= 2 )
179 : {
180 0 : tmp7 = 0;
181 : }
182 :
183 0 : Word16 yaw_fx_16 = BASOP_util_atan2( tmp3, tmp7, 0 ); // Q13
184 0 : *yaw_fx = Mpy_32_16_1( 961263669 /*_180_OVER_PI in Q24*/, yaw_fx_16 );
185 :
186 : Word32 p_fx;
187 0 : Word32 tmp8 = W_extract_l( W_shr( W_mult0_32_32( quat.w_fx, quat.y_fx ), Q22 ) ); // Q22
188 0 : Word32 tmp9 = W_extract_l( W_shr( W_mult0_32_32( quat.z_fx, quat.x_fx ), Q22 ) ); // Q22
189 0 : p_fx = L_shl( L_sub( tmp8, tmp9 ), 1 );
190 0 : p_fx = max( L_negate( ONE_IN_Q22 ), min( ONE_IN_Q22, p_fx ) ); // Q22
191 :
192 0 : Word32 p_fx_sq = W_extract_l( W_shr( W_mult0_32_32( p_fx, p_fx ), Q22 ) );
193 0 : Word16 res_exp = 0;
194 0 : Word32 one_minus_p_fx_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( p_fx_sq ), 31 - Q22, &res_exp );
195 0 : Word32 sqrt_one_minus_p_fx_sq = Sqrt32( one_minus_p_fx_sq, &res_exp );
196 :
197 0 : Word16 pitch_fx_16 = BASOP_util_atan2( p_fx, sqrt_one_minus_p_fx_sq, ( 31 - Q22 ) - res_exp );
198 :
199 0 : *pitch_fx = Mpy_32_16_1( 961263669 /*_180_OVER_PI in Q24*/, pitch_fx_16 );
200 :
201 0 : Word32 tmp10 = W_extract_l( W_shr( W_mult0_32_32( quat.w_fx, quat.z_fx ), Q22 ) ); // Q22
202 0 : Word32 tmp11 = W_extract_l( W_shr( W_mult0_32_32( quat.x_fx, quat.y_fx ), Q22 ) ); // Q22
203 0 : Word32 tmp12 = L_shl( L_add( tmp10, tmp11 ), 1 ); // Q22
204 :
205 0 : Word32 tmp13 = W_extract_l( W_shr( W_mult0_32_32( quat.y_fx, quat.y_fx ), Q22 ) ); // Q22
206 0 : Word32 tmp14 = W_extract_l( W_shr( W_mult0_32_32( quat.z_fx, quat.z_fx ), Q22 ) ); // Q22
207 0 : Word32 tmp15 = L_shl( L_add( tmp13, tmp14 ), 1 ); // Q22
208 0 : Word32 tmp16 = L_sub( ONE_IN_Q22, tmp15 ); // Q22
209 :
210 0 : IF( tmp12 >= -2 && tmp12 <= 2 )
211 : {
212 0 : tmp12 = 0;
213 : }
214 0 : IF( tmp16 >= -2 && tmp16 <= 2 )
215 : {
216 0 : tmp16 = 0;
217 : }
218 0 : Word16 roll_fx_16 = BASOP_util_atan2( tmp12, tmp16, 0 ); // Q13
219 0 : *roll_fx = Mpy_32_16_1( 961263669 /*_180_OVER_PI in Q24*/, roll_fx_16 );
220 : }
221 : ELSE
222 : {
223 : /* Euler angles in R_X(roll)*R_Y(pitch)*R_Z(yaw) convention
224 : *
225 : * yaw: rotate scene counter-clockwise in the horizontal plane
226 : * pitch: rotate scene in the median plane, increase elevation with positive values
227 : * roll: rotate scene from the right ear to the top
228 : */
229 0 : *yaw_fx = quat.z_fx;
230 0 : *pitch_fx = quat.y_fx;
231 0 : *roll_fx = quat.x_fx;
232 : }
233 :
234 0 : return;
235 : }
236 :
237 :
238 : /*-------------------------------------------------------------------------
239 : * deg2rad()
240 : *
241 : * Converts degrees to normalized radians
242 : *------------------------------------------------------------------------*/
243 456 : Word32 deg2rad_fx(
244 : Word32 degrees // Q22
245 : )
246 : {
247 456 : WHILE( GE_32( degrees, DEGREE_180 ) )
248 : {
249 0 : degrees = L_sub( degrees, DEGREE_360 );
250 : }
251 456 : WHILE( LE_32( degrees, -DEGREE_180 ) )
252 : {
253 0 : degrees = L_add( degrees, DEGREE_360 );
254 : }
255 :
256 456 : return Mpy_32_32( PI_OVER_180_FX, degrees ); // Q22
257 : }
258 :
259 0 : float deg2rad(
260 : float degrees )
261 : {
262 0 : while ( degrees >= 180.0f )
263 : {
264 0 : degrees = degrees - 360.0f;
265 : }
266 0 : while ( degrees <= -180.0f )
267 : {
268 0 : degrees = degrees + 360.0f;
269 : }
270 :
271 0 : return PI_OVER_180 * degrees;
272 : }
|
