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 614 : void ivas_headTrack_close_fx(
163 : HEAD_TRACK_DATA_HANDLE *hHeadTrackData /* i/o: head track handle */
164 : )
165 : {
166 614 : test();
167 614 : IF( hHeadTrackData == NULL || *hHeadTrackData == NULL )
168 : {
169 532 : 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 3508030 : 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 3508030 : IF( GT_16( abs_s( azi_in ), 180 ) )
303 : {
304 1059127 : IF( azi_in > 0 )
305 : {
306 1059127 : azi_in = sub( azi_in, 360 );
307 : }
308 : ELSE
309 : {
310 0 : azi_in = add( azi_in, 360 );
311 : }
312 : }
313 3508030 : temp_16 = ele_in;
314 3508030 : move16();
315 3508030 : w_fx = cosine_table_Q31[abs_s( temp_16 )]; // Q31
316 3508030 : move32();
317 3508030 : temp_16 = azi_in;
318 3508030 : move16();
319 3508030 : dv_fx[0] = Mpy_32_32( w_fx, cosine_table_Q31[abs_s( temp_16 )] ); // Q31
320 3508030 : move32();
321 3508030 : temp_16 = azi_in;
322 3508030 : move16();
323 3508030 : dv_fx[1] = Mpy_32_32( w_fx, sine_table_Q31[add( temp_16, 180 )] ); // Q31
324 3508030 : move32();
325 3508030 : temp_16 = ele_in;
326 3508030 : move16();
327 3508030 : dv_fx[2] = sine_table_Q31[add( temp_16, 180 )]; // Q31
328 3508030 : move32();
329 : /*Rotation mtx multiplication*/
330 14032120 : FOR( n = 0; n < 3; n++ )
331 : {
332 10524090 : 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 10524090 : dv_r_fx[n] = L_add( Mpy_32_32( dv_fx[2], Rmat_fx[n][2] ), temp ); // Q30
334 10524090 : move32();
335 : }
336 :
337 : /*Conversion cartesian to spherical coordinates*/
338 3508030 : y = dv_r_fx[1];
339 3508030 : move32();
340 3508030 : x = dv_r_fx[0];
341 3508030 : move32();
342 3508030 : radian = atan2_fx( L_abs( L_shr( y, 15 ) ), L_abs( L_shr( x, 15 ) ) ); // Q14
343 :
344 3508030 : if ( y <= 0 )
345 : {
346 1760257 : radian = negate( radian );
347 : }
348 :
349 3508030 : IF( x < 0 )
350 : {
351 1272058 : IF( radian < 0 )
352 : {
353 603173 : angle = negate( add( 180, extract_l( L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), ( 24 ) ) ) ) ); // Q0
354 : }
355 : ELSE
356 : {
357 668885 : angle = sub( 180, extract_l( L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), ( 24 ) ) ) ); // Q0
358 : }
359 1272058 : IF( radian == 0 )
360 : {
361 1017 : if ( y < 0 )
362 : {
363 3 : angle = negate( angle );
364 : }
365 : }
366 : }
367 : ELSE
368 : {
369 2235972 : angle = extract_l( L_shr( Mpy_32_16_1( _180_OVER_PI_Q25, radian ), 24 ) ); // Q0
370 : }
371 :
372 3508030 : *azi = s_max( -180, s_min( 180, angle ) );
373 3508030 : move16();
374 :
375 3508030 : IF( isPlanar == 0 )
376 : {
377 3308030 : 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 3308030 : y = dv_r_fx[2];
379 3308030 : move32();
380 3308030 : x = sqrt_fx;
381 3308030 : move32();
382 3308030 : radian = atan2_fx( L_abs( L_shr( y, 15 ) ), L_abs( L_shr( x, 15 ) ) ); // Q14
383 3308030 : if ( y <= 0 )
384 : {
385 1922275 : radian = negate( radian );
386 : }
387 :
388 3308030 : 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 3308030 : angle = extract_l( L_shr( Mpy_32_16_r( _180_OVER_PI_Q25, radian ), ( 24 ) ) ); // Q0
409 : }
410 :
411 3308030 : *ele = s_max( -90, s_min( 90, angle ) ); // Q0
412 3308030 : move16();
413 : }
414 : ELSE
415 : {
416 200000 : *ele = 0;
417 200000 : move16();
418 : }
419 :
420 3508030 : 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 :
1026 : #ifdef FIX_1113_EXTREND_ISAR
1027 19311 : assert( ( nInChannels == HOA3_CHANNELS || nInChannels == HOA2_CHANNELS || nInChannels == FOA_CHANNELS ) &&
1028 : "Number of channels must correspond to an ambisonics order!" );
1029 : #else
1030 : assert( nInChannels == HEADROT_SHMAT_DIM && "Number of channels must be 16!" );
1031 : #endif
1032 :
1033 : /* initialize rotation matrices with zeros */
1034 328287 : FOR( i = 0; i < HEADROT_SHMAT_DIM; i++ )
1035 : {
1036 308976 : set16_fx( SHrotmat[i], 0, HEADROT_SHMAT_DIM );
1037 : }
1038 :
1039 : /* calculate Ambisonics rotation matrix from the quaternion */
1040 19311 : SHrotmatgen_fx( SHrotmat, Rmat, shd_rot_max_order );
1041 :
1042 : #ifdef DEBUGGING
1043 : dbgwrite_txt( (const float *) SHrotmat[0], HEADROT_SHMAT_DIM * HEADROT_SHMAT_DIM, "Fixed_SHrotmat.txt", NULL );
1044 : #endif
1045 :
1046 : /* rotation by mtx multiplication */
1047 96434 : FOR( i = 0; i < numTimeSlots; i++ )
1048 : {
1049 4704503 : FOR( iBand = 0; iBand < CLDFB_NO_CHANNELS_MAX; iBand++ )
1050 : {
1051 : /*As the rotation matrix becomes block diagonal in a SH basis, we can
1052 : apply each angular-momentum block individually to save complexity. */
1053 :
1054 : /* loop over l blocks */
1055 4627380 : m1 = 1;
1056 4627380 : move16();
1057 4627380 : m2 = 4;
1058 4627380 : move16();
1059 18509520 : FOR( l = 1; l <= shd_rot_max_order; l++ )
1060 : {
1061 : /* compute mtx-vector product for this l */
1062 83292840 : FOR( n = m1; n < m2; n++ )
1063 : {
1064 69410700 : realRot[n - m1] = 0;
1065 69410700 : move32();
1066 69410700 : imagRot[n - m1] = 0;
1067 69410700 : move32();
1068 453483240 : FOR( m = m1; m < m2; m++ )
1069 : {
1070 384072540 : realRot[n - m1] = Madd_32_16_r( realRot[n - m1], Cldfb_RealBuffer[m][i][iBand], SHrotmat[n][m] ); // Q(x + 14 - 15)
1071 384072540 : move32();
1072 384072540 : imagRot[n - m1] = Madd_32_16_r( imagRot[n - m1], Cldfb_ImagBuffer[m][i][iBand], SHrotmat[n][m] ); // Q(x + 14 - 15)
1073 384072540 : move32();
1074 : }
1075 : }
1076 : /* write back the result */
1077 83292840 : FOR( n = m1; n < m2; n++ )
1078 : {
1079 69410700 : Cldfb_RealBuffer[n][i][iBand] = realRot[n - m1]; // Qx - 1
1080 69410700 : move32();
1081 69410700 : Cldfb_ImagBuffer[n][i][iBand] = imagRot[n - m1]; // Qx - 1
1082 69410700 : move32();
1083 : }
1084 13882140 : m1 = m2;
1085 13882140 : move16();
1086 13882140 : m2 = add( m2, add( shl( add( l, 1 ), 1 ), 1 ) );
1087 : }
1088 :
1089 : /* unoptimized code for reference (full matrix multiplication)
1090 : for (n = 0; n < nInChannels; n++)
1091 : {
1092 : realRot[n] = 0.f;
1093 : imagRot[n] = 0.f;
1094 :
1095 : for (m = 0; m < nInChannels; m++)
1096 : {
1097 : realRot[n] += SHrotmat[n][m] * Cldfb_RealBuffer[m][i][iBand];
1098 : imagRot[n] += SHrotmat[n][m] * Cldfb_ImagBuffer[m][i][iBand];
1099 : }
1100 : }
1101 : for (n = 0; n < nInChannels; n++)
1102 : {
1103 : Cldfb_RealBuffer[n][i][iBand] = realRot[n];
1104 : Cldfb_ImagBuffer[n][i][iBand] = imagRot[n];
1105 : }
1106 : */
1107 : }
1108 : }
1109 : // To make the Q factor same as tha of the remaining vector values
1110 96555 : FOR( j = 0; j < MAX_PARAM_SPATIAL_SUBFRAMES; j++ )
1111 : {
1112 4711884 : FOR( k = 0; k < CLDFB_NO_CHANNELS_MAX; k++ )
1113 : {
1114 4634640 : Cldfb_RealBuffer[0][j][k] = L_shr( Cldfb_RealBuffer[0][j][k], 1 ); // Qx - 1
1115 4634640 : move32();
1116 4634640 : Cldfb_ImagBuffer[0][j][k] = L_shr( Cldfb_ImagBuffer[0][j][k], 1 ); // Qx - 1
1117 4634640 : move32();
1118 : }
1119 : }
1120 19311 : return;
1121 : }
1122 : /*-------------------------------------------------------------------------
1123 : * rotateFrame_sd_cldfb()
1124 : *
1125 : * Apply rotation to signals in Spatial Domain and in CLDFB
1126 : *------------------------------------------------------------------------*/
1127 :
1128 8000 : void rotateFrame_sd_cldfb_fixed(
1129 : Word32 Rmat_fx[3][3], /* i : real-space rotation matrix (Q30) */
1130 : Word32 Cldfb_RealBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain real part Qx */
1131 : Word32 Cldfb_ImagBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain imag part Qx */
1132 : const IVAS_OUTPUT_SETUP_HANDLE hOutputSetup, /* i : output format setup number of channels */
1133 : const EFAP_HANDLE hEFAPdata, /* i : EFAP structure */
1134 : const Word16 numTimeSlots, /* i : number of time slots to process */
1135 : const Word16 nb_band /* i : number of CLDFB bands to process */
1136 : )
1137 : {
1138 : Word16 iBlock, iBand, m, n;
1139 : Word32 gains_fx[MAX_CICP_CHANNELS - 1][MAX_CICP_CHANNELS - 1];
1140 : Word16 azimuth, elevation;
1141 : Word32 g1_fx;
1142 : Word32 realRot_fx[MAX_CICP_CHANNELS - 1][MAX_PARAM_SPATIAL_SUBFRAMES * CLDFB_NO_CHANNELS_MAX];
1143 : Word32 imagRot_fx[MAX_CICP_CHANNELS - 1][MAX_PARAM_SPATIAL_SUBFRAMES * CLDFB_NO_CHANNELS_MAX];
1144 : Word32 *p_realRot_fx, *p_imagRot_fx;
1145 : Word32 *p_real_fx, *p_imag_fx;
1146 : Word16 nInChannels;
1147 : Word16 isPlanar;
1148 8000 : push_wmops( "rotateFrame_sd_cldfb" );
1149 :
1150 8000 : nInChannels = hOutputSetup->nchan_out_woLFE;
1151 8000 : move16();
1152 8000 : isPlanar = 1;
1153 8000 : move16();
1154 48000 : FOR( n = 0; n < nInChannels; n++ )
1155 : {
1156 40000 : IF( hOutputSetup->ls_elevation_fx[n] != 0 )
1157 : {
1158 0 : isPlanar = 0;
1159 0 : move16();
1160 0 : BREAK;
1161 : }
1162 : }
1163 :
1164 : /* rotation of Euler angles */
1165 48000 : FOR( n = 0; n < nInChannels; n++ )
1166 : {
1167 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 );
1168 40000 : test();
1169 40000 : test();
1170 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 ) ) )
1171 : {
1172 36781 : efap_determine_gains_fx( hEFAPdata, gains_fx[n], L_shl( azimuth, Q22 ), L_shl( elevation, Q22 ), EFAP_MODE_EFAP );
1173 36781 : scale_sig32( gains_fx[n], hEFAPdata->numSpk, Q1 ); // gains_fx: Q31
1174 : }
1175 : ELSE
1176 : {
1177 3219 : set32_fx( gains_fx[n], 0, nInChannels );
1178 3219 : gains_fx[n][n] = 0x7fffffff; // 1 in Q31
1179 3219 : move32();
1180 : }
1181 : }
1182 :
1183 : /* Apply panning gains by mtx multiplication*/
1184 48000 : FOR( n = 0; n < nInChannels; n++ )
1185 : {
1186 40000 : set32_fx( realRot_fx[n], 0, shl( nb_band, 2 ) );
1187 40000 : set32_fx( imagRot_fx[n], 0, shl( nb_band, 2 ) );
1188 240000 : FOR( m = 0; m < nInChannels; m++ )
1189 : {
1190 200000 : g1_fx = gains_fx[m][n]; // Q31
1191 200000 : move32();
1192 200000 : p_realRot_fx = realRot_fx[n];
1193 200000 : p_imagRot_fx = imagRot_fx[n];
1194 :
1195 200000 : IF( g1_fx > 0 )
1196 : {
1197 382375 : FOR( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
1198 : {
1199 305900 : p_real_fx = Cldfb_RealBuffer[m][iBlock];
1200 305900 : p_imag_fx = Cldfb_ImagBuffer[m][iBlock];
1201 :
1202 15600900 : FOR( iBand = 0; iBand < nb_band; iBand++ )
1203 : {
1204 15295000 : *( p_realRot_fx ) = L_add( *p_realRot_fx, Mpy_32_32( g1_fx, *( p_real_fx++ ) ) ); // Qx
1205 15295000 : move32();
1206 15295000 : *( p_imagRot_fx ) = L_add( *p_imagRot_fx, Mpy_32_32( g1_fx, *( p_imag_fx++ ) ) ); // Qx
1207 15295000 : move32();
1208 15295000 : p_realRot_fx++;
1209 15295000 : p_imagRot_fx++;
1210 : }
1211 : }
1212 : }
1213 : }
1214 : }
1215 :
1216 48000 : FOR( n = 0; n < nInChannels; n++ )
1217 : {
1218 40000 : p_realRot_fx = realRot_fx[n];
1219 40000 : p_imagRot_fx = imagRot_fx[n];
1220 :
1221 200000 : FOR( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
1222 : {
1223 160000 : p_real_fx = Cldfb_RealBuffer[n][iBlock];
1224 160000 : p_imag_fx = Cldfb_ImagBuffer[n][iBlock];
1225 :
1226 8160000 : FOR( iBand = 0; iBand < nb_band; iBand++ )
1227 : {
1228 8000000 : *( p_real_fx++ ) = *( p_realRot_fx++ );
1229 8000000 : move32();
1230 8000000 : *( p_imag_fx++ ) = *( p_imagRot_fx++ );
1231 8000000 : move32();
1232 : }
1233 1760000 : FOR( ; iBand < CLDFB_NO_CHANNELS_MAX; iBand++ )
1234 : {
1235 1600000 : *( p_real_fx++ ) = 0;
1236 1600000 : move32();
1237 1600000 : *( p_imag_fx++ ) = 0;
1238 1600000 : move32();
1239 : }
1240 : }
1241 : }
1242 8000 : pop_wmops();
1243 :
1244 8000 : return;
1245 : }
1246 :
1247 : /*-----------------------------------------------------------------------*
1248 : * ivas_external_orientation_open()
1249 : *
1250 : * Allocate and initialize external orientation handle
1251 : *-----------------------------------------------------------------------*/
1252 31 : ivas_error ivas_external_orientation_open(
1253 : EXTERNAL_ORIENTATION_HANDLE *hExtOrientationData, /* o : external orientation handle */
1254 : const Word16 num_subframes /* i : number of subframes */
1255 : )
1256 : {
1257 :
1258 : Word16 i;
1259 : IVAS_QUATERNION identity;
1260 31 : identity.w_fx = ONE_IN_Q31;
1261 31 : move32();
1262 31 : identity.x_fx = 0;
1263 31 : move32();
1264 31 : identity.y_fx = 0;
1265 31 : move32();
1266 31 : identity.z_fx = 0;
1267 31 : move32();
1268 31 : identity.q_fact = 31;
1269 31 : move16();
1270 : /* Allocate handle */
1271 31 : IF( ( *hExtOrientationData = (EXTERNAL_ORIENTATION_HANDLE) malloc( sizeof( EXTERNAL_ORIENTATION_DATA ) ) ) == NULL )
1272 : {
1273 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for external orientation memory\n" ) );
1274 : }
1275 31 : ( *hExtOrientationData )->num_subframes = num_subframes;
1276 31 : move16();
1277 : /* Enable head rotation and disable external orientation as default */
1278 155 : FOR( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
1279 : {
1280 124 : ( *hExtOrientationData )->enableHeadRotation[i] = 1;
1281 124 : move16();
1282 124 : ( *hExtOrientationData )->enableExternalOrientation[i] = 0;
1283 124 : move16();
1284 124 : ( *hExtOrientationData )->enableRotationInterpolation[i] = 0;
1285 124 : move16();
1286 124 : ( *hExtOrientationData )->numFramesToTargetOrientation[i] = 0;
1287 124 : move16();
1288 :
1289 124 : ( *hExtOrientationData )->Quaternions[i] = identity;
1290 : }
1291 31 : return IVAS_ERR_OK;
1292 : }
1293 :
1294 : /*-----------------------------------------------------------------------*
1295 : * ivas_external_orientation_close()
1296 : *
1297 : * Deallocate external orientation handle
1298 : *-----------------------------------------------------------------------*/
1299 1280 : void ivas_external_orientation_close_fx(
1300 : EXTERNAL_ORIENTATION_HANDLE *hExtOrientationData /* i/o: external orientation handle */
1301 : )
1302 : {
1303 1280 : test();
1304 1280 : IF( hExtOrientationData == NULL || *hExtOrientationData == NULL )
1305 : {
1306 1249 : return;
1307 : }
1308 :
1309 31 : free( ( *hExtOrientationData ) );
1310 31 : *hExtOrientationData = NULL;
1311 :
1312 31 : return;
1313 : }
1314 :
1315 :
1316 : /*-----------------------------------------------------------------------*
1317 : * ivas_combined_orientation_open()
1318 : *
1319 : * Allocate and initialize combined orientation handle
1320 : *-----------------------------------------------------------------------*/
1321 176 : ivas_error ivas_combined_orientation_open(
1322 : COMBINED_ORIENTATION_HANDLE *hCombinedOrientationData, /* o : combined orientation handle */
1323 : const Word32 fs, /* i : sampling rate */
1324 : const Word16 num_subframes /* i : number of subframes */
1325 : )
1326 : {
1327 : Word16 i;
1328 : Word16 j;
1329 : IVAS_QUATERNION identity;
1330 : IVAS_VECTOR3 origo;
1331 : Word16 pos_idx;
1332 176 : identity.w_fx = ONE_IN_Q31;
1333 176 : move32();
1334 176 : identity.x_fx = 0;
1335 176 : move32();
1336 176 : identity.y_fx = 0;
1337 176 : move32();
1338 176 : identity.z_fx = 0;
1339 176 : move32();
1340 176 : identity.q_fact = 31;
1341 176 : move16();
1342 176 : origo.x_fx = 0;
1343 176 : move32();
1344 176 : origo.y_fx = 0;
1345 176 : move32();
1346 176 : origo.z_fx = 0;
1347 176 : move32();
1348 176 : origo.q_fact = 31;
1349 176 : move16();
1350 : /* Allocate handle */
1351 176 : IF( ( *hCombinedOrientationData = (COMBINED_ORIENTATION_HANDLE) malloc( sizeof( COMBINED_ORIENTATION_DATA ) ) ) == NULL )
1352 : {
1353 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for combined orientation memory\n" ) );
1354 : }
1355 :
1356 : /* Initialization */
1357 176 : ( *hCombinedOrientationData )->num_subframes = num_subframes;
1358 176 : move16();
1359 176 : ( *hCombinedOrientationData )->interpolationCoefficient_fx = ONE_IN_Q30; // Q30
1360 176 : move32();
1361 176 : ( *hCombinedOrientationData )->interpolationIncrement_fx = ONE_IN_Q30; // Q30
1362 176 : move32();
1363 176 : ( *hCombinedOrientationData )->maximumFramesToTargetOrientation = 500;
1364 176 : move16();
1365 176 : ( *hCombinedOrientationData )->lrSwitchedNext = 0;
1366 176 : move16();
1367 176 : ( *hCombinedOrientationData )->lrSwitchedCurrent = 0;
1368 176 : move16();
1369 :
1370 176 : ( *hCombinedOrientationData )->lrSwitchInterpVal_fx = 0;
1371 176 : move32();
1372 176 : ( *hCombinedOrientationData )->isInterpolationOngoing = FALSE;
1373 176 : move16();
1374 176 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_start = identity;
1375 176 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_target = identity;
1376 :
1377 : /* Initialise orientations to identity */
1378 880 : FOR( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
1379 : {
1380 704 : ( *hCombinedOrientationData )->enableCombinedOrientation[i] = 0;
1381 704 : move16();
1382 704 : ( *hCombinedOrientationData )->Quaternions[i] = identity;
1383 704 : ( *hCombinedOrientationData )->listenerPos[i] = origo;
1384 :
1385 2816 : FOR( j = 0; j < 3; j++ )
1386 : {
1387 2112 : set32_fx( ( *hCombinedOrientationData )->Rmat_fx[i][j], 0, 3 );
1388 2112 : ( *hCombinedOrientationData )->Rmat_fx[i][j][j] = ONE_IN_Q30; // Q30
1389 2112 : move32();
1390 : }
1391 : }
1392 :
1393 1584 : FOR( pos_idx = 0; pos_idx < MAX_HEAD_ROT_POSES; pos_idx++ )
1394 : {
1395 5632 : FOR( j = 0; j < 3; j++ )
1396 : {
1397 4224 : set32_fx( ( *hCombinedOrientationData )->Rmat_prev_fx[pos_idx][j], 0, 3 );
1398 4224 : ( *hCombinedOrientationData )->Rmat_prev_fx[pos_idx][j][j] = ONE_IN_Q30;
1399 4224 : move32();
1400 : }
1401 : }
1402 176 : ( *hCombinedOrientationData )->sr_pose_pred_axis = DEFAULT_AXIS;
1403 176 : ( *hCombinedOrientationData )->sr_low_res_flag = 0;
1404 176 : move32();
1405 176 : move16();
1406 :
1407 176 : ( *hCombinedOrientationData )
1408 176 : ->Quaternion_prev_extOrientation = identity;
1409 176 : ( *hCombinedOrientationData )->Quaternion_frozen_ext = identity;
1410 176 : ( *hCombinedOrientationData )->Quaternion_frozen_head = identity;
1411 176 : set_zero_fx( ( *hCombinedOrientationData )->chEneIIR_fx[0], MASA_FREQUENCY_BANDS );
1412 176 : set_zero_fx( ( *hCombinedOrientationData )->chEneIIR_fx[1], MASA_FREQUENCY_BANDS );
1413 176 : set_zero_fx( ( *hCombinedOrientationData )->procChEneIIR_fx[0], MASA_FREQUENCY_BANDS );
1414 176 : set_zero_fx( ( *hCombinedOrientationData )->procChEneIIR_fx[1], MASA_FREQUENCY_BANDS );
1415 176 : set16_fx( ( *hCombinedOrientationData )->q_chEneIIR[0], Q31, MASA_FREQUENCY_BANDS );
1416 176 : set16_fx( ( *hCombinedOrientationData )->q_chEneIIR[1], Q31, MASA_FREQUENCY_BANDS );
1417 176 : set16_fx( ( *hCombinedOrientationData )->q_procChEneIIR[0], Q31, MASA_FREQUENCY_BANDS );
1418 176 : set16_fx( ( *hCombinedOrientationData )->q_procChEneIIR[1], Q31, MASA_FREQUENCY_BANDS );
1419 :
1420 176 : ( *hCombinedOrientationData )->isExtOrientationFrozen = 0;
1421 176 : move16();
1422 176 : ( *hCombinedOrientationData )->isHeadRotationFrozen = 0;
1423 176 : move16();
1424 :
1425 176 : ( *hCombinedOrientationData )->subframe_idx = 0;
1426 176 : move16();
1427 : /* ( *hCombinedOrientationData )->subframe_size = (int16_t) ( fs / ( FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES ) ); */
1428 176 : ( *hCombinedOrientationData )->subframe_size = extract_l( Mpy_32_32_r( fs, 10737418 /* 1 / ( FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES ) in Q31 */ ) );
1429 176 : move16();
1430 176 : ( *hCombinedOrientationData )->cur_subframe_samples_rendered = 0;
1431 176 : move16();
1432 :
1433 176 : return IVAS_ERR_OK;
1434 : }
1435 : /*-----------------------------------------------------------------------*
1436 : * ivas_combined_orientation_close()
1437 : *
1438 : * Deallocate combined orientation handle
1439 : *-----------------------------------------------------------------------*/
1440 :
1441 1280 : void ivas_combined_orientation_close_fx(
1442 : COMBINED_ORIENTATION_HANDLE *hCombinedOrientationData /* i/o: combined orientation handle */
1443 : )
1444 : {
1445 1280 : test();
1446 1280 : IF( hCombinedOrientationData == NULL || *hCombinedOrientationData == NULL )
1447 : {
1448 1104 : return;
1449 : }
1450 :
1451 176 : free( ( *hCombinedOrientationData ) );
1452 176 : *hCombinedOrientationData = NULL;
1453 :
1454 176 : return;
1455 : }
1456 :
1457 :
1458 : /*-------------------------------------------------------------------------
1459 : * combine_external_and_head_orientations_dec()
1460 : *
1461 : *
1462 : *------------------------------------------------------------------------*/
1463 :
1464 69653 : ivas_error combine_external_and_head_orientations_dec(
1465 : HEAD_TRACK_DATA_HANDLE hHeadTrackData, /* i : head track handle */
1466 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1467 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1468 : )
1469 : {
1470 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis;
1471 69653 : IVAS_QUATERNION *pHeadRotQuaternion = NULL;
1472 69653 : IVAS_VECTOR3 *listenerPos = NULL;
1473 :
1474 69653 : IF( hHeadTrackData != NULL )
1475 : {
1476 69653 : pHeadRotQuaternion = hHeadTrackData->Quaternions;
1477 69653 : listenerPos = hHeadTrackData->Pos;
1478 69653 : sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
1479 : }
1480 : ELSE
1481 : {
1482 0 : sr_pose_pred_axis = DEFAULT_AXIS;
1483 : }
1484 :
1485 69653 : return combine_external_and_head_orientations( pHeadRotQuaternion, listenerPos, sr_pose_pred_axis, hExtOrientationData, hCombinedOrientationData );
1486 : }
1487 :
1488 :
1489 : /*-------------------------------------------------------------------------
1490 : * combine_external_and_head_orientations_rend()
1491 : *
1492 : *
1493 : *------------------------------------------------------------------------*/
1494 :
1495 1126140 : ivas_error combine_external_and_head_orientations_rend(
1496 : IVAS_REND_HeadRotData *hHeadTrackData, /* i : head track handle */
1497 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1498 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1499 : )
1500 : {
1501 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis;
1502 1126140 : IVAS_QUATERNION *headRotQuaternions = NULL;
1503 1126140 : IVAS_VECTOR3 *listenerPos = NULL;
1504 : Word16 i;
1505 :
1506 1126140 : sr_pose_pred_axis = DEFAULT_AXIS;
1507 :
1508 1126140 : IF( hHeadTrackData != NULL )
1509 : {
1510 1126140 : IF( hHeadTrackData->headRotEnabled )
1511 : {
1512 180024 : headRotQuaternions = hHeadTrackData->headPositions;
1513 180024 : listenerPos = hHeadTrackData->Pos;
1514 : }
1515 1126140 : sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
1516 : }
1517 0 : ELSE IF( hExtOrientationData != NULL )
1518 : {
1519 : /* Head rotation data not available, use the freezed value or disable */
1520 0 : FOR( i = 0; i < hExtOrientationData->num_subframes; i++ )
1521 : {
1522 0 : IF( NE_16( hExtOrientationData->enableHeadRotation[i], 2 ) )
1523 : {
1524 0 : hExtOrientationData->enableHeadRotation[i] = 0;
1525 0 : move16();
1526 : }
1527 : }
1528 : }
1529 :
1530 1126140 : return combine_external_and_head_orientations( headRotQuaternions, listenerPos, sr_pose_pred_axis, hExtOrientationData, hCombinedOrientationData );
1531 : }
1532 :
1533 :
1534 : /*-------------------------------------------------------------------------
1535 : * combine_external_and_head_orientations()
1536 : *
1537 : * Combine the external orientations and the head orientation.
1538 : * NOTE that the external orientations are inversed.
1539 : *------------------------------------------------------------------------*/
1540 :
1541 1195793 : ivas_error combine_external_and_head_orientations(
1542 : IVAS_QUATERNION *headRotQuaternions, /* i : quaternions for head rotation */
1543 : IVAS_VECTOR3 *listenerPos, /* i : listener position */
1544 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis, /* i : split rend pose prediction axis */
1545 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1546 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1547 : )
1548 : {
1549 : Word16 i;
1550 : Word16 j;
1551 : IVAS_QUATERNION identity;
1552 : IVAS_VECTOR3 origo;
1553 1195793 : identity.w_fx = ONE_IN_Q31;
1554 1195793 : move32();
1555 1195793 : identity.x_fx = 0;
1556 1195793 : move32();
1557 1195793 : identity.y_fx = 0;
1558 1195793 : move32();
1559 1195793 : identity.z_fx = 0;
1560 1195793 : move32();
1561 1195793 : identity.q_fact = 31;
1562 1195793 : move16();
1563 1195793 : origo.x_fx = 0;
1564 1195793 : move32();
1565 1195793 : origo.y_fx = 0;
1566 1195793 : move32();
1567 1195793 : origo.z_fx = 0;
1568 1195793 : move32();
1569 1195793 : origo.q_fact = 31;
1570 1195793 : move16();
1571 :
1572 : /* Form combined orientations or return if no data available */
1573 1195793 : test();
1574 1195793 : test();
1575 1195793 : IF( hCombinedOrientationData == NULL )
1576 : {
1577 946116 : test();
1578 946116 : IF( headRotQuaternions != NULL || hExtOrientationData != NULL )
1579 : {
1580 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
1581 : }
1582 : ELSE
1583 : {
1584 946116 : return IVAS_ERR_OK;
1585 : }
1586 : }
1587 249677 : ELSE IF( headRotQuaternions == NULL && hExtOrientationData == NULL )
1588 : {
1589 : /* Reset the combined orientations and rotations */
1590 0 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1591 0 : move16();
1592 0 : hCombinedOrientationData->interpolationCoefficient_fx = ONE_IN_Q30;
1593 0 : move32();
1594 0 : hCombinedOrientationData->interpolationIncrement_fx = ONE_IN_Q30;
1595 0 : move32();
1596 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
1597 0 : hCombinedOrientationData->Quaternions_ext_interpolation_target = identity;
1598 0 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1599 : {
1600 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1601 0 : move16();
1602 0 : hCombinedOrientationData->Quaternions[i] = identity;
1603 0 : hCombinedOrientationData->listenerPos[i] = origo;
1604 :
1605 0 : FOR( j = 0; j < 3; j++ )
1606 : {
1607 0 : set_zero_fx( hCombinedOrientationData->Rmat_fx[i][j], 3 );
1608 0 : hCombinedOrientationData->Rmat_fx[i][j][j] = ONE_IN_Q30;
1609 0 : move32();
1610 : }
1611 : }
1612 : }
1613 249677 : ELSE IF( hExtOrientationData == NULL && headRotQuaternions != NULL )
1614 : {
1615 : /* Head rotation only */
1616 670349 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1617 : {
1618 449872 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1619 : }
1620 : }
1621 :
1622 249677 : IF( hExtOrientationData != NULL )
1623 : {
1624 : /* External orientations */
1625 146000 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1626 : {
1627 : /* Check for frozen external orientation */
1628 116800 : IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
1629 : {
1630 17391 : IF( NE_16( hCombinedOrientationData->isExtOrientationFrozen, 1 ) )
1631 : {
1632 31 : hCombinedOrientationData->Quaternion_frozen_ext = hExtOrientationData->Quaternions[i];
1633 31 : hCombinedOrientationData->isExtOrientationFrozen = 1;
1634 31 : move16();
1635 : }
1636 : }
1637 : ELSE
1638 : {
1639 99409 : hCombinedOrientationData->Quaternion_frozen_ext = identity;
1640 99409 : hCombinedOrientationData->isExtOrientationFrozen = 0;
1641 99409 : move16();
1642 : }
1643 116800 : test();
1644 116800 : IF( EQ_16( hExtOrientationData->enableRotationInterpolation[i], 1 ) && hExtOrientationData->enableExternalOrientation[i] > 0 )
1645 : {
1646 60970 : test();
1647 60970 : test();
1648 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 ) )
1649 : {
1650 : /* Continue interpolation */
1651 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
1652 50636 : move32();
1653 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
1654 50636 : move32();
1655 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
1656 50636 : move32();
1657 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
1658 50636 : move32();
1659 :
1660 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
1661 50636 : move32();
1662 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
1663 50636 : move32();
1664 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
1665 50636 : move32();
1666 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
1667 50636 : move32();
1668 :
1669 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact = Q29;
1670 50636 : move16();
1671 50636 : hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact = Q29;
1672 50636 : move16();
1673 50636 : QuaternionSlerp_fx( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient_fx, &hCombinedOrientationData->Quaternions[i] );
1674 50636 : hCombinedOrientationData->interpolationCoefficient_fx = L_add( hCombinedOrientationData->interpolationIncrement_fx, hCombinedOrientationData->interpolationCoefficient_fx );
1675 50636 : move32();
1676 : }
1677 : ELSE
1678 : {
1679 : /* Stop interpolation or check for new interpolation */
1680 10334 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1681 10334 : move16();
1682 10334 : hCombinedOrientationData->interpolationCoefficient_fx = ONE_IN_Q30;
1683 10334 : move32();
1684 10334 : hCombinedOrientationData->interpolationIncrement_fx = ONE_IN_Q30;
1685 10334 : move32();
1686 10334 : external_target_interpolation_fx( hExtOrientationData, hCombinedOrientationData, i );
1687 10334 : Word16 l_shift = 0;
1688 10334 : move16();
1689 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 ) ) );
1690 10334 : hCombinedOrientationData->Quaternions[i].w_fx = L_shl( hCombinedOrientationData->Quaternions[i].w_fx, l_shift ); // q_fact+l_shift
1691 10334 : move32();
1692 10334 : hCombinedOrientationData->Quaternions[i].x_fx = L_shl( hCombinedOrientationData->Quaternions[i].x_fx, l_shift ); // q_fact+l_shift
1693 10334 : move32();
1694 10334 : hCombinedOrientationData->Quaternions[i].y_fx = L_shl( hCombinedOrientationData->Quaternions[i].y_fx, l_shift ); // q_fact+l_shift
1695 10334 : move32();
1696 10334 : hCombinedOrientationData->Quaternions[i].z_fx = L_shl( hCombinedOrientationData->Quaternions[i].z_fx, l_shift ); // q_fact+l_shift
1697 10334 : move32();
1698 10334 : hCombinedOrientationData->Quaternions[i].q_fact = add( hCombinedOrientationData->Quaternions[i].q_fact, l_shift );
1699 10334 : move16();
1700 : }
1701 : }
1702 : ELSE
1703 : {
1704 : /* Interpolation disabled, use the current orientation values */
1705 :
1706 : /* Use the most recent external orientation */
1707 55830 : IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 1 ) )
1708 : {
1709 36393 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
1710 : }
1711 : /* Use the freezed external orientation */
1712 19437 : ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
1713 : {
1714 12803 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_ext;
1715 : }
1716 : }
1717 : }
1718 : }
1719 249677 : test();
1720 249677 : IF( hExtOrientationData != NULL && headRotQuaternions != NULL )
1721 : {
1722 : /* Combine head and external orientations */
1723 146000 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1724 : {
1725 : /* Check for frozen head rotation */
1726 116800 : IF( EQ_16( hExtOrientationData->enableHeadRotation[i], 2 ) )
1727 : {
1728 12400 : IF( NE_16( hCombinedOrientationData->isHeadRotationFrozen, 1 ) )
1729 : {
1730 62 : hCombinedOrientationData->Quaternion_frozen_head = headRotQuaternions[i];
1731 62 : hCombinedOrientationData->isHeadRotationFrozen = 1;
1732 62 : move16();
1733 : }
1734 : }
1735 : ELSE
1736 : {
1737 104400 : hCombinedOrientationData->Quaternion_frozen_head = identity;
1738 104400 : hCombinedOrientationData->isHeadRotationFrozen = 0;
1739 104400 : move16();
1740 : }
1741 : /* Use the most recent head rotation */
1742 116800 : IF( EQ_16( hExtOrientationData->enableHeadRotation[i], 1 ) )
1743 : {
1744 83595 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 )
1745 : {
1746 76961 : QuaternionProduct_fx( hCombinedOrientationData->Quaternions[i], headRotQuaternions[i], &hCombinedOrientationData->Quaternions[i] );
1747 : }
1748 : ELSE
1749 : {
1750 6634 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1751 : }
1752 : }
1753 : /* Use the freezed head rotation */
1754 33205 : ELSE IF( EQ_16( hExtOrientationData->enableHeadRotation[i], 2 ) )
1755 : {
1756 12400 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 )
1757 : {
1758 12400 : QuaternionProduct_fx( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Quaternion_frozen_head, &hCombinedOrientationData->Quaternions[i] );
1759 : }
1760 : ELSE
1761 : {
1762 0 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_head;
1763 : }
1764 : }
1765 : /* Reset the combined orientations to identity */
1766 116800 : test();
1767 116800 : if ( hExtOrientationData->enableHeadRotation[i] == 0 && hExtOrientationData->enableExternalOrientation[i] == 0 )
1768 : {
1769 0 : hCombinedOrientationData->Quaternions[i] = identity;
1770 : }
1771 : }
1772 : }
1773 249677 : test();
1774 249677 : IF( headRotQuaternions != NULL || hExtOrientationData != NULL )
1775 : {
1776 : /* Calculate the combined rotation matrix */
1777 816349 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1778 : {
1779 566672 : QuatToRotMat_fx( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Rmat_fx[i] );
1780 : }
1781 : }
1782 :
1783 : /* Save the current orientations */
1784 249677 : IF( hExtOrientationData != NULL )
1785 : {
1786 29200 : IF( hExtOrientationData->enableExternalOrientation[hExtOrientationData->num_subframes - 1] > 0 )
1787 : {
1788 27557 : hCombinedOrientationData->Quaternion_prev_extOrientation = hExtOrientationData->Quaternions[hExtOrientationData->num_subframes - 1];
1789 : }
1790 : ELSE
1791 : {
1792 1643 : hCombinedOrientationData->Quaternion_prev_extOrientation = identity;
1793 : }
1794 : }
1795 249677 : IF( headRotQuaternions != NULL )
1796 : {
1797 816349 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1798 : {
1799 566672 : hCombinedOrientationData->listenerPos[i] = listenerPos[i];
1800 : }
1801 : }
1802 : /* Check if combined orientation is enabled */
1803 249677 : test();
1804 249677 : test();
1805 249677 : test();
1806 249677 : IF( headRotQuaternions != NULL && hExtOrientationData == NULL )
1807 : {
1808 670349 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1809 : {
1810 449872 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1811 449872 : move16();
1812 : }
1813 : }
1814 29200 : ELSE IF( headRotQuaternions == NULL && hExtOrientationData != NULL )
1815 : {
1816 0 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1817 : {
1818 0 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 )
1819 : {
1820 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1821 0 : move16();
1822 : }
1823 : ELSE
1824 : {
1825 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1826 0 : move16();
1827 : }
1828 : }
1829 : }
1830 29200 : ELSE IF( headRotQuaternions != NULL && hExtOrientationData != NULL )
1831 : {
1832 146000 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1833 : {
1834 116800 : test();
1835 116800 : IF( hExtOrientationData->enableExternalOrientation[i] > 0 || ( hExtOrientationData->enableHeadRotation[i] > 0 ) )
1836 : {
1837 116800 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1838 116800 : move16();
1839 : }
1840 : ELSE
1841 : {
1842 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1843 0 : move16();
1844 : }
1845 : }
1846 : }
1847 : ELSE
1848 : {
1849 0 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1850 : {
1851 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1852 0 : move16();
1853 : }
1854 : }
1855 :
1856 249677 : hCombinedOrientationData->subframe_idx = 0;
1857 249677 : move16();
1858 249677 : hCombinedOrientationData->cur_subframe_samples_rendered = 0;
1859 249677 : move16();
1860 249677 : hCombinedOrientationData->subframe_idx_start = 0;
1861 249677 : move16();
1862 249677 : hCombinedOrientationData->cur_subframe_samples_rendered_start = 0;
1863 249677 : move16();
1864 816349 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1865 : {
1866 2266688 : FOR( j = 0; j < 3; j++ )
1867 : {
1868 6800064 : FOR( Word16 k = 0; k < 3; k++ )
1869 : {
1870 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
1871 5100048 : move32();
1872 : }
1873 : }
1874 : }
1875 :
1876 249677 : Word16 l_shift = 0;
1877 249677 : move16();
1878 816349 : FOR( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1879 : {
1880 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 ) ) );
1881 566672 : hCombinedOrientationData->Quaternions[i].w_fx = L_shl( hCombinedOrientationData->Quaternions[i].w_fx, l_shift ); // q_fact+l_shift
1882 566672 : move32();
1883 566672 : hCombinedOrientationData->Quaternions[i].x_fx = L_shl( hCombinedOrientationData->Quaternions[i].x_fx, l_shift ); // q_fact+l_shift
1884 566672 : move32();
1885 566672 : hCombinedOrientationData->Quaternions[i].y_fx = L_shl( hCombinedOrientationData->Quaternions[i].y_fx, l_shift ); // q_fact+l_shift
1886 566672 : move32();
1887 566672 : hCombinedOrientationData->Quaternions[i].z_fx = L_shl( hCombinedOrientationData->Quaternions[i].z_fx, l_shift ); // q_fact+l_shift
1888 566672 : move32();
1889 566672 : hCombinedOrientationData->Quaternions[i].q_fact = add( hCombinedOrientationData->Quaternions[i].q_fact, l_shift );
1890 566672 : move16();
1891 : }
1892 :
1893 249677 : hCombinedOrientationData->sr_pose_pred_axis = sr_pose_pred_axis;
1894 249677 : return IVAS_ERR_OK;
1895 : }
1896 :
1897 : /*-------------------------------------------------------------------------
1898 : * external_target_interpolation()
1899 : *
1900 : *
1901 : *------------------------------------------------------------------------*/
1902 10334 : static void external_target_interpolation_fx(
1903 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1904 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1905 : const Word16 i /* i : subframe index */
1906 : )
1907 : {
1908 : /* Sanity check for number of frames */
1909 10334 : hExtOrientationData->numFramesToTargetOrientation[i] = s_min( hExtOrientationData->numFramesToTargetOrientation[i], hCombinedOrientationData->maximumFramesToTargetOrientation );
1910 10334 : move16();
1911 10334 : hExtOrientationData->numFramesToTargetOrientation[i] = s_max( hExtOrientationData->numFramesToTargetOrientation[i], 0 );
1912 10334 : move16();
1913 :
1914 : /* Interpolate from the current orientation to the target orientation */
1915 10334 : IF( hExtOrientationData->numFramesToTargetOrientation[i] > 0 )
1916 : {
1917 3349 : IF( EQ_16( are_orientations_same_fx( &hCombinedOrientationData->Quaternions_ext_interpolation_target, &hExtOrientationData->Quaternions[i] ), false ) )
1918 : {
1919 : /* Target orientation is different from the previous target, update the values */
1920 :
1921 : /* Set the received orientation as the target */
1922 249 : hCombinedOrientationData->Quaternions_ext_interpolation_target = hExtOrientationData->Quaternions[i];
1923 :
1924 : /* Use the most recent external orientation as the starting orientation */
1925 249 : IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 1 ) )
1926 : {
1927 218 : IF( i > 0 )
1928 : {
1929 141 : IF( hExtOrientationData->enableExternalOrientation[i - 1] == 0 )
1930 : {
1931 : IVAS_QUATERNION identity;
1932 31 : identity.w_fx = ONE_IN_Q31;
1933 31 : move32();
1934 31 : identity.x_fx = identity.y_fx = identity.z_fx = 0;
1935 31 : move32();
1936 31 : move32();
1937 31 : move32();
1938 31 : identity.q_fact = 31;
1939 31 : move16();
1940 31 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
1941 : }
1942 110 : ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i - 1], 2 ) )
1943 : {
1944 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1945 : }
1946 : ELSE
1947 : {
1948 110 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hExtOrientationData->Quaternions[i - 1];
1949 : }
1950 : }
1951 : ELSE
1952 : {
1953 77 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_prev_extOrientation;
1954 : }
1955 : }
1956 31 : ELSE IF( EQ_16( hExtOrientationData->enableExternalOrientation[i], 2 ) )
1957 : {
1958 31 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1959 : }
1960 249 : Word16 tmp_e = 0;
1961 249 : move16();
1962 : Word32 tmp;
1963 : /* Calculate the interpolation increment and coefficient */
1964 249 : tmp = BASOP_Util_Divide3232_Scale_newton( ONE_IN_Q30, L_shl( L_deposit_l( hExtOrientationData->numFramesToTargetOrientation[i] ), 2 ), &tmp_e );
1965 249 : hCombinedOrientationData->interpolationIncrement_fx = L_shl( tmp, sub( tmp_e, 31 ) ); /* Q30 */
1966 249 : move32();
1967 249 : hCombinedOrientationData->interpolationCoefficient_fx = hCombinedOrientationData->interpolationIncrement_fx;
1968 249 : move32();
1969 : }
1970 :
1971 : /* Interpolate */
1972 3349 : hCombinedOrientationData->isInterpolationOngoing = TRUE;
1973 3349 : move16();
1974 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
1975 3349 : move32();
1976 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
1977 3349 : move32();
1978 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
1979 3349 : move32();
1980 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
1981 3349 : move32();
1982 :
1983 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
1984 3349 : move32();
1985 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
1986 3349 : move32();
1987 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
1988 3349 : move32();
1989 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
1990 3349 : move32();
1991 :
1992 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_start.q_fact = Q29;
1993 3349 : move16();
1994 3349 : hCombinedOrientationData->Quaternions_ext_interpolation_target.q_fact = Q29;
1995 3349 : move16();
1996 3349 : QuaternionSlerp_fx( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient_fx, &hCombinedOrientationData->Quaternions[i] );
1997 3349 : hCombinedOrientationData->interpolationCoefficient_fx = L_add_sat( hCombinedOrientationData->interpolationCoefficient_fx, hCombinedOrientationData->interpolationIncrement_fx );
1998 3349 : move32();
1999 : }
2000 : ELSE
2001 : {
2002 : /* Use the target orientation immediately */
2003 6985 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
2004 6985 : move16();
2005 6985 : hCombinedOrientationData->interpolationCoefficient_fx = ONE_IN_Q30;
2006 6985 : move32();
2007 6985 : hCombinedOrientationData->interpolationIncrement_fx = ONE_IN_Q30;
2008 6985 : move32();
2009 6985 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
2010 : }
2011 :
2012 10334 : return;
2013 : }
2014 :
2015 : /*-------------------------------------------------------------------------
2016 : * are_orientations_same()
2017 : *
2018 : *
2019 : *------------------------------------------------------------------------*/
2020 54203 : static bool are_orientations_same_fx(
2021 : const IVAS_QUATERNION *orientation1,
2022 : const IVAS_QUATERNION *orientation2 )
2023 : {
2024 54203 : bool orientationsAreSame = true;
2025 54203 : move16();
2026 54203 : Word32 error_margin_fx = 107374182; // 0.05f in Q31
2027 54203 : move32();
2028 54203 : Word16 error_margin_e = 0;
2029 54203 : move16();
2030 54203 : Word16 w_e = 0, x_e = 0, y_e = 0, z_e = 0;
2031 54203 : move16();
2032 54203 : move16();
2033 54203 : move16();
2034 54203 : move16();
2035 54203 : Word32 w_result = 0, x_result = 0, y_result = 0, z_result = 0;
2036 54203 : move32();
2037 54203 : move32();
2038 54203 : move32();
2039 54203 : move32();
2040 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 ) );
2041 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 ) );
2042 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 ) );
2043 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 ) );
2044 54203 : Word16 Flag_1 = BASOP_Util_Cmp_Mant32Exp( w_result, w_e, error_margin_fx, error_margin_e );
2045 54203 : Word16 Flag_2 = BASOP_Util_Cmp_Mant32Exp( x_result, x_e, error_margin_fx, error_margin_e );
2046 54203 : Word16 Flag_3 = BASOP_Util_Cmp_Mant32Exp( y_result, y_e, error_margin_fx, error_margin_e );
2047 54203 : Word16 Flag_4 = BASOP_Util_Cmp_Mant32Exp( z_result, z_e, error_margin_fx, error_margin_e );
2048 :
2049 54203 : test();
2050 54203 : test();
2051 54203 : test();
2052 108073 : if ( EQ_16( Flag_1, 1 ) ||
2053 107740 : EQ_16( Flag_2, 1 ) ||
2054 107740 : EQ_16( Flag_3, 1 ) ||
2055 53870 : EQ_16( Flag_4, 1 ) )
2056 : {
2057 467 : orientationsAreSame = false;
2058 467 : move16();
2059 : }
2060 :
2061 54203 : return orientationsAreSame;
2062 : }
2063 :
2064 :
2065 : /*-----------------------------------------------------------------------*
2066 : * Local Function definitions
2067 : *-----------------------------------------------------------------------*/
2068 : /*-------------------------------------------------------------------------
2069 : * Helper functions used by SHrotmatgen,
2070 : * an implementation of the algorithm in
2071 : * Ivanic, J. & Ruedenberg, K., J. Phys. Chem. 100, 6342 (1996)
2072 : *------------------------------------------------------------------------*/
2073 11623851 : static Word32 SHrot_p_fx(
2074 : const Word16 i,
2075 : const Word16 l,
2076 : const Word16 a,
2077 : const Word16 b,
2078 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2079 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] ) // Q14
2080 : {
2081 :
2082 11623851 : Word16 ri1 = 0, rim1 = 0, ri0 = 0, R_lm1_1 = 0, R_lm1_2 = 0;
2083 11623851 : move16();
2084 11623851 : move16();
2085 11623851 : move16();
2086 11623851 : move16();
2087 11623851 : move16();
2088 :
2089 11623851 : Word32 p = 0;
2090 11623851 : move32();
2091 :
2092 11623851 : ri1 = SHrotmat[i + 1 + 1][1 + 1 + 1]; // Q14
2093 11623851 : move16();
2094 11623851 : rim1 = SHrotmat[i + 1 + 1][-1 + 1 + 1]; // Q14
2095 11623851 : move16();
2096 11623851 : ri0 = SHrotmat[i + 1 + 1][0 + 1 + 1]; // Q14
2097 11623851 : move16();
2098 :
2099 11623851 : IF( EQ_16( b, -l ) )
2100 : {
2101 1915131 : R_lm1_1 = R_lm1[a + l - 1][0]; // Q14
2102 1915131 : move16();
2103 1915131 : R_lm1_2 = R_lm1[a + l - 1][2 * l - 2]; // Q14
2104 1915131 : move16();
2105 1915131 : p = L_mac0( L_mult0( ri1, R_lm1_1 ), rim1, R_lm1_2 ); // Q28
2106 : }
2107 : ELSE
2108 : {
2109 9708720 : IF( EQ_16( b, l ) )
2110 : {
2111 1915131 : R_lm1_1 = R_lm1[a + l - 1][2 * l - 2]; // Q14
2112 1915131 : move16();
2113 1915131 : R_lm1_2 = R_lm1[a + l - 1][0]; // Q14
2114 1915131 : move16();
2115 1915131 : p = L_msu0( L_mult0( ri1, R_lm1_1 ), rim1, R_lm1_2 ); // Q28
2116 : }
2117 : ELSE
2118 : {
2119 7793589 : R_lm1_1 = R_lm1[a + l - 1][b + l - 1];
2120 7793589 : move16();
2121 7793589 : p = L_mult0( ri0, R_lm1_1 ); // Q28
2122 : }
2123 : }
2124 :
2125 11623851 : return p; // Q28
2126 : }
2127 :
2128 2586525 : static Word32 SHrot_u_fx(
2129 : const Word16 l,
2130 : const Word16 m,
2131 : const Word16 n,
2132 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2133 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] // Q14
2134 : )
2135 : {
2136 2586525 : return SHrot_p_fx( 0, l, m, n, SHrotmat, R_lm1 );
2137 : }
2138 :
2139 3895299 : static Word32 SHrot_v_fx(
2140 : const Word16 l,
2141 : const Word16 m,
2142 : const Word16 n,
2143 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2144 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] // Q14
2145 : )
2146 : {
2147 :
2148 : Word32 result;
2149 : Word32 p0, p1;
2150 : Word16 d;
2151 :
2152 3895299 : IF( m == 0 )
2153 : {
2154 654387 : p0 = SHrot_p_fx( 1, l, 1, n, SHrotmat, R_lm1 ); // Q28
2155 654387 : p1 = SHrot_p_fx( -1, l, -1, n, SHrotmat, R_lm1 ); // Q28
2156 654387 : result = L_shr( L_add( p0, p1 ), 2 ); // converting to Q27
2157 : }
2158 : ELSE
2159 : {
2160 3240912 : IF( m > 0 )
2161 : {
2162 1620456 : d = 0;
2163 1620456 : move16();
2164 1620456 : if ( EQ_16( m, 1 ) )
2165 : {
2166 654387 : d = 1;
2167 654387 : move16();
2168 : }
2169 1620456 : p0 = SHrot_p_fx( 1, l, sub( m, 1 ), n, SHrotmat, R_lm1 ); // Q28
2170 1620456 : p1 = SHrot_p_fx( -1, l, add( negate( m ), 1 ), n, SHrotmat, R_lm1 ); // Q28
2171 1620456 : result = Msub_32_16_r( Mpy_32_16_r( p0, square_root16_table[1 + d] ), p1, shl( sub( 1, d ), 14 ) ); // Q27
2172 : }
2173 : ELSE
2174 : {
2175 1620456 : d = 0;
2176 1620456 : move16();
2177 1620456 : if ( EQ_16( m, -1 ) )
2178 : {
2179 654387 : d = 1;
2180 654387 : move16();
2181 : }
2182 1620456 : p0 = SHrot_p_fx( 1, l, add( m, 1 ), n, SHrotmat, R_lm1 );
2183 1620456 : p1 = SHrot_p_fx( -1, l, negate( add( m, 1 ) ), n, SHrotmat, R_lm1 );
2184 1620456 : result = Madd_32_16_r( Mpy_32_16_r( p0, shl( sub( 1, d ), 14 ) ), p1, square_root16_table[1 + d] ); // Q27
2185 : }
2186 : }
2187 3895299 : return result; // Q27
2188 : }
2189 :
2190 623364 : static Word32 SHrot_w_fx(
2191 : const Word16 l,
2192 : const Word16 m,
2193 : const Word16 n,
2194 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], // Q14
2195 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] // Q14
2196 : )
2197 : {
2198 : Word32 result, p0, p1;
2199 :
2200 623364 : IF( m == 0 )
2201 : {
2202 0 : assert( 0 && "ERROR should not be called\n" );
2203 : return 0;
2204 : }
2205 : ELSE
2206 : {
2207 623364 : IF( m > 0 )
2208 : {
2209 311682 : p0 = SHrot_p_fx( 1, l, add( m, 1 ), n, SHrotmat, R_lm1 ); // Q28
2210 311682 : p1 = SHrot_p_fx( -1, l, negate( add( m, 1 ) ), n, SHrotmat, R_lm1 ); // Q28
2211 311682 : result = L_add( p0, p1 ); // Q28
2212 : }
2213 : ELSE
2214 : {
2215 311682 : p0 = SHrot_p_fx( 1, l, sub( m, 1 ), n, SHrotmat, R_lm1 ); // Q28
2216 311682 : p1 = SHrot_p_fx( -1, l, sub( 1, m ), n, SHrotmat, R_lm1 ); // Q28
2217 311682 : result = L_sub( p0, p1 ); // Q28
2218 : }
2219 : }
2220 :
2221 623364 : return result; // Q28
2222 : }
2223 :
2224 :
2225 : /*-------------------------------------------------------------------------
2226 : * SHrotmatgen_fx()
2227 : *
2228 : *
2229 : *------------------------------------------------------------------------*/
2230 :
2231 92556 : void SHrotmatgen_fx(
2232 : Word16 SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], /* o : rotation matrix in SHD Q14 */
2233 : Word32 Rmat[3][3], /* i : real-space rotation matrix Q30 */
2234 : const Word16 order /* i : ambisonics order */
2235 : )
2236 : {
2237 92556 : Word16 d = 0;
2238 92556 : move16();
2239 92556 : Word16 band_idx = 0;
2240 92556 : move16();
2241 : Word16 i, j;
2242 : Word16 l, m, n;
2243 : Word16 absm;
2244 92556 : Word16 sqdenom = 0, sql2mm2 = 0, sqdabsm = 0, sqlabsm = 0;
2245 92556 : Word16 u = 0, v = 0, w = 0;
2246 92556 : move16();
2247 92556 : move16();
2248 92556 : move16();
2249 92556 : move16();
2250 92556 : move16();
2251 92556 : move16();
2252 92556 : move16();
2253 :
2254 92556 : Word32 u32_fx = 0, v32_fx = 0, w32_fx = 0;
2255 92556 : move32();
2256 92556 : move32();
2257 92556 : move32();
2258 :
2259 : Word16 R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
2260 :
2261 1573452 : FOR( i = 0; i < HEADROT_SHMAT_DIM; i++ )
2262 : {
2263 1480896 : set16_fx( R_lm1[i], 0, HEADROT_SHMAT_DIM );
2264 : }
2265 : Word16 R_l[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
2266 : Word32 result;
2267 92556 : SHrotmat[0][0] = ONE_IN_Q14;
2268 92556 : move16();
2269 :
2270 92556 : SHrotmat[1][1] = extract_h( Rmat[1][1] ); // Q14
2271 92556 : move16();
2272 92556 : SHrotmat[1][2] = extract_h( Rmat[1][2] );
2273 92556 : move16();
2274 92556 : SHrotmat[1][3] = extract_h( Rmat[1][0] );
2275 92556 : move16();
2276 :
2277 92556 : SHrotmat[2][1] = extract_h( Rmat[2][1] ); // Q14
2278 92556 : move16();
2279 92556 : SHrotmat[2][2] = extract_h( Rmat[2][2] );
2280 92556 : move16();
2281 92556 : SHrotmat[2][3] = extract_h( Rmat[2][0] );
2282 92556 : move16();
2283 :
2284 92556 : SHrotmat[3][1] = extract_h( Rmat[0][1] ); // Q14
2285 92556 : move16();
2286 92556 : SHrotmat[3][2] = extract_h( Rmat[0][2] );
2287 92556 : move16();
2288 92556 : SHrotmat[3][3] = extract_h( Rmat[0][0] );
2289 92556 : move16();
2290 :
2291 370224 : FOR( i = 0; i < 2 * 1 + 1; i++ )
2292 : {
2293 1110672 : FOR( j = 0; j < 2 * 1 + 1; j++ )
2294 : {
2295 833004 : R_lm1[i][j] = SHrotmat[i + 1][j + 1]; // Q14
2296 833004 : move16();
2297 : }
2298 : }
2299 92556 : band_idx = 4;
2300 92556 : move16();
2301 205623 : FOR( l = 2; l <= order; l++ )
2302 : {
2303 113067 : set16_fx( &R_l[0][0], 0, HEADROT_SHMAT_DIM2 );
2304 767454 : FOR( m = -l; m <= l; m++ )
2305 : {
2306 654387 : d = 0;
2307 654387 : move16();
2308 654387 : if ( m == 0 )
2309 : {
2310 113067 : d = 1;
2311 113067 : move16();
2312 : }
2313 654387 : absm = extract_l( L_abs( m ) );
2314 :
2315 654387 : sql2mm2 = square_root30_q12[l * l - m * m]; // Q12
2316 654387 : move16();
2317 654387 : sqdabsm = square_root30_q12[( 1 + d ) * ( l + absm - 1 ) * ( l + absm )]; // Q12
2318 654387 : move16();
2319 654387 : sqlabsm = square_root30_q12[( ( l - ( absm + 1 ) ) * ( l - absm ) )]; // Q12
2320 654387 : move16();
2321 :
2322 4549686 : FOR( n = -l; n <= l; n++ )
2323 : {
2324 3895299 : IF( EQ_16( abs_s( n ), l ) )
2325 : {
2326 1308774 : sqdenom = square_root30_q12[( ( 2 * l ) * ( 2 * l - 1 ) )]; // Q12
2327 1308774 : move16();
2328 : }
2329 : ELSE
2330 : {
2331 2586525 : sqdenom = square_root30_q12[( l * l - n * n )]; // Q12
2332 2586525 : move16();
2333 : }
2334 :
2335 :
2336 3895299 : u = div_l( L_shl( (Word32) sql2mm2, 15 ), sqdenom ); // Q14
2337 3895299 : v = imult1616( div_l( L_shl( (Word32) sqdabsm, 14 ), sqdenom ), sub( 1, shl( d, 1 ) ) ); // Q14
2338 3895299 : w = imult1616( div_l( L_shl( (Word32) sqlabsm, 14 ), sqdenom ), negate( sub( 1, d ) ) ); // Q14
2339 :
2340 3895299 : IF( u != 0 )
2341 : {
2342 2586525 : result = SHrot_u_fx( l, m, n, SHrotmat, R_lm1 ); // Q28
2343 2586525 : u32_fx = Mpy_32_16_r( result, u ); // Q27
2344 : }
2345 3895299 : IF( v != 0 )
2346 : {
2347 3895299 : result = SHrot_v_fx( l, m, n, SHrotmat, R_lm1 );
2348 3895299 : v32_fx = Mpy_32_16_r( result, v ); // Q26
2349 : }
2350 3895299 : IF( w != 0 )
2351 : {
2352 623364 : result = SHrot_w_fx( l, m, n, SHrotmat, R_lm1 );
2353 623364 : w32_fx = Mpy_32_16_r( result, w ); // Q27
2354 : }
2355 : // Addind and converting to 16 bit integer of Q14
2356 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
2357 3895299 : move16();
2358 : }
2359 : }
2360 :
2361 767454 : FOR( i = 0; i < 2 * l + 1; i++ )
2362 : {
2363 4549686 : FOR( j = 0; j < 2 * l + 1; j++ )
2364 : {
2365 3895299 : SHrotmat[band_idx + i][band_idx + j] = R_l[i][j]; // Q14
2366 3895299 : move16();
2367 : }
2368 : }
2369 :
2370 767454 : FOR( i = 0; i < 2 * l + 1; i++ )
2371 : {
2372 4549686 : FOR( j = 0; j < 2 * l + 1; j++ )
2373 : {
2374 3895299 : R_lm1[i][j] = R_l[i][j];
2375 3895299 : move16();
2376 : }
2377 : }
2378 :
2379 113067 : band_idx = add( band_idx, add( shl( l, 1 ), 1 ) );
2380 : }
2381 :
2382 92556 : return;
2383 : }
2384 :
2385 :
2386 : /*-------------------------------------------------------------------------
2387 : * ivas_combined_orientation_update_index()
2388 : *
2389 : * update read index based on the number of rendered samples
2390 : *------------------------------------------------------------------------*/
2391 :
2392 2172790 : void ivas_combined_orientation_update_index(
2393 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
2394 : const Word16 samples_rendered /* i : samples rendered since the last call */
2395 : )
2396 : {
2397 : Word16 exp, div_result;
2398 2172790 : IF( hCombinedOrientationData != NULL )
2399 : {
2400 551081 : IF( EQ_16( hCombinedOrientationData->num_subframes, 1 ) || hCombinedOrientationData->sr_low_res_flag )
2401 : {
2402 : /* only one orientation available anyway or split rendering with low resolution*/
2403 135601 : hCombinedOrientationData->subframe_idx = 0;
2404 135601 : move16();
2405 : }
2406 : ELSE
2407 : {
2408 415480 : hCombinedOrientationData->cur_subframe_samples_rendered = add( hCombinedOrientationData->cur_subframe_samples_rendered, samples_rendered );
2409 415480 : move16();
2410 415480 : div_result = BASOP_Util_Divide3216_Scale( hCombinedOrientationData->cur_subframe_samples_rendered, hCombinedOrientationData->subframe_size, &exp );
2411 415480 : hCombinedOrientationData->subframe_idx = add( hCombinedOrientationData->subframe_idx, shl( div_result, add( exp, 1 ) ) );
2412 415480 : move16();
2413 415480 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size;
2414 415480 : move16();
2415 415480 : hCombinedOrientationData->subframe_idx = s_min( hCombinedOrientationData->subframe_idx, sub( hCombinedOrientationData->num_subframes, 1 ) );
2416 415480 : move16();
2417 : }
2418 : }
2419 :
2420 2172790 : return;
2421 : }
2422 :
2423 :
2424 : /*-------------------------------------------------------------------------
2425 : * ivas_combined_orientation_set_to_start_index()
2426 : *
2427 : * update read index based on the number of rendered samples
2428 : *------------------------------------------------------------------------*/
2429 :
2430 1956299 : void ivas_combined_orientation_set_to_start_index(
2431 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
2432 : )
2433 : {
2434 1956299 : IF( hCombinedOrientationData != NULL )
2435 : {
2436 385062 : hCombinedOrientationData->subframe_idx = hCombinedOrientationData->subframe_idx_start;
2437 385062 : move16();
2438 385062 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered_start;
2439 385062 : move16();
2440 : }
2441 :
2442 1956299 : return;
2443 : }
2444 : /*-------------------------------------------------------------------------
2445 : * ivas_combined_orientation_update_start_index()
2446 : *
2447 : * update start index based on the number of rendered samples
2448 : *------------------------------------------------------------------------*/
2449 :
2450 1552835 : void ivas_combined_orientation_update_start_index(
2451 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
2452 : const Word16 samples_rendered /* i : samples rendered since the last call */
2453 : )
2454 : {
2455 1552835 : IF( hCombinedOrientationData != NULL )
2456 : {
2457 249762 : IF( EQ_16( hCombinedOrientationData->num_subframes, 1 ) || hCombinedOrientationData->sr_low_res_flag )
2458 : {
2459 : /* only one orientation available anyway or split rendering with low resolution*/
2460 144012 : hCombinedOrientationData->subframe_idx = 0;
2461 144012 : move16();
2462 : }
2463 : ELSE
2464 : {
2465 105750 : hCombinedOrientationData->cur_subframe_samples_rendered_start = add( hCombinedOrientationData->cur_subframe_samples_rendered_start, samples_rendered );
2466 105750 : move16();
2467 105750 : hCombinedOrientationData->subframe_idx_start = add( hCombinedOrientationData->subframe_idx_start, mult( hCombinedOrientationData->cur_subframe_samples_rendered, div_s( 1, hCombinedOrientationData->subframe_size ) ) );
2468 105750 : move16();
2469 105750 : hCombinedOrientationData->cur_subframe_samples_rendered_start = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size; /* No operator to calculate modulo*/
2470 105750 : move16();
2471 105750 : hCombinedOrientationData->subframe_idx_start = s_min( hCombinedOrientationData->subframe_idx, sub( hCombinedOrientationData->num_subframes, 1 ) );
2472 105750 : move16();
2473 : }
2474 : }
2475 :
2476 1552835 : return;
2477 : }
|
