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 2345 : 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 2345 : 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 2345 : ( *hHeadTrackData )->lrSwitchInterpVal = 0.0f;
81 2345 : ( *hHeadTrackData )->lrSwitchedCurrent = 0;
82 2345 : ( *hHeadTrackData )->lrSwitchedNext = 0;
83 2345 : 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 2345 : 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 9380 : for ( i = 0; i < 3; i++ )
95 : {
96 7035 : set_zero( ( *hHeadTrackData )->Rmat_prev[i], 3 );
97 7035 : ( *hHeadTrackData )->Rmat_prev[i][i] = 1.0f;
98 : }
99 :
100 2345 : ( *hHeadTrackData )->sr_pose_pred_axis = DEFAULT_AXIS;
101 :
102 2345 : set_zero( ( *hHeadTrackData )->chEneIIR[0], MASA_FREQUENCY_BANDS );
103 2345 : set_zero( ( *hHeadTrackData )->chEneIIR[1], MASA_FREQUENCY_BANDS );
104 2345 : set_zero( ( *hHeadTrackData )->procChEneIIR[0], MASA_FREQUENCY_BANDS );
105 2345 : set_zero( ( *hHeadTrackData )->procChEneIIR[1], MASA_FREQUENCY_BANDS );
106 :
107 2345 : return IVAS_ERR_OK;
108 : }
109 :
110 :
111 : /*-----------------------------------------------------------------------*
112 : * ivas_headTrack_close()
113 : *
114 : * Deallocate Head-Tracking handle
115 : *-----------------------------------------------------------------------*/
116 :
117 86027 : void ivas_headTrack_close(
118 : HEAD_TRACK_DATA_HANDLE *hHeadTrackData /* i/o: head track handle */
119 : )
120 : {
121 86027 : if ( hHeadTrackData == NULL || *hHeadTrackData == NULL )
122 : {
123 83682 : return;
124 : }
125 :
126 2345 : if ( ( *hHeadTrackData )->OrientationTracker != NULL )
127 : {
128 2345 : free( ( *hHeadTrackData )->OrientationTracker );
129 2345 : ( *hHeadTrackData )->OrientationTracker = NULL;
130 : }
131 :
132 2345 : free( ( *hHeadTrackData ) );
133 2345 : *hHeadTrackData = NULL;
134 :
135 2345 : return;
136 : }
137 :
138 :
139 : /*----------------------------------------------------------------------------------
140 : * QuatToRotMat()
141 : *
142 : * Quaternion handling: calculate rotation matrices in real-space and SHD
143 : *---------------------------------------------------------------------------------*/
144 :
145 20330773 : 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 20330773 : if ( quat.w == -3.0 )
151 : {
152 : IVAS_QUATERNION quat_local;
153 6105022 : Euler2Quat( deg2rad( quat.x ), deg2rad( quat.y ), deg2rad( quat.z ), &quat_local );
154 6105022 : QuatToRotMat( quat_local, Rmat );
155 : }
156 : else
157 : {
158 14225751 : Rmat[0][0] = quat.w * quat.w + quat.x * quat.x - quat.y * quat.y - quat.z * quat.z;
159 14225751 : Rmat[0][1] = 2.0f * ( quat.x * quat.y - quat.w * quat.z );
160 14225751 : Rmat[0][2] = 2.0f * ( quat.x * quat.z + quat.w * quat.y );
161 :
162 14225751 : Rmat[1][0] = 2.0f * ( quat.x * quat.y + quat.w * quat.z );
163 14225751 : Rmat[1][1] = quat.w * quat.w - quat.x * quat.x + quat.y * quat.y - quat.z * quat.z;
164 14225751 : Rmat[1][2] = 2.0f * ( quat.y * quat.z - quat.w * quat.x );
165 :
166 14225751 : Rmat[2][0] = 2.0f * ( quat.x * quat.z - quat.w * quat.y );
167 14225751 : Rmat[2][1] = 2.0f * ( quat.y * quat.z + quat.w * quat.x );
168 14225751 : Rmat[2][2] = quat.w * quat.w - quat.x * quat.x - quat.y * quat.y + quat.z * quat.z;
169 : }
170 :
171 20330773 : return;
172 : }
173 :
174 :
175 : /*-------------------------------------------------------------------------
176 : * rad2deg()
177 : *
178 : * Converts normalized radians to degrees
179 : *------------------------------------------------------------------------*/
180 :
181 1368 : float rad2deg(
182 : float radians )
183 : {
184 1380 : while ( radians >= EVS_PI )
185 : {
186 12 : radians = radians - EVS_PI;
187 : }
188 1368 : while ( radians <= -EVS_PI )
189 : {
190 0 : radians = radians + EVS_PI;
191 : }
192 :
193 1368 : return _180_OVER_PI * radians;
194 : }
195 :
196 :
197 : /*-------------------------------------------------------------------------
198 : * rotateAziEle()
199 : *
200 : * Apply rotation to direction parameters azimuth and elevation
201 : *------------------------------------------------------------------------*/
202 :
203 87284308 : 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 87284308 : w = cosf( ele_in * PI_OVER_180 );
218 87284308 : dv[0] = w * cosf( azi_in * PI_OVER_180 );
219 87284308 : dv[1] = w * sinf( azi_in * PI_OVER_180 );
220 87284308 : dv[2] = sinf( ele_in * PI_OVER_180 );
221 :
222 : /*Rotation mtx multiplication*/
223 349137232 : for ( n = 0; n < 3; n++ )
224 : {
225 261852924 : 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 87284308 : *azi = (int16_t) roundf( max( -180.0f, min( 180.0f, atan2f( dv_r[1], dv_r[0] ) * _180_OVER_PI ) ) );
230 87284308 : if ( isPlanar == 0 )
231 : {
232 86515684 : *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 768624 : *ele = 0;
237 : }
238 :
239 87284308 : return;
240 : }
241 :
242 :
243 : /*-------------------------------------------------------------------------
244 : * rotateFrame_shd()
245 : *
246 : * Apply rotation to signals in Spherical Harmonic Domain
247 : *------------------------------------------------------------------------*/
248 :
249 131754 : 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 131754 : shd_rot_max_order = hTransSetup.ambisonics_order;
268 :
269 131754 : tmp = 1.0f / ( subframe_len - 1 );
270 31752714 : for ( i = 0; i < subframe_len; i++ )
271 : {
272 31620960 : cross_fade[i] = i * tmp;
273 : }
274 :
275 : /* initialize rotation matrices with zeros */
276 2239818 : for ( i = 0; i < HEADROT_SHMAT_DIM; i++ )
277 : {
278 2108064 : set_zero( SHrotmat_prev[i], HEADROT_SHMAT_DIM );
279 2108064 : set_zero( SHrotmat[i], HEADROT_SHMAT_DIM );
280 : }
281 :
282 : /* calculate ambisonics rotation matrices for the previous and current frames */
283 131754 : SHrotmatgen( SHrotmat_prev, hCombinedOrientationData->Rmat_prev[0], shd_rot_max_order );
284 :
285 131754 : SHrotmatgen( SHrotmat, hCombinedOrientationData->Rmat[hCombinedOrientationData->subframe_idx], shd_rot_max_order );
286 :
287 31752714 : 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 31620960 : m1 = 1;
294 31620960 : m2 = 4;
295 126483840 : for ( l = 1; l <= shd_rot_max_order; l++ )
296 : {
297 : /* compute mtx-vector product for this l */
298 569177280 : for ( n = m1; n < m2; n++ )
299 : {
300 474314400 : tmpRot[n - m1] = 0.f;
301 :
302 3098854080 : for ( m = m1; m < m2; m++ )
303 : {
304 : /* crossfade with previous rotation gains */
305 2624539680 : 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 569177280 : for ( n = m1; n < m2; n++ )
311 : {
312 474314400 : output[n][subframe_idx * subframe_len + i] = tmpRot[n - m1];
313 : }
314 94862880 : m1 = m2;
315 94862880 : 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 527016 : for ( i = 0; i < 3; i++ )
337 : {
338 395262 : mvr2r(
339 395262 : hCombinedOrientationData->Rmat[hCombinedOrientationData->subframe_idx][i],
340 395262 : hCombinedOrientationData->Rmat_prev[0][i],
341 : 3 );
342 : }
343 :
344 131754 : return;
345 : }
346 :
347 :
348 : /*-------------------------------------------------------------------------
349 : * rotateFrame_sd()
350 : *
351 : * Apply rotation to signals in Spatial Domain
352 : *------------------------------------------------------------------------*/
353 :
354 48000 : 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 48000 : push_wmops( "rotateFrame_sd" );
375 :
376 48000 : nchan = hTransSetup.nchan_out_woLFE + hTransSetup.num_lfe;
377 48000 : index_lfe = hTransSetup.index_lfe[0];
378 :
379 48000 : tmp = 1.0f / ( subframe_len - 1 );
380 11568000 : for ( i = 0; i < subframe_len; i++ )
381 : {
382 11520000 : cross_fade[i] = i * tmp;
383 : }
384 :
385 336000 : for ( ch_in = 0; ch_in < nchan; ch_in++ )
386 : {
387 : /* zero output and gain buffers */
388 288000 : set_zero( &output_tmp[ch_in][subframe_idx * subframe_len], subframe_len );
389 288000 : set_zero( gains_prev[ch_in], nchan );
390 288000 : set_zero( gains[ch_in], nchan );
391 :
392 : /* set gains to passthrough by default */
393 288000 : gains_prev[ch_in][ch_in] = 1.0f;
394 288000 : gains[ch_in][ch_in] = 1.0f;
395 :
396 : /* skip LFE */
397 288000 : if ( ch_in == index_lfe )
398 : {
399 48000 : continue;
400 : }
401 :
402 : /* input channel index without LFE */
403 240000 : ch_in_woLFE = ( ch_in >= index_lfe ) ? ch_in - 1 : ch_in;
404 :
405 : /* gains for previous subframe rotation */
406 240000 : 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 240000 : if ( hEFAPdata != NULL && ( hTransSetup.ls_azimuth[ch_in_woLFE] != azimuth || hTransSetup.ls_elevation[ch_in_woLFE] != elevation ) )
409 : {
410 201195 : efap_determine_gains( hEFAPdata, tmp_gains, azimuth, elevation, EFAP_MODE_EFAP );
411 1408365 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
412 : {
413 : /* skip LFE */
414 1207170 : if ( ch_out == index_lfe )
415 : {
416 201195 : continue;
417 : }
418 :
419 : /* output channel index without LFE */
420 1005975 : ch_out_woLFE = ( ch_out >= index_lfe ) ? ch_out - 1 : ch_out;
421 :
422 1005975 : gains_prev[ch_in][ch_out] = tmp_gains[ch_out_woLFE];
423 : }
424 : }
425 :
426 : /* gains for current subframe rotation */
427 240000 : 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 240000 : if ( hEFAPdata != NULL && ( hTransSetup.ls_azimuth[ch_in_woLFE] != azimuth || hTransSetup.ls_elevation[ch_in_woLFE] != elevation ) )
430 : {
431 201240 : efap_determine_gains( hEFAPdata, tmp_gains, azimuth, elevation, EFAP_MODE_EFAP );
432 :
433 1408680 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
434 : {
435 : /* skip LFE */
436 1207440 : if ( ch_out == index_lfe )
437 : {
438 201240 : continue;
439 : }
440 :
441 : /* output channel index without LFE */
442 1006200 : ch_out_woLFE = ( ch_out >= index_lfe ) ? ch_out - 1 : ch_out;
443 :
444 1006200 : gains[ch_in][ch_out] = tmp_gains[ch_out_woLFE];
445 : }
446 : }
447 : }
448 :
449 : /* apply panning gains by mtx multiplication */
450 336000 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
451 : {
452 2016000 : for ( ch_in = 0; ch_in < nchan; ch_in++ )
453 : {
454 : /* crossfade with previous rotation gains */
455 416448000 : for ( i = subframe_idx * subframe_len, j = 0; j < subframe_len; i++, j++ )
456 : {
457 414720000 : 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 192000 : for ( i = 0; i < 3; i++ )
464 : {
465 144000 : mvr2r(
466 144000 : hCombinedOrientationData->Rmat[hCombinedOrientationData->subframe_idx][i],
467 144000 : hCombinedOrientationData->Rmat_prev[0][i],
468 : 3 );
469 : }
470 :
471 : /* copy to output */
472 336000 : for ( ch_out = 0; ch_out < nchan; ch_out++ )
473 : {
474 288000 : mvr2r( &output_tmp[ch_out][subframe_idx * subframe_len], &output[ch_out][subframe_idx * subframe_len], subframe_len );
475 : }
476 :
477 48000 : pop_wmops();
478 48000 : 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 1372213 : 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 1372213 : int16_t n = 0;
498 1372213 : int16_t m = 0;
499 1372213 : int16_t i = 0;
500 1372213 : int16_t iBand = 0;
501 1372213 : 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 1372213 : 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 23327621 : for ( i = 0; i < HEADROT_SHMAT_DIM; i++ )
510 : {
511 21955408 : set_zero( SHrotmat[i], HEADROT_SHMAT_DIM );
512 : }
513 :
514 : /* calculate Ambisonics rotation matrix from the quaternion */
515 1372213 : SHrotmatgen( SHrotmat, Rmat, shd_rot_max_order );
516 :
517 : /* rotation by mtx multiplication */
518 6696498 : for ( i = 0; i < numTimeSlots; i++ )
519 : {
520 324781385 : 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 319457100 : m1 = 1;
527 319457100 : m2 = 4;
528 1277828400 : for ( l = 1; l <= shd_rot_max_order; l++ )
529 : {
530 : /* compute mtx-vector product for this l */
531 5750227800 : for ( n = m1; n < m2; n++ )
532 : {
533 4791856500 : realRot[n - m1] = 0.f;
534 4791856500 : imagRot[n - m1] = 0.f;
535 :
536 31306795800 : for ( m = m1; m < m2; m++ )
537 : {
538 26514939300 : realRot[n - m1] += SHrotmat[n][m] * Cldfb_RealBuffer[m][i][iBand];
539 26514939300 : imagRot[n - m1] += SHrotmat[n][m] * Cldfb_ImagBuffer[m][i][iBand];
540 : }
541 : }
542 : /* write back the result */
543 5750227800 : for ( n = m1; n < m2; n++ )
544 : {
545 4791856500 : Cldfb_RealBuffer[n][i][iBand] = realRot[n - m1];
546 4791856500 : Cldfb_ImagBuffer[n][i][iBand] = imagRot[n - m1];
547 : }
548 958371300 : m1 = m2;
549 958371300 : 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 1372213 : return;
574 : }
575 :
576 :
577 : /*-------------------------------------------------------------------------
578 : * rotateFrame_sd_cldfb()
579 : *
580 : * Apply rotation to signals in Spatial Domain and in CLDFB
581 : *------------------------------------------------------------------------*/
582 :
583 24000 : 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 24000 : push_wmops( "rotateFrame_sd_cldfb" );
604 :
605 24000 : nInChannels = hOutputSetup->nchan_out_woLFE;
606 24000 : isPlanar = 1;
607 144000 : for ( n = 0; n < nInChannels; n++ )
608 : {
609 120000 : if ( hOutputSetup->ls_elevation[n] != 0 )
610 : {
611 0 : isPlanar = 0;
612 0 : break;
613 : }
614 : }
615 :
616 : /* rotation of Euler angles */
617 144000 : for ( n = 0; n < nInChannels; n++ )
618 : {
619 120000 : rotateAziEle( hOutputSetup->ls_azimuth[n], hOutputSetup->ls_elevation[n], &azimuth, &elevation, Rmat, isPlanar );
620 120000 : if ( hEFAPdata != NULL && ( hOutputSetup->ls_azimuth[n] != azimuth || hOutputSetup->ls_elevation[n] != elevation ) )
621 : {
622 104685 : efap_determine_gains( hEFAPdata, gains[n], azimuth, elevation, EFAP_MODE_EFAP );
623 : }
624 : else
625 : {
626 15315 : set_zero( gains[n], nInChannels );
627 15315 : gains[n][n] = 1.0f;
628 : }
629 : }
630 :
631 : /* Apply panning gains by mtx multiplication*/
632 144000 : for ( n = 0; n < nInChannels; n++ )
633 : {
634 120000 : set_zero( realRot[n], MAX_PARAM_SPATIAL_SUBFRAMES * nb_band );
635 120000 : set_zero( imagRot[n], MAX_PARAM_SPATIAL_SUBFRAMES * nb_band );
636 720000 : for ( m = 0; m < nInChannels; m++ )
637 : {
638 600000 : g1 = gains[m][n];
639 600000 : p_realRot = realRot[n];
640 600000 : p_imagRot = imagRot[n];
641 600000 : if ( g1 > 0.f )
642 : {
643 1118760 : for ( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
644 : {
645 895008 : p_real = Cldfb_RealBuffer[m][iBlock];
646 895008 : p_imag = Cldfb_ImagBuffer[m][iBlock];
647 45645408 : for ( iBand = 0; iBand < nb_band; iBand++ )
648 : {
649 44750400 : *( p_realRot ) = *p_realRot + g1 * *( p_real++ );
650 44750400 : *( p_imagRot ) = *p_imagRot + g1 * *( p_imag++ );
651 44750400 : p_realRot++;
652 44750400 : p_imagRot++;
653 : }
654 : }
655 : }
656 : }
657 : }
658 :
659 144000 : for ( n = 0; n < nInChannels; n++ )
660 : {
661 120000 : p_realRot = realRot[n];
662 120000 : p_imagRot = imagRot[n];
663 600000 : for ( iBlock = 0; iBlock < numTimeSlots; iBlock++ )
664 : {
665 480000 : p_real = Cldfb_RealBuffer[n][iBlock];
666 480000 : p_imag = Cldfb_ImagBuffer[n][iBlock];
667 24480000 : for ( iBand = 0; iBand < nb_band; iBand++ )
668 : {
669 24000000 : *( p_real++ ) = *( p_realRot++ );
670 24000000 : *( p_imag++ ) = *( p_imagRot++ );
671 : }
672 5280000 : for ( ; iBand < CLDFB_NO_CHANNELS_MAX; iBand++ )
673 : {
674 4800000 : *( p_real++ ) = 0.f;
675 4800000 : *( p_imag++ ) = 0.f;
676 : }
677 : }
678 : }
679 24000 : pop_wmops();
680 :
681 24000 : 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 95133 : void ivas_external_orientation_close(
731 : EXTERNAL_ORIENTATION_HANDLE *hExtOrientationData /* i/o: external orientation handle */
732 : )
733 : {
734 95133 : if ( hExtOrientationData == NULL || *hExtOrientationData == NULL )
735 : {
736 95040 : 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 4311 : 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 4311 : identity.w = 1.0f;
765 4311 : identity.x = identity.y = identity.z = 0.0f;
766 4311 : origo.x = origo.y = origo.z = 0.0f;
767 :
768 : /* Allocate handle */
769 4311 : 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 4311 : ( *hCombinedOrientationData )->num_subframes = num_subframes;
776 4311 : ( *hCombinedOrientationData )->interpolationCoefficient = 1.0f;
777 4311 : ( *hCombinedOrientationData )->interpolationIncrement = 1.0f;
778 4311 : ( *hCombinedOrientationData )->maximumFramesToTargetOrientation = 500;
779 4311 : ( *hCombinedOrientationData )->lrSwitchedNext = 0;
780 4311 : ( *hCombinedOrientationData )->lrSwitchedCurrent = 0;
781 4311 : ( *hCombinedOrientationData )->lrSwitchInterpVal = 0.0f;
782 4311 : ( *hCombinedOrientationData )->isInterpolationOngoing = FALSE;
783 4311 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_start = identity;
784 4311 : ( *hCombinedOrientationData )->Quaternions_ext_interpolation_target = identity;
785 :
786 : /* Initialise orientations to identity */
787 21555 : for ( i = 0; i < MAX_PARAM_SPATIAL_SUBFRAMES; i++ )
788 : {
789 17244 : ( *hCombinedOrientationData )->enableCombinedOrientation[i] = 0;
790 17244 : ( *hCombinedOrientationData )->Quaternions[i] = identity;
791 17244 : ( *hCombinedOrientationData )->listenerPos[i] = origo;
792 :
793 68976 : for ( j = 0; j < 3; j++ )
794 : {
795 51732 : set_zero( ( *hCombinedOrientationData )->Rmat[i][j], 3 );
796 51732 : ( *hCombinedOrientationData )->Rmat[i][j][j] = 1.0f;
797 : }
798 : }
799 :
800 38799 : for ( pos_idx = 0; pos_idx < MAX_HEAD_ROT_POSES; pos_idx++ )
801 : {
802 137952 : for ( j = 0; j < 3; j++ )
803 : {
804 103464 : set_zero( ( *hCombinedOrientationData )->Rmat_prev[pos_idx][j], 3 );
805 103464 : ( *hCombinedOrientationData )->Rmat_prev[pos_idx][j][j] = 1.0f;
806 : }
807 : }
808 4311 : ( *hCombinedOrientationData )->sr_pose_pred_axis = DEFAULT_AXIS;
809 4311 : ( *hCombinedOrientationData )->sr_low_res_flag = 0;
810 :
811 4311 : ( *hCombinedOrientationData )->Quaternion_prev_extOrientation = identity;
812 4311 : ( *hCombinedOrientationData )->Quaternion_frozen_ext = identity;
813 4311 : ( *hCombinedOrientationData )->Quaternion_frozen_head = identity;
814 :
815 :
816 4311 : set_zero( ( *hCombinedOrientationData )->chEneIIR[0], MASA_FREQUENCY_BANDS );
817 4311 : set_zero( ( *hCombinedOrientationData )->chEneIIR[1], MASA_FREQUENCY_BANDS );
818 4311 : set_zero( ( *hCombinedOrientationData )->procChEneIIR[0], MASA_FREQUENCY_BANDS );
819 4311 : set_zero( ( *hCombinedOrientationData )->procChEneIIR[1], MASA_FREQUENCY_BANDS );
820 :
821 4311 : ( *hCombinedOrientationData )->isExtOrientationFrozen = 0;
822 4311 : ( *hCombinedOrientationData )->isHeadRotationFrozen = 0;
823 :
824 4311 : ( *hCombinedOrientationData )->subframe_idx = 0;
825 4311 : ( *hCombinedOrientationData )->subframe_size = (int16_t) ( fs / ( FRAMES_PER_SEC * MAX_PARAM_SPATIAL_SUBFRAMES ) );
826 4311 : ( *hCombinedOrientationData )->cur_subframe_samples_rendered = 0;
827 : #ifdef RTP_S4_251135_CR26253_0016_REV1
828 :
829 25866 : for ( i = 0; i < ( 1 + IVAS_MAX_NUM_OBJECTS ); i++ )
830 : {
831 21555 : ( *hCombinedOrientationData )->isDiegeticInputPI[i] = true;
832 : }
833 4311 : ( *hCombinedOrientationData )->isDiegeticInputPISet = false;
834 : #endif
835 :
836 4311 : return IVAS_ERR_OK;
837 : }
838 :
839 :
840 : /*-----------------------------------------------------------------------*
841 : * ivas_combined_orientation_close()
842 : *
843 : * Deallocate combined orientation handle
844 : *-----------------------------------------------------------------------*/
845 :
846 95133 : void ivas_combined_orientation_close(
847 : COMBINED_ORIENTATION_HANDLE *hCombinedOrientationData /* i/o: combined orientation handle */
848 : )
849 : {
850 95133 : if ( hCombinedOrientationData == NULL || *hCombinedOrientationData == NULL )
851 : {
852 90822 : return;
853 : }
854 :
855 4311 : free( ( *hCombinedOrientationData ) );
856 4311 : *hCombinedOrientationData = NULL;
857 :
858 4311 : return;
859 : }
860 :
861 :
862 : /*-------------------------------------------------------------------------
863 : * combine_external_and_head_orientations_dec()
864 : *
865 : *
866 : *------------------------------------------------------------------------*/
867 :
868 795281 : ivas_error combine_external_and_head_orientations_dec(
869 : HEAD_TRACK_DATA_HANDLE hHeadTrackData, /* i : head track handle */
870 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
871 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
872 : )
873 : {
874 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis;
875 795281 : IVAS_QUATERNION *pHeadRotQuaternion = NULL;
876 795281 : IVAS_VECTOR3 *listenerPos = NULL;
877 :
878 795281 : if ( hHeadTrackData != NULL )
879 : {
880 795281 : pHeadRotQuaternion = hHeadTrackData->Quaternions;
881 795281 : listenerPos = hHeadTrackData->Pos;
882 795281 : sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
883 : }
884 : else
885 : {
886 0 : sr_pose_pred_axis = DEFAULT_AXIS;
887 : }
888 :
889 795281 : return combine_external_and_head_orientations( pHeadRotQuaternion, listenerPos, sr_pose_pred_axis, hExtOrientationData, hCombinedOrientationData );
890 : }
891 :
892 :
893 : /*-------------------------------------------------------------------------
894 : * combine_external_and_head_orientations_rend()
895 : *
896 : *
897 : *------------------------------------------------------------------------*/
898 :
899 7589985 : ivas_error combine_external_and_head_orientations_rend(
900 : IVAS_REND_HeadRotData *hHeadTrackData, /* i : head track handle */
901 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
902 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
903 : )
904 : {
905 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis;
906 7589985 : IVAS_QUATERNION *headRotQuaternions = NULL;
907 7589985 : IVAS_VECTOR3 *listenerPos = NULL;
908 : int16_t i;
909 :
910 7589985 : sr_pose_pred_axis = DEFAULT_AXIS;
911 7589985 : if ( hHeadTrackData->headRotEnabled )
912 : {
913 1379414 : headRotQuaternions = hHeadTrackData->headPositions;
914 1379414 : listenerPos = hHeadTrackData->Pos;
915 1379414 : sr_pose_pred_axis = hHeadTrackData->sr_pose_pred_axis;
916 : }
917 6210571 : else if ( hExtOrientationData != NULL )
918 : {
919 : /* Head rotation data not available, use the freezed value or disable */
920 0 : for ( i = 0; i < hExtOrientationData->num_subframes; i++ )
921 : {
922 0 : if ( hExtOrientationData->enableHeadRotation[i] != 2 )
923 : {
924 0 : hExtOrientationData->enableHeadRotation[i] = 0;
925 : }
926 : }
927 : }
928 :
929 7589985 : return combine_external_and_head_orientations( headRotQuaternions, listenerPos, sr_pose_pred_axis, hExtOrientationData, hCombinedOrientationData );
930 : }
931 :
932 :
933 : /*-------------------------------------------------------------------------
934 : * combine_external_and_head_orientations()
935 : *
936 : * Combine the external orientations and the head orientation.
937 : * NOTE that the external orientations are inversed.
938 : *------------------------------------------------------------------------*/
939 :
940 8385266 : ivas_error combine_external_and_head_orientations(
941 : IVAS_QUATERNION *headRotQuaternions, /* i : quaternions for head rotation */
942 : IVAS_VECTOR3 *listenerPos, /* i : listener position */
943 : ISAR_SPLIT_REND_ROT_AXIS sr_pose_pred_axis, /* i : split rend pose prediction axis */
944 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
945 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
946 : )
947 : {
948 : int16_t i;
949 : int16_t j;
950 : IVAS_QUATERNION identity;
951 : IVAS_VECTOR3 origo;
952 :
953 8385266 : identity.w = 1.0f;
954 8385266 : identity.x = identity.y = identity.z = 0.0f;
955 8385266 : origo.x = origo.y = origo.z = 0.0f;
956 :
957 : /* Form combined orientations or return if no data available */
958 8385266 : if ( hCombinedOrientationData == NULL )
959 : {
960 6210571 : if ( headRotQuaternions != NULL || hExtOrientationData != NULL )
961 : {
962 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
963 : }
964 : else
965 : {
966 6210571 : return IVAS_ERR_OK;
967 : }
968 : }
969 2174695 : else if ( headRotQuaternions == NULL && hExtOrientationData == NULL )
970 : {
971 : /* Reset the combined orientations and rotations */
972 0 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
973 0 : hCombinedOrientationData->interpolationCoefficient = 1.0f;
974 0 : hCombinedOrientationData->interpolationIncrement = 1.0f;
975 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
976 0 : hCombinedOrientationData->Quaternions_ext_interpolation_target = identity;
977 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
978 : {
979 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
980 0 : hCombinedOrientationData->Quaternions[i] = identity;
981 0 : hCombinedOrientationData->listenerPos[i] = origo;
982 :
983 0 : for ( j = 0; j < 3; j++ )
984 : {
985 0 : set_zero( hCombinedOrientationData->Rmat[i][j], 3 );
986 0 : hCombinedOrientationData->Rmat[i][j][j] = 1.0f;
987 : }
988 : }
989 : }
990 2174695 : else if ( hExtOrientationData == NULL && headRotQuaternions != NULL )
991 : {
992 : #ifdef RTP_S4_251135_CR26253_0016_REV1
993 : /* Disable head rotation if diegetic PI data indicating non-diegetic audio is received */
994 1970295 : if ( hCombinedOrientationData->isDiegeticInputPISet && !hCombinedOrientationData->isDiegeticInputPI[0] && !hCombinedOrientationData->isDiegeticInputPI[1] && !hCombinedOrientationData->isDiegeticInputPI[2] && !hCombinedOrientationData->isDiegeticInputPI[3] && !hCombinedOrientationData->isDiegeticInputPI[4] )
995 : {
996 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
997 : {
998 0 : hCombinedOrientationData->Quaternions[i] = identity;
999 : }
1000 : }
1001 : else
1002 : {
1003 : /* Head rotation only */
1004 5959581 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1005 : {
1006 3989286 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1007 : }
1008 : }
1009 : #else
1010 : /* Head rotation only */
1011 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1012 : {
1013 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1014 : }
1015 : #endif
1016 : }
1017 :
1018 2174695 : if ( hExtOrientationData != NULL )
1019 : {
1020 : /* External orientations */
1021 554800 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1022 : {
1023 : /* Check for frozen external orientation */
1024 350400 : if ( hExtOrientationData->enableExternalOrientation[i] == 2 )
1025 : {
1026 52173 : if ( hCombinedOrientationData->isExtOrientationFrozen != 1 )
1027 : {
1028 93 : hCombinedOrientationData->Quaternion_frozen_ext = hExtOrientationData->Quaternions[i];
1029 93 : hCombinedOrientationData->isExtOrientationFrozen = 1;
1030 : }
1031 : }
1032 : else
1033 : {
1034 298227 : hCombinedOrientationData->Quaternion_frozen_ext = identity;
1035 298227 : hCombinedOrientationData->isExtOrientationFrozen = 0;
1036 : }
1037 :
1038 350400 : if ( hExtOrientationData->enableRotationInterpolation[i] == 1 && hExtOrientationData->enableExternalOrientation[i] > 0 )
1039 : {
1040 182910 : if ( hCombinedOrientationData->isInterpolationOngoing == true && hCombinedOrientationData->interpolationCoefficient <= 1.0f && are_orientations_same( &hCombinedOrientationData->Quaternions_ext_interpolation_target, &hExtOrientationData->Quaternions[i] ) == true )
1041 : {
1042 : /* Continue interpolation */
1043 151908 : QuaternionSlerp( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i] );
1044 151908 : hCombinedOrientationData->interpolationCoefficient += hCombinedOrientationData->interpolationIncrement;
1045 : }
1046 : else
1047 : {
1048 : /* Stop interpolation or check for new interpolation */
1049 31002 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1050 31002 : hCombinedOrientationData->interpolationCoefficient = 1.0f;
1051 31002 : hCombinedOrientationData->interpolationIncrement = 1.0f;
1052 31002 : external_target_interpolation( hExtOrientationData, hCombinedOrientationData, i );
1053 : }
1054 : }
1055 : else
1056 : {
1057 : /* Interpolation disabled, use the current orientation values */
1058 :
1059 : /* Use the most recent external orientation */
1060 167490 : if ( hExtOrientationData->enableExternalOrientation[i] == 1 )
1061 : {
1062 109179 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
1063 : }
1064 : /* Use the freezed external orientation */
1065 58311 : else if ( hExtOrientationData->enableExternalOrientation[i] == 2 )
1066 : {
1067 38409 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_ext;
1068 : }
1069 : }
1070 : }
1071 : }
1072 :
1073 2174695 : if ( hExtOrientationData != NULL && headRotQuaternions != NULL )
1074 : {
1075 : /* Combine head and external orientations */
1076 554800 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1077 : {
1078 : /* Check for frozen head rotation */
1079 350400 : if ( hExtOrientationData->enableHeadRotation[i] == 2 )
1080 : {
1081 37200 : if ( hCombinedOrientationData->isHeadRotationFrozen != 1 )
1082 : {
1083 186 : hCombinedOrientationData->Quaternion_frozen_head = headRotQuaternions[i];
1084 186 : hCombinedOrientationData->isHeadRotationFrozen = 1;
1085 : }
1086 : }
1087 : else
1088 : {
1089 313200 : hCombinedOrientationData->Quaternion_frozen_head = identity;
1090 313200 : hCombinedOrientationData->isHeadRotationFrozen = 0;
1091 : }
1092 : #ifdef RTP_S4_251135_CR26253_0016_REV1
1093 : /* Disable head rotation if diegetic PI data indicating non-diegetic audio is received */
1094 350400 : if ( hCombinedOrientationData->isDiegeticInputPISet && !hCombinedOrientationData->isDiegeticInputPI[0] && !hCombinedOrientationData->isDiegeticInputPI[1] && !hCombinedOrientationData->isDiegeticInputPI[2] && !hCombinedOrientationData->isDiegeticInputPI[3] && !hCombinedOrientationData->isDiegeticInputPI[4] )
1095 : {
1096 0 : continue;
1097 : }
1098 : else
1099 : {
1100 : /* Use the most recent head rotation */
1101 350400 : if ( hExtOrientationData->enableHeadRotation[i] == 1 )
1102 : {
1103 250785 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1104 : {
1105 230883 : QuaternionProduct( hCombinedOrientationData->Quaternions[i], headRotQuaternions[i], &hCombinedOrientationData->Quaternions[i] );
1106 : }
1107 : else
1108 : {
1109 19902 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1110 : }
1111 : }
1112 : /* Use the freezed head rotation */
1113 99615 : else if ( hExtOrientationData->enableHeadRotation[i] == 2 )
1114 : {
1115 37200 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1116 : {
1117 37200 : QuaternionProduct( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Quaternion_frozen_head, &hCombinedOrientationData->Quaternions[i] );
1118 : }
1119 : else
1120 : {
1121 0 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_head;
1122 : }
1123 : }
1124 : }
1125 : #else
1126 : /* Use the most recent head rotation */
1127 : if ( hExtOrientationData->enableHeadRotation[i] == 1 )
1128 : {
1129 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1130 : {
1131 : QuaternionProduct( hCombinedOrientationData->Quaternions[i], headRotQuaternions[i], &hCombinedOrientationData->Quaternions[i] );
1132 : }
1133 : else
1134 : {
1135 : hCombinedOrientationData->Quaternions[i] = headRotQuaternions[i];
1136 : }
1137 : }
1138 : /* Use the freezed head rotation */
1139 : else if ( hExtOrientationData->enableHeadRotation[i] == 2 )
1140 : {
1141 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1142 : {
1143 : QuaternionProduct( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Quaternion_frozen_head, &hCombinedOrientationData->Quaternions[i] );
1144 : }
1145 : else
1146 : {
1147 : hCombinedOrientationData->Quaternions[i] = hCombinedOrientationData->Quaternion_frozen_head;
1148 : }
1149 : }
1150 : #endif
1151 :
1152 : /* Reset the combined orientations to identity */
1153 350400 : if ( hExtOrientationData->enableHeadRotation[i] == 0 && hExtOrientationData->enableExternalOrientation[i] == 0 )
1154 : {
1155 0 : hCombinedOrientationData->Quaternions[i] = identity;
1156 : }
1157 : }
1158 : }
1159 :
1160 2174695 : if ( headRotQuaternions != NULL || hExtOrientationData != NULL )
1161 : {
1162 : /* Calculate the combined rotation matrix */
1163 6514381 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1164 : {
1165 4339686 : QuatToRotMat( hCombinedOrientationData->Quaternions[i], hCombinedOrientationData->Rmat[i] );
1166 : }
1167 : }
1168 :
1169 : /* Save the current orientations */
1170 2174695 : if ( hExtOrientationData != NULL )
1171 : {
1172 204400 : if ( hExtOrientationData->enableExternalOrientation[hExtOrientationData->num_subframes - 1] > 0 )
1173 : {
1174 192806 : hCombinedOrientationData->Quaternion_prev_extOrientation = hExtOrientationData->Quaternions[hExtOrientationData->num_subframes - 1];
1175 : }
1176 : else
1177 : {
1178 11594 : hCombinedOrientationData->Quaternion_prev_extOrientation = identity;
1179 : }
1180 : }
1181 2174695 : if ( headRotQuaternions != NULL )
1182 : {
1183 6514381 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1184 : {
1185 4339686 : hCombinedOrientationData->listenerPos[i] = listenerPos[i];
1186 : }
1187 : }
1188 :
1189 : /* Check if combined orientation is enabled */
1190 2174695 : if ( headRotQuaternions != NULL && hExtOrientationData == NULL )
1191 : {
1192 5959581 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1193 : {
1194 3989286 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1195 : }
1196 : }
1197 204400 : else if ( headRotQuaternions == NULL && hExtOrientationData != NULL )
1198 : {
1199 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1200 : {
1201 0 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 )
1202 : {
1203 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1204 : }
1205 : else
1206 : {
1207 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1208 : }
1209 : }
1210 : }
1211 204400 : else if ( headRotQuaternions != NULL && hExtOrientationData != NULL )
1212 : {
1213 554800 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1214 : {
1215 350400 : if ( hExtOrientationData->enableExternalOrientation[i] > 0 || ( hExtOrientationData->enableHeadRotation[i] > 0 ) )
1216 : {
1217 350400 : hCombinedOrientationData->enableCombinedOrientation[i] = 1;
1218 : }
1219 : else
1220 : {
1221 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1222 : }
1223 : }
1224 : }
1225 : else
1226 : {
1227 0 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1228 : {
1229 0 : hCombinedOrientationData->enableCombinedOrientation[i] = 0;
1230 : }
1231 : }
1232 :
1233 : #ifdef DEBUG_MODE_ORIENTATION
1234 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1235 : {
1236 : dbgwrite( &hCombinedOrientationData->enableCombinedOrientation[i], sizeof( int16_t ), 1, 1, "res/dec_orientation_enabled.dat" );
1237 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].w ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_w.dat" );
1238 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].x ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_x.dat" );
1239 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].y ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_y.dat" );
1240 : dbgwrite( &( hCombinedOrientationData->Quaternions[i].z ), sizeof( float ), 1, 1, "res/dec_orientation_quaternion_z.dat" );
1241 : }
1242 : #endif
1243 2174695 : hCombinedOrientationData->sr_pose_pred_axis = sr_pose_pred_axis;
1244 2174695 : hCombinedOrientationData->subframe_idx = 0;
1245 2174695 : hCombinedOrientationData->cur_subframe_samples_rendered = 0;
1246 2174695 : hCombinedOrientationData->subframe_idx_start = 0;
1247 2174695 : hCombinedOrientationData->cur_subframe_samples_rendered_start = 0;
1248 :
1249 : #ifdef IVAS_RTPDUMP
1250 : /* Reset external orientations */
1251 2174695 : if ( hExtOrientationData != NULL )
1252 : {
1253 554800 : for ( i = 0; i < hCombinedOrientationData->num_subframes; i++ )
1254 : {
1255 350400 : hExtOrientationData->Quaternions[i] = identity;
1256 : }
1257 : }
1258 : #endif
1259 2174695 : return IVAS_ERR_OK;
1260 : }
1261 :
1262 :
1263 : /*-------------------------------------------------------------------------
1264 : * external_target_interpolation()
1265 : *
1266 : *
1267 : *------------------------------------------------------------------------*/
1268 :
1269 31002 : static void external_target_interpolation(
1270 : EXTERNAL_ORIENTATION_HANDLE hExtOrientationData, /* i : external orientation handle */
1271 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1272 : const int16_t i /* i : subframe index */
1273 : )
1274 : {
1275 : /* Sanity check for number of frames */
1276 31002 : hExtOrientationData->numFramesToTargetOrientation[i] = min( hExtOrientationData->numFramesToTargetOrientation[i], hCombinedOrientationData->maximumFramesToTargetOrientation );
1277 31002 : hExtOrientationData->numFramesToTargetOrientation[i] = max( hExtOrientationData->numFramesToTargetOrientation[i], 0 );
1278 :
1279 : /* Interpolate from the current orientation to the target orientation */
1280 31002 : if ( hExtOrientationData->numFramesToTargetOrientation[i] > 0 )
1281 : {
1282 10047 : if ( are_orientations_same( &hCombinedOrientationData->Quaternions_ext_interpolation_target, &hExtOrientationData->Quaternions[i] ) == false )
1283 : {
1284 : /* Target orientation is different from the previous target, update the values */
1285 :
1286 : /* Set the received orientation as the target */
1287 747 : hCombinedOrientationData->Quaternions_ext_interpolation_target = hExtOrientationData->Quaternions[i];
1288 :
1289 : /* Use the most recent external orientation as the starting orientation */
1290 747 : if ( hExtOrientationData->enableExternalOrientation[i] == 1 )
1291 : {
1292 654 : if ( i > 0 )
1293 : {
1294 227 : if ( hExtOrientationData->enableExternalOrientation[i - 1] == 0 )
1295 : {
1296 : IVAS_QUATERNION identity;
1297 62 : identity.w = 1.0f;
1298 62 : identity.x = identity.y = identity.z = 0.0f;
1299 62 : hCombinedOrientationData->Quaternions_ext_interpolation_start = identity;
1300 : }
1301 165 : else if ( hExtOrientationData->enableExternalOrientation[i - 1] == 2 )
1302 : {
1303 0 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1304 : }
1305 : else
1306 : {
1307 165 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hExtOrientationData->Quaternions[i - 1];
1308 : }
1309 : }
1310 : else
1311 : {
1312 427 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_prev_extOrientation;
1313 : }
1314 : }
1315 93 : else if ( hExtOrientationData->enableExternalOrientation[i] == 2 )
1316 : {
1317 93 : hCombinedOrientationData->Quaternions_ext_interpolation_start = hCombinedOrientationData->Quaternion_frozen_ext;
1318 : }
1319 :
1320 : /* Calculate the interpolation increment and coefficient */
1321 747 : hCombinedOrientationData->interpolationIncrement = 1.0f / ( (float) hExtOrientationData->numFramesToTargetOrientation[i] * (float) MAX_PARAM_SPATIAL_SUBFRAMES );
1322 747 : hCombinedOrientationData->interpolationCoefficient = hCombinedOrientationData->interpolationIncrement;
1323 : }
1324 :
1325 : /* Interpolate */
1326 10047 : hCombinedOrientationData->isInterpolationOngoing = TRUE;
1327 10047 : QuaternionSlerp( hCombinedOrientationData->Quaternions_ext_interpolation_start, hCombinedOrientationData->Quaternions_ext_interpolation_target, hCombinedOrientationData->interpolationCoefficient, &hCombinedOrientationData->Quaternions[i] );
1328 10047 : hCombinedOrientationData->interpolationCoefficient += hCombinedOrientationData->interpolationIncrement;
1329 : }
1330 : else
1331 : {
1332 : /* Use the target orientation immediately */
1333 20955 : hCombinedOrientationData->isInterpolationOngoing = FALSE;
1334 20955 : hCombinedOrientationData->interpolationCoefficient = 1.0f;
1335 20955 : hCombinedOrientationData->interpolationIncrement = 1.0f;
1336 20955 : hCombinedOrientationData->Quaternions[i] = hExtOrientationData->Quaternions[i];
1337 : }
1338 :
1339 31002 : return;
1340 : }
1341 :
1342 :
1343 : /*-------------------------------------------------------------------------
1344 : * are_orientations_same()
1345 : *
1346 : *
1347 : *------------------------------------------------------------------------*/
1348 :
1349 162609 : static bool are_orientations_same(
1350 : const IVAS_QUATERNION *orientation1,
1351 : const IVAS_QUATERNION *orientation2 )
1352 : {
1353 162609 : bool orientationsAreSame = true;
1354 162609 : float error_margin = 0.05f;
1355 :
1356 162609 : if ( fabsf( orientation1->w - orientation2->w ) > error_margin ||
1357 161610 : fabsf( orientation1->x - orientation2->x ) > error_margin ||
1358 161610 : fabsf( orientation1->y - orientation2->y ) > error_margin ||
1359 161610 : fabsf( orientation1->z - orientation2->z ) > error_margin )
1360 : {
1361 1401 : orientationsAreSame = false;
1362 : }
1363 :
1364 162609 : return orientationsAreSame;
1365 : }
1366 :
1367 :
1368 : /*-----------------------------------------------------------------------*
1369 : * Local Function definitions
1370 : *-----------------------------------------------------------------------*/
1371 :
1372 : /*-------------------------------------------------------------------------
1373 : * Helper functions used by SHrotmatgen,
1374 : * an implementation of the algorithm in
1375 : * Ivanic, J. & Ruedenberg, K., J. Phys. Chem. 100, 6342 (1996)
1376 : *------------------------------------------------------------------------*/
1377 :
1378 549438919 : static float SHrot_p(
1379 : const int16_t i,
1380 : const int16_t l,
1381 : const int16_t a,
1382 : const int16_t b,
1383 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM],
1384 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] )
1385 : {
1386 549438919 : float ri1 = 0.0f, rim1 = 0.0f, ri0 = 0.0f, p = 0.0f, R_lm1_1 = 0.0f, R_lm1_2 = 0.0f;
1387 :
1388 549438919 : ri1 = SHrotmat[i + 1 + 1][1 + 1 + 1];
1389 549438919 : rim1 = SHrotmat[i + 1 + 1][-1 + 1 + 1];
1390 549438919 : ri0 = SHrotmat[i + 1 + 1][0 + 1 + 1];
1391 :
1392 549438919 : if ( b == -l )
1393 : {
1394 88555707 : R_lm1_1 = R_lm1[a + l - 1][0];
1395 88555707 : R_lm1_2 = R_lm1[a + l - 1][2 * l - 2];
1396 88555707 : p = ri1 * R_lm1_1 + rim1 * R_lm1_2;
1397 : }
1398 : else
1399 : {
1400 460883212 : if ( b == l )
1401 : {
1402 88555707 : R_lm1_1 = R_lm1[a + l - 1][2 * l - 2];
1403 88555707 : R_lm1_2 = R_lm1[a + l - 1][0];
1404 88555707 : p = ri1 * R_lm1_1 - rim1 * R_lm1_2;
1405 : }
1406 : else
1407 : {
1408 372327505 : R_lm1_1 = R_lm1[a + l - 1][b + l - 1];
1409 372327505 : p = ri0 * R_lm1_1;
1410 : }
1411 : }
1412 :
1413 549438919 : return p;
1414 : }
1415 :
1416 121799465 : static float SHrot_u(
1417 : const int16_t l,
1418 : const int16_t m,
1419 : const int16_t n,
1420 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM],
1421 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] )
1422 : {
1423 121799465 : return SHrot_p( 0, l, m, n, SHrotmat, R_lm1 );
1424 : }
1425 :
1426 181357871 : static float SHrot_v(
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 :
1434 : float result, d, p0, p1;
1435 :
1436 181357871 : if ( m == 0 )
1437 : {
1438 29779203 : p0 = SHrot_p( 1, l, 1, n, SHrotmat, R_lm1 );
1439 29779203 : p1 = SHrot_p( -1, l, -1, n, SHrotmat, R_lm1 );
1440 29779203 : result = p0 + p1;
1441 : }
1442 : else
1443 : {
1444 151578668 : if ( m > 0 )
1445 : {
1446 75789334 : d = ( m == 1 ) ? 1.0f : 0.0f;
1447 75789334 : p0 = SHrot_p( 1, l, m - 1, n, SHrotmat, R_lm1 );
1448 75789334 : p1 = SHrot_p( -1, l, -m + 1, n, SHrotmat, R_lm1 );
1449 75789334 : result = p0 * sqrtf( 1.0f + d ) - p1 * ( 1.0f - d );
1450 : }
1451 : else
1452 : {
1453 75789334 : d = ( m == -1 ) ? 1.0f : 0.0f;
1454 75789334 : p0 = SHrot_p( 1, l, m + 1, n, SHrotmat, R_lm1 );
1455 75789334 : p1 = SHrot_p( -1, l, -m - 1, n, SHrotmat, R_lm1 );
1456 75789334 : result = p0 * ( 1.0f - d ) + p1 * sqrtf( 1.0f + d );
1457 : }
1458 : }
1459 :
1460 181357871 : return result;
1461 : }
1462 :
1463 32461856 : static float SHrot_w(
1464 : const int16_t l,
1465 : const int16_t m,
1466 : const int16_t n,
1467 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM],
1468 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM] )
1469 : {
1470 : float result, p0, p1;
1471 :
1472 32461856 : if ( m == 0 )
1473 : {
1474 0 : assert( 0 && "ERROR should not be called\n" );
1475 : return 0.0f;
1476 : }
1477 : else
1478 : {
1479 32461856 : if ( m > 0 )
1480 : {
1481 16230928 : p0 = SHrot_p( 1, l, m + 1, n, SHrotmat, R_lm1 );
1482 16230928 : p1 = SHrot_p( -1, l, -m - 1, n, SHrotmat, R_lm1 );
1483 16230928 : result = p0 + p1;
1484 : }
1485 : else
1486 : {
1487 16230928 : p0 = SHrot_p( 1, l, m - 1, n, SHrotmat, R_lm1 );
1488 16230928 : p1 = SHrot_p( -1, l, -m + 1, n, SHrotmat, R_lm1 );
1489 16230928 : result = p0 - p1;
1490 : }
1491 : }
1492 :
1493 32461856 : return result;
1494 : }
1495 :
1496 :
1497 : /*-------------------------------------------------------------------------
1498 : * rotateFrame_sd_cldfb()
1499 : *
1500 : *
1501 : *------------------------------------------------------------------------*/
1502 :
1503 3281566 : void SHrotmatgen(
1504 : float SHrotmat[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM], /* o : rotation matrix in SHD */
1505 : float Rmat[3][3], /* i : real-space rotation matrix */
1506 : const int16_t order /* i : ambisonics order */
1507 : )
1508 : {
1509 3281566 : int16_t d = 0;
1510 3281566 : int16_t band_idx = 0;
1511 : int16_t i, j;
1512 : int16_t l, m, n;
1513 : int16_t absm;
1514 3281566 : float sqdenom = 0.0f, sql2mm2 = 0.0f, sqdabsm = 0.0f, sqlabsm = 0.0f;
1515 3281566 : float u = 0.0f, v = 0.0f, w = 0.0f;
1516 : float R_lm1[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
1517 : float R_l[HEADROT_SHMAT_DIM][HEADROT_SHMAT_DIM];
1518 :
1519 3281566 : SHrotmat[0][0] = 1.0f;
1520 :
1521 3281566 : SHrotmat[1][1] = Rmat[1][1];
1522 3281566 : SHrotmat[1][2] = Rmat[1][2];
1523 3281566 : SHrotmat[1][3] = Rmat[1][0];
1524 :
1525 3281566 : SHrotmat[2][1] = Rmat[2][1];
1526 3281566 : SHrotmat[2][2] = Rmat[2][2];
1527 3281566 : SHrotmat[2][3] = Rmat[2][0];
1528 :
1529 3281566 : SHrotmat[3][1] = Rmat[0][1];
1530 3281566 : SHrotmat[3][2] = Rmat[0][2];
1531 3281566 : SHrotmat[3][3] = Rmat[0][0];
1532 :
1533 13126264 : for ( i = 0; i < 2 * 1 + 1; i++ )
1534 : {
1535 39378792 : for ( j = 0; j < 2 * 1 + 1; j++ )
1536 : {
1537 29534094 : R_lm1[i][j] = SHrotmat[i + 1][j + 1];
1538 : }
1539 : }
1540 :
1541 3281566 : band_idx = 4;
1542 8309925 : for ( l = 2; l <= order; l++ )
1543 : {
1544 5028359 : set_zero( &R_l[0][0], HEADROT_SHMAT_DIM2 );
1545 :
1546 34807562 : for ( m = -l; m <= l; m++ )
1547 : {
1548 29779203 : d = ( m == 0 ) ? 1 : 0;
1549 29779203 : absm = (int16_t) abs( m );
1550 29779203 : sql2mm2 = sqrtf( (float) ( l * l - m * m ) );
1551 29779203 : sqdabsm = sqrtf( (float) ( ( 1 + d ) * ( l + absm - 1 ) * ( l + absm ) ) );
1552 29779203 : sqlabsm = sqrtf( (float) ( ( l - absm - 1 ) * ( l - absm ) ) );
1553 :
1554 211137074 : for ( n = -l; n <= l; n++ )
1555 : {
1556 181357871 : if ( abs( n ) == l )
1557 : {
1558 59558406 : sqdenom = sqrtf( (float) ( ( 2 * l ) * ( 2 * l - 1 ) ) );
1559 : }
1560 : else
1561 : {
1562 121799465 : sqdenom = sqrtf( (float) ( l * l - n * n ) );
1563 : }
1564 :
1565 181357871 : u = sql2mm2 / sqdenom;
1566 181357871 : v = sqdabsm / sqdenom * ( 1 - 2 * d ) * 0.5f;
1567 181357871 : w = sqlabsm / sqdenom * ( 1 - d ) * ( -0.5f );
1568 :
1569 181357871 : if ( u != 0 )
1570 : {
1571 121799465 : u = u * SHrot_u( l, m, n, SHrotmat, R_lm1 );
1572 : }
1573 181357871 : if ( v != 0 )
1574 : {
1575 181357871 : v = v * SHrot_v( l, m, n, SHrotmat, R_lm1 );
1576 : }
1577 181357871 : if ( w != 0 )
1578 : {
1579 32461856 : w = w * SHrot_w( l, m, n, SHrotmat, R_lm1 );
1580 : }
1581 181357871 : R_l[m + l][n + l] = u + v + w;
1582 : }
1583 : }
1584 :
1585 34807562 : for ( i = 0; i < 2 * l + 1; i++ )
1586 : {
1587 211137074 : for ( j = 0; j < 2 * l + 1; j++ )
1588 : {
1589 181357871 : SHrotmat[band_idx + i][band_idx + j] = R_l[i][j];
1590 : }
1591 : }
1592 :
1593 34807562 : for ( i = 0; i < 2 * l + 1; i++ )
1594 : {
1595 211137074 : for ( j = 0; j < 2 * l + 1; j++ )
1596 : {
1597 181357871 : R_lm1[i][j] = R_l[i][j];
1598 : }
1599 : }
1600 :
1601 5028359 : band_idx += 2 * l + 1;
1602 : }
1603 :
1604 3281566 : return;
1605 : }
1606 :
1607 :
1608 : /*-------------------------------------------------------------------------
1609 : * ivas_combined_orientation_update_index()
1610 : *
1611 : * update read index based on the number of rendered samples
1612 : *------------------------------------------------------------------------*/
1613 :
1614 35003969 : void ivas_combined_orientation_update_index(
1615 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1616 : const int16_t samples_rendered /* i : samples rendered since the last call */
1617 : )
1618 : {
1619 35003969 : if ( hCombinedOrientationData != NULL )
1620 : {
1621 8251499 : if ( hCombinedOrientationData->num_subframes == 1 || hCombinedOrientationData->sr_low_res_flag )
1622 : {
1623 : /* only one orientation available anyway or split rendering with low resolution*/
1624 1725991 : hCombinedOrientationData->subframe_idx = 0;
1625 : }
1626 : else
1627 : {
1628 6525508 : hCombinedOrientationData->cur_subframe_samples_rendered += samples_rendered;
1629 6525508 : hCombinedOrientationData->subframe_idx += hCombinedOrientationData->cur_subframe_samples_rendered / hCombinedOrientationData->subframe_size;
1630 6525508 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size;
1631 6525508 : hCombinedOrientationData->subframe_idx = min( hCombinedOrientationData->subframe_idx, hCombinedOrientationData->num_subframes - 1 );
1632 : }
1633 : }
1634 :
1635 35003969 : return;
1636 : }
1637 :
1638 :
1639 : /*-------------------------------------------------------------------------
1640 : * ivas_combined_orientation_update_index()
1641 : *
1642 : * update read index based on the number of rendered samples
1643 : *------------------------------------------------------------------------*/
1644 :
1645 21290763 : void ivas_combined_orientation_set_to_start_index(
1646 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData /* i/o: combined orientation handle */
1647 : )
1648 : {
1649 21290763 : if ( hCombinedOrientationData != NULL )
1650 : {
1651 5170755 : hCombinedOrientationData->subframe_idx = hCombinedOrientationData->subframe_idx_start;
1652 5170755 : hCombinedOrientationData->cur_subframe_samples_rendered = hCombinedOrientationData->cur_subframe_samples_rendered_start;
1653 : }
1654 :
1655 21290763 : return;
1656 : }
1657 :
1658 :
1659 : /*-------------------------------------------------------------------------
1660 : * ivas_combined_orientation_update_start_index()
1661 : *
1662 : * update start index based on the number of rendered samples
1663 : *------------------------------------------------------------------------*/
1664 :
1665 15043251 : void ivas_combined_orientation_update_start_index(
1666 : COMBINED_ORIENTATION_HANDLE hCombinedOrientationData, /* i/o: combined orientation handle */
1667 : const int16_t samples_rendered /* i : samples rendered since the last call */
1668 : )
1669 : {
1670 15043251 : if ( hCombinedOrientationData != NULL )
1671 : {
1672 2221518 : if ( hCombinedOrientationData->num_subframes == 1 || hCombinedOrientationData->sr_low_res_flag )
1673 : {
1674 : /* only one orientation available anyway or split rendering with low resolution*/
1675 1348034 : hCombinedOrientationData->subframe_idx = 0;
1676 : }
1677 : else
1678 : {
1679 873484 : hCombinedOrientationData->cur_subframe_samples_rendered_start += samples_rendered;
1680 873484 : hCombinedOrientationData->subframe_idx_start += hCombinedOrientationData->cur_subframe_samples_rendered / hCombinedOrientationData->subframe_size;
1681 873484 : hCombinedOrientationData->cur_subframe_samples_rendered_start = hCombinedOrientationData->cur_subframe_samples_rendered % hCombinedOrientationData->subframe_size;
1682 873484 : hCombinedOrientationData->subframe_idx_start = min( hCombinedOrientationData->subframe_idx, hCombinedOrientationData->num_subframes - 1 );
1683 : }
1684 : }
1685 :
1686 15043251 : return;
1687 : }
|
