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