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