Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : #include "ivas_cnst.h"
34 : #include <assert.h>
35 : #include <stdint.h>
36 : #include "options.h"
37 : #include <math.h>
38 : #include "cnst.h"
39 : #include "prot.h"
40 : #include "ivas_prot.h"
41 : #include "ivas_prot_rend.h"
42 : #ifdef DEBUGGING
43 : #include "debug.h"
44 : #endif
45 : #include "wmc_auto.h"
46 :
47 :
48 : /*-----------------------------------------------------------------------*
49 : * Local funtion declarations
50 : *-----------------------------------------------------------------------*/
51 :
52 :
53 : static ivas_error combine_external_and_head_orientations( IVAS_QUATERNION *headRotQuaternions, IVAS_VECTOR3 *listenerPos, ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis, EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, COMBINED_ORIENTATION_HANDLE hCombinedOrientationData );
54 :
55 : static void external_target_interpolation( EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, const int16_t i );
56 :
57 : static bool are_orientations_same( const IVAS_QUATERNION *orientation1, const IVAS_QUATERNION *orientation2 );
58 :
59 :
60 : /*-----------------------------------------------------------------------*
61 : * ivas_headTrack_open()
62 : *
63 : * Allocate and initialize Head-Tracking handle
64 : *-----------------------------------------------------------------------*/
65 :
66 249 : ivas_error ivas_headTrack_open(
67 : HEAD_TRACK_DATA_HANDLE *hHeadTrackData /* o : head track handle */
68 : )
69 : {
70 : int16_t i;
71 : ivas_error error;
72 :
73 : /* Allocate Head-Tracking handle */
74 249 : if ( ( *hHeadTrackData = (HEAD_TRACK_DATA_HANDLE) malloc( sizeof( HEAD_TRACK_DATA ) ) ) == NULL )
75 : {
76 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for head-tracking memory\n" ) );
77 : }
78 :
79 : /* Initialization */
80 249 : ( *hHeadTrackData )->lrSwitchInterpVal = 0.0f;
81 249 : ( *hHeadTrackData )->lrSwitchedCurrent = 0;
82 249 : ( *hHeadTrackData )->lrSwitchedNext = 0;
83 249 : if ( ( ( *hHeadTrackData )->OrientationTracker = (ivas_orient_trk_state_t *) malloc( sizeof( ivas_orient_trk_state_t ) ) ) == NULL )
84 : {
85 0 : return IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for Orientation tracking" );
86 : }
87 :
88 249 : if ( ( error = ivas_orient_trk_Init( ( *hHeadTrackData )->OrientationTracker ) ) != IVAS_ERR_OK )
89 : {
90 0 : return error;
91 : }
92 :
93 : /* Initialise Rmat_prev to I, Rmat will be computed later */
94 996 : for ( i = 0; i < 3; i++ )
95 : {
96 747 : set_zero( ( *hHeadTrackData )->Rmat_prev[i], 3 );
97 747 : ( *hHeadTrackData )->Rmat_prev[i][i] = 1.0f;
98 : }
99 :
100 249 : ( *hHeadTrackData )->sr_pose_pred_axis = DEFAULT_AXIS;
101 :
102 249 : set_zero( ( *hHeadTrackData )->chEneIIR[0], MASA_FREQUENCY_BANDS );
103 249 : set_zero( ( *hHeadTrackData )->chEneIIR[1], MASA_FREQUENCY_BANDS );
104 249 : set_zero( ( *hHeadTrackData )->procChEneIIR[0], MASA_FREQUENCY_BANDS );
105 249 : set_zero( ( *hHeadTrackData )->procChEneIIR[1], MASA_FREQUENCY_BANDS );
106 :
107 249 : return IVAS_ERR_OK;
108 : }
109 :
110 :
111 : /*-----------------------------------------------------------------------*
112 : * ivas_headTrack_close()
113 : *
114 : * Deallocate Head-Tracking handle
115 : *-----------------------------------------------------------------------*/
116 :
117 1939 : void ivas_headTrack_close(
118 : HEAD_TRACK_DATA_HANDLE *hHeadTrackData /* i/o: head track handle */
119 : )
120 : {
121 1939 : if ( hHeadTrackData == NULL || *hHeadTrackData == NULL )
122 : {
123 1690 : return;
124 : }
125 :
126 249 : if ( ( *hHeadTrackData )->OrientationTracker != NULL )
127 : {
128 249 : free( ( *hHeadTrackData )->OrientationTracker );
129 249 : ( *hHeadTrackData )->OrientationTracker = NULL;
130 : }
131 :
132 249 : free( ( *hHeadTrackData ) );
133 249 : *hHeadTrackData = NULL;
134 :
135 249 : return;
136 : }
137 :
138 :
139 : /*----------------------------------------------------------------------------------
140 : * QuatToRotMat()
141 : *
142 : * Quaternion handling: calculate rotation matrices in real-space and SHD
143 : *---------------------------------------------------------------------------------*/
144 :
145 59062999 : void QuatToRotMat(
146 : const IVAS_QUATERNION quat, /* i : quaternion describing the rotation */
147 : float Rmat[3][3] /* o : real-space rotation matrix for this rotation */
148 : )
149 : {
150 59062999 : if ( quat.w == -3.0 )
151 : {
152 : IVAS_QUATERNION quat_local;
153 0 : Euler2Quat( deg2rad( quat.x ), deg2rad( quat.y ), deg2rad( quat.z ), &quat_local );
154 0 : QuatToRotMat( quat_local, Rmat );
155 : }
156 : else
157 : {
158 59062999 : Rmat[0][0] = quat.w * quat.w + quat.x * quat.x - quat.y * quat.y - quat.z * quat.z;
159 59062999 : Rmat[0][1] = 2.0f * ( quat.x * quat.y - quat.w * quat.z );
160 59062999 : Rmat[0][2] = 2.0f * ( quat.x * quat.z + quat.w * quat.y );
161 :
162 59062999 : Rmat[1][0] = 2.0f * ( quat.x * quat.y + quat.w * quat.z );
163 59062999 : Rmat[1][1] = quat.w * quat.w - quat.x * quat.x + quat.y * quat.y - quat.z * quat.z;
164 59062999 : Rmat[1][2] = 2.0f * ( quat.y * quat.z - quat.w * quat.x );
165 :
166 59062999 : Rmat[2][0] = 2.0f * ( quat.x * quat.z - quat.w * quat.y );
167 59062999 : Rmat[2][1] = 2.0f * ( quat.y * quat.z + quat.w * quat.x );
168 59062999 : Rmat[2][2] = quat.w * quat.w - quat.x * quat.x - quat.y * quat.y + quat.z * quat.z;
169 : }
170 :
171 59062999 : return;
172 : }
173 :
174 :
175 : /*-------------------------------------------------------------------------
176 : * rad2deg()
177 : *
178 : * Converts normalized radians to degrees
179 : *------------------------------------------------------------------------*/
180 :
181 6984 : float rad2deg(
182 : float radians )
183 : {
184 7056 : while ( radians >= EVS_PI )
185 : {
186 72 : radians = radians - EVS_PI;
187 : }
188 6984 : while ( radians <= -EVS_PI )
189 : {
190 0 : radians = radians + EVS_PI;
191 : }
192 :
193 6984 : return _180_OVER_PI * radians;
194 : }
195 :
196 :
197 : /*-------------------------------------------------------------------------
198 : * rotateAziEle()
199 : *
200 : * Apply rotation to direction parameters azimuth and elevation
201 : *------------------------------------------------------------------------*/
202 :
203 456094954 : void rotateAziEle(
204 : float azi_in, /* i : output elevation */
205 : float ele_in, /* i : input elevation */
206 : int16_t *azi, /* o : rotated azimuth */
207 : int16_t *ele, /* o : rotated elevation */
208 : float Rmat[3][3], /* i : real-space rotation matrix */
209 : const int16_t isPlanar /* i : is rotation planar and elevation meaningless? */
210 : )
211 : {
212 : int16_t n;
213 : float dv[3], dv_r[3];
214 : float w;
215 :
216 : /*Conversion spherical to cartesian coordinates*/
217 456094954 : w = cosf( ele_in * PI_OVER_180 );
218 456094954 : dv[0] = w * cosf( azi_in * PI_OVER_180 );
219 456094954 : dv[1] = w * sinf( azi_in * PI_OVER_180 );
220 456094954 : dv[2] = sinf( ele_in * PI_OVER_180 );
221 :
222 : /*Rotation mtx multiplication*/
223 1824379816 : for ( n = 0; n < 3; n++ )
224 : {
225 1368284862 : dv_r[n] = Rmat[n][0] * dv[0] + Rmat[n][1] * dv[1] + Rmat[n][2] * dv[2];
226 : }
227 :
228 : /*Conversion cartesian to spherical coordinates*/
229 456094954 : *azi = (int16_t) roundf( max( -180.0f, min( 180.0f, atan2f( dv_r[1], dv_r[0] ) * _180_OVER_PI ) ) );
230 456094954 : if ( isPlanar == 0 )
231 : {
232 448091014 : *ele = (int16_t) roundf( max( -90.0f, min( 90.0f, atan2f( dv_r[2], sqrtf( dv_r[0] * dv_r[0] + dv_r[1] * dv_r[1] ) ) * _180_OVER_PI ) ) );
233 : }
234 : else
235 : {
236 8003940 : *ele = 0;
237 : }
238 :
239 456094954 : return;
240 : }
241 :
242 :
243 : /*-------------------------------------------------------------------------
244 : * rotateFrame_shd()
245 : *
246 : * Apply rotation to signals in Spherical Harmonic Domain
247 : *------------------------------------------------------------------------*/
248 :
249 127005 : void rotateFrame_shd(
250 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i : head and external orientation combined handle */
251 : float *output[], /* i/o: unrotated HOA3 signal buffer in TD */
252 : const int16_t subframe_len, /* i : subframe length per channel */
253 : const IVAS_OUTPUT_SETUP hTransSetup, /* i : format for rotation */
254 : const int16_t subframe_idx /* i : subframe index */
255 : )
256 : {
257 : int16_t i, l, n, m;
258 : int16_t m1, m2;
259 : int16_t shd_rot_max_order;
260 :
261 : float tmp;
262 : float tmpRot[2 * HEADROT_ORDER + 1];
263 : float SHrotmat_prev[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
264 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
265 : float cross_fade[L_FRAME48k / MAX_PARAM_SPATIAL_SUBFRAMES];
266 :
267 127005 : shd_rot_max_order = hTransSetup.ambisonics_order;
268 :
269 127005 : tmp = 1.0f / ( subframe_len - 1 );
270 30608205 : for ( i = 0; i < subframe_len; i++ )
271 : {
272 30481200 : cross_fade[i] = i * tmp;
273 : }
274 :
275 : /* initialize rotation matrices with zeros */
276 2159085 : for ( i = 0; i < HEADROT_SHMAT_DIM; i++ )
277 : {
278 2032080 : set_zero( SHrotmat_prev[i], HEADROT_SHMAT_DIM );
279 2032080 : set_zero( SHrotmat[i], HEADROT_SHMAT_DIM );
280 : }
281 :
282 : /* calculate ambisonics rotation matrices for the previous and current frames */
283 127005 : SHrotmatgen( SHrotmat_prev, hCombinedOrientationData->Rmat_prev[0], shd_rot_max_order );
284 :
285 127005 : SHrotmatgen( SHrotmat, hCombinedOrientationData->Rmat[hCombinedOrientationData->subframe_idx], shd_rot_max_order );
286 :
287 30608205 : for ( i = 0; i < subframe_len; i++ )
288 : {
289 : /*As the rotation matrix becomes block diagonal in a SH basis, we can
290 : apply each angular-momentum block individually to save complexity. */
291 :
292 : /* loop over l blocks */
293 30481200 : m1 = 1;
294 30481200 : m2 = 4;
295 121924800 : for ( l = 1; l <= shd_rot_max_order; l++ )
296 : {
297 : /* compute mtx-vector product for this l */
298 548661600 : for ( n = m1; n < m2; n++ )
299 : {
300 457218000 : tmpRot[n - m1] = 0.f;
301 :
302 2987157600 : for ( m = m1; m < m2; m++ )
303 : {
304 : /* crossfade with previous rotation gains */
305 2529939600 : tmpRot[n - m1] += cross_fade[i] * SHrotmat[n][m] * output[m][subframe_idx * subframe_len + i] + ( 1 - cross_fade[i] ) * SHrotmat_prev[n][m] * output[m][subframe_idx * subframe_len + i];
306 : }
307 : }
308 :
309 : /* write back the result */
310 548661600 : for ( n = m1; n < m2; n++ )
311 : {
312 457218000 : output[n][subframe_idx * subframe_len + i] = tmpRot[n - m1];
313 : }
314 91443600 : m1 = m2;
315 91443600 : m2 += 2 * ( l + 1 ) + 1;
316 : }
317 :
318 : /* unoptimized code for reference (full matrix multiplication)
319 : for ( n = 0; n < nchan; n++ )
320 : {
321 : tmpRot[n] = 0.f;
322 :
323 : for ( m = 0; m < nchan; m++ )
324 : {
325 : tmpRot[n] += SHrotmat[n][m] * output[m][i];
326 : }
327 : }
328 : for ( n = 0; n < nchan; n++ )
329 : {
330 : output[n][i] = tmpRot[n];
331 : }
332 : */
333 : }
334 :
335 : /* move Rmat to Rmat_prev */
336 508020 : for ( i = 0; i < 3; i++ )
337 : {
338 381015 : mvr2r(
339 381015 : hCombinedOrientationData->Rmat[hCombinedOrientationData->subframe_idx][i],
340 381015 : hCombinedOrientationData->Rmat_prev[0][i],
341 : 3 );
342 : }
343 :
344 127005 : return;
345 : }
346 :
347 :
348 : /*-------------------------------------------------------------------------
349 : * rotateFrame_sd()
350 : *
351 : * Apply rotation to signals in Spatial Domain
352 : *------------------------------------------------------------------------*/
353 :
354 390432 : void rotateFrame_sd(
355 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i : head and external orientation combined handle */
356 : float *output[], /* i/o: unrotated SD signal buffer in TD */
357 : const int16_t subframe_len, /* i : subframe length per channel */
358 : const IVAS_OUTPUT_SETUP hTransSetup, /* i : format for rotation */
359 : const EFAP_HANDLE hEFAPdata, /* i : EFAP structure */
360 : const int16_t subframe_idx /* i : subframe index */
361 : )
362 : {
363 : int16_t i, j;
364 : int16_t nchan, index_lfe;
365 : int16_t ch_in, ch_in_woLFE, ch_out, ch_out_woLFE;
366 : int16_t azimuth, elevation;
367 :
368 : float tmp;
369 : float tmp_gains[MAX_LS_CHANNELS - 1];
370 : float gains[MAX_LS_CHANNELS][MAX_LS_CHANNELS];
371 : float gains_prev[MAX_LS_CHANNELS][MAX_LS_CHANNELS];
372 : float output_tmp[MAX_LS_CHANNELS][L_FRAME48k];
373 : float cross_fade[L_FRAME48k / MAX_PARAM_SPATIAL_SUBFRAMES];
374 390432 : push_wmops( "rotateFrame_sd" );
375 :
376 390432 : nchan = hTransSetup.nchan_out_woLFE + hTransSetup.num_lfe;
377 390432 : index_lfe = hTransSetup.index_lfe[0];
378 :
379 390432 : tmp = 1.0f / ( subframe_len - 1 );
380 94094112 : for ( i = 0; i < subframe_len; i++ )
381 : {
382 93703680 : cross_fade[i] = i * tmp;
383 : }
384 :
385 2733024 : for ( ch_in = 0; ch_in < nchan; ch_in++ )
386 : {
387 : /* zero output and gain buffers */
388 2342592 : set_zero( &output_tmp[ch_in][subframe_idx * subframe_len], subframe_len );
389 2342592 : set_zero( gains_prev[ch_in], nchan );
390 2342592 : set_zero( gains[ch_in], nchan );
391 :
392 : /* set gains to passthrough by default */
393 2342592 : gains_prev[ch_in][ch_in] = 1.0f;
394 2342592 : gains[ch_in][ch_in] = 1.0f;
395 :
396 : /* skip LFE */
397 2342592 : if ( ch_in == index_lfe )
398 : {
399 390432 : continue;
400 : }
401 :
402 : /* input channel index without LFE */
403 1952160 : ch_in_woLFE = ( ch_in >= index_lfe ) ? ch_in - 1 : ch_in;
404 :
405 : /* gains for previous subframe rotation */
406 1952160 : rotateAziEle( hTransSetup.ls_azimuth[ch_in_woLFE], hTransSetup.ls_elevation[ch_in_woLFE], &azimuth, &elevation, hCombinedOrientationData->Rmat_prev[0], hTransSetup.is_planar_setup );
407 :
408 1952160 : if ( hEFAPdata != NULL && ( hTransSetup.ls_azimuth[ch_in_woLFE] != azimuth || hTransSetup.ls_elevation[ch_in_woLFE] != elevation ) )
409 : {
410 1438395 : efap_determine_gains( hEFAPdata, tmp_gains, azimuth, elevation, EFAP_MODE_EFAP );
411 10068765 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
412 : {
413 : /* skip LFE */
414 8630370 : if ( ch_out == index_lfe )
415 : {
416 1438395 : continue;
417 : }
418 :
419 : /* output channel index without LFE */
420 7191975 : ch_out_woLFE = ( ch_out >= index_lfe ) ? ch_out - 1 : ch_out;
421 :
422 7191975 : gains_prev[ch_in][ch_out] = tmp_gains[ch_out_woLFE];
423 : }
424 : }
425 :
426 : /* gains for current subframe rotation */
427 1952160 : rotateAziEle( hTransSetup.ls_azimuth[ch_in_woLFE], hTransSetup.ls_elevation[ch_in_woLFE], &azimuth, &elevation, hCombinedOrientationData->Rmat[hCombinedOrientationData->subframe_idx], hTransSetup.is_planar_setup );
428 :
429 1952160 : if ( hEFAPdata != NULL && ( hTransSetup.ls_azimuth[ch_in_woLFE] != azimuth || hTransSetup.ls_elevation[ch_in_woLFE] != elevation ) )
430 : {
431 1438455 : efap_determine_gains( hEFAPdata, tmp_gains, azimuth, elevation, EFAP_MODE_EFAP );
432 :
433 10069185 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
434 : {
435 : /* skip LFE */
436 8630730 : if ( ch_out == index_lfe )
437 : {
438 1438455 : continue;
439 : }
440 :
441 : /* output channel index without LFE */
442 7192275 : ch_out_woLFE = ( ch_out >= index_lfe ) ? ch_out - 1 : ch_out;
443 :
444 7192275 : gains[ch_in][ch_out] = tmp_gains[ch_out_woLFE];
445 : }
446 : }
447 : }
448 :
449 : /* apply panning gains by mtx multiplication */
450 2733024 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
451 : {
452 16398144 : for ( ch_in = 0; ch_in < nchan; ch_in++ )
453 : {
454 : /* crossfade with previous rotation gains */
455 3387388032 : for ( i = subframe_idx * subframe_len, j = 0; j < subframe_len; i++, j++ )
456 : {
457 3373332480 : output_tmp[ch_out][i] += ( cross_fade[j] ) * gains[ch_in][ch_out] * output[ch_in][i] + ( 1 - cross_fade[j] ) * gains_prev[ch_in][ch_out] * output[ch_in][i];
458 : }
459 : }
460 : }
461 :
462 : /* move Rmat to Rmat_prev */
463 1561728 : for ( i = 0; i < 3; i++ )
464 : {
465 1171296 : mvr2r(
466 1171296 : hCombinedOrientationData->Rmat[hCombinedOrientationData->subframe_idx][i],
467 1171296 : hCombinedOrientationData->Rmat_prev[0][i],
468 : 3 );
469 : }
470 :
471 : /* copy to output */
472 2733024 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
473 : {
474 2342592 : mvr2r( &output_tmp[ch_out][subframe_idx * subframe_len], &output[ch_out][subframe_idx * subframe_len], subframe_len );
475 : }
476 :
477 390432 : pop_wmops();
478 390432 : return;
479 : }
480 :
481 :
482 : /*-------------------------------------------------------------------------
483 : * rotateFrame_shd_cldfb()
484 : *
485 : * Apply rotation to signals in Spherical Harmonic Domain and in CLDFB
486 : *------------------------------------------------------------------------*/
487 :
488 784077 : void rotateFrame_shd_cldfb(
489 : float Cldfb_RealBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain real part */
490 : float Cldfb_ImagBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain imag part */
491 : float Rmat[3][3], /* i : real-space rotation matrix */
492 : const int16_t nInChannels, /* i : number of channels */
493 : const int16_t numTimeSlots, /* i : number of time slots to process */
494 : const int16_t shd_rot_max_order /* i : split-order rotation method */
495 : )
496 : {
497 784077 : int16_t n = 0;
498 784077 : int16_t m = 0;
499 784077 : int16_t i = 0;
500 784077 : int16_t iBand = 0;
501 784077 : int16_t l = 0, m1 = 0, m2 = 0;
502 : float realRot[2 * HEADROT_ORDER + 1], imagRot[2 * HEADROT_ORDER + 1];
503 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
504 :
505 784077 : assert( ( nInChannels == HOA3_CHANNELS || nInChannels == HOA2_CHANNELS || nInChannels == FOA_CHANNELS ) &&
506 : "Number of channels must correspond to an ambisonics order!" );
507 :
508 : /* initialize rotation matrices with zeros */
509 13329309 : for ( i = 0; i < HEADROT_SHMAT_DIM; i++ )
510 : {
511 12545232 : set_zero( SHrotmat[i], HEADROT_SHMAT_DIM );
512 : }
513 :
514 : /* calculate Ambisonics rotation matrix from the quaternion */
515 784077 : SHrotmatgen( SHrotmat, Rmat, shd_rot_max_order );
516 :
517 : /* rotation by mtx multiplication */
518 3869523 : for ( i = 0; i < numTimeSlots; i++ )
519 : {
520 188212206 : for ( iBand = 0; iBand < CLDFB_NO_CHANNELS_MAX; iBand++ )
521 : {
522 : /*As the rotation matrix becomes block diagonal in a SH basis, we can
523 : apply each angular-momentum block individually to save complexity. */
524 :
525 : /* loop over l blocks */
526 185126760 : m1 = 1;
527 185126760 : m2 = 4;
528 740507040 : for ( l = 1; l <= shd_rot_max_order; l++ )
529 : {
530 : /* compute mtx-vector product for this l */
531 3332281680 : for ( n = m1; n < m2; n++ )
532 : {
533 2776901400 : realRot[n - m1] = 0.f;
534 2776901400 : imagRot[n - m1] = 0.f;
535 :
536 18142422480 : for ( m = m1; m < m2; m++ )
537 : {
538 15365521080 : realRot[n - m1] += SHrotmat[n][m] * Cldfb_RealBuffer[m][i][iBand];
539 15365521080 : imagRot[n - m1] += SHrotmat[n][m] * Cldfb_ImagBuffer[m][i][iBand];
540 : }
541 : }
542 : /* write back the result */
543 3332281680 : for ( n = m1; n < m2; n++ )
544 : {
545 2776901400 : Cldfb_RealBuffer[n][i][iBand] = realRot[n - m1];
546 2776901400 : Cldfb_ImagBuffer[n][i][iBand] = imagRot[n - m1];
547 : }
548 555380280 : m1 = m2;
549 555380280 : m2 += 2 * ( l + 1 ) + 1;
550 : }
551 :
552 : /* unoptimized code for reference (full matrix multiplication)
553 : for (n = 0; n < nInChannels; n++)
554 : {
555 : realRot[n] = 0.f;
556 : imagRot[n] = 0.f;
557 :
558 : for (m = 0; m < nInChannels; m++)
559 : {
560 : realRot[n] += SHrotmat[n][m] * Cldfb_RealBuffer[m][i][iBand];
561 : imagRot[n] += SHrotmat[n][m] * Cldfb_ImagBuffer[m][i][iBand];
562 : }
563 : }
564 : for (n = 0; n < nInChannels; n++)
565 : {
566 : Cldfb_RealBuffer[n][i][iBand] = realRot[n];
567 : Cldfb_ImagBuffer[n][i][iBand] = imagRot[n];
568 : }
569 : */
570 : }
571 : }
572 :
573 784077 : return;
574 : }
575 :
576 :
577 : /*-------------------------------------------------------------------------
578 : * rotateFrame_sd_cldfb()
579 : *
580 : * Apply rotation to signals in Spatial Domain and in CLDFB
581 : *------------------------------------------------------------------------*/
582 :
583 195216 : void rotateFrame_sd_cldfb(
584 : float Rmat[3][3], /* i : real-space rotation matrix */
585 : float Cldfb_RealBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain real part */
586 : float Cldfb_ImagBuffer[][MAX_PARAM_SPATIAL_SUBFRAMES][CLDFB_NO_CHANNELS_MAX], /* i/o: unrotated HOA3 signal buffer in cldfb domain imag part */
587 : const IVAS_OUTPUT_SETUP_HANDLE hOutputSetup, /* i : output format setup number of channels */
588 : const EFAP_HANDLE hEFAPdata, /* i : EFAP structure */
589 : const int16_t numTimeSlots, /* i : number of time slots to process */
590 : const int16_t nb_band /* i : number of CLDFB bands to process */
591 : )
592 : {
593 : int16_t iBlock, iBand, m, n;
594 : float gains[MAX_LS_CHANNELS - 1][MAX_LS_CHANNELS - 1];
595 : int16_t azimuth, elevation;
596 : float g1;
597 : float realRot[MAX_LS_CHANNELS - 1][MAX_PARAM_SPATIAL_SUBFRAMES * CLDFB_NO_CHANNELS_MAX];
598 : float imagRot[MAX_LS_CHANNELS - 1][MAX_PARAM_SPATIAL_SUBFRAMES * CLDFB_NO_CHANNELS_MAX];
599 : float *p_realRot, *p_imagRot;
600 : float *p_real, *p_imag;
601 : int16_t nInChannels;
602 : int16_t isPlanar;
603 195216 : push_wmops( "rotateFrame_sd_cldfb" );
604 :
605 195216 : nInChannels = hOutputSetup->nchan_out_woLFE;
606 195216 : isPlanar = 1;
607 1171296 : for ( n = 0; n < nInChannels; n++ )
608 : {
609 976080 : if ( hOutputSetup->ls_elevation[n] != 0 )
610 : {
611 0 : isPlanar = 0;
612 0 : break;
613 : }
614 : }
615 :
616 : /* rotation of Euler angles */
617 1171296 : for ( n = 0; n < nInChannels; n++ )
618 : {
619 976080 : rotateAziEle( hOutputSetup->ls_azimuth[n], hOutputSetup->ls_elevation[n], &azimuth, &elevation, Rmat, isPlanar );
620 976080 : if ( hEFAPdata != NULL && ( hOutputSetup->ls_azimuth[n] != azimuth || hOutputSetup->ls_elevation[n] != elevation ) )
621 : {
622 837300 : efap_determine_gains( hEFAPdata, gains[n], azimuth, elevation, EFAP_MODE_EFAP );
623 : }
624 : else
625 : {
626 138780 : set_zero( gains[n], nInChannels );
627 138780 : gains[n][n] = 1.0f;
628 : }
629 : }
630 :
631 : /* Apply panning gains by mtx multiplication*/
632 1171296 : for ( n = 0; n < nInChannels; n++ )
633 : {
634 976080 : set_zero( realRot[n], MAX_PARAM_SPATIAL_SUBFRAMES * nb_band );
635 976080 : set_zero( imagRot[n], MAX_PARAM_SPATIAL_SUBFRAMES * nb_band );
636 5856480 : for ( m = 0; m < nInChannels; m++ )
637 : {
638 4880400 : g1 = gains[m][n];
639 4880400 : p_realRot = realRot[n];
640 4880400 : p_imagRot = imagRot[n];
641 4880400 : if ( g1 > 0.f )
642 : {
643 9031200 : for ( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
644 : {
645 7224960 : p_real = Cldfb_RealBuffer[m][iBlock];
646 7224960 : p_imag = Cldfb_ImagBuffer[m][iBlock];
647 368472960 : for ( iBand = 0; iBand < nb_band; iBand++ )
648 : {
649 361248000 : *( p_realRot ) = *p_realRot + g1 * *( p_real++ );
650 361248000 : *( p_imagRot ) = *p_imagRot + g1 * *( p_imag++ );
651 361248000 : p_realRot++;
652 361248000 : p_imagRot++;
653 : }
654 : }
655 : }
656 : }
657 : }
658 :
659 1171296 : for ( n = 0; n < nInChannels; n++ )
660 : {
661 976080 : p_realRot = realRot[n];
662 976080 : p_imagRot = imagRot[n];
663 4880400 : for ( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
664 : {
665 3904320 : p_real = Cldfb_RealBuffer[n][iBlock];
666 3904320 : p_imag = Cldfb_ImagBuffer[n][iBlock];
667 199120320 : for ( iBand = 0; iBand < nb_band; iBand++ )
668 : {
669 195216000 : *( p_real++ ) = *( p_realRot++ );
670 195216000 : *( p_imag++ ) = *( p_imagRot++ );
671 : }
672 42947520 : for ( ; iBand < CLDFB_NO_CHANNELS_MAX; iBand++ )
673 : {
674 39043200 : *( p_real++ ) = 0.f;
675 39043200 : *( p_imag++ ) = 0.f;
676 : }
677 : }
678 : }
679 195216 : pop_wmops();
680 :
681 195216 : return;
682 : }
683 :
684 :
685 : /*-----------------------------------------------------------------------*
686 : * ivas_external_orientation_open()
687 : *
688 : * Allocate and initialize external orientation handle
689 : *-----------------------------------------------------------------------*/
690 :
691 93 : ivas_error ivas_external_orientation_open(
692 : EXTERNAL_ORIENTATION_HANDLE *hExtOrientationData, /* o : external orientation handle */
693 : const int16_t num_subframes /* i : number of subframes */
694 : )
695 : {
696 :
697 : int16_t i;
698 : IVAS_QUATERNION identity;
699 :
700 93 : identity.w = 1.0f;
701 93 : identity.x = identity.y = identity.z = 0.0f;
702 :
703 : /* Allocate handle */
704 93 : if ( ( *hExtOrientationData = (EXTERNAL_ORIENTATION_HANDLE) malloc( sizeof( EXTERNAL_ORIENTATION_DATA ) ) ) == NULL )
705 : {
706 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for external orientation memory\n" ) );
707 : }
708 93 : ( *hExtOrientationData )->num_subframes = num_subframes;
709 :
710 : /* Enable head rotation and disable external orientation as default */
711 465 : for ( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
712 : {
713 372 : ( *hExtOrientationData )->enableHeadRotation[i] = 1;
714 372 : ( *hExtOrientationData )->enableExternalOrientation[i] = 0;
715 372 : ( *hExtOrientationData )->enableRotationInterpolation[i] = 0;
716 372 : ( *hExtOrientationData )->numFramesToTargetOrientation[i] = 0;
717 372 : ( *hExtOrientationData )->Quaternions[i] = identity;
718 : }
719 :
720 93 : return IVAS_ERR_OK;
721 : }
722 :
723 :
724 : /*-----------------------------------------------------------------------*
725 : * ivas_external_orientation_close()
726 : *
727 : * Deallocate external orientation handle
728 : *-----------------------------------------------------------------------*/
729 :
730 6970 : void ivas_external_orientation_close(
731 : EXTERNAL_ORIENTATION_HANDLE *hExtOrientationData /* i/o: external orientation handle */
732 : )
733 : {
734 6970 : if ( hExtOrientationData == NULL || *hExtOrientationData == NULL )
735 : {
736 6877 : return;
737 : }
738 :
739 93 : free( ( *hExtOrientationData ) );
740 93 : *hExtOrientationData = NULL;
741 :
742 93 : return;
743 : }
744 :
745 :
746 : /*-----------------------------------------------------------------------*
747 : * ivas_combined_orientation_open()
748 : *
749 : * Allocate and initialize combined orientation handle
750 : *-----------------------------------------------------------------------*/
751 :
752 1077 : ivas_error ivas_combined_orientation_open(
753 : COMBINED_ORIENTATION_HANDLE *hCombinedOrientationData, /* o : combined orientation handle */
754 : const int32_t fs, /* i : sampling rate */
755 : const int16_t num_subframes /* i : number of subframes */
756 : )
757 : {
758 : int16_t i;
759 : int16_t j;
760 : IVAS_QUATERNION identity;
761 : IVAS_VECTOR3 origo;
762 : int16_t pos_idx;
763 :
764 1077 : identity.w = 1.0f;
765 1077 : identity.x = identity.y = identity.z = 0.0f;
766 1077 : origo.x = origo.y = origo.z = 0.0f;
767 :
768 : /* Allocate handle */
769 1077 : if ( ( *hCombinedOrientationData = (COMBINED_ORIENTATION_HANDLE) malloc( sizeof( COMBINED_ORIENTATION_DATA ) ) ) == NULL )
770 : {
771 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for combined orientation memory\n" ) );
772 : }
773 :
774 : /* Initialization */
775 1077 : ( *hCombinedOrientationData )->num_subframes = num_subframes;
776 1077 : ( *hCombinedOrientationData )->interpolationCoefficient = 1.0f;
777 1077 : ( *hCombinedOrientationData )->interpolationIncrement = 1.0f;
778 1077 : ( *hCombinedOrientationData )->maximumFramesToTargetOrientation = 500;
779 1077 : ( *hCombinedOrientationData )->lrSwitchedNext = 0;
780 1077 : ( *hCombinedOrientationData )->lrSwitchedCurrent = 0;
781 1077 : ( *hCombinedOrientationData )->lrSwitchInterpVal = 0.0f;
782 1077 : ( *hCombinedOrientationData )->isInterpolationOngoing = FALSE;
783 1077 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_start = identity;
784 1077 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_target = identity;
785 :
786 : /* Initialise orientations to identity */
787 5385 : for ( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
788 : {
789 4308 : ( *hCombinedOrientationData )->enableCombinedOrientation[i] = 0;
790 4308 : ( *hCombinedOrientationData )->Quaternions[i] = identity;
791 4308 : ( *hCombinedOrientationData )->listenerPos[i] = origo;
792 :
793 17232 : for ( j = 0; j < 3; j++ )
794 : {
795 12924 : set_zero( ( *hCombinedOrientationData )->Rmat[i][j], 3 );
796 12924 : ( *hCombinedOrientationData )->Rmat[i][j][j] = 1.0f;
797 : }
798 : }
799 :
800 9693 : for ( pos_idx = 0; pos_idx < MAX_HEAD_ROT_POSES; pos_idx++ )
801 : {
802 34464 : for ( j = 0; j < 3; j++ )
803 : {
804 25848 : set_zero( ( *hCombinedOrientationData )->Rmat_prev[pos_idx][j], 3 );
805 25848 : ( *hCombinedOrientationData )->Rmat_prev[pos_idx][j][j] = 1.0f;
806 : }
807 : }
808 1077 : ( *hCombinedOrientationData )->sr_pose_pred_axis = DEFAULT_AXIS;
809 1077 : ( *hCombinedOrientationData )->sr_low_res_flag = 0;
810 :
811 1077 : ( *hCombinedOrientationData )->Quaternion_prev_extOrientation = identity;
812 1077 : ( *hCombinedOrientationData )->Quaternion_frozen_ext = identity;
813 1077 : ( *hCombinedOrientationData )->Quaternion_frozen_head = identity;
814 :
815 :
816 1077 : set_zero( ( *hCombinedOrientationData )->chEneIIR[0], MASA_FREQUENCY_BANDS );
817 1077 : set_zero( ( *hCombinedOrientationData )->chEneIIR[1], MASA_FREQUENCY_BANDS );
818 1077 : set_zero( ( *hCombinedOrientationData )->procChEneIIR[0], MASA_FREQUENCY_BANDS );
819 1077 : set_zero( ( *hCombinedOrientationData )->procChEneIIR[1], MASA_FREQUENCY_BANDS );
820 :
821 1077 : ( *hCombinedOrientationData )->isExtOrientationFrozen = 0;
822 1077 : ( *hCombinedOrientationData )->isHeadRotationFrozen = 0;
823 :
824 1077 : ( *hCombinedOrientationData )->subframe_idx = 0;
825 1077 : ( *hCombinedOrientationData )->subframe_size = (int16_t) ( fs / ( FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES ) );
826 1077 : ( *hCombinedOrientationData )->cur_subframe_samples_rendered = 0;
827 :
828 6462 : for ( i = 0; i < ( 1 + IVAS_MAX_NUM_OBJECTS ); i++ )
829 : {
830 5385 : ( *hCombinedOrientationData )->isDiegeticInputPI[i] = true;
831 : }
832 1077 : ( *hCombinedOrientationData )->isDiegeticInputPISet = false;
833 :
834 1077 : return IVAS_ERR_OK;
835 : }
836 :
837 :
838 : /*-----------------------------------------------------------------------*
839 : * ivas_combined_orientation_close()
840 : *
841 : * Deallocate combined orientation handle
842 : *-----------------------------------------------------------------------*/
843 :
844 6970 : void ivas_combined_orientation_close(
845 : COMBINED_ORIENTATION_HANDLE *hCombinedOrientationData /* i/o: combined orientation handle */
846 : )
847 : {
848 6970 : if ( hCombinedOrientationData == NULL || *hCombinedOrientationData == NULL )
849 : {
850 5893 : return;
851 : }
852 :
853 1077 : free( ( *hCombinedOrientationData ) );
854 1077 : *hCombinedOrientationData = NULL;
855 :
856 1077 : return;
857 : }
858 :
859 :
860 : /*-------------------------------------------------------------------------
861 : * combine_external_and_head_orientations_dec()
862 : *
863 : *
864 : *------------------------------------------------------------------------*/
865 :
866 4622919 : ivas_error combine_external_and_head_orientations_dec(
867 : HEAD_TRACK_DATA_HANDLE hHeadTrackData, /* i : head track handle */
868 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
869 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
870 : )
871 : {
872 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis;
873 4622919 : IVAS_QUATERNION *pHeadRotQuaternion = NULL;
874 4622919 : IVAS_VECTOR3 *listenerPos = NULL;
875 :
876 4622919 : if ( hHeadTrackData != NULL )
877 : {
878 4622919 : pHeadRotQuaternion = hHeadTrackData->Quaternions;
879 4622919 : listenerPos = hHeadTrackData->Pos;
880 4622919 : sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
881 : }
882 : else
883 : {
884 0 : sr_pose_pred_axis = DEFAULT_AXIS;
885 : }
886 :
887 4622919 : return combine_external_and_head_orientations( pHeadRotQuaternion, listenerPos, sr_pose_pred_axis, hExtOrientationData, hCombinedOrientationData );
888 : }
889 :
890 :
891 : /*-------------------------------------------------------------------------
892 : * combine_external_and_head_orientations_rend()
893 : *
894 : *
895 : *------------------------------------------------------------------------*/
896 :
897 90457902 : ivas_error combine_external_and_head_orientations_rend(
898 : IVAS_REND_HeadRotData *hHeadTrackData, /* i : head track handle */
899 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
900 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
901 : )
902 : {
903 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis;
904 90457902 : IVAS_QUATERNION *headRotQuaternions = NULL;
905 90457902 : IVAS_VECTOR3 *listenerPos = NULL;
906 : int16_t i;
907 :
908 90457902 : sr_pose_pred_axis = DEFAULT_AXIS;
909 90457902 : if ( hHeadTrackData->headRotEnabled )
910 : {
911 17763141 : headRotQuaternions = hHeadTrackData->headPositions;
912 17763141 : listenerPos = hHeadTrackData->Pos;
913 17763141 : sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
914 : }
915 72694761 : else if ( hExtOrientationData != NULL )
916 : {
917 : /* Head rotation data not available, use the freezed value or disable */
918 0 : for ( i = 0; i < hExtOrientationData->num_subframes; i++ )
919 : {
920 0 : if ( hExtOrientationData->enableHeadRotation[i] != 2 )
921 : {
922 0 : hExtOrientationData->enableHeadRotation[i] = 0;
923 : }
924 : }
925 : }
926 :
927 90457902 : return combine_external_and_head_orientations( headRotQuaternions, listenerPos, sr_pose_pred_axis, hExtOrientationData, hCombinedOrientationData );
928 : }
929 :
930 :
931 : /*-------------------------------------------------------------------------
932 : * combine_external_and_head_orientations()
933 : *
934 : * Combine the external orientations and the head orientation.
935 : * NOTE that the external orientations are inversed.
936 : *------------------------------------------------------------------------*/
937 :
938 95080821 : ivas_error combine_external_and_head_orientations(
939 : IVAS_QUATERNION *headRotQuaternions, /* i : quaternions for head rotation */
940 : IVAS_VECTOR3 *listenerPos, /* i : listener position */
941 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis, /* i : split rend pose prediction axis */
942 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
943 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
944 : )
945 : {
946 : int16_t i;
947 : int16_t j;
948 : IVAS_QUATERNION identity;
949 : IVAS_VECTOR3 origo;
950 :
951 95080821 : identity.w = 1.0f;
952 95080821 : identity.x = identity.y = identity.z = 0.0f;
953 95080821 : origo.x = origo.y = origo.z = 0.0f;
954 :
955 : /* Form combined orientations or return if no data available */
956 95080821 : if ( hCombinedOrientationData == NULL )
957 : {
958 72694761 : if ( headRotQuaternions != NULL || hExtOrientationData != NULL )
959 : {
960 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
961 : }
962 : else
963 : {
964 72694761 : return IVAS_ERR_OK;
965 : }
966 : }
967 22386060 : else if ( headRotQuaternions == NULL && hExtOrientationData == NULL )
968 : {
969 : /* Reset the combined orientations and rotations */
970 0 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
971 0 : hCombinedOrientationData->interpolationCoefficient = 1.0f;
972 0 : hCombinedOrientationData->interpolationIncrement = 1.0f;
973 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
974 0 : hCombinedOrientationData->Quaternions_ext_interpolation_target = identity;
975 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
976 : {
977 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
978 0 : hCombinedOrientationData->Quaternions[i] = identity;
979 0 : hCombinedOrientationData->listenerPos[i] = origo;
980 :
981 0 : for ( j = 0; j < 3; j++ )
982 : {
983 0 : set_zero( hCombinedOrientationData->Rmat[i][j], 3 );
984 0 : hCombinedOrientationData->Rmat[i][j][j] = 1.0f;
985 : }
986 : }
987 : }
988 22386060 : else if ( hExtOrientationData == NULL && headRotQuaternions != NULL )
989 : {
990 : /* Disable head rotation if diegetic PI data indicating non-diegetic audio is received */
991 20650970 : if ( hCombinedOrientationData->isDiegeticInputPISet && !hCombinedOrientationData->isDiegeticInputPI[0] && !hCombinedOrientationData->isDiegeticInputPI[1] && !hCombinedOrientationData->isDiegeticInputPI[2] && !hCombinedOrientationData->isDiegeticInputPI[3] && !hCombinedOrientationData->isDiegeticInputPI[4] )
992 : {
993 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
994 : {
995 0 : hCombinedOrientationData->Quaternions[i] = identity;
996 : }
997 : }
998 : else
999 : {
1000 : /* Head rotation only */
1001 50884202 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1002 : {
1003 30233232 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1004 : }
1005 : }
1006 : }
1007 :
1008 22386060 : if ( hExtOrientationData != NULL )
1009 : {
1010 : /* External orientations */
1011 4709530 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1012 : {
1013 : /* Check for frozen external orientation */
1014 2974440 : if ( hExtOrientationData->enableExternalOrientation[i] == 2 )
1015 : {
1016 535890 : if ( hCombinedOrientationData->isExtOrientationFrozen != 1 )
1017 : {
1018 960 : hCombinedOrientationData->Quaternion_frozen_ext = hExtOrientationData->Quaternions[i];
1019 960 : hCombinedOrientationData->isExtOrientationFrozen = 1;
1020 : }
1021 : }
1022 : else
1023 : {
1024 2438550 : hCombinedOrientationData->Quaternion_frozen_ext = identity;
1025 2438550 : hCombinedOrientationData->isExtOrientationFrozen = 0;
1026 : }
1027 :
1028 2974440 : if ( hExtOrientationData->enableRotationInterpolation[i] == 1 && hExtOrientationData->enableExternalOrientation[i] > 0 )
1029 : {
1030 1308636 : if ( hCombinedOrientationData->isInterpolationOngoing == true && hCombinedOrientationData->interpolationCoefficient <= 1.0f && are_orientations_same( &hCombinedOrientationData->Quaternions_ext_interpolation_target, &hExtOrientationData->Quaternions[i] ) == true )
1031 : {
1032 : /* Continue interpolation */
1033 1163064 : QuaternionSlerp( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i] );
1034 1163064 : hCombinedOrientationData->interpolationCoefficient += hCombinedOrientationData->interpolationIncrement;
1035 : }
1036 : else
1037 : {
1038 : /* Stop interpolation or check for new interpolation */
1039 145572 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1040 145572 : hCombinedOrientationData->interpolationCoefficient = 1.0f;
1041 145572 : hCombinedOrientationData->interpolationIncrement = 1.0f;
1042 145572 : external_target_interpolation( hExtOrientationData, hCombinedOrientationData, i );
1043 : }
1044 : }
1045 : else
1046 : {
1047 : /* Interpolation disabled, use the current orientation values */
1048 :
1049 : /* Use the most recent external orientation */
1050 1665804 : if ( hExtOrientationData->enableExternalOrientation[i] == 1 )
1051 : {
1052 1054176 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
1053 : }
1054 : /* Use the freezed external orientation */
1055 611628 : else if ( hExtOrientationData->enableExternalOrientation[i] == 2 )
1056 : {
1057 394698 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_ext;
1058 : }
1059 : }
1060 : }
1061 : }
1062 :
1063 22386060 : if ( hExtOrientationData != NULL && headRotQuaternions != NULL )
1064 : {
1065 : /* Combine head and external orientations */
1066 4709530 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1067 : {
1068 : /* Check for frozen head rotation */
1069 2974440 : if ( hExtOrientationData->enableHeadRotation[i] == 2 )
1070 : {
1071 385314 : if ( hCombinedOrientationData->isHeadRotationFrozen != 1 )
1072 : {
1073 1950 : hCombinedOrientationData->Quaternion_frozen_head = headRotQuaternions[i];
1074 1950 : hCombinedOrientationData->isHeadRotationFrozen = 1;
1075 : }
1076 : }
1077 : else
1078 : {
1079 2589126 : hCombinedOrientationData->Quaternion_frozen_head = identity;
1080 2589126 : hCombinedOrientationData->isHeadRotationFrozen = 0;
1081 : }
1082 :
1083 : /* Disable head rotation if diegetic PI data indicating non-diegetic audio is received */
1084 2974440 : if ( hCombinedOrientationData->isDiegeticInputPISet && !hCombinedOrientationData->isDiegeticInputPI[0] && !hCombinedOrientationData->isDiegeticInputPI[1] && !hCombinedOrientationData->isDiegeticInputPI[2] && !hCombinedOrientationData->isDiegeticInputPI[3] && !hCombinedOrientationData->isDiegeticInputPI[4] )
1085 : {
1086 0 : continue;
1087 : }
1088 : else
1089 : {
1090 : /* Use the most recent head rotation */
1091 2974440 : if ( hExtOrientationData->enableHeadRotation[i] == 1 )
1092 : {
1093 2015271 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1094 : {
1095 1798341 : QuaternionProduct( hCombinedOrientationData->Quaternions[i], headRotQuaternions[i], &hCombinedOrientationData->Quaternions[i] );
1096 : }
1097 : else
1098 : {
1099 216930 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1100 : }
1101 : }
1102 : /* Use the freezed head rotation */
1103 959169 : else if ( hExtOrientationData->enableHeadRotation[i] == 2 )
1104 : {
1105 385314 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1106 : {
1107 385314 : QuaternionProduct( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Quaternion_frozen_head, &hCombinedOrientationData->Quaternions[i] );
1108 : }
1109 : else
1110 : {
1111 0 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_head;
1112 : }
1113 : }
1114 : }
1115 :
1116 : /* Reset the combined orientations to identity */
1117 2974440 : if ( hExtOrientationData->enableHeadRotation[i] == 0 && hExtOrientationData->enableExternalOrientation[i] == 0 )
1118 : {
1119 0 : hCombinedOrientationData->Quaternions[i] = identity;
1120 : }
1121 : }
1122 : }
1123 :
1124 22386060 : if ( headRotQuaternions != NULL || hExtOrientationData != NULL )
1125 : {
1126 : /* Calculate the combined rotation matrix */
1127 55593732 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1128 : {
1129 33207672 : QuatToRotMat( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Rmat[i] );
1130 : }
1131 : }
1132 :
1133 : /* Save the current orientations */
1134 22386060 : if ( hExtOrientationData != NULL )
1135 : {
1136 1735090 : if ( hExtOrientationData->enableExternalOrientation[hExtOrientationData->num_subframes - 1] > 0 )
1137 : {
1138 1608712 : hCombinedOrientationData->Quaternion_prev_extOrientation = hExtOrientationData->Quaternions[hExtOrientationData->num_subframes - 1];
1139 : }
1140 : else
1141 : {
1142 126378 : hCombinedOrientationData->Quaternion_prev_extOrientation = identity;
1143 : }
1144 : }
1145 22386060 : if ( headRotQuaternions != NULL )
1146 : {
1147 55593732 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1148 : {
1149 33207672 : hCombinedOrientationData->listenerPos[i] = listenerPos[i];
1150 : }
1151 : }
1152 :
1153 : /* Check if combined orientation is enabled */
1154 22386060 : if ( headRotQuaternions != NULL && hExtOrientationData == NULL )
1155 : {
1156 50884202 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1157 : {
1158 30233232 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1159 : }
1160 : }
1161 1735090 : else if ( headRotQuaternions == NULL && hExtOrientationData != NULL )
1162 : {
1163 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1164 : {
1165 0 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1166 : {
1167 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1168 : }
1169 : else
1170 : {
1171 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1172 : }
1173 : }
1174 : }
1175 1735090 : else if ( headRotQuaternions != NULL && hExtOrientationData != NULL )
1176 : {
1177 4709530 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1178 : {
1179 2974440 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 || ( hExtOrientationData->enableHeadRotation[i] > 0 ) )
1180 : {
1181 2974440 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1182 : }
1183 : else
1184 : {
1185 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1186 : }
1187 : }
1188 : }
1189 : else
1190 : {
1191 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1192 : {
1193 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1194 : }
1195 : }
1196 :
1197 : #ifdef DEBUG_MODE_ORIENTATION
1198 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1199 : {
1200 : dbgwrite( &hCombinedOrientationData->enableCombinedOrientation[i], sizeof( int16_t ), 1, 1, "res/dec_orientation_enabled.dat" );
1201 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].w ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_w.dat" );
1202 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].x ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_x.dat" );
1203 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].y ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_y.dat" );
1204 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].z ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_z.dat" );
1205 : }
1206 : #endif
1207 22386060 : hCombinedOrientationData->sr_pose_pred_axis = sr_pose_pred_axis;
1208 22386060 : hCombinedOrientationData->subframe_idx = 0;
1209 22386060 : hCombinedOrientationData->cur_subframe_samples_rendered = 0;
1210 22386060 : hCombinedOrientationData->subframe_idx_start = 0;
1211 22386060 : hCombinedOrientationData->cur_subframe_samples_rendered_start = 0;
1212 :
1213 : /* Reset external orientations */
1214 22386060 : if ( hExtOrientationData != NULL )
1215 : {
1216 4709530 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1217 : {
1218 2974440 : hExtOrientationData->Quaternions[i] = identity;
1219 : }
1220 : }
1221 :
1222 22386060 : return IVAS_ERR_OK;
1223 : }
1224 :
1225 :
1226 : /*-------------------------------------------------------------------------
1227 : * external_target_interpolation()
1228 : *
1229 : *
1230 : *------------------------------------------------------------------------*/
1231 :
1232 145572 : static void external_target_interpolation(
1233 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1234 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1235 : const int16_t i /* i : subframe index */
1236 : )
1237 : {
1238 : /* Sanity check for number of frames */
1239 145572 : hExtOrientationData->numFramesToTargetOrientation[i] = min( hExtOrientationData->numFramesToTargetOrientation[i], hCombinedOrientationData->maximumFramesToTargetOrientation );
1240 145572 : hExtOrientationData->numFramesToTargetOrientation[i] = max( hExtOrientationData->numFramesToTargetOrientation[i], 0 );
1241 :
1242 : /* Interpolate from the current orientation to the target orientation */
1243 145572 : if ( hExtOrientationData->numFramesToTargetOrientation[i] > 0 )
1244 : {
1245 15270 : if ( are_orientations_same( &hCombinedOrientationData->Quaternions_ext_interpolation_target, &hExtOrientationData->Quaternions[i] ) == false )
1246 : {
1247 : /* Target orientation is different from the previous target, update the values */
1248 :
1249 : /* Set the received orientation as the target */
1250 5970 : hCombinedOrientationData->Quaternions_ext_interpolation_target = hExtOrientationData->Quaternions[i];
1251 :
1252 : /* Use the most recent external orientation as the starting orientation */
1253 5970 : if ( hExtOrientationData->enableExternalOrientation[i] == 1 )
1254 : {
1255 5016 : if ( i > 0 )
1256 : {
1257 1696 : if ( hExtOrientationData->enableExternalOrientation[i - 1] == 0 )
1258 : {
1259 : IVAS_QUATERNION identity;
1260 670 : identity.w = 1.0f;
1261 670 : identity.x = identity.y = identity.z = 0.0f;
1262 670 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
1263 : }
1264 1026 : else if ( hExtOrientationData->enableExternalOrientation[i - 1] == 2 )
1265 : {
1266 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1267 : }
1268 : else
1269 : {
1270 1026 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hExtOrientationData->Quaternions[i - 1];
1271 : }
1272 : }
1273 : else
1274 : {
1275 3320 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_prev_extOrientation;
1276 : }
1277 : }
1278 954 : else if ( hExtOrientationData->enableExternalOrientation[i] == 2 )
1279 : {
1280 954 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1281 : }
1282 :
1283 : /* Calculate the interpolation increment and coefficient */
1284 5970 : hCombinedOrientationData->interpolationIncrement = 1.0f / ( (float) hExtOrientationData->numFramesToTargetOrientation[i] * (float) MAX_PARAM_SPATIAL_SUBFRAMES );
1285 5970 : hCombinedOrientationData->interpolationCoefficient = hCombinedOrientationData->interpolationIncrement;
1286 : }
1287 :
1288 : /* Interpolate */
1289 15270 : hCombinedOrientationData->isInterpolationOngoing = TRUE;
1290 15270 : QuaternionSlerp( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i] );
1291 15270 : hCombinedOrientationData->interpolationCoefficient += hCombinedOrientationData->interpolationIncrement;
1292 : }
1293 : else
1294 : {
1295 : /* Use the target orientation immediately */
1296 130302 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1297 130302 : hCombinedOrientationData->interpolationCoefficient = 1.0f;
1298 130302 : hCombinedOrientationData->interpolationIncrement = 1.0f;
1299 130302 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
1300 : }
1301 :
1302 145572 : return;
1303 : }
1304 :
1305 :
1306 : /*-------------------------------------------------------------------------
1307 : * are_orientations_same()
1308 : *
1309 : *
1310 : *------------------------------------------------------------------------*/
1311 :
1312 1184211 : static bool are_orientations_same(
1313 : const IVAS_QUATERNION *orientation1,
1314 : const IVAS_QUATERNION *orientation2 )
1315 : {
1316 1184211 : bool orientationsAreSame = true;
1317 1184211 : float error_margin = 0.05f;
1318 :
1319 1184211 : if ( fabsf( orientation1->w - orientation2->w ) > error_margin ||
1320 1175349 : fabsf( orientation1->x - orientation2->x ) > error_margin ||
1321 1175349 : fabsf( orientation1->y - orientation2->y ) > error_margin ||
1322 1175349 : fabsf( orientation1->z - orientation2->z ) > error_margin )
1323 : {
1324 11847 : orientationsAreSame = false;
1325 : }
1326 :
1327 1184211 : return orientationsAreSame;
1328 : }
1329 :
1330 :
1331 : /*-----------------------------------------------------------------------*
1332 : * Local Function definitions
1333 : *-----------------------------------------------------------------------*/
1334 :
1335 : /*-------------------------------------------------------------------------
1336 : * Helper functions used by SHrotmatgen,
1337 : * an implementation of the algorithm in
1338 : * Ivanic, J. & Ruedenberg, K., J. Phys. Chem. 100, 6342 (1996)
1339 : *------------------------------------------------------------------------*/
1340 :
1341 1392763509 : static float SHrot_p(
1342 : const int16_t i,
1343 : const int16_t l,
1344 : const int16_t a,
1345 : const int16_t b,
1346 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM],
1347 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] )
1348 : {
1349 1392763509 : float ri1 = 0.0f, rim1 = 0.0f, ri0 = 0.0f, p = 0.0f, R_lm1_1 = 0.0f, R_lm1_2 = 0.0f;
1350 :
1351 1392763509 : ri1 = SHrotmat[i + 1 + 1][1 + 1 + 1];
1352 1392763509 : rim1 = SHrotmat[i + 1 + 1][-1 + 1 + 1];
1353 1392763509 : ri0 = SHrotmat[i + 1 + 1][0 + 1 + 1];
1354 :
1355 1392763509 : if ( b == -l )
1356 : {
1357 232387065 : R_lm1_1 = R_lm1[a + l - 1][0];
1358 232387065 : R_lm1_2 = R_lm1[a + l - 1][2 * l - 2];
1359 232387065 : p = ri1 * R_lm1_1 + rim1 * R_lm1_2;
1360 : }
1361 : else
1362 : {
1363 1160376444 : if ( b == l )
1364 : {
1365 232387065 : R_lm1_1 = R_lm1[a + l - 1][2 * l - 2];
1366 232387065 : R_lm1_2 = R_lm1[a + l - 1][0];
1367 232387065 : p = ri1 * R_lm1_1 - rim1 * R_lm1_2;
1368 : }
1369 : else
1370 : {
1371 927989379 : R_lm1_1 = R_lm1[a + l - 1][b + l - 1];
1372 927989379 : p = ri0 * R_lm1_1;
1373 : }
1374 : }
1375 :
1376 1392763509 : return p;
1377 : }
1378 :
1379 310598955 : static float SHrot_u(
1380 : const int16_t l,
1381 : const int16_t m,
1382 : const int16_t n,
1383 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM],
1384 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] )
1385 : {
1386 310598955 : return SHrot_p( 0, l, m, n, SHrotmat, R_lm1 );
1387 : }
1388 :
1389 470830221 : static float SHrot_v(
1390 : const int16_t l,
1391 : const int16_t m,
1392 : const int16_t n,
1393 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM],
1394 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] )
1395 : {
1396 :
1397 : float result, d, p0, p1;
1398 :
1399 470830221 : if ( m == 0 )
1400 : {
1401 80115633 : p0 = SHrot_p( 1, l, 1, n, SHrotmat, R_lm1 );
1402 80115633 : p1 = SHrot_p( -1, l, -1, n, SHrotmat, R_lm1 );
1403 80115633 : result = p0 + p1;
1404 : }
1405 : else
1406 : {
1407 390714588 : if ( m > 0 )
1408 : {
1409 195357294 : d = ( m == 1 ) ? 1.0f : 0.0f;
1410 195357294 : p0 = SHrot_p( 1, l, m - 1, n, SHrotmat, R_lm1 );
1411 195357294 : p1 = SHrot_p( -1, l, -m + 1, n, SHrotmat, R_lm1 );
1412 195357294 : result = p0 * sqrtf( 1.0f + d ) - p1 * ( 1.0f - d );
1413 : }
1414 : else
1415 : {
1416 195357294 : d = ( m == -1 ) ? 1.0f : 0.0f;
1417 195357294 : p0 = SHrot_p( 1, l, m + 1, n, SHrotmat, R_lm1 );
1418 195357294 : p1 = SHrot_p( -1, l, -m - 1, n, SHrotmat, R_lm1 );
1419 195357294 : result = p0 * ( 1.0f - d ) + p1 * sqrtf( 1.0f + d );
1420 : }
1421 : }
1422 :
1423 470830221 : return result;
1424 : }
1425 :
1426 70252056 : static float SHrot_w(
1427 : const int16_t l,
1428 : const int16_t m,
1429 : const int16_t n,
1430 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM],
1431 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] )
1432 : {
1433 : float result, p0, p1;
1434 :
1435 70252056 : if ( m == 0 )
1436 : {
1437 0 : assert( 0 && "ERROR should not be called\n" );
1438 : return 0.0f;
1439 : }
1440 : else
1441 : {
1442 70252056 : if ( m > 0 )
1443 : {
1444 35126028 : p0 = SHrot_p( 1, l, m + 1, n, SHrotmat, R_lm1 );
1445 35126028 : p1 = SHrot_p( -1, l, -m - 1, n, SHrotmat, R_lm1 );
1446 35126028 : result = p0 + p1;
1447 : }
1448 : else
1449 : {
1450 35126028 : p0 = SHrot_p( 1, l, m - 1, n, SHrotmat, R_lm1 );
1451 35126028 : p1 = SHrot_p( -1, l, -m + 1, n, SHrotmat, R_lm1 );
1452 35126028 : result = p0 - p1;
1453 : }
1454 : }
1455 :
1456 70252056 : return result;
1457 : }
1458 :
1459 :
1460 : /*-------------------------------------------------------------------------
1461 : * rotateFrame_sd_cldfb()
1462 : *
1463 : *
1464 : *------------------------------------------------------------------------*/
1465 :
1466 13054518 : void SHrotmatgen(
1467 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], /* o : rotation matrix in SHD */
1468 : float Rmat[3][3], /* i : real-space rotation matrix */
1469 : const int16_t order /* i : ambisonics order */
1470 : )
1471 : {
1472 13054518 : int16_t d = 0;
1473 13054518 : int16_t band_idx = 0;
1474 : int16_t i, j;
1475 : int16_t l, m, n;
1476 : int16_t absm;
1477 13054518 : float sqdenom = 0.0f, sql2mm2 = 0.0f, sqdabsm = 0.0f, sqlabsm = 0.0f;
1478 13054518 : float u = 0.0f, v = 0.0f, w = 0.0f;
1479 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
1480 : float R_l[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
1481 :
1482 13054518 : SHrotmat[0][0] = 1.0f;
1483 :
1484 13054518 : SHrotmat[1][1] = Rmat[1][1];
1485 13054518 : SHrotmat[1][2] = Rmat[1][2];
1486 13054518 : SHrotmat[1][3] = Rmat[1][0];
1487 :
1488 13054518 : SHrotmat[2][1] = Rmat[2][1];
1489 13054518 : SHrotmat[2][2] = Rmat[2][2];
1490 13054518 : SHrotmat[2][3] = Rmat[2][0];
1491 :
1492 13054518 : SHrotmat[3][1] = Rmat[0][1];
1493 13054518 : SHrotmat[3][2] = Rmat[0][2];
1494 13054518 : SHrotmat[3][3] = Rmat[0][0];
1495 :
1496 52218072 : for ( i = 0; i < 2 * 1 + 1; i++ )
1497 : {
1498 156654216 : for ( j = 0; j < 2 * 1 + 1; j++ )
1499 : {
1500 117490662 : R_lm1[i][j] = SHrotmat[i + 1][j + 1];
1501 : }
1502 : }
1503 :
1504 13054518 : band_idx = 4;
1505 27070443 : for ( l = 2; l <= order; l++ )
1506 : {
1507 14015925 : set_zero( &R_l[0][0], HEADROT_SHMAT_DIM2 );
1508 :
1509 94131558 : for ( m = -l; m <= l; m++ )
1510 : {
1511 80115633 : d = ( m == 0 ) ? 1 : 0;
1512 80115633 : absm = (int16_t) abs( m );
1513 80115633 : sql2mm2 = sqrtf( (float) ( l * l - m * m ) );
1514 80115633 : sqdabsm = sqrtf( (float) ( ( 1 + d ) * ( l + absm - 1 ) * ( l + absm ) ) );
1515 80115633 : sqlabsm = sqrtf( (float) ( ( l - absm - 1 ) * ( l - absm ) ) );
1516 :
1517 550945854 : for ( n = -l; n <= l; n++ )
1518 : {
1519 470830221 : if ( abs( n ) == l )
1520 : {
1521 160231266 : sqdenom = sqrtf( (float) ( ( 2 * l ) * ( 2 * l - 1 ) ) );
1522 : }
1523 : else
1524 : {
1525 310598955 : sqdenom = sqrtf( (float) ( l * l - n * n ) );
1526 : }
1527 :
1528 470830221 : u = sql2mm2 / sqdenom;
1529 470830221 : v = sqdabsm / sqdenom * ( 1 - 2 * d ) * 0.5f;
1530 470830221 : w = sqlabsm / sqdenom * ( 1 - d ) * ( -0.5f );
1531 :
1532 470830221 : if ( u != 0 )
1533 : {
1534 310598955 : u = u * SHrot_u( l, m, n, SHrotmat, R_lm1 );
1535 : }
1536 470830221 : if ( v != 0 )
1537 : {
1538 470830221 : v = v * SHrot_v( l, m, n, SHrotmat, R_lm1 );
1539 : }
1540 470830221 : if ( w != 0 )
1541 : {
1542 70252056 : w = w * SHrot_w( l, m, n, SHrotmat, R_lm1 );
1543 : }
1544 470830221 : R_l[m + l][n + l] = u + v + w;
1545 : }
1546 : }
1547 :
1548 94131558 : for ( i = 0; i < 2 * l + 1; i++ )
1549 : {
1550 550945854 : for ( j = 0; j < 2 * l + 1; j++ )
1551 : {
1552 470830221 : SHrotmat[band_idx + i][band_idx + j] = R_l[i][j];
1553 : }
1554 : }
1555 :
1556 94131558 : for ( i = 0; i < 2 * l + 1; i++ )
1557 : {
1558 550945854 : for ( j = 0; j < 2 * l + 1; j++ )
1559 : {
1560 470830221 : R_lm1[i][j] = R_l[i][j];
1561 : }
1562 : }
1563 :
1564 14015925 : band_idx += 2 * l + 1;
1565 : }
1566 :
1567 13054518 : return;
1568 : }
1569 :
1570 :
1571 : /*-------------------------------------------------------------------------
1572 : * ivas_combined_orientation_update_index()
1573 : *
1574 : * update read index based on the number of rendered samples
1575 : *------------------------------------------------------------------------*/
1576 :
1577 200522535 : void ivas_combined_orientation_update_index(
1578 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1579 : const int16_t samples_rendered /* i : samples rendered since the last call */
1580 : )
1581 : {
1582 200522535 : if ( hCombinedOrientationData != NULL )
1583 : {
1584 36638137 : if ( hCombinedOrientationData->num_subframes == 1 || hCombinedOrientationData->sr_low_res_flag )
1585 : {
1586 : /* only one orientation available anyway or split rendering with low resolution*/
1587 17784437 : hCombinedOrientationData->subframe_idx = 0;
1588 : }
1589 : else
1590 : {
1591 18853700 : hCombinedOrientationData->cur_subframe_samples_rendered += samples_rendered;
1592 18853700 : hCombinedOrientationData->subframe_idx += hCombinedOrientationData->cur_subframe_samples_rendered / hCombinedOrientationData->subframe_size;
1593 18853700 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size;
1594 18853700 : hCombinedOrientationData->subframe_idx = min( hCombinedOrientationData->subframe_idx, hCombinedOrientationData->num_subframes - 1 );
1595 : }
1596 : }
1597 :
1598 200522535 : return;
1599 : }
1600 :
1601 :
1602 : /*-------------------------------------------------------------------------
1603 : * ivas_combined_orientation_update_index()
1604 : *
1605 : * update read index based on the number of rendered samples
1606 : *------------------------------------------------------------------------*/
1607 :
1608 217573796 : void ivas_combined_orientation_set_to_start_index(
1609 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1610 : )
1611 : {
1612 217573796 : if ( hCombinedOrientationData != NULL )
1613 : {
1614 42060334 : hCombinedOrientationData->subframe_idx = hCombinedOrientationData->subframe_idx_start;
1615 42060334 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered_start;
1616 : }
1617 :
1618 217573796 : return;
1619 : }
1620 :
1621 :
1622 : /*-------------------------------------------------------------------------
1623 : * ivas_combined_orientation_update_start_index()
1624 : *
1625 : * update start index based on the number of rendered samples
1626 : *------------------------------------------------------------------------*/
1627 :
1628 124623864 : void ivas_combined_orientation_update_start_index(
1629 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1630 : const int16_t samples_rendered /* i : samples rendered since the last call */
1631 : )
1632 : {
1633 124623864 : if ( hCombinedOrientationData != NULL )
1634 : {
1635 22418656 : if ( hCombinedOrientationData->num_subframes == 1 || hCombinedOrientationData->sr_low_res_flag )
1636 : {
1637 : /* only one orientation available anyway or split rendering with low resolution*/
1638 17908616 : hCombinedOrientationData->subframe_idx = 0;
1639 : }
1640 : else
1641 : {
1642 4510040 : hCombinedOrientationData->cur_subframe_samples_rendered_start += samples_rendered;
1643 4510040 : hCombinedOrientationData->subframe_idx_start += hCombinedOrientationData->cur_subframe_samples_rendered / hCombinedOrientationData->subframe_size;
1644 4510040 : hCombinedOrientationData->cur_subframe_samples_rendered_start = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size;
1645 4510040 : hCombinedOrientationData->subframe_idx_start = min( hCombinedOrientationData->subframe_idx, hCombinedOrientationData->num_subframes - 1 );
1646 : }
1647 : }
1648 :
1649 124623864 : return;
1650 : }
|
