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 <assert.h>
34 : #include <stdint.h>
35 : #include "basop_util.h"
36 : #include "ivas_cnst.h"
37 : #include "options.h"
38 : #include <math.h>
39 : #include "cnst.h"
40 : #include "prot_fx.h"
41 : #include "ivas_prot_rend_fx.h"
42 : #include "wmc_auto.h"
43 : #include <stdio.h>
44 : #include "ivas_prot_fx.h"
45 : #include "debug.h"
46 : #include "ivas_rom_binaural_crend_head.h"
47 :
48 : Word16 square_root16_table[] = { 0, 0x4000, 0x5A82 }; // Q14
49 : Word16 square_root30_q12[31] = {
50 : // Q12
51 : 0,
52 : 4096,
53 : 5793,
54 : 7094,
55 : 8192,
56 : 9159,
57 : 10033,
58 : 10837,
59 : 11585,
60 : 12288,
61 : 12953,
62 : 13585,
63 : 14189,
64 : 14768,
65 : 15326,
66 : 15864,
67 : 16384,
68 : 16888,
69 : 17378,
70 : 17854,
71 : 18318,
72 : 18770,
73 : 19212,
74 : 19644,
75 : 20066,
76 : 20480,
77 : 20886,
78 : 21283,
79 : 21674,
80 : 22058,
81 : 22435,
82 : };
83 : /*-----------------------------------------------------------------------*
84 : * Local funtion declarations
85 : *-----------------------------------------------------------------------*/
86 :
87 : static ivas_error combine_external_and_head_orientations(
88 : IVAS_QUATERNION *headRotQuaternions,
89 : IVAS_VECTOR3 *listenerPos,
90 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData,
91 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData );
92 : static void external_target_interpolation_fx(
93 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData,
94 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData,
95 : const Word16 i );
96 :
97 : static bool are_orientations_same_fx( const IVAS_QUATERNION *orientation1, const IVAS_QUATERNION *orientation2 );
98 :
99 : /*-----------------------------------------------------------------------*
100 : * ivas_headTrack_open()
101 : *
102 : * Allocate and initialize Head-Tracking handle
103 : *-----------------------------------------------------------------------*/
104 :
105 81 : ivas_error ivas_headTrack_open_fx(
106 : HEAD_TRACK_DATA_HANDLE *hHeadTrackData /* o : head track handle */
107 : )
108 : {
109 : Word16 i;
110 : ivas_error error;
111 :
112 : /* Allocate Head-Tracking handle */
113 81 : IF( ( *hHeadTrackData = (HEAD_TRACK_DATA_HANDLE) malloc( sizeof( HEAD_TRACK_DATA ) ) ) == NULL )
114 : {
115 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for head-tracking memory\n" ) );
116 : }
117 :
118 : /* Initialization */
119 81 : ( *hHeadTrackData )->lrSwitchInterpVal_fx = 0; // Q30
120 81 : move32();
121 81 : ( *hHeadTrackData )->lrSwitchedCurrent = 0;
122 81 : move32();
123 81 : ( *hHeadTrackData )->lrSwitchedNext = 0;
124 81 : move32();
125 81 : IF( ( ( *hHeadTrackData )->OrientationTracker = (ivas_orient_trk_state_t *) malloc( sizeof( ivas_orient_trk_state_t ) ) ) == NULL )
126 : {
127 0 : return IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for Orientation tracking" );
128 : }
129 :
130 81 : IF( NE_32( ( error = ivas_orient_trk_Init_fx( ( *hHeadTrackData )->OrientationTracker ) ), IVAS_ERR_OK ) )
131 : {
132 0 : return error;
133 : }
134 :
135 : /* Initialise Rmat_prev to I, Rmat will be computed later */
136 324 : FOR( i = 0; i < 3; i++ )
137 : {
138 243 : set32_fx( ( *hHeadTrackData )->Rmat_prev_fx[i], 0, 3 );
139 243 : ( *hHeadTrackData )->Rmat_prev_fx[i][i] = ONE_IN_Q31; // Q31
140 243 : move32();
141 : }
142 :
143 :
144 81 : set32_fx( ( *hHeadTrackData )->chEneIIR_fx[0], 0, MASA_FREQUENCY_BANDS );
145 81 : set32_fx( ( *hHeadTrackData )->chEneIIR_fx[1], 0, MASA_FREQUENCY_BANDS );
146 81 : set32_fx( ( *hHeadTrackData )->procChEneIIR_fx[0], 0, MASA_FREQUENCY_BANDS );
147 81 : set32_fx( ( *hHeadTrackData )->procChEneIIR_fx[1], 0, MASA_FREQUENCY_BANDS );
148 :
149 81 : return IVAS_ERR_OK;
150 : }
151 :
152 :
153 : /*-----------------------------------------------------------------------*
154 : * ivas_headTrack_close()
155 : *
156 : * Deallocate Head-Tracking handle
157 : *-----------------------------------------------------------------------*/
158 604 : void ivas_headTrack_close_fx(
159 : HEAD_TRACK_DATA_HANDLE *hHeadTrackData /* i/o: head track handle */
160 : )
161 : {
162 604 : test();
163 604 : IF( hHeadTrackData == NULL || *hHeadTrackData == NULL )
164 : {
165 523 : return;
166 : }
167 :
168 81 : IF( ( *hHeadTrackData )->OrientationTracker != NULL )
169 : {
170 81 : free( ( *hHeadTrackData )->OrientationTracker );
171 81 : ( *hHeadTrackData )->OrientationTracker = NULL;
172 : }
173 :
174 81 : free( ( *hHeadTrackData ) );
175 81 : *hHeadTrackData = NULL;
176 :
177 81 : return;
178 : }
179 :
180 : /*----------------------------------------------------------------------------------
181 : * QuatToRotMat()
182 : *
183 : * Quaternion handling: calculate rotation matrices in real-space and SHD
184 : *---------------------------------------------------------------------------------*/
185 :
186 2549413 : void QuatToRotMat_fx(
187 : const IVAS_QUATERNION quat, /* i : quaternion describing the rotation Qx */
188 : Word32 Rmat[3][3] /* o : real-space rotation matrix for this rotation 2*Qx-32 */
189 : )
190 : {
191 2549413 : Word32 w = quat.w_fx; // Qx
192 2549413 : move32();
193 2549413 : Word32 x = quat.x_fx;
194 2549413 : move32();
195 2549413 : Word32 y = quat.y_fx;
196 2549413 : move32();
197 2549413 : Word32 z = quat.z_fx;
198 2549413 : move32();
199 :
200 : // Adding a guard bit to squared terms since 2*x is not being done in those
201 : // statements (R[0][0], R[1][1], R[2][2]). This is done to avoid L_shl.
202 2549413 : Word32 ww = L_shr( Mpy_32_32( w, w ), 1 ); // 2 * Qx - 31 - 1 = 2*Qx-32
203 2549413 : Word32 xx = L_shr( Mpy_32_32( x, x ), 1 );
204 2549413 : Word32 yy = L_shr( Mpy_32_32( y, y ), 1 );
205 2549413 : Word32 zz = L_shr( Mpy_32_32( z, z ), 1 );
206 :
207 2549413 : Word32 wx = Mpy_32_32( w, x ); // 2 * Qx - 31
208 2549413 : Word32 wz = Mpy_32_32( w, z );
209 2549413 : Word32 wy = Mpy_32_32( w, y );
210 :
211 2549413 : Word32 xy = Mpy_32_32( x, y );
212 2549413 : Word32 xz = Mpy_32_32( x, z );
213 :
214 2549413 : Word32 yz = Mpy_32_32( y, z );
215 :
216 2549413 : Rmat[0][0] = L_sub( L_sub( L_add( ww, xx ), yy ), zz ); // 2 * Qx - 31 - 1 = 2*Qx-32
217 2549413 : move32();
218 2549413 : Rmat[0][1] = L_sub( xy, wz );
219 2549413 : move32();
220 2549413 : Rmat[0][2] = L_add( xz, wy );
221 2549413 : move32();
222 :
223 2549413 : Rmat[1][0] = L_add( xy, wz ); // 2 * Qx - 32
224 2549413 : move32();
225 2549413 : Rmat[1][1] = L_sub( L_add( L_sub( ww, xx ), yy ), zz );
226 2549413 : move32();
227 2549413 : Rmat[1][2] = L_sub( yz, wx );
228 2549413 : move32();
229 :
230 2549413 : Rmat[2][0] = L_sub( xz, wy ); // 2 * Qx - 32
231 2549413 : move32();
232 2549413 : Rmat[2][1] = L_add( yz, wx );
233 2549413 : move32();
234 2549413 : Rmat[2][2] = L_add( L_sub( L_sub( ww, xx ), yy ), zz );
235 2549413 : move32();
236 :
237 2549413 : return;
238 : }
239 :
240 :
241 0 : void Euler2Quat_fx(
242 : const Word32 yaw, /* i : yaw (x) Q22 */
243 : const Word32 pitch, /* i : pitch (y) Q22 */
244 : const Word32 roll, /* i : roll (z) Q22 */
245 : IVAS_QUATERNION *quat /* o : quaternion describing the rotation Q19 */
246 : )
247 : {
248 0 : Word16 cr = getCosWord16( extract_l( L_shr_r( roll, 10 ) ) ); // Q14
249 0 : Word16 sr = getSinWord16( extract_l( L_shr_r( roll, 10 ) ) );
250 0 : Word16 cp = getCosWord16( extract_l( L_shr_r( pitch, 10 ) ) );
251 0 : Word16 sp = getSinWord16( extract_l( L_shr_r( pitch, 10 ) ) );
252 0 : Word16 cy = getCosWord16( extract_l( L_shr_r( yaw, 10 ) ) );
253 0 : Word16 sy = getSinWord16( extract_l( L_shr_r( yaw, 10 ) ) );
254 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 ); // Q19
255 0 : move32();
256 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 ); // Q19
257 0 : move32();
258 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 ); // Q19
259 0 : move32();
260 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 ); // Q19
261 0 : move32();
262 :
263 0 : return;
264 : }
265 :
266 :
267 : /*-------------------------------------------------------------------------
268 : * deg2rad()
269 : *
270 : * Converts degrees to normalized radians
271 : *------------------------------------------------------------------------*/
272 396 : Word32 deg2rad_fx(
273 : Word32 degrees // Q23
274 : )
275 : {
276 396 : WHILE( GE_32( degrees, DEGREE_180 ) )
277 : {
278 0 : degrees = L_sub( degrees, DEGREE_360 );
279 : }
280 396 : WHILE( LE_32( degrees, -DEGREE_180 ) )
281 : {
282 0 : degrees = L_add( degrees, DEGREE_360 );
283 : }
284 :
285 396 : return Mpy_32_32( PI_OVER_180_FX, degrees ); // Q23
286 : }
287 :
288 : /*-------------------------------------------------------------------------
289 : * rad2deg()
290 : *
291 : * Converts normalized radians to degrees
292 : *------------------------------------------------------------------------*/
293 396 : Word32 rad2deg_fx(
294 : Word32 radians // Q13
295 : )
296 : {
297 :
298 399 : WHILE( GE_32( radians, EVS_PI_FX ) )
299 : {
300 3 : radians = L_sub( radians, EVS_PI_FX );
301 : }
302 396 : WHILE( LE_32( radians, -EVS_PI_FX ) )
303 : {
304 0 : radians = L_add( radians, EVS_PI_FX );
305 : }
306 :
307 396 : return W_extract_l( W_mult0_32_32( radians, _180_OVER_PI_FX ) ); // Q23
308 : }
309 : /*-------------------------------------------------------------------------
310 : * rotateAziEle()
311 : *
312 : * Apply rotation to direction parameters azimuth and elevation
313 : *------------------------------------------------------------------------*/
314 :
315 3472585 : void rotateAziEle_fx(
316 : Word16 azi_in, /* i : output elevation Q0 */
317 : Word16 ele_in, /* i : input elevation Q0 */
318 : Word16 *azi, /* o : rotated azimuth Q0 */
319 : Word16 *ele, /* o : rotated elevation Q0 */
320 : Word32 Rmat_fx[3][3], /* i : real-space rotation matrix Q30*/
321 : const Word16 isPlanar /* i : is rotation planar and elevation meaningless? Q0*/
322 : )
323 : {
324 : Word16 n;
325 : Word32 dv_fx[3], dv_r_fx[3];
326 : Word32 w_fx;
327 : Word32 temp;
328 : Word16 temp_16;
329 : Word32 y, x, sqrt_fx;
330 : Word16 radian, angle;
331 : /*Conversion spherical to cartesian coordinates*/
332 3472585 : IF( GT_16( abs_s( azi_in ), 180 ) )
333 : {
334 1061852 : IF( azi_in > 0 )
335 : {
336 1061852 : azi_in = sub( azi_in, 360 );
337 : }
338 : ELSE
339 : {
340 0 : azi_in = add( azi_in, 360 );
341 : }
342 : }
343 3472585 : temp_16 = ele_in;
344 3472585 : move16();
345 3472585 : w_fx = cosine_table_Q31[abs_s( temp_16 )]; // Q31
346 3472585 : move32();
347 3472585 : temp_16 = azi_in;
348 3472585 : move16();
349 3472585 : dv_fx[0] = Mpy_32_32( w_fx, cosine_table_Q31[abs_s( temp_16 )] ); // Q31
350 3472585 : move32();
351 3472585 : temp_16 = azi_in;
352 3472585 : move16();
353 3472585 : dv_fx[1] = Mpy_32_32( w_fx, sine_table_Q31[add( temp_16, 180 )] ); // Q31
354 3472585 : move32();
355 3472585 : temp_16 = ele_in;
356 3472585 : move16();
357 3472585 : dv_fx[2] = sine_table_Q31[add( temp_16, 180 )]; // Q31
358 3472585 : move32();
359 : /*Rotation mtx multiplication*/
360 13890340 : FOR( n = 0; n < 3; n++ )
361 : {
362 10417755 : temp = L_add( Mpy_32_32( dv_fx[0], Rmat_fx[n][0] ), Mpy_32_32( dv_fx[1], Rmat_fx[n][1] ) ); // Q30
363 10417755 : dv_r_fx[n] = L_add( Mpy_32_32( dv_fx[2], Rmat_fx[n][2] ), temp ); // Q30
364 10417755 : move32();
365 : }
366 :
367 : /*Conversion cartesian to spherical coordinates*/
368 3472585 : y = dv_r_fx[1];
369 3472585 : move32();
370 3472585 : x = dv_r_fx[0];
371 3472585 : move32();
372 3472585 : radian = atan2_fx( L_abs( L_shr( y, 15 ) ), L_abs( L_shr( x, 15 ) ) ); // Q14
373 :
374 3472585 : if ( y <= 0 )
375 : {
376 1735529 : radian = negate( radian );
377 : }
378 :
379 3472585 : IF( x < 0 )
380 : {
381 1256302 : IF( radian < 0 )
382 : {
383 592703 : angle = negate( add( 180, extract_l( L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), ( 24 ) ) ) ) ); // Q0
384 : }
385 : ELSE
386 : {
387 663599 : angle = sub( 180, extract_l( L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), ( 24 ) ) ) ); // Q0
388 : }
389 1256302 : IF( radian == 0 )
390 : {
391 844 : if ( y < 0 )
392 : {
393 3 : angle = negate( angle );
394 : }
395 : }
396 : }
397 : ELSE
398 : {
399 2216283 : angle = extract_l( L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), 24 ) ); // Q0
400 : }
401 :
402 3472585 : *azi = s_max( -180, s_min( 180, angle ) );
403 3472585 : move16();
404 :
405 3472585 : IF( isPlanar == 0 )
406 : {
407 3272585 : sqrt_fx = L_Frac_sqrtQ31( L_add( Mpy_32_32( dv_r_fx[0], dv_r_fx[0] ), Mpy_32_32( dv_r_fx[1], dv_r_fx[1] ) ) );
408 3272585 : y = dv_r_fx[2];
409 3272585 : move32();
410 3272585 : x = sqrt_fx;
411 3272585 : move32();
412 3272585 : radian = atan2_fx( L_abs( L_shr( y, 15 ) ), L_abs( L_shr( x, 15 ) ) ); // Q14
413 3272585 : if ( y <= 0 )
414 : {
415 1908914 : radian = negate( radian );
416 : }
417 :
418 3272585 : IF( x < 0 )
419 : {
420 0 : IF( radian < 0 )
421 : {
422 0 : angle = negate( add( 180, extract_l( L_shr( Mpy_32_16_r( _180_OVER_PI_Q25, radian ), ( 24 ) ) ) ) ); // Q0
423 : }
424 : ELSE
425 : {
426 0 : angle = sub( 180, extract_l( L_shr( Mpy_32_16_r( _180_OVER_PI_Q25, radian ), ( 24 ) ) ) ); // Q0
427 : }
428 0 : IF( radian == 0 )
429 : {
430 0 : if ( y < 0 )
431 : {
432 0 : angle = negate( angle );
433 : }
434 : }
435 : }
436 : ELSE
437 : {
438 3272585 : angle = extract_l( L_shr( Mpy_32_16_r( _180_OVER_PI_Q25, radian ), ( 24 ) ) ); // Q0
439 : }
440 :
441 3272585 : *ele = s_max( -90, s_min( 90, angle ) ); // Q0
442 3272585 : move16();
443 : }
444 : ELSE
445 : {
446 200000 : *ele = 0;
447 200000 : move16();
448 : }
449 :
450 3472585 : return;
451 : }
452 :
453 : /*-------------------------------------------------------------------------
454 : * rotateAziEle_fx_frac_az_el()
455 : *
456 : * Apply rotation to direction parameters azimuth and elevation
457 : *------------------------------------------------------------------------*/
458 150000 : void rotateAziEle_fx_frac_az_el(
459 : Word32 azi_in, /* i : output elevation Q22 */
460 : Word32 ele_in, /* i : input elevation Q22 */
461 : Word32 *azi, /* o : rotated azimuth Q22 */
462 : Word32 *ele, /* o : rotated elevation Q22 */
463 : Word32 Rmat_fx[3][3], /* i : real-space rotation matrix Q30 */
464 : const Word16 isPlanar /* i : is rotation planar and elevation meaningless? Q0*/
465 : )
466 : {
467 : Word16 n, radian; // temp_16;
468 : Word32 dv_fx[3], dv_r_fx[3];
469 : Word16 w_fx;
470 : Word32 temp;
471 : Word32 y, x, sqrt_fx;
472 : Word32 angle;
473 : Word16 azi_in_q13, ele_in_q13;
474 : /*Conversion spherical to cartesian coordinates*/
475 150000 : IF( GT_32( L_abs( azi_in ), _180_IN_Q22 ) )
476 : {
477 71744 : IF( azi_in > 0 )
478 : {
479 71744 : azi_in = L_sub( azi_in, _360_IN_Q22 );
480 : }
481 : ELSE
482 : {
483 0 : azi_in = L_add( azi_in, _360_IN_Q22 );
484 : }
485 : }
486 150000 : azi_in_q13 = extract_l( L_shr( Mpy_32_32( azi_in, PI_OVER_180_FX ), Q9 ) );
487 150000 : ele_in_q13 = extract_l( L_shr( Mpy_32_32( ele_in, PI_OVER_180_FX ), Q9 ) );
488 150000 : w_fx = getCosWord16( ele_in_q13 ); // Q14
489 150000 : dv_fx[0] = L_mult( w_fx, getCosWord16( azi_in_q13 ) ); // Q28
490 150000 : move32();
491 150000 : IF( EQ_32( dv_fx[0], ONE_IN_Q29 ) )
492 : {
493 2414 : move32();
494 2414 : dv_fx[0] = ONE_IN_Q31;
495 : }
496 : ELSE
497 : {
498 147586 : dv_fx[0] = L_shl( dv_fx[0], 2 );
499 147586 : move32();
500 : }
501 150000 : dv_fx[1] = L_mult( w_fx, getSinWord16( azi_in_q13 ) ); // Q28
502 150000 : move32();
503 150000 : IF( EQ_32( dv_fx[1], ONE_IN_Q30 ) )
504 : {
505 0 : move32();
506 0 : dv_fx[1] = ONE_IN_Q31;
507 : }
508 : ELSE
509 : {
510 150000 : dv_fx[1] = L_shl( dv_fx[1], 1 );
511 150000 : move32();
512 : }
513 150000 : dv_fx[2] = L_deposit_l( getSinWord16( ele_in_q13 ) ); // Q14
514 150000 : move32();
515 150000 : IF( EQ_32( dv_fx[2], ONE_IN_Q15 ) )
516 : {
517 0 : move32();
518 0 : dv_fx[2] = ONE_IN_Q31;
519 : }
520 : ELSE
521 : {
522 150000 : dv_fx[2] = L_shl( dv_fx[2], 16 );
523 150000 : move32();
524 : }
525 :
526 : /*Rotation mtx multiplication*/
527 600000 : FOR( n = 0; n < 3; n++ )
528 : {
529 450000 : temp = L_add( Mpy_32_32( dv_fx[0], Rmat_fx[n][0] ), Mpy_32_32( dv_fx[1], Rmat_fx[n][1] ) );
530 450000 : dv_r_fx[n] = L_add( Mpy_32_32( dv_fx[2], Rmat_fx[n][2] ), temp ); // Q30
531 450000 : move32();
532 : }
533 :
534 : /*Conversion cartesian to spherical coordinates*/
535 150000 : move32();
536 150000 : y = dv_r_fx[1];
537 150000 : move32();
538 150000 : x = dv_r_fx[0];
539 150000 : radian = BASOP_util_atan2( y, x, 0 ); // Q13
540 :
541 150000 : angle = L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), 1 ); // Q22
542 :
543 :
544 150000 : *azi = L_max( L_shl( -180, 22 ), L_min( L_shl( 180, 22 ), angle ) ); // Q22
545 150000 : move32();
546 150000 : *azi = L_shl( L_shr( L_add( *azi, ONE_IN_Q21 ), Q22 ), Q22 );
547 150000 : move32();
548 150000 : if ( LT_32( L_abs( *azi ), ONE_IN_Q22 ) )
549 : {
550 532 : move32();
551 532 : *azi = 0;
552 : }
553 150000 : IF( isPlanar == 0 )
554 : {
555 150000 : sqrt_fx = L_Frac_sqrtQ31( L_add( Mpy_32_32( dv_r_fx[0], dv_r_fx[0] ), Mpy_32_32( dv_r_fx[1], dv_r_fx[1] ) ) );
556 150000 : y = dv_r_fx[2];
557 150000 : move32();
558 150000 : x = sqrt_fx;
559 150000 : move32();
560 150000 : radian = BASOP_util_atan2( y, x, 0 ); // Q13
561 :
562 150000 : angle = L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), 1 ); // Q22
563 :
564 :
565 150000 : *ele = L_max( L_shl( -90, 22 ), L_min( L_shl( 90, 22 ), angle ) ); // Q22
566 150000 : move32();
567 150000 : *ele = L_shl( L_shr( L_add( *ele, ONE_IN_Q21 ), Q22 ), Q22 );
568 150000 : move32();
569 150000 : if ( LT_32( L_abs( *ele ), ONE_IN_Q22 ) )
570 : {
571 18942 : *ele = 0;
572 18942 : move32();
573 : }
574 : }
575 : ELSE
576 : {
577 0 : *ele = 0;
578 0 : move32();
579 : }
580 :
581 150000 : return;
582 : }
583 :
584 : /*-------------------------------------------------------------------------
585 : * rotateAziEle()
586 : *
587 : * Apply rotation to direction parameters azimuth and elevation
588 : *------------------------------------------------------------------------*/
589 :
590 449760 : void rotateAziEle_fixed(
591 : Word16 azi_in, /* i : output elevation Q0 */
592 : Word16 ele_in, /* i : input elevation Q0 */
593 : Word32 *azi, /* o : rotated azimuth Q22 */
594 : Word32 *ele, /* o : rotated elevation Q22 */
595 : Word32 Rmat_fx[3][3], /* i : real-space rotation matrix Q30 */
596 : const Word16 isPlanar /* i : is rotation planar and elevation meaningless? Q0*/
597 : )
598 : {
599 : Word16 n, radian, temp_16;
600 : Word32 dv_fx[3], dv_r_fx[3];
601 : Word32 w_fx;
602 : Word32 temp;
603 : Word32 y, x, sqrt_fx;
604 : Word32 angle;
605 : /*Conversion spherical to cartesian coordinates*/
606 449760 : IF( GT_16( abs_s( azi_in ), 180 ) )
607 : {
608 0 : IF( azi_in > 0 )
609 : {
610 0 : azi_in = sub( azi_in, 360 );
611 : }
612 : ELSE
613 : {
614 0 : azi_in = add( azi_in, 360 );
615 : }
616 : }
617 449760 : temp_16 = ele_in;
618 449760 : move16();
619 449760 : w_fx = cosine_table_Q31[abs( temp_16 )];
620 449760 : move32();
621 449760 : temp_16 = azi_in;
622 449760 : move16();
623 449760 : dv_fx[0] = Mpy_32_32( w_fx, cosine_table_Q31[abs( temp_16 )] );
624 449760 : move32();
625 449760 : temp_16 = azi_in;
626 449760 : move16();
627 449760 : dv_fx[1] = Mpy_32_32( w_fx, sine_table_Q31[temp_16 + 180] );
628 449760 : move32();
629 449760 : temp_16 = ele_in;
630 449760 : move16();
631 449760 : dv_fx[2] = sine_table_Q31[temp_16 + 180];
632 449760 : move32();
633 : /*Rotation mtx multiplication*/
634 1799040 : FOR( n = 0; n < 3; n++ )
635 : {
636 1349280 : temp = L_add( Mpy_32_32( dv_fx[0], Rmat_fx[n][0] ), Mpy_32_32( dv_fx[1], Rmat_fx[n][1] ) );
637 1349280 : dv_r_fx[n] = L_add( Mpy_32_32( dv_fx[2], Rmat_fx[n][2] ), temp ); // Q30
638 1349280 : move32();
639 : }
640 :
641 : /*Conversion cartesian to spherical coordinates*/
642 449760 : y = dv_r_fx[1];
643 449760 : move32();
644 449760 : x = dv_r_fx[0];
645 449760 : move32();
646 449760 : radian = BASOP_util_atan2( y, x, 0 ); // Q13
647 :
648 449760 : angle = L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), 1 ); // Q22
649 : /* Handeled roudning off operation*/
650 449760 : IF( angle > 0 )
651 : {
652 242804 : temp = L_add( angle, ONE_IN_Q21 /* 0.5 in Q22*/ );
653 242804 : temp_16 = extract_l( L_shr( temp, Q22 ) );
654 242804 : angle = L_shl( temp_16, Q22 ); // Q22
655 : }
656 : ELSE
657 : {
658 206956 : temp = L_sub( angle, ONE_IN_Q21 /* 0.5 in Q22*/ );
659 206956 : temp_16 = add( extract_l( L_shr( temp, Q22 ) ), 1 );
660 206956 : angle = L_shl( temp_16, Q22 ); // Q22
661 : }
662 449760 : *azi = L_max( L_shl( -180, 22 ), L_min( L_shl( 180, 22 ), angle ) ); // Q22
663 449760 : move32();
664 449760 : if ( LE_32( L_abs( *azi ), ONE_IN_Q22 ) )
665 : {
666 4142 : *azi = 0;
667 4142 : move32();
668 : }
669 449760 : IF( isPlanar == 0 )
670 : {
671 401360 : sqrt_fx = L_Frac_sqrtQ31( L_add( Mpy_32_32( dv_r_fx[0], dv_r_fx[0] ), Mpy_32_32( dv_r_fx[1], dv_r_fx[1] ) ) );
672 401360 : y = dv_r_fx[2];
673 401360 : move32();
674 401360 : x = sqrt_fx;
675 401360 : move32();
676 401360 : radian = BASOP_util_atan2( y, x, 0 ); // Q13
677 :
678 401360 : angle = L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), 1 ); // Q22
679 :
680 401360 : *ele = L_max( L_shl( -90, 22 ), L_min( L_shl( 90, 22 ), angle ) ); // Q22
681 401360 : move32();
682 401360 : if ( LT_32( L_abs( *ele ), ONE_IN_Q22 ) )
683 : {
684 208480 : *ele = 0;
685 208480 : move32();
686 : }
687 : }
688 : ELSE
689 : {
690 48400 : *ele = 0;
691 48400 : move32();
692 : }
693 :
694 449760 : return;
695 : }
696 :
697 : /*-------------------------------------------------------------------------
698 : * rotateFrame_shd()
699 : *
700 : * Apply rotation to signals in Spherical Harmonic Domain
701 : *------------------------------------------------------------------------*/
702 :
703 600 : void rotateFrame_shd(
704 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i : head and external orientation combined handle */
705 : Word32 *output[], /* i/o: unrotated HOA3 signal buffer in TD Q11 */
706 : const Word16 subframe_len, /* i : subframe length per channel Q0 */
707 : const IVAS_OUTPUT_SETUP hTransSetup, /* i : format for rotation */
708 : const Word16 subframe_idx /* i : subframe index Q0 */
709 : )
710 : {
711 : // Not yet tested, no test cases entering the function.
712 : Word16 i, l, n, m, offset;
713 : Word16 m1, m2;
714 : Word16 shd_rot_max_order;
715 :
716 600 : Word32 tmp = Q31_BY_SUB_FRAME_240;
717 600 : move32();
718 : Word32 tmpRot[2 * HEADROT_ORDER + 1];
719 : Word16 SHrotmat_prev[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
720 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
721 : Word32 cross_fade[L_FRAME48k / MAX_PARAM_SPATIAL_SUBFRAMES];
722 600 : shd_rot_max_order = hTransSetup.ambisonics_order;
723 600 : move16();
724 600 : offset = imult1616( subframe_idx, subframe_len );
725 :
726 600 : SWITCH( subframe_len )
727 : {
728 600 : case L_SUBFRAME_48k:
729 600 : tmp = Q31_BY_SUB_FRAME_240; // Q31
730 600 : move32();
731 600 : BREAK;
732 0 : case L_SUBFRAME_32k:
733 0 : tmp = Q31_BY_SUB_FRAME_180;
734 0 : move32();
735 0 : BREAK;
736 0 : case L_SUBFRAME_16k:
737 0 : tmp = Q31_BY_SUB_FRAME_80;
738 0 : move32();
739 0 : BREAK;
740 0 : case L_SUBFRAME_8k:
741 0 : tmp = Q31_BY_SUB_FRAME_40;
742 0 : move32();
743 0 : BREAK;
744 0 : default:
745 0 : BREAK;
746 : }
747 144600 : FOR( i = 0; i < subframe_len; i++ )
748 : {
749 144000 : cross_fade[i] = UL_Mpy_32_32( i, tmp ); // Q31
750 144000 : move32();
751 : }
752 : /* initialize rotation matrices with zeros */
753 10200 : FOR( i = 0; i < HEADROT_SHMAT_DIM; i++ )
754 : {
755 9600 : set16_fx( SHrotmat_prev[i], 0, HEADROT_SHMAT_DIM );
756 9600 : set16_fx( SHrotmat[i], 0, HEADROT_SHMAT_DIM );
757 : }
758 :
759 : /* calculate ambisonics rotation matrices for the previous and current frames */
760 600 : SHrotmatgen_fx( SHrotmat_prev, hCombinedOrientationData->Rmat_prev_fx, shd_rot_max_order );
761 :
762 600 : SHrotmatgen_fx( SHrotmat /*Q14*/, hCombinedOrientationData->Rmat_fx[hCombinedOrientationData->subframe_idx], shd_rot_max_order );
763 :
764 144600 : FOR( i = 0; i < subframe_len; i++ )
765 : {
766 : /*As the rotation matrix becomes block diagonal in a SH basis, we can
767 : apply each angular-momentum block individually to save complexity. */
768 :
769 : /* loop over l blocks */
770 144000 : m1 = 1;
771 144000 : move16();
772 144000 : m2 = 4;
773 144000 : move16();
774 576000 : FOR( l = 1; l <= shd_rot_max_order; l++ )
775 : {
776 : /* compute mtx-vector product for this l */
777 2592000 : FOR( n = m1; n < m2; n++ )
778 : {
779 2160000 : tmpRot[n - m1] = 0;
780 2160000 : move32();
781 14112000 : FOR( m = m1; m < m2; m++ )
782 : {
783 : /* crossfade with previous rotation gains */
784 11952000 : tmp = L_add( Mpy_32_32( Mpy_32_16_r( cross_fade[i], SHrotmat[n][m] ) /*Q30*/, output[m][offset + i] /*Q11*/ ), Mpy_32_32( Mpy_32_16_r( L_sub( ONE_IN_Q31, cross_fade[i] ), SHrotmat_prev[n][m] ) /*Q30*/, output[m][offset + i] /*Q11*/ ) );
785 11952000 : tmpRot[n - m1] = L_add( L_shl( tmp, 1 ), tmpRot[n - m1] ); // Q11
786 : }
787 : }
788 :
789 : /* write back the result */
790 2592000 : FOR( n = m1; n < m2; n++ )
791 : {
792 2160000 : output[n][offset + i] = tmpRot[n - m1]; // Q11
793 2160000 : move32();
794 : }
795 432000 : m1 = m2;
796 432000 : move16();
797 432000 : m2 = add( m2, add( shl( add( l, 1 ), 1 ), 1 ) );
798 : }
799 :
800 : /* unoptimized code for reference (full matrix multiplication)
801 : for ( n = 0; n < nchan; n++ )
802 : {
803 : tmpRot[n] = 0.f;
804 :
805 : for ( m = 0; m < nchan; m++ )
806 : {
807 : tmpRot[n] += SHrotmat[n][m] * output[m][i];
808 : }
809 : }
810 : for ( n = 0; n < nchan; n++ )
811 : {
812 : output[n][i] = tmpRot[n];
813 : }
814 : */
815 : }
816 :
817 : /* move Rmat to Rmat_prev */
818 2400 : FOR( i = 0; i < 3; i++ )
819 : {
820 1800 : MVR2R_WORD32(
821 : hCombinedOrientationData->Rmat_fx[subframe_idx][i],
822 : hCombinedOrientationData->Rmat_prev_fx[i],
823 : 3 ); // Q14
824 : }
825 :
826 600 : return;
827 : }
828 :
829 : /*-------------------------------------------------------------------------
830 : * rotateFrame_sd()
831 : *
832 : * Apply rotation to signals in Spatial Domain
833 : *------------------------------------------------------------------------*/
834 16000 : void rotateFrame_sd(
835 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i : head and external orientation combined handle */
836 : Word32 *output[], /* i/o: unrotated SD signal buffer in TD Q11 */
837 : const Word16 subframe_len, /* i : subframe length per channel */
838 : const IVAS_OUTPUT_SETUP hTransSetup, /* i : format for rotation */
839 : const EFAP_HANDLE hEFAPdata, /* i : EFAP structure */
840 : const Word16 subframe_idx /* i : subframe index */
841 : )
842 : {
843 : Word16 i, j, offset;
844 : Word16 nchan, index_lfe;
845 : Word16 ch_in, ch_in_woLFE, ch_out, ch_out_woLFE;
846 : Word16 azimuth, elevation;
847 : Word32 azimuth_fx, elevation_fx;
848 16000 : Word32 tmp = Q31_BY_SUB_FRAME_240; // Q31
849 : Word32 out_temp;
850 : Word32 tmp_gains_fx[MAX_CICP_CHANNELS - 1];
851 : Word32 gains_fx[MAX_CICP_CHANNELS][MAX_CICP_CHANNELS];
852 : Word32 gains_prev_fx[MAX_CICP_CHANNELS][MAX_CICP_CHANNELS];
853 : Word32 output_tmp_fx[MAX_CICP_CHANNELS][L_FRAME48k];
854 : Word32 cross_fade_fx[L_FRAME48k / MAX_PARAM_SPATIAL_SUBFRAMES];
855 :
856 16000 : push_wmops( "rotateFrame_sd" );
857 :
858 16000 : move32();
859 :
860 16000 : nchan = add( hTransSetup.nchan_out_woLFE, hTransSetup.num_lfe );
861 16000 : index_lfe = hTransSetup.index_lfe[0];
862 16000 : move16();
863 16000 : offset = imult1616( subframe_idx, subframe_len );
864 16000 : SWITCH( subframe_len )
865 : {
866 16000 : case L_SUBFRAME_48k:
867 16000 : tmp = Q31_BY_SUB_FRAME_240;
868 16000 : move32();
869 16000 : BREAK;
870 0 : case L_SUBFRAME_32k:
871 0 : tmp = Q31_BY_SUB_FRAME_180;
872 0 : move32();
873 0 : BREAK;
874 0 : case L_SUBFRAME_16k:
875 0 : tmp = Q31_BY_SUB_FRAME_80;
876 0 : move32();
877 0 : BREAK;
878 0 : case L_SUBFRAME_8k:
879 0 : tmp = Q31_BY_SUB_FRAME_40;
880 0 : move32();
881 0 : BREAK;
882 0 : default:
883 0 : BREAK;
884 : }
885 :
886 3856000 : FOR( i = 0; i < subframe_len; i++ )
887 : {
888 3840000 : cross_fade_fx[i] = UL_Mpy_32_32( i, tmp ); // Q31
889 3840000 : move32();
890 : }
891 :
892 112000 : FOR( ch_in = 0; ch_in < nchan; ch_in++ )
893 : {
894 : /* zero output and gain buffers */
895 96000 : set32_fx( &output_tmp_fx[ch_in][offset], 0, subframe_len );
896 96000 : set32_fx( gains_prev_fx[ch_in], 0, nchan );
897 96000 : set32_fx( gains_fx[ch_in], 0, nchan );
898 : /* set gains to passthrough by default */
899 96000 : gains_prev_fx[ch_in][ch_in] = ONE_IN_Q30;
900 96000 : move32();
901 96000 : gains_fx[ch_in][ch_in] = ONE_IN_Q30;
902 96000 : move32();
903 : /* skip LFE */
904 96000 : IF( EQ_16( ch_in, index_lfe ) )
905 : {
906 16000 : CONTINUE;
907 : }
908 :
909 : /* input channel index without LFE */
910 80000 : IF( GE_16( ch_in, index_lfe ) )
911 : {
912 32000 : ch_in_woLFE = sub( ch_in, 1 );
913 : }
914 : ELSE
915 : {
916 48000 : ch_in_woLFE = ch_in;
917 48000 : move16();
918 : }
919 :
920 : /* gains for previous subframe rotation */
921 80000 : rotateAziEle_fx( extract_l( L_shr( hTransSetup.ls_azimuth_fx[ch_in_woLFE], Q22 ) ), extract_l( L_shr( hTransSetup.ls_elevation_fx[ch_in_woLFE], Q22 ) ), &azimuth, &elevation, hCombinedOrientationData->Rmat_prev_fx, hTransSetup.is_planar_setup );
922 :
923 80000 : test();
924 80000 : test();
925 80000 : IF( hEFAPdata != NULL && ( NE_16( extract_l( L_shr( hTransSetup.ls_azimuth_fx[ch_in_woLFE], Q22 ) ), azimuth ) || NE_16( extract_l( L_shr( hTransSetup.ls_elevation_fx[ch_in_woLFE], Q22 ) ), elevation ) ) )
926 : {
927 76747 : azimuth_fx = L_shl( L_deposit_l( azimuth ), Q22 ); // Q0->Q22
928 76747 : elevation_fx = L_shl( L_deposit_l( elevation ), Q22 ); // Q0->Q22
929 76747 : efap_determine_gains_fx( hEFAPdata, tmp_gains_fx, azimuth_fx, elevation_fx, EFAP_MODE_EFAP );
930 :
931 :
932 537229 : FOR( ch_out = 0; ch_out < nchan; ch_out++ )
933 : {
934 : /* skip LFE */
935 460482 : IF( EQ_16( ch_out, index_lfe ) )
936 : {
937 76747 : CONTINUE;
938 : }
939 :
940 : /* output channel index without LFE */
941 383735 : IF( GE_16( ch_out, index_lfe ) )
942 : {
943 153494 : ch_out_woLFE = sub( ch_out, 1 );
944 : }
945 : ELSE
946 : {
947 230241 : ch_out_woLFE = ch_out;
948 230241 : move16();
949 : }
950 383735 : gains_prev_fx[ch_in][ch_out] = tmp_gains_fx[ch_out_woLFE]; // Q30
951 383735 : move32();
952 : }
953 : }
954 :
955 : /* gains for current subframe rotation */
956 80000 : rotateAziEle_fx( extract_l( L_shr( hTransSetup.ls_azimuth_fx[ch_in_woLFE], Q22 ) ), extract_l( L_shr( hTransSetup.ls_elevation_fx[ch_in_woLFE], Q22 ) ), &azimuth, &elevation, hCombinedOrientationData->Rmat_fx[hCombinedOrientationData->subframe_idx], hTransSetup.is_planar_setup );
957 80000 : test();
958 80000 : test();
959 80000 : IF( hEFAPdata != NULL && ( NE_16( extract_l( L_shr( hTransSetup.ls_azimuth_fx[ch_in_woLFE], Q22 ) ), azimuth ) || NE_16( extract_l( L_shr( hTransSetup.ls_elevation_fx[ch_in_woLFE], Q22 ) ), elevation ) ) )
960 : {
961 :
962 76763 : azimuth_fx = L_shl( L_deposit_l( azimuth ), Q22 ); // Q0->Q22
963 76763 : elevation_fx = L_shl( L_deposit_l( elevation ), Q22 ); // Q0->Q22
964 76763 : efap_determine_gains_fx( hEFAPdata, tmp_gains_fx, azimuth_fx, elevation_fx, EFAP_MODE_EFAP );
965 :
966 537341 : FOR( ch_out = 0; ch_out < nchan; ch_out++ )
967 : {
968 : /* skip LFE */
969 460578 : IF( EQ_16( ch_out, index_lfe ) )
970 : {
971 76763 : CONTINUE;
972 : }
973 :
974 : /* output channel index without LFE */
975 383815 : IF( GE_16( ch_out, index_lfe ) )
976 : {
977 153526 : ch_out_woLFE = sub( ch_out, 1 );
978 : }
979 : ELSE
980 : {
981 230289 : ch_out_woLFE = ch_out;
982 230289 : move16();
983 : }
984 :
985 383815 : gains_fx[ch_in][ch_out] = tmp_gains_fx[ch_out_woLFE]; // Q30
986 383815 : move32();
987 : }
988 : }
989 : }
990 :
991 : /* apply panning gains by mtx multiplication */
992 112000 : FOR( ch_out = 0; ch_out < nchan; ch_out++ )
993 : {
994 672000 : FOR( ch_in = 0; ch_in < nchan; ch_in++ )
995 : {
996 576000 : j = 0;
997 : /* crossfade with previous rotation gains */
998 138816000 : FOR( i = subframe_idx * subframe_len; j < subframe_len; i++ )
999 : {
1000 138240000 : out_temp = output[ch_in][i];
1001 138240000 : move32();
1002 138240000 : Word32 temp = Mpy_32_32( Mpy_32_32( ( cross_fade_fx[j] ), gains_fx[ch_in][ch_out] ), out_temp ); // Q10
1003 138240000 : Word32 temp1 = Mpy_32_32( Mpy_32_32( L_sub( ONE_IN_Q31, cross_fade_fx[j] ), gains_prev_fx[ch_in][ch_out] ), out_temp ); // Q10
1004 138240000 : output_tmp_fx[ch_out][i] = L_add( L_shl( L_add( temp, temp1 ), 1 ), output_tmp_fx[ch_out][i] ); // Q11
1005 138240000 : move32();
1006 138240000 : j = add( j, 1 );
1007 : }
1008 : }
1009 : }
1010 :
1011 : /* move Rmat to Rmat_prev */
1012 64000 : FOR( i = 0; i < 3; i++ )
1013 : {
1014 48000 : MVR2R_WORD32(
1015 : hCombinedOrientationData->Rmat_fx[hCombinedOrientationData->subframe_idx][i], // Q30
1016 : hCombinedOrientationData->Rmat_prev_fx[i],
1017 : 3 );
1018 : }
1019 : /* copy to output */
1020 112000 : FOR( ch_out = 0; ch_out < nchan; ch_out++ )
1021 : {
1022 96000 : MVR2R_WORD32( &output_tmp_fx[ch_out][offset], &output[ch_out][offset], subframe_len ); // Q11
1023 : }
1024 :
1025 16000 : pop_wmops();
1026 16000 : return;
1027 : }
1028 : /*-------------------------------------------------------------------------
1029 : * rotateFrame_shd_cldfb()
1030 : *
1031 : * Apply rotation to signals in Spherical Harmonic Domain and in CLDFB
1032 : *------------------------------------------------------------------------*/
1033 18520 : void rotateFrame_shd_cldfb(
1034 : Word32 Cldfb_RealBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain real part Qx -> Qx-1 */
1035 : Word32 Cldfb_ImagBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain imag part Qx -> Qx-1 */
1036 : Word32 Rmat[3][3], /* i : real-space rotation matrix Q30 */
1037 : const Word16 nInChannels, /* i : number of channels */
1038 : const Word16 numTimeSlots, /* i : number of time slots to process */
1039 : const Word16 shd_rot_max_order /* i : split-order rotation method */
1040 : )
1041 : {
1042 18520 : Word16 n = 0;
1043 18520 : Word16 m = 0;
1044 18520 : Word16 i = 0, j = 0, k = 0;
1045 18520 : Word16 iBand = 0;
1046 18520 : Word16 l = 0, m1 = 0, m2 = 0;
1047 : Word32 realRot[2 * HEADROT_ORDER + 1], imagRot[2 * HEADROT_ORDER + 1];
1048 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
1049 : #ifndef OPT_HEAD_ROT_REND_V1_BE
1050 : Word32 temp1, temp2;
1051 : #endif /* OPT_HEAD_ROT_REND_V1_BE */
1052 18520 : move16();
1053 18520 : move16();
1054 18520 : move16();
1055 18520 : move16();
1056 18520 : move16();
1057 18520 : move16();
1058 18520 : move16();
1059 18520 : move16();
1060 18520 : move16();
1061 18520 : assert( nInChannels == HEADROT_SHMAT_DIM && "Number of channels must be 16!" );
1062 :
1063 : /* initialize rotation matrices with zeros */
1064 314840 : FOR( i = 0; i < HEADROT_SHMAT_DIM; i++ )
1065 : {
1066 296320 : set16_fx( SHrotmat[i], 0, HEADROT_SHMAT_DIM );
1067 : }
1068 :
1069 : /* calculate Ambisonics rotation matrix from the quaternion */
1070 18520 : SHrotmatgen_fx( SHrotmat, Rmat, shd_rot_max_order );
1071 :
1072 : #ifdef DEBUGGING
1073 : dbgwrite_txt( (const float *) SHrotmat[0], HEADROT_SHMAT_DIM * HEADROT_SHMAT_DIM, "Fixed_SHrotmat.txt", NULL );
1074 : #endif
1075 :
1076 : /* rotation by mtx multiplication */
1077 92600 : FOR( i = 0; i < numTimeSlots; i++ )
1078 : {
1079 4518880 : FOR( iBand = 0; iBand < CLDFB_NO_CHANNELS_MAX; iBand++ )
1080 : {
1081 : /*As the rotation matrix becomes block diagonal in a SH basis, we can
1082 : apply each angular-momentum block individually to save complexity. */
1083 :
1084 : /* loop over l blocks */
1085 4444800 : m1 = 1;
1086 4444800 : move16();
1087 4444800 : m2 = 4;
1088 4444800 : move16();
1089 17779200 : FOR( l = 1; l <= shd_rot_max_order; l++ )
1090 : {
1091 : /* compute mtx-vector product for this l */
1092 80006400 : FOR( n = m1; n < m2; n++ )
1093 : {
1094 66672000 : realRot[n - m1] = 0;
1095 66672000 : move32();
1096 66672000 : imagRot[n - m1] = 0;
1097 66672000 : move32();
1098 435590400 : FOR( m = m1; m < m2; m++ )
1099 : {
1100 : #ifdef OPT_HEAD_ROT_REND_V1_BE
1101 368918400 : realRot[n - m1] = Madd_32_16_r( realRot[n - m1], Cldfb_RealBuffer[m][i][iBand], SHrotmat[n][m] ); // Q(x + 14 - 15)
1102 368918400 : move32();
1103 368918400 : imagRot[n - m1] = Madd_32_16_r( imagRot[n - m1], Cldfb_ImagBuffer[m][i][iBand], SHrotmat[n][m] ); // Q(x + 14 - 15)
1104 368918400 : move32();
1105 : #else /* OPT_HEAD_ROT_REND_V1_BE */
1106 : temp1 = Mpy_32_16_r( Cldfb_RealBuffer[m][i][iBand], SHrotmat[n][m] ); // Q(x + 14 - 15)
1107 : temp2 = Mpy_32_16_r( Cldfb_ImagBuffer[m][i][iBand], SHrotmat[n][m] ); // Q(x + 14 - 15)
1108 : realRot[n - m1] = L_add( temp1, realRot[n - m1] ); // Q(x + 14 - 15)
1109 : move32();
1110 : imagRot[n - m1] = L_add( temp2, imagRot[n - m1] ); // Q(x + 14 - 15)
1111 : move32();
1112 : #endif /* OPT_HEAD_ROT_REND_V1_BE */
1113 : }
1114 : }
1115 : /* write back the result */
1116 80006400 : FOR( n = m1; n < m2; n++ )
1117 : {
1118 66672000 : Cldfb_RealBuffer[n][i][iBand] = realRot[n - m1]; // Qx - 1
1119 66672000 : move32();
1120 66672000 : Cldfb_ImagBuffer[n][i][iBand] = imagRot[n - m1]; // Qx - 1
1121 66672000 : move32();
1122 : }
1123 13334400 : m1 = m2;
1124 13334400 : move16();
1125 13334400 : m2 = add( m2, add( shl( add( l, 1 ), 1 ), 1 ) );
1126 : }
1127 :
1128 : /* unoptimized code for reference (full matrix multiplication)
1129 : for (n = 0; n < nInChannels; n++)
1130 : {
1131 : realRot[n] = 0.f;
1132 : imagRot[n] = 0.f;
1133 :
1134 : for (m = 0; m < nInChannels; m++)
1135 : {
1136 : realRot[n] += SHrotmat[n][m] * Cldfb_RealBuffer[m][i][iBand];
1137 : imagRot[n] += SHrotmat[n][m] * Cldfb_ImagBuffer[m][i][iBand];
1138 : }
1139 : }
1140 : for (n = 0; n < nInChannels; n++)
1141 : {
1142 : Cldfb_RealBuffer[n][i][iBand] = realRot[n];
1143 : Cldfb_ImagBuffer[n][i][iBand] = imagRot[n];
1144 : }
1145 : */
1146 : }
1147 : }
1148 : // To make the Q factor same as tha of the remaining vector values
1149 92600 : FOR( j = 0; j < MAX_PARAM_SPATIAL_SUBFRAMES; j++ )
1150 : {
1151 4518880 : FOR( k = 0; k < CLDFB_NO_CHANNELS_MAX; k++ )
1152 : {
1153 4444800 : Cldfb_RealBuffer[0][j][k] = L_shr( Cldfb_RealBuffer[0][j][k], 1 ); // Qx - 1
1154 4444800 : move32();
1155 4444800 : Cldfb_ImagBuffer[0][j][k] = L_shr( Cldfb_ImagBuffer[0][j][k], 1 ); // Qx - 1
1156 4444800 : move32();
1157 : }
1158 : }
1159 18520 : return;
1160 : }
1161 : /*-------------------------------------------------------------------------
1162 : * rotateFrame_sd_cldfb()
1163 : *
1164 : * Apply rotation to signals in Spatial Domain and in CLDFB
1165 : *------------------------------------------------------------------------*/
1166 :
1167 8000 : void rotateFrame_sd_cldfb_fixed(
1168 : Word32 Rmat_fx[3][3], /* i : real-space rotation matrix (Q30) */
1169 : Word32 Cldfb_RealBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain real part Qx */
1170 : Word32 Cldfb_ImagBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain imag part Qx */
1171 : const IVAS_OUTPUT_SETUP_HANDLE hOutputSetup, /* i : output format setup number of channels */
1172 : const EFAP_HANDLE hEFAPdata, /* i : EFAP structure */
1173 : const Word16 numTimeSlots, /* i : number of time slots to process */
1174 : const Word16 nb_band /* i : number of CLDFB bands to process */
1175 : )
1176 : {
1177 : Word16 iBlock, iBand, m, n;
1178 : Word32 gains_fx[MAX_CICP_CHANNELS - 1][MAX_CICP_CHANNELS - 1];
1179 : Word16 azimuth, elevation;
1180 : Word32 g1_fx;
1181 : Word32 realRot_fx[MAX_CICP_CHANNELS - 1][MAX_PARAM_SPATIAL_SUBFRAMES * CLDFB_NO_CHANNELS_MAX];
1182 : Word32 imagRot_fx[MAX_CICP_CHANNELS - 1][MAX_PARAM_SPATIAL_SUBFRAMES * CLDFB_NO_CHANNELS_MAX];
1183 : Word32 *p_realRot_fx, *p_imagRot_fx;
1184 : Word32 *p_real_fx, *p_imag_fx;
1185 : Word16 nInChannels;
1186 : Word16 isPlanar;
1187 8000 : push_wmops( "rotateFrame_sd_cldfb" );
1188 :
1189 8000 : nInChannels = hOutputSetup->nchan_out_woLFE;
1190 8000 : move16();
1191 8000 : isPlanar = 1;
1192 8000 : move16();
1193 48000 : FOR( n = 0; n < nInChannels; n++ )
1194 : {
1195 40000 : IF( hOutputSetup->ls_elevation_fx[n] != 0 )
1196 : {
1197 0 : isPlanar = 0;
1198 0 : move16();
1199 0 : BREAK;
1200 : }
1201 : }
1202 :
1203 : /* rotation of Euler angles */
1204 48000 : FOR( n = 0; n < nInChannels; n++ )
1205 : {
1206 40000 : rotateAziEle_fx( extract_l( L_shr( hOutputSetup->ls_azimuth_fx[n], Q22 ) ), extract_l( L_shr( hOutputSetup->ls_elevation_fx[n], Q22 ) ), &azimuth, &elevation, Rmat_fx, isPlanar );
1207 40000 : test();
1208 40000 : test();
1209 40000 : IF( hEFAPdata != NULL && ( NE_16( extract_l( L_shr( hOutputSetup->ls_azimuth_fx[n], Q22 ) ), azimuth ) || NE_16( extract_l( L_shr( hOutputSetup->ls_elevation_fx[n], Q22 ) ), elevation ) ) )
1210 : {
1211 0 : efap_determine_gains_fx( hEFAPdata, gains_fx[n], L_shl( azimuth, Q22 ), L_shl( elevation, Q22 ), EFAP_MODE_EFAP );
1212 : }
1213 : ELSE
1214 : {
1215 40000 : set32_fx( gains_fx[n], 0, nInChannels );
1216 40000 : gains_fx[n][n] = 0x7fffffff; // 1 in Q31
1217 40000 : move32();
1218 : }
1219 : }
1220 :
1221 : /* Apply panning gains by mtx multiplication*/
1222 48000 : FOR( n = 0; n < nInChannels; n++ )
1223 : {
1224 40000 : set32_fx( realRot_fx[n], 0, shl( nb_band, 2 ) );
1225 40000 : set32_fx( imagRot_fx[n], 0, shl( nb_band, 2 ) );
1226 240000 : FOR( m = 0; m < nInChannels; m++ )
1227 : {
1228 200000 : g1_fx = gains_fx[m][n]; // Q31
1229 200000 : move32();
1230 200000 : p_realRot_fx = realRot_fx[n];
1231 200000 : p_imagRot_fx = imagRot_fx[n];
1232 :
1233 200000 : IF( g1_fx > 0 )
1234 : {
1235 200000 : FOR( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
1236 : {
1237 160000 : p_real_fx = Cldfb_RealBuffer[m][iBlock];
1238 160000 : p_imag_fx = Cldfb_ImagBuffer[m][iBlock];
1239 :
1240 8160000 : FOR( iBand = 0; iBand < nb_band; iBand++ )
1241 : {
1242 8000000 : *( p_realRot_fx ) = L_add( *p_realRot_fx, Mpy_32_32( g1_fx, *( p_real_fx++ ) ) ); // Qx
1243 8000000 : move32();
1244 8000000 : *( p_imagRot_fx ) = L_add( *p_imagRot_fx, Mpy_32_32( g1_fx, *( p_imag_fx++ ) ) ); // Qx
1245 8000000 : move32();
1246 8000000 : p_realRot_fx++;
1247 8000000 : p_imagRot_fx++;
1248 : }
1249 : }
1250 : }
1251 : }
1252 : }
1253 :
1254 48000 : FOR( n = 0; n < nInChannels; n++ )
1255 : {
1256 40000 : p_realRot_fx = realRot_fx[n];
1257 40000 : p_imagRot_fx = imagRot_fx[n];
1258 :
1259 200000 : FOR( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
1260 : {
1261 160000 : p_real_fx = Cldfb_RealBuffer[n][iBlock];
1262 160000 : p_imag_fx = Cldfb_ImagBuffer[n][iBlock];
1263 :
1264 8160000 : FOR( iBand = 0; iBand < nb_band; iBand++ )
1265 : {
1266 8000000 : *( p_real_fx++ ) = *( p_realRot_fx++ );
1267 8000000 : move32();
1268 8000000 : *( p_imag_fx++ ) = *( p_imagRot_fx++ );
1269 8000000 : move32();
1270 : }
1271 1760000 : FOR( ; iBand < CLDFB_NO_CHANNELS_MAX; iBand++ )
1272 : {
1273 1600000 : *( p_real_fx++ ) = 0;
1274 1600000 : move32();
1275 1600000 : *( p_imag_fx++ ) = 0;
1276 1600000 : move32();
1277 : }
1278 : }
1279 : }
1280 8000 : pop_wmops();
1281 :
1282 8000 : return;
1283 : }
1284 :
1285 : /*-----------------------------------------------------------------------*
1286 : * ivas_external_orientation_open()
1287 : *
1288 : * Allocate and initialize external orientation handle
1289 : *-----------------------------------------------------------------------*/
1290 697 : ivas_error ivas_external_orientation_open(
1291 : EXTERNAL_ORIENTATION_HANDLE *hExtOrientationData, /* o : external orientation handle */
1292 : const Word16 num_subframes /* i : number of subframes */
1293 : )
1294 : {
1295 :
1296 : Word16 i;
1297 : IVAS_QUATERNION identity;
1298 697 : identity.w_fx = ONE_IN_Q31;
1299 697 : move32();
1300 697 : identity.x_fx = 0;
1301 697 : move32();
1302 697 : identity.y_fx = 0;
1303 697 : move32();
1304 697 : identity.z_fx = 0;
1305 697 : move32();
1306 697 : identity.q_fact = 31;
1307 697 : move16();
1308 : /* Allocate handle */
1309 697 : IF( ( *hExtOrientationData = (EXTERNAL_ORIENTATION_HANDLE) malloc( sizeof( EXTERNAL_ORIENTATION_DATA ) ) ) == NULL )
1310 : {
1311 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for external orientation memory\n" ) );
1312 : }
1313 697 : ( *hExtOrientationData )->num_subframes = num_subframes;
1314 697 : move16();
1315 : /* Enable head rotation and disable external orientation as default */
1316 3485 : FOR( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
1317 : {
1318 2788 : ( *hExtOrientationData )->enableHeadRotation[i] = 1;
1319 2788 : move16();
1320 2788 : ( *hExtOrientationData )->enableExternalOrientation[i] = 0;
1321 2788 : move16();
1322 2788 : ( *hExtOrientationData )->enableRotationInterpolation[i] = 0;
1323 2788 : move16();
1324 2788 : ( *hExtOrientationData )->numFramesToTargetOrientation[i] = 0;
1325 2788 : move16();
1326 :
1327 2788 : ( *hExtOrientationData )->Quaternions[i] = identity;
1328 : }
1329 697 : return IVAS_ERR_OK;
1330 : }
1331 :
1332 : /*-----------------------------------------------------------------------*
1333 : * ivas_external_orientation_close()
1334 : *
1335 : * Deallocate external orientation handle
1336 : *-----------------------------------------------------------------------*/
1337 1270 : void ivas_external_orientation_close_fx(
1338 : EXTERNAL_ORIENTATION_HANDLE *hExtOrientationData /* i/o: external orientation handle */
1339 : )
1340 : {
1341 1270 : test();
1342 1270 : IF( hExtOrientationData == NULL || *hExtOrientationData == NULL )
1343 : {
1344 573 : return;
1345 : }
1346 :
1347 697 : free( ( *hExtOrientationData ) );
1348 697 : *hExtOrientationData = NULL;
1349 :
1350 697 : return;
1351 : }
1352 :
1353 :
1354 : /*-----------------------------------------------------------------------*
1355 : * ivas_combined_orientation_open()
1356 : *
1357 : * Allocate and initialize combined orientation handle
1358 : *-----------------------------------------------------------------------*/
1359 747 : ivas_error ivas_combined_orientation_open(
1360 : COMBINED_ORIENTATION_HANDLE *hCombinedOrientationData, /* o : combined orientation handle */
1361 : const Word32 fs, /* i : sampling rate */
1362 : const Word16 num_subframes /* i : number of subframes */
1363 : )
1364 : {
1365 : Word16 i;
1366 : Word16 j;
1367 : IVAS_QUATERNION identity;
1368 : IVAS_VECTOR3 origo;
1369 747 : identity.w_fx = ONE_IN_Q31;
1370 747 : move32();
1371 747 : identity.x_fx = 0;
1372 747 : move32();
1373 747 : identity.y_fx = 0;
1374 747 : move32();
1375 747 : identity.z_fx = 0;
1376 747 : move32();
1377 747 : identity.q_fact = 31;
1378 747 : move16();
1379 747 : origo.x_fx = 0;
1380 747 : move32();
1381 747 : origo.y_fx = 0;
1382 747 : move32();
1383 747 : origo.z_fx = 0;
1384 747 : move32();
1385 747 : origo.q_fact = 31;
1386 747 : move16();
1387 : /* Allocate handle */
1388 747 : IF( ( *hCombinedOrientationData = (COMBINED_ORIENTATION_HANDLE) malloc( sizeof( COMBINED_ORIENTATION_DATA ) ) ) == NULL )
1389 : {
1390 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for combined orientation memory\n" ) );
1391 : }
1392 :
1393 : /* Initialization */
1394 747 : ( *hCombinedOrientationData )->num_subframes = num_subframes;
1395 747 : move16();
1396 747 : ( *hCombinedOrientationData )->interpolationCoefficient_fx = ONE_IN_Q30; // Q30
1397 747 : move32();
1398 747 : ( *hCombinedOrientationData )->interpolationIncrement_fx = ONE_IN_Q30; // Q30
1399 747 : move32();
1400 747 : IF( EQ_16( num_subframes, 1 ) )
1401 : {
1402 304 : ( *hCombinedOrientationData )->maximumFramesToTargetOrientation = 2000;
1403 304 : move16();
1404 : }
1405 : ELSE
1406 : {
1407 443 : ( *hCombinedOrientationData )->maximumFramesToTargetOrientation = 500;
1408 443 : move16();
1409 : }
1410 747 : ( *hCombinedOrientationData )->lrSwitchedNext = 0;
1411 747 : move16();
1412 747 : ( *hCombinedOrientationData )->lrSwitchedCurrent = 0;
1413 747 : move16();
1414 :
1415 747 : ( *hCombinedOrientationData )->lrSwitchInterpVal_fx = 0;
1416 747 : move32();
1417 747 : ( *hCombinedOrientationData )->isInterpolationOngoing = FALSE;
1418 747 : move16();
1419 747 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_start = identity;
1420 747 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_target = identity;
1421 :
1422 : /* Initialise orientations to identity */
1423 3735 : FOR( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
1424 : {
1425 2988 : ( *hCombinedOrientationData )->enableCombinedOrientation[i] = 0;
1426 2988 : move16();
1427 2988 : ( *hCombinedOrientationData )->Quaternions[i] = identity;
1428 2988 : ( *hCombinedOrientationData )->listenerPos[i] = origo;
1429 :
1430 11952 : FOR( j = 0; j < 3; j++ )
1431 : {
1432 8964 : set32_fx( ( *hCombinedOrientationData )->Rmat_fx[i][j], 0, 3 );
1433 8964 : ( *hCombinedOrientationData )->Rmat_fx[i][j][j] = ONE_IN_Q30; // Q30
1434 8964 : move32();
1435 : }
1436 : }
1437 :
1438 2988 : FOR( j = 0; j < 3; j++ )
1439 : {
1440 2241 : set32_fx( ( *hCombinedOrientationData )->Rmat_prev_fx[j], 0, 3 );
1441 2241 : ( *hCombinedOrientationData )->Rmat_prev_fx[j][j] = ONE_IN_Q30;
1442 2241 : move32();
1443 : }
1444 747 : ( *hCombinedOrientationData )->Quaternion_prev_extOrientation = identity;
1445 747 : ( *hCombinedOrientationData )->Quaternion_frozen_ext = identity;
1446 747 : ( *hCombinedOrientationData )->Quaternion_frozen_head = identity;
1447 747 : set_zero_fx( ( *hCombinedOrientationData )->chEneIIR_fx[0], MASA_FREQUENCY_BANDS );
1448 747 : set_zero_fx( ( *hCombinedOrientationData )->chEneIIR_fx[1], MASA_FREQUENCY_BANDS );
1449 747 : set_zero_fx( ( *hCombinedOrientationData )->procChEneIIR_fx[0], MASA_FREQUENCY_BANDS );
1450 747 : set_zero_fx( ( *hCombinedOrientationData )->procChEneIIR_fx[1], MASA_FREQUENCY_BANDS );
1451 747 : set16_fx( ( *hCombinedOrientationData )->q_chEneIIR[0], Q31, MASA_FREQUENCY_BANDS );
1452 747 : set16_fx( ( *hCombinedOrientationData )->q_chEneIIR[1], Q31, MASA_FREQUENCY_BANDS );
1453 747 : set16_fx( ( *hCombinedOrientationData )->q_procChEneIIR[0], Q31, MASA_FREQUENCY_BANDS );
1454 747 : set16_fx( ( *hCombinedOrientationData )->q_procChEneIIR[1], Q31, MASA_FREQUENCY_BANDS );
1455 :
1456 747 : ( *hCombinedOrientationData )->isExtOrientationFrozen = 0;
1457 747 : move16();
1458 747 : ( *hCombinedOrientationData )->isHeadRotationFrozen = 0;
1459 747 : move16();
1460 :
1461 747 : ( *hCombinedOrientationData )->subframe_idx = 0;
1462 747 : move16();
1463 : /* ( *hCombinedOrientationData )->subframe_size = (int16_t) ( fs / ( FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES ) ); */
1464 747 : ( *hCombinedOrientationData )->subframe_size = extract_l( Mpy_32_32_r( fs, 10737418 /* 1 / ( FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES ) in Q31 */ ) );
1465 747 : move16();
1466 747 : ( *hCombinedOrientationData )->cur_subframe_samples_rendered = 0;
1467 747 : move16();
1468 :
1469 747 : return IVAS_ERR_OK;
1470 : }
1471 : /*-----------------------------------------------------------------------*
1472 : * ivas_combined_orientation_close()
1473 : *
1474 : * Deallocate combined orientation handle
1475 : *-----------------------------------------------------------------------*/
1476 :
1477 1270 : void ivas_combined_orientation_close_fx(
1478 : COMBINED_ORIENTATION_HANDLE *hCombinedOrientationData /* i/o: combined orientation handle */
1479 : )
1480 : {
1481 1270 : test();
1482 1270 : IF( hCombinedOrientationData == NULL || *hCombinedOrientationData == NULL )
1483 : {
1484 523 : return;
1485 : }
1486 :
1487 747 : free( ( *hCombinedOrientationData ) );
1488 747 : *hCombinedOrientationData = NULL;
1489 :
1490 747 : return;
1491 : }
1492 :
1493 :
1494 : /*-------------------------------------------------------------------------
1495 : * combine_external_and_head_orientations_dec()
1496 : *
1497 : *
1498 : *------------------------------------------------------------------------*/
1499 :
1500 69350 : ivas_error combine_external_and_head_orientations_dec(
1501 : HEAD_TRACK_DATA_HANDLE hHeadTrackData, /* i : head track handle */
1502 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1503 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1504 : )
1505 : {
1506 69350 : IVAS_QUATERNION *pHeadRotQuaternion = NULL;
1507 69350 : IVAS_VECTOR3 *listenerPos = NULL;
1508 :
1509 69350 : IF( hHeadTrackData != NULL )
1510 : {
1511 69350 : pHeadRotQuaternion = hHeadTrackData->Quaternions;
1512 69350 : listenerPos = hHeadTrackData->Pos;
1513 : }
1514 69350 : return combine_external_and_head_orientations( pHeadRotQuaternion, listenerPos,
1515 : hExtOrientationData, hCombinedOrientationData );
1516 : }
1517 :
1518 :
1519 : /*-------------------------------------------------------------------------
1520 : * combine_external_and_head_orientations_rend()
1521 : *
1522 : *
1523 : *------------------------------------------------------------------------*/
1524 :
1525 1126140 : ivas_error combine_external_and_head_orientations_rend(
1526 : IVAS_REND_HeadRotData *hHeadTrackData, /* i : head track handle */
1527 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1528 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1529 : )
1530 : {
1531 1126140 : IVAS_QUATERNION *headRotQuaternions = NULL;
1532 1126140 : IVAS_VECTOR3 *listenerPos = NULL;
1533 : Word16 i;
1534 :
1535 1126140 : IF( hHeadTrackData != NULL )
1536 : {
1537 1126140 : IF( hHeadTrackData->headRotEnabled )
1538 : {
1539 180024 : headRotQuaternions = hHeadTrackData->headPositions;
1540 180024 : listenerPos = hHeadTrackData->Pos;
1541 : }
1542 : }
1543 0 : ELSE IF( hExtOrientationData != NULL )
1544 : {
1545 : /* Head rotation data not available, use the freezed value or disable */
1546 0 : FOR( i = 0; i < hExtOrientationData->num_subframes; i++ )
1547 : {
1548 0 : IF( NE_16( hExtOrientationData->enableHeadRotation[i], 2 ) )
1549 : {
1550 0 : hExtOrientationData->enableHeadRotation[i] = 0;
1551 0 : move16();
1552 : }
1553 : }
1554 : }
1555 :
1556 1126140 : return combine_external_and_head_orientations( headRotQuaternions, listenerPos,
1557 : hExtOrientationData, hCombinedOrientationData );
1558 : }
1559 :
1560 :
1561 : /*-------------------------------------------------------------------------
1562 : * combine_external_and_head_orientations()
1563 : *
1564 : * Combine the external orientations and the head orientation.
1565 : * NOTE that the external orientations are inversed.
1566 : *------------------------------------------------------------------------*/
1567 1195490 : ivas_error combine_external_and_head_orientations(
1568 : IVAS_QUATERNION *headRotQuaternions, /* i : quaternions for head rotation */
1569 : IVAS_VECTOR3 *listenerPos, /* i : listener position */
1570 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1571 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1572 : )
1573 : {
1574 : Word16 i;
1575 : Word16 j;
1576 : IVAS_QUATERNION identity;
1577 : IVAS_VECTOR3 origo;
1578 1195490 : identity.w_fx = ONE_IN_Q31;
1579 1195490 : move32();
1580 1195490 : identity.x_fx = 0;
1581 1195490 : move32();
1582 1195490 : identity.y_fx = 0;
1583 1195490 : move32();
1584 1195490 : identity.z_fx = 0;
1585 1195490 : move32();
1586 1195490 : identity.q_fact = 31;
1587 1195490 : move16();
1588 1195490 : origo.x_fx = 0;
1589 1195490 : move32();
1590 1195490 : origo.y_fx = 0;
1591 1195490 : move32();
1592 1195490 : origo.z_fx = 0;
1593 1195490 : move32();
1594 1195490 : origo.q_fact = 31;
1595 1195490 : move16();
1596 :
1597 : /* Form combined orientations or return if no data available */
1598 1195490 : test();
1599 1195490 : test();
1600 1195490 : IF( hCombinedOrientationData == NULL )
1601 : {
1602 0 : test();
1603 0 : IF( headRotQuaternions != NULL || hExtOrientationData != NULL )
1604 : {
1605 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
1606 : }
1607 : ELSE
1608 : {
1609 0 : return IVAS_ERR_OK;
1610 : }
1611 : }
1612 1195490 : ELSE IF( headRotQuaternions == NULL && hExtOrientationData == NULL )
1613 : {
1614 : /* Reset the combined orientations and rotations */
1615 0 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1616 0 : move16();
1617 0 : hCombinedOrientationData->interpolationCoefficient_fx = ONE_IN_Q30;
1618 0 : move32();
1619 0 : hCombinedOrientationData->interpolationIncrement_fx = ONE_IN_Q30;
1620 0 : move32();
1621 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
1622 0 : hCombinedOrientationData->Quaternions_ext_interpolation_target = identity;
1623 0 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1624 : {
1625 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1626 0 : move16();
1627 0 : hCombinedOrientationData->Quaternions[i] = identity;
1628 0 : hCombinedOrientationData->listenerPos[i] = origo;
1629 :
1630 0 : FOR( j = 0; j < 3; j++ )
1631 : {
1632 0 : set_zero_fx( hCombinedOrientationData->Rmat_fx[i][j], 3 );
1633 0 : hCombinedOrientationData->Rmat_fx[i][j][j] = ONE_IN_Q30;
1634 0 : move32();
1635 : }
1636 : }
1637 : }
1638 1195490 : ELSE IF( hExtOrientationData == NULL && headRotQuaternions != NULL )
1639 : {
1640 : /* Head rotation only */
1641 200750 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1642 : {
1643 160600 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1644 : }
1645 : }
1646 :
1647 1195490 : IF( hExtOrientationData != NULL )
1648 : {
1649 : /* External orientations */
1650 3190508 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1651 : {
1652 : /* Check for frozen external orientation */
1653 2035168 : IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
1654 : {
1655 17391 : IF( NE_16( hCombinedOrientationData->isExtOrientationFrozen, 1 ) )
1656 : {
1657 31 : hCombinedOrientationData->Quaternion_frozen_ext = hExtOrientationData->Quaternions[i];
1658 31 : hCombinedOrientationData->isExtOrientationFrozen = 1;
1659 31 : move16();
1660 : }
1661 : }
1662 : ELSE
1663 : {
1664 2017777 : hCombinedOrientationData->Quaternion_frozen_ext = identity;
1665 2017777 : hCombinedOrientationData->isExtOrientationFrozen = 0;
1666 2017777 : move16();
1667 : }
1668 2035168 : test();
1669 2035168 : IF( EQ_16( hExtOrientationData->enableRotationInterpolation[i], 1 ) && hExtOrientationData->enableExternalOrientation[i] > 0 )
1670 : {
1671 60970 : test();
1672 60970 : test();
1673 60970 : IF( EQ_16( hCombinedOrientationData->isInterpolationOngoing, true ) && LE_32( hCombinedOrientationData->interpolationCoefficient_fx, ONE_IN_Q30 ) && EQ_16( are_orientations_same_fx( &hCombinedOrientationData->Quaternions_ext_interpolation_target, &hExtOrientationData->Quaternions[i] ), true ) )
1674 : {
1675 : /* Continue interpolation */
1676 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_start.w_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.w_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
1677 50636 : move32();
1678 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_start.x_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.x_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
1679 50636 : move32();
1680 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_start.y_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.y_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
1681 50636 : move32();
1682 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_start.z_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.z_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
1683 50636 : move32();
1684 :
1685 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_target.w_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.w_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
1686 50636 : move32();
1687 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_target.x_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.x_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
1688 50636 : move32();
1689 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_target.y_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.y_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
1690 50636 : move32();
1691 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_target.z_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.z_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
1692 50636 : move32();
1693 :
1694 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact = Q29;
1695 50636 : move16();
1696 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact = Q29;
1697 50636 : move16();
1698 50636 : QuaternionSlerp_fx( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient_fx, &hCombinedOrientationData->Quaternions[i] );
1699 50636 : hCombinedOrientationData->interpolationCoefficient_fx = L_add( hCombinedOrientationData->interpolationIncrement_fx, hCombinedOrientationData->interpolationCoefficient_fx );
1700 50636 : move32();
1701 : }
1702 : ELSE
1703 : {
1704 : /* Stop interpolation or check for new interpolation */
1705 10334 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1706 10334 : move16();
1707 10334 : hCombinedOrientationData->interpolationCoefficient_fx = ONE_IN_Q30;
1708 10334 : move32();
1709 10334 : hCombinedOrientationData->interpolationIncrement_fx = ONE_IN_Q30;
1710 10334 : move32();
1711 10334 : external_target_interpolation_fx( hExtOrientationData, hCombinedOrientationData, i );
1712 10334 : Word16 l_shift = 0;
1713 10334 : move16();
1714 10334 : l_shift = s_min( s_min( Q_factor_L_32( hCombinedOrientationData->Quaternions[i].w_fx ), Q_factor_L_32( hCombinedOrientationData->Quaternions[i].x_fx ) ), s_min( Q_factor_L_32( hCombinedOrientationData->Quaternions[i].y_fx ), Q_factor_L_32( hCombinedOrientationData->Quaternions[i].z_fx ) ) );
1715 10334 : hCombinedOrientationData->Quaternions[i].w_fx = L_shl( hCombinedOrientationData->Quaternions[i].w_fx, l_shift ); // q_fact+l_shift
1716 10334 : move32();
1717 10334 : hCombinedOrientationData->Quaternions[i].x_fx = L_shl( hCombinedOrientationData->Quaternions[i].x_fx, l_shift ); // q_fact+l_shift
1718 10334 : move32();
1719 10334 : hCombinedOrientationData->Quaternions[i].y_fx = L_shl( hCombinedOrientationData->Quaternions[i].y_fx, l_shift ); // q_fact+l_shift
1720 10334 : move32();
1721 10334 : hCombinedOrientationData->Quaternions[i].z_fx = L_shl( hCombinedOrientationData->Quaternions[i].z_fx, l_shift ); // q_fact+l_shift
1722 10334 : move32();
1723 10334 : hCombinedOrientationData->Quaternions[i].q_fact = add( hCombinedOrientationData->Quaternions[i].q_fact, l_shift );
1724 10334 : move16();
1725 : }
1726 : }
1727 : ELSE
1728 : {
1729 : /* Interpolation disabled, use the current orientation values */
1730 :
1731 : /* Use the most recent external orientation */
1732 1974198 : IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 1 ) )
1733 : {
1734 36393 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
1735 : }
1736 : /* Use the freezed external orientation */
1737 1937805 : ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
1738 : {
1739 12803 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_ext;
1740 : }
1741 : }
1742 : }
1743 : }
1744 1195490 : test();
1745 1195490 : IF( hExtOrientationData != NULL && headRotQuaternions != NULL )
1746 : {
1747 : /* Combine head and external orientations */
1748 614084 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1749 : {
1750 : /* Check for frozen head rotation */
1751 404860 : IF( EQ_16( hExtOrientationData->enableHeadRotation[i], 2 ) )
1752 : {
1753 12400 : IF( NE_16( hCombinedOrientationData->isHeadRotationFrozen, 1 ) )
1754 : {
1755 62 : hCombinedOrientationData->Quaternion_frozen_head = headRotQuaternions[i];
1756 62 : hCombinedOrientationData->isHeadRotationFrozen = 1;
1757 62 : move16();
1758 : }
1759 : }
1760 : ELSE
1761 : {
1762 392460 : hCombinedOrientationData->Quaternion_frozen_head = identity;
1763 392460 : hCombinedOrientationData->isHeadRotationFrozen = 0;
1764 392460 : move16();
1765 : }
1766 : /* Use the most recent head rotation */
1767 404860 : IF( EQ_16( hExtOrientationData->enableHeadRotation[i], 1 ) )
1768 : {
1769 371655 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 )
1770 : {
1771 76961 : QuaternionProduct_fx( hCombinedOrientationData->Quaternions[i], headRotQuaternions[i], &hCombinedOrientationData->Quaternions[i] );
1772 : }
1773 : ELSE
1774 : {
1775 294694 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1776 : }
1777 : }
1778 : /* Use the freezed head rotation */
1779 33205 : ELSE IF( EQ_16( hExtOrientationData->enableHeadRotation[i], 2 ) )
1780 : {
1781 12400 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 )
1782 : {
1783 12400 : QuaternionProduct_fx( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Quaternion_frozen_head, &hCombinedOrientationData->Quaternions[i] );
1784 : }
1785 : ELSE
1786 : {
1787 0 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_head;
1788 : }
1789 : }
1790 : /* Reset the combined orientations to identity */
1791 404860 : test();
1792 404860 : if ( hExtOrientationData->enableHeadRotation[i] == 0 && hExtOrientationData->enableExternalOrientation[i] == 0 )
1793 : {
1794 0 : hCombinedOrientationData->Quaternions[i] = identity;
1795 : }
1796 : }
1797 : }
1798 1195490 : test();
1799 1195490 : IF( headRotQuaternions != NULL || hExtOrientationData != NULL )
1800 : {
1801 : /* Calculate the combined rotation matrix */
1802 3391258 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1803 : {
1804 2195768 : QuatToRotMat_fx( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Rmat_fx[i] );
1805 : }
1806 : }
1807 :
1808 : /* Save the current orientations */
1809 1195490 : IF( hExtOrientationData != NULL )
1810 : {
1811 1155340 : IF( hExtOrientationData->enableExternalOrientation[hExtOrientationData->num_subframes - 1] > 0 )
1812 : {
1813 27557 : hCombinedOrientationData->Quaternion_prev_extOrientation = hExtOrientationData->Quaternions[hExtOrientationData->num_subframes - 1];
1814 : }
1815 : ELSE
1816 : {
1817 1127783 : hCombinedOrientationData->Quaternion_prev_extOrientation = identity;
1818 : }
1819 : }
1820 1195490 : IF( headRotQuaternions != NULL )
1821 : {
1822 814834 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1823 : {
1824 565460 : hCombinedOrientationData->listenerPos[i] = listenerPos[i];
1825 : }
1826 : }
1827 : /* Check if combined orientation is enabled */
1828 1195490 : test();
1829 1195490 : test();
1830 1195490 : test();
1831 1195490 : IF( headRotQuaternions != NULL && hExtOrientationData == NULL )
1832 : {
1833 200750 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1834 : {
1835 160600 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1836 160600 : move16();
1837 : }
1838 : }
1839 1155340 : ELSE IF( headRotQuaternions == NULL && hExtOrientationData != NULL )
1840 : {
1841 2576424 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1842 : {
1843 1630308 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 )
1844 : {
1845 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1846 0 : move16();
1847 : }
1848 : ELSE
1849 : {
1850 1630308 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1851 1630308 : move16();
1852 : }
1853 : }
1854 : }
1855 209224 : ELSE IF( headRotQuaternions != NULL && hExtOrientationData != NULL )
1856 : {
1857 614084 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1858 : {
1859 404860 : test();
1860 404860 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 || ( hExtOrientationData->enableHeadRotation[i] > 0 ) )
1861 : {
1862 404860 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1863 404860 : move16();
1864 : }
1865 : ELSE
1866 : {
1867 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1868 0 : move16();
1869 : }
1870 : }
1871 : }
1872 : ELSE
1873 : {
1874 0 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1875 : {
1876 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1877 0 : move16();
1878 : }
1879 : }
1880 :
1881 1195490 : hCombinedOrientationData->subframe_idx = 0;
1882 1195490 : move16();
1883 1195490 : hCombinedOrientationData->cur_subframe_samples_rendered = 0;
1884 1195490 : move16();
1885 1195490 : hCombinedOrientationData->subframe_idx_start = 0;
1886 1195490 : move16();
1887 1195490 : hCombinedOrientationData->cur_subframe_samples_rendered_start = 0;
1888 1195490 : move16();
1889 3391258 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1890 : {
1891 8783072 : FOR( j = 0; j < 3; j++ )
1892 : {
1893 26349216 : FOR( Word16 k = 0; k < 3; k++ )
1894 : {
1895 19761912 : hCombinedOrientationData->Rmat_fx[i][j][k] = L_shl( hCombinedOrientationData->Rmat_fx[i][j][k], sub( 62, shl( hCombinedOrientationData->Quaternions[i].q_fact, 1 ) ) ); // Q30
1896 19761912 : move32();
1897 : }
1898 : }
1899 : }
1900 :
1901 1195490 : Word16 l_shift = 0;
1902 1195490 : move16();
1903 3391258 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1904 : {
1905 2195768 : l_shift = s_min( s_min( Q_factor_L_32( hCombinedOrientationData->Quaternions[i].w_fx ), Q_factor_L_32( hCombinedOrientationData->Quaternions[i].x_fx ) ), s_min( Q_factor_L_32( hCombinedOrientationData->Quaternions[i].y_fx ), Q_factor_L_32( hCombinedOrientationData->Quaternions[i].z_fx ) ) );
1906 2195768 : hCombinedOrientationData->Quaternions[i].w_fx = L_shl( hCombinedOrientationData->Quaternions[i].w_fx, l_shift ); // q_fact+l_shift
1907 2195768 : move32();
1908 2195768 : hCombinedOrientationData->Quaternions[i].x_fx = L_shl( hCombinedOrientationData->Quaternions[i].x_fx, l_shift ); // q_fact+l_shift
1909 2195768 : move32();
1910 2195768 : hCombinedOrientationData->Quaternions[i].y_fx = L_shl( hCombinedOrientationData->Quaternions[i].y_fx, l_shift ); // q_fact+l_shift
1911 2195768 : move32();
1912 2195768 : hCombinedOrientationData->Quaternions[i].z_fx = L_shl( hCombinedOrientationData->Quaternions[i].z_fx, l_shift ); // q_fact+l_shift
1913 2195768 : move32();
1914 2195768 : hCombinedOrientationData->Quaternions[i].q_fact = add( hCombinedOrientationData->Quaternions[i].q_fact, l_shift );
1915 2195768 : move16();
1916 : }
1917 1195490 : return IVAS_ERR_OK;
1918 : }
1919 :
1920 : /*-------------------------------------------------------------------------
1921 : * external_target_interpolation()
1922 : *
1923 : *
1924 : *------------------------------------------------------------------------*/
1925 10334 : static void external_target_interpolation_fx(
1926 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1927 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1928 : const Word16 i /* i : subframe index */
1929 : )
1930 : {
1931 : /* Sanity check for number of frames */
1932 10334 : hExtOrientationData->numFramesToTargetOrientation[i] = s_min( hExtOrientationData->numFramesToTargetOrientation[i], hCombinedOrientationData->maximumFramesToTargetOrientation );
1933 10334 : move16();
1934 10334 : hExtOrientationData->numFramesToTargetOrientation[i] = s_max( hExtOrientationData->numFramesToTargetOrientation[i], 0 );
1935 10334 : move16();
1936 :
1937 : /* Interpolate from the current orientation to the target orientation */
1938 10334 : IF( hExtOrientationData->numFramesToTargetOrientation[i] > 0 )
1939 : {
1940 3349 : IF( EQ_16( are_orientations_same_fx( &hCombinedOrientationData->Quaternions_ext_interpolation_target, &hExtOrientationData->Quaternions[i] ), false ) )
1941 : {
1942 : /* Target orientation is different from the previous target, update the values */
1943 :
1944 : /* Set the received orientation as the target */
1945 249 : hCombinedOrientationData->Quaternions_ext_interpolation_target = hExtOrientationData->Quaternions[i];
1946 :
1947 : /* Use the most recent external orientation as the starting orientation */
1948 249 : IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 1 ) )
1949 : {
1950 218 : IF( i > 0 )
1951 : {
1952 141 : IF( hExtOrientationData->enableExternalOrientation[i - 1] == 0 )
1953 : {
1954 : IVAS_QUATERNION identity;
1955 31 : identity.w_fx = ONE_IN_Q31;
1956 31 : move32();
1957 31 : identity.x_fx = identity.y_fx = identity.z_fx = 0;
1958 31 : move32();
1959 31 : move32();
1960 31 : move32();
1961 31 : identity.q_fact = 31;
1962 31 : move16();
1963 31 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
1964 : }
1965 110 : ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i - 1], 2 ) )
1966 : {
1967 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1968 : }
1969 : ELSE
1970 : {
1971 110 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hExtOrientationData->Quaternions[i - 1];
1972 : }
1973 : }
1974 : ELSE
1975 : {
1976 77 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_prev_extOrientation;
1977 : }
1978 : }
1979 31 : ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
1980 : {
1981 31 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1982 : }
1983 249 : Word16 tmp_e = 0;
1984 249 : move16();
1985 : Word32 tmp;
1986 : /* Calculate the interpolation increment and coefficient */
1987 249 : tmp = BASOP_Util_Divide3232_Scale_newton( ONE_IN_Q30, L_shl( L_deposit_l( hExtOrientationData->numFramesToTargetOrientation[i] ), 2 ), &tmp_e );
1988 249 : hCombinedOrientationData->interpolationIncrement_fx = L_shl( tmp, sub( tmp_e, 31 ) ); /* Q30 */
1989 249 : move32();
1990 249 : hCombinedOrientationData->interpolationCoefficient_fx = hCombinedOrientationData->interpolationIncrement_fx;
1991 249 : move32();
1992 : }
1993 :
1994 : /* Interpolate */
1995 3349 : hCombinedOrientationData->isInterpolationOngoing = TRUE;
1996 3349 : move16();
1997 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_start.w_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.w_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
1998 3349 : move32();
1999 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_start.x_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.x_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
2000 3349 : move32();
2001 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_start.y_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.y_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
2002 3349 : move32();
2003 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_start.z_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_start.z_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact, Q29 ) ); // Q29
2004 3349 : move32();
2005 :
2006 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_target.w_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.w_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
2007 3349 : move32();
2008 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_target.x_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.x_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
2009 3349 : move32();
2010 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_target.y_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.y_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
2011 3349 : move32();
2012 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_target.z_fx = L_shr( hCombinedOrientationData->Quaternions_ext_interpolation_target.z_fx, sub( hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact, Q29 ) ); // Q29
2013 3349 : move32();
2014 :
2015 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact = Q29;
2016 3349 : move16();
2017 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact = Q29;
2018 3349 : move16();
2019 3349 : QuaternionSlerp_fx( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient_fx, &hCombinedOrientationData->Quaternions[i] );
2020 3349 : hCombinedOrientationData->interpolationCoefficient_fx = L_add_sat( hCombinedOrientationData->interpolationCoefficient_fx, hCombinedOrientationData->interpolationIncrement_fx );
2021 3349 : move32();
2022 : }
2023 : ELSE
2024 : {
2025 : /* Use the target orientation immediately */
2026 6985 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
2027 6985 : move16();
2028 6985 : hCombinedOrientationData->interpolationCoefficient_fx = ONE_IN_Q30;
2029 6985 : move32();
2030 6985 : hCombinedOrientationData->interpolationIncrement_fx = ONE_IN_Q30;
2031 6985 : move32();
2032 6985 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
2033 : }
2034 :
2035 10334 : return;
2036 : }
2037 :
2038 : /*-------------------------------------------------------------------------
2039 : * are_orientations_same()
2040 : *
2041 : *
2042 : *------------------------------------------------------------------------*/
2043 54203 : static bool are_orientations_same_fx(
2044 : const IVAS_QUATERNION *orientation1,
2045 : const IVAS_QUATERNION *orientation2 )
2046 : {
2047 54203 : bool orientationsAreSame = true;
2048 54203 : move16();
2049 54203 : Word32 error_margin_fx = 107374182; // 0.05f in Q31
2050 54203 : move32();
2051 54203 : Word16 error_margin_e = 0;
2052 54203 : move16();
2053 54203 : Word16 w_e = 0, x_e = 0, y_e = 0, z_e = 0;
2054 54203 : move16();
2055 54203 : move16();
2056 54203 : move16();
2057 54203 : move16();
2058 54203 : Word32 w_result = 0, x_result = 0, y_result = 0, z_result = 0;
2059 54203 : move32();
2060 54203 : move32();
2061 54203 : move32();
2062 54203 : move32();
2063 54203 : w_result = L_abs( BASOP_Util_Add_Mant32Exp( orientation1->w_fx, sub( 31, orientation1->q_fact ), L_negate( orientation2->w_fx ), sub( 31, orientation2->q_fact ), &w_e ) );
2064 54203 : x_result = L_abs( BASOP_Util_Add_Mant32Exp( orientation1->x_fx, sub( 31, orientation1->q_fact ), L_negate( orientation2->x_fx ), sub( 31, orientation2->q_fact ), &x_e ) );
2065 54203 : y_result = L_abs( BASOP_Util_Add_Mant32Exp( orientation1->y_fx, sub( 31, orientation1->q_fact ), L_negate( orientation2->y_fx ), sub( 31, orientation2->q_fact ), &y_e ) );
2066 54203 : z_result = L_abs( BASOP_Util_Add_Mant32Exp( orientation1->z_fx, sub( 31, orientation1->q_fact ), L_negate( orientation2->z_fx ), sub( 31, orientation2->q_fact ), &z_e ) );
2067 54203 : Word16 Flag_1 = BASOP_Util_Cmp_Mant32Exp( w_result, w_e, error_margin_fx, error_margin_e );
2068 54203 : Word16 Flag_2 = BASOP_Util_Cmp_Mant32Exp( x_result, x_e, error_margin_fx, error_margin_e );
2069 54203 : Word16 Flag_3 = BASOP_Util_Cmp_Mant32Exp( y_result, y_e, error_margin_fx, error_margin_e );
2070 54203 : Word16 Flag_4 = BASOP_Util_Cmp_Mant32Exp( z_result, z_e, error_margin_fx, error_margin_e );
2071 :
2072 54203 : test();
2073 54203 : test();
2074 54203 : test();
2075 108073 : if ( EQ_16( Flag_1, 1 ) ||
2076 107740 : EQ_16( Flag_2, 1 ) ||
2077 107740 : EQ_16( Flag_3, 1 ) ||
2078 53870 : EQ_16( Flag_4, 1 ) )
2079 : {
2080 467 : orientationsAreSame = false;
2081 467 : move16();
2082 : }
2083 :
2084 54203 : return orientationsAreSame;
2085 : }
2086 :
2087 :
2088 : /*-----------------------------------------------------------------------*
2089 : * Local Function definitions
2090 : *-----------------------------------------------------------------------*/
2091 : /*-------------------------------------------------------------------------
2092 : * Helper functions used by SHrotmatgen,
2093 : * an implementation of the algorithm in
2094 : * Ivanic, J. & Ruedenberg, K., J. Phys. Chem. 100, 6342 (1996)
2095 : *------------------------------------------------------------------------*/
2096 11445085 : static Word32 SHrot_p_fx(
2097 : const Word16 i,
2098 : const Word16 l,
2099 : const Word16 a,
2100 : const Word16 b,
2101 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2102 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] ) // Q14
2103 : {
2104 :
2105 11445085 : Word16 ri1 = 0, rim1 = 0, ri0 = 0, R_lm1_1 = 0, R_lm1_2 = 0;
2106 11445085 : move16();
2107 11445085 : move16();
2108 11445085 : move16();
2109 11445085 : move16();
2110 11445085 : move16();
2111 :
2112 11445085 : Word32 p = 0;
2113 11445085 : move32();
2114 :
2115 11445085 : ri1 = SHrotmat[i + 1 + 1][1 + 1 + 1]; // Q14
2116 11445085 : move16();
2117 11445085 : rim1 = SHrotmat[i + 1 + 1][-1 + 1 + 1]; // Q14
2118 11445085 : move16();
2119 11445085 : ri0 = SHrotmat[i + 1 + 1][0 + 1 + 1]; // Q14
2120 11445085 : move16();
2121 :
2122 11445085 : IF( EQ_16( b, -l ) )
2123 : {
2124 1886655 : R_lm1_1 = R_lm1[a + l - 1][0]; // Q14
2125 1886655 : move16();
2126 1886655 : R_lm1_2 = R_lm1[a + l - 1][2 * l - 2]; // Q14
2127 1886655 : move16();
2128 1886655 : p = L_mac0( L_mult0( ri1, R_lm1_1 ), rim1, R_lm1_2 ); // Q28
2129 : }
2130 : ELSE
2131 : {
2132 9558430 : IF( EQ_16( b, l ) )
2133 : {
2134 1886655 : R_lm1_1 = R_lm1[a + l - 1][2 * l - 2]; // Q14
2135 1886655 : move16();
2136 1886655 : R_lm1_2 = R_lm1[a + l - 1][0]; // Q14
2137 1886655 : move16();
2138 1886655 : p = L_msu0( L_mult0( ri1, R_lm1_1 ), rim1, R_lm1_2 ); // Q28
2139 : }
2140 : ELSE
2141 : {
2142 7671775 : R_lm1_1 = R_lm1[a + l - 1][b + l - 1];
2143 7671775 : move16();
2144 7671775 : p = L_mult0( ri0, R_lm1_1 ); // Q28
2145 : }
2146 : }
2147 :
2148 11445085 : return p; // Q28
2149 : }
2150 :
2151 2546975 : static Word32 SHrot_u_fx(
2152 : const Word16 l,
2153 : const Word16 m,
2154 : const Word16 n,
2155 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2156 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] // Q14
2157 : )
2158 : {
2159 2546975 : return SHrot_p_fx( 0, l, m, n, SHrotmat, R_lm1 );
2160 : }
2161 :
2162 3836765 : static Word32 SHrot_v_fx(
2163 : const Word16 l,
2164 : const Word16 m,
2165 : const Word16 n,
2166 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2167 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] // Q14
2168 : )
2169 : {
2170 :
2171 : Word32 result;
2172 : Word32 p0, p1;
2173 : Word16 d;
2174 :
2175 3836765 : IF( m == 0 )
2176 : {
2177 644895 : p0 = SHrot_p_fx( 1, l, 1, n, SHrotmat, R_lm1 ); // Q28
2178 644895 : p1 = SHrot_p_fx( -1, l, -1, n, SHrotmat, R_lm1 ); // Q28
2179 644895 : result = L_shr( L_add( p0, p1 ), 2 ); // converting to Q27
2180 : }
2181 : ELSE
2182 : {
2183 3191870 : IF( m > 0 )
2184 : {
2185 1595935 : d = 0;
2186 1595935 : move16();
2187 1595935 : if ( EQ_16( m, 1 ) )
2188 : {
2189 644895 : d = 1;
2190 644895 : move16();
2191 : }
2192 1595935 : p0 = SHrot_p_fx( 1, l, sub( m, 1 ), n, SHrotmat, R_lm1 ); // Q28
2193 1595935 : p1 = SHrot_p_fx( -1, l, add( negate( m ), 1 ), n, SHrotmat, R_lm1 ); // Q28
2194 1595935 : result = Msub_32_16_r( Mpy_32_16_r( p0, square_root16_table[1 + d] ), p1, shl( sub( 1, d ), 14 ) ); // Q27
2195 : }
2196 : ELSE
2197 : {
2198 1595935 : d = 0;
2199 1595935 : move16();
2200 1595935 : if ( EQ_16( m, -1 ) )
2201 : {
2202 644895 : d = 1;
2203 644895 : move16();
2204 : }
2205 1595935 : p0 = SHrot_p_fx( 1, l, add( m, 1 ), n, SHrotmat, R_lm1 );
2206 1595935 : p1 = SHrot_p_fx( -1, l, negate( add( m, 1 ) ), n, SHrotmat, R_lm1 );
2207 1595935 : result = Madd_32_16_r( Mpy_32_16_r( p0, shl( sub( 1, d ), 14 ) ), p1, square_root16_table[1 + d] ); // Q27
2208 : }
2209 : }
2210 3836765 : return result; // Q27
2211 : }
2212 :
2213 612290 : static Word32 SHrot_w_fx(
2214 : const Word16 l,
2215 : const Word16 m,
2216 : const Word16 n,
2217 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2218 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] // Q14
2219 : )
2220 : {
2221 : Word32 result, p0, p1;
2222 :
2223 612290 : IF( m == 0 )
2224 : {
2225 0 : printf( "ERROR should not be called\n" );
2226 0 : return 0;
2227 : }
2228 : ELSE
2229 : {
2230 612290 : IF( m > 0 )
2231 : {
2232 306145 : p0 = SHrot_p_fx( 1, l, add( m, 1 ), n, SHrotmat, R_lm1 ); // Q28
2233 306145 : p1 = SHrot_p_fx( -1, l, negate( add( m, 1 ) ), n, SHrotmat, R_lm1 ); // Q28
2234 306145 : result = L_add( p0, p1 ); // Q28
2235 : }
2236 : ELSE
2237 : {
2238 306145 : p0 = SHrot_p_fx( 1, l, sub( m, 1 ), n, SHrotmat, R_lm1 ); // Q28
2239 306145 : p1 = SHrot_p_fx( -1, l, sub( 1, m ), n, SHrotmat, R_lm1 ); // Q28
2240 306145 : result = L_sub( p0, p1 ); // Q28
2241 : }
2242 : }
2243 :
2244 612290 : return result; // Q28
2245 : }
2246 :
2247 :
2248 : /*-------------------------------------------------------------------------
2249 : * SHrotmatgen_fx()
2250 : *
2251 : *
2252 : *------------------------------------------------------------------------*/
2253 :
2254 91765 : void SHrotmatgen_fx(
2255 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], /* o : rotation matrix in SHD Q14 */
2256 : Word32 Rmat[3][3], /* i : real-space rotation matrix Q30 */
2257 : const Word16 order /* i : ambisonics order */
2258 : )
2259 : {
2260 91765 : Word16 d = 0;
2261 91765 : move16();
2262 91765 : Word16 band_idx = 0;
2263 91765 : move16();
2264 : Word16 i, j;
2265 : Word16 l, m, n;
2266 : Word16 absm;
2267 91765 : Word16 sqdenom = 0, sql2mm2 = 0, sqdabsm = 0, sqlabsm = 0;
2268 91765 : Word16 u = 0, v = 0, w = 0;
2269 91765 : move16();
2270 91765 : move16();
2271 91765 : move16();
2272 91765 : move16();
2273 91765 : move16();
2274 91765 : move16();
2275 91765 : move16();
2276 :
2277 91765 : Word32 u32_fx = 0, v32_fx = 0, w32_fx = 0;
2278 91765 : move32();
2279 91765 : move32();
2280 91765 : move32();
2281 :
2282 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
2283 :
2284 1560005 : FOR( i = 0; i < HEADROT_SHMAT_DIM; i++ )
2285 : {
2286 1468240 : set16_fx( R_lm1[i], 0, HEADROT_SHMAT_DIM );
2287 : }
2288 : Word16 R_l[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
2289 : Word32 result;
2290 91765 : SHrotmat[0][0] = ONE_IN_Q14;
2291 91765 : move16();
2292 :
2293 91765 : SHrotmat[1][1] = extract_h( Rmat[1][1] ); // Q14
2294 91765 : move16();
2295 91765 : SHrotmat[1][2] = extract_h( Rmat[1][2] );
2296 91765 : move16();
2297 91765 : SHrotmat[1][3] = extract_h( Rmat[1][0] );
2298 91765 : move16();
2299 :
2300 91765 : SHrotmat[2][1] = extract_h( Rmat[2][1] ); // Q14
2301 91765 : move16();
2302 91765 : SHrotmat[2][2] = extract_h( Rmat[2][2] );
2303 91765 : move16();
2304 91765 : SHrotmat[2][3] = extract_h( Rmat[2][0] );
2305 91765 : move16();
2306 :
2307 91765 : SHrotmat[3][1] = extract_h( Rmat[0][1] ); // Q14
2308 91765 : move16();
2309 91765 : SHrotmat[3][2] = extract_h( Rmat[0][2] );
2310 91765 : move16();
2311 91765 : SHrotmat[3][3] = extract_h( Rmat[0][0] );
2312 91765 : move16();
2313 :
2314 367060 : FOR( i = 0; i < 2 * 1 + 1; i++ )
2315 : {
2316 1101180 : FOR( j = 0; j < 2 * 1 + 1; j++ )
2317 : {
2318 825885 : R_lm1[i][j] = SHrotmat[i + 1][j + 1]; // Q14
2319 825885 : move16();
2320 : }
2321 : }
2322 91765 : band_idx = 4;
2323 91765 : move16();
2324 203250 : FOR( l = 2; l <= order; l++ )
2325 : {
2326 111485 : set16_fx( &R_l[0][0], 0, HEADROT_SHMAT_DIM2 );
2327 756380 : FOR( m = -l; m <= l; m++ )
2328 : {
2329 644895 : d = 0;
2330 644895 : move16();
2331 644895 : if ( m == 0 )
2332 : {
2333 111485 : d = 1;
2334 111485 : move16();
2335 : }
2336 644895 : absm = extract_l( L_abs( m ) );
2337 :
2338 644895 : sql2mm2 = square_root30_q12[l * l - m * m]; // Q12
2339 644895 : move16();
2340 644895 : sqdabsm = square_root30_q12[( 1 + d ) * ( l + absm - 1 ) * ( l + absm )]; // Q12
2341 644895 : move16();
2342 644895 : sqlabsm = square_root30_q12[( ( l - ( absm + 1 ) ) * ( l - absm ) )]; // Q12
2343 644895 : move16();
2344 :
2345 4481660 : FOR( n = -l; n <= l; n++ )
2346 : {
2347 3836765 : IF( EQ_16( abs_s( n ), l ) )
2348 : {
2349 1289790 : sqdenom = square_root30_q12[( ( 2 * l ) * ( 2 * l - 1 ) )]; // Q12
2350 1289790 : move16();
2351 : }
2352 : ELSE
2353 : {
2354 2546975 : sqdenom = square_root30_q12[( l * l - n * n )]; // Q12
2355 2546975 : move16();
2356 : }
2357 :
2358 :
2359 3836765 : u = div_l( L_shl( (Word32) sql2mm2, 15 ), sqdenom ); // Q14
2360 3836765 : v = imult1616( div_l( L_shl( (Word32) sqdabsm, 14 ), sqdenom ), sub( 1, shl( d, 1 ) ) ); // Q14
2361 3836765 : w = imult1616( div_l( L_shl( (Word32) sqlabsm, 14 ), sqdenom ), negate( sub( 1, d ) ) ); // Q14
2362 :
2363 3836765 : IF( u != 0 )
2364 : {
2365 2546975 : result = SHrot_u_fx( l, m, n, SHrotmat, R_lm1 ); // Q28
2366 2546975 : u32_fx = Mpy_32_16_r( result, u ); // Q27
2367 : }
2368 3836765 : IF( v != 0 )
2369 : {
2370 3836765 : result = SHrot_v_fx( l, m, n, SHrotmat, R_lm1 );
2371 3836765 : v32_fx = Mpy_32_16_r( result, v ); // Q26
2372 : }
2373 3836765 : IF( w != 0 )
2374 : {
2375 612290 : result = SHrot_w_fx( l, m, n, SHrotmat, R_lm1 );
2376 612290 : w32_fx = Mpy_32_16_r( result, w ); // Q27
2377 : }
2378 : // Addind and converting to 16 bit integer of Q14
2379 3836765 : R_l[m + l][n + l] = extract_h( L_shl( L_add( L_add( L_shr( u32_fx, 1 ), v32_fx ), L_shr( w32_fx, 1 ) ), 4 ) ); // Q14
2380 3836765 : move16();
2381 : }
2382 : }
2383 :
2384 756380 : FOR( i = 0; i < 2 * l + 1; i++ )
2385 : {
2386 4481660 : FOR( j = 0; j < 2 * l + 1; j++ )
2387 : {
2388 3836765 : SHrotmat[band_idx + i][band_idx + j] = R_l[i][j]; // Q14
2389 3836765 : move16();
2390 : }
2391 : }
2392 :
2393 756380 : FOR( i = 0; i < 2 * l + 1; i++ )
2394 : {
2395 4481660 : FOR( j = 0; j < 2 * l + 1; j++ )
2396 : {
2397 3836765 : R_lm1[i][j] = R_l[i][j];
2398 3836765 : move16();
2399 : }
2400 : }
2401 :
2402 111485 : band_idx = add( band_idx, add( shl( l, 1 ), 1 ) );
2403 : }
2404 :
2405 91765 : return;
2406 : }
2407 :
2408 :
2409 : /*-------------------------------------------------------------------------
2410 : * ivas_combined_orientation_update_index()
2411 : *
2412 : * update read index based on the number of rendered samples
2413 : *------------------------------------------------------------------------*/
2414 :
2415 2032459 : void ivas_combined_orientation_update_index(
2416 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
2417 : const Word16 samples_rendered /* i : samples rendered since the last call */
2418 : )
2419 : {
2420 : Word16 exp, div_result;
2421 2032459 : IF( hCombinedOrientationData != NULL )
2422 : {
2423 795410 : IF( EQ_16( hCombinedOrientationData->num_subframes, 1 ) )
2424 : {
2425 : /* only one orientation available anyway or split rendering with low resolution*/
2426 263202 : hCombinedOrientationData->subframe_idx = 0;
2427 263202 : move16();
2428 : }
2429 : ELSE
2430 : {
2431 532208 : hCombinedOrientationData->cur_subframe_samples_rendered = add( hCombinedOrientationData->cur_subframe_samples_rendered, samples_rendered );
2432 532208 : move16();
2433 532208 : div_result = BASOP_Util_Divide3216_Scale( hCombinedOrientationData->cur_subframe_samples_rendered, hCombinedOrientationData->subframe_size, &exp );
2434 532208 : hCombinedOrientationData->subframe_idx = add( hCombinedOrientationData->subframe_idx, shl( div_result, add( exp, 1 ) ) );
2435 532208 : move16();
2436 532208 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size;
2437 532208 : move16();
2438 532208 : hCombinedOrientationData->subframe_idx = s_min( hCombinedOrientationData->subframe_idx, sub( hCombinedOrientationData->num_subframes, 1 ) );
2439 532208 : move16();
2440 : }
2441 : }
2442 :
2443 2032459 : return;
2444 : }
2445 :
2446 :
2447 : /*-------------------------------------------------------------------------
2448 : * ivas_combined_orientation_set_to_start_index()
2449 : *
2450 : * update read index based on the number of rendered samples
2451 : *------------------------------------------------------------------------*/
2452 :
2453 1946663 : void ivas_combined_orientation_set_to_start_index(
2454 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
2455 : )
2456 : {
2457 1946663 : IF( hCombinedOrientationData != NULL )
2458 : {
2459 1946440 : hCombinedOrientationData->subframe_idx = hCombinedOrientationData->subframe_idx_start;
2460 1946440 : move16();
2461 1946440 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered_start;
2462 1946440 : move16();
2463 : }
2464 :
2465 1946663 : return;
2466 : }
2467 : /*-------------------------------------------------------------------------
2468 : * ivas_combined_orientation_update_start_index()
2469 : *
2470 : * update start index based on the number of rendered samples
2471 : *------------------------------------------------------------------------*/
2472 :
2473 1538230 : void ivas_combined_orientation_update_start_index(
2474 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
2475 : const Word16 samples_rendered /* i : samples rendered since the last call */
2476 : )
2477 : {
2478 1538230 : IF( hCombinedOrientationData != NULL )
2479 : {
2480 1195490 : IF( EQ_16( hCombinedOrientationData->num_subframes, 1 ) )
2481 : {
2482 : /* only one orientation available anyway or split rendering with low resolution*/
2483 862064 : hCombinedOrientationData->subframe_idx = 0;
2484 862064 : move16();
2485 : }
2486 : ELSE
2487 : {
2488 333426 : hCombinedOrientationData->cur_subframe_samples_rendered_start = add( hCombinedOrientationData->cur_subframe_samples_rendered_start, samples_rendered );
2489 333426 : move16();
2490 333426 : hCombinedOrientationData->subframe_idx_start = add( hCombinedOrientationData->subframe_idx_start, mult( hCombinedOrientationData->cur_subframe_samples_rendered, div_s( 1, hCombinedOrientationData->subframe_size ) ) );
2491 333426 : move16();
2492 333426 : hCombinedOrientationData->cur_subframe_samples_rendered_start = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size; /* No operator to calculate modulo*/
2493 333426 : move16();
2494 333426 : hCombinedOrientationData->subframe_idx_start = s_min( hCombinedOrientationData->subframe_idx, sub( hCombinedOrientationData->num_subframes, 1 ) );
2495 333426 : move16();
2496 : }
2497 : }
2498 :
2499 1538230 : return;
2500 : }
|
