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 <stdint.h>
34 : #include "options.h"
35 : #include "ivas_prot.h"
36 : #include "ivas_cnst.h"
37 : #ifdef DEBUGGING
38 : #include "debug.h"
39 : #endif
40 : #include <math.h>
41 : #include <assert.h>
42 : #include "ivas_rom_com.h"
43 : #include "wmc_auto.h"
44 : #include "prot.h"
45 :
46 :
47 : /*---------------------------------------------------------------------*
48 : * eye_matrix()
49 : *
50 : *---------------------------------------------------------------------*/
51 :
52 2026048 : void eye_matrix(
53 : float *mat,
54 : const int16_t n,
55 : const float d )
56 : {
57 : int16_t i;
58 :
59 34442816 : for ( i = 0; i < n * n; i++ )
60 : {
61 32416768 : mat[i] = 0.0f;
62 : }
63 10130240 : for ( i = 0; i < n; i++ )
64 : {
65 8104192 : mat[i * n + i] = d;
66 : }
67 :
68 2026048 : return;
69 : }
70 :
71 :
72 : /*---------------------------------------------------------------------*
73 : * cov()
74 : *
75 : * Compute covariance of input channels
76 : *---------------------------------------------------------------------*/
77 :
78 3500 : void cov_subfr(
79 : float **ptr_sig,
80 : float *r,
81 : const int16_t n_channels,
82 : const int16_t len )
83 : {
84 : float s;
85 : int16_t j, k, l;
86 : float *t, *tt;
87 :
88 : /* Compute diagonal r[k][k] */
89 17500 : for ( k = 0; k < n_channels; k++ )
90 : {
91 14000 : t = &ptr_sig[k][0];
92 14000 : s = t[0] * t[0];
93 5760000 : for ( j = 1; j < len; j++ )
94 : {
95 5746000 : s += t[j] * t[j];
96 : }
97 14000 : r[k * n_channels + k] = s;
98 : }
99 :
100 : /* Compute off-diagonal r[k][l] = r[l][k] */
101 14000 : for ( k = 0; k < ( n_channels - 1 ); k++ )
102 : {
103 10500 : t = &ptr_sig[k][0];
104 31500 : for ( l = k + 1; l < n_channels; l++ )
105 : {
106 21000 : tt = &ptr_sig[l][0];
107 21000 : s = t[0] * tt[0];
108 8640000 : for ( j = 1; j < len; j++ )
109 : {
110 8619000 : s += t[j] * tt[j];
111 : }
112 21000 : r[k * n_channels + l] = s;
113 21000 : r[l * n_channels + k] = s;
114 : }
115 : }
116 :
117 3500 : return;
118 : }
119 :
120 :
121 70000 : static void house_refl(
122 : const float *px,
123 : const int16_t sizex,
124 : float *pu,
125 : float *normu )
126 : {
127 : int16_t i;
128 :
129 70000 : mvr2r( px, pu, sizex );
130 70000 : ( *normu ) = dotp( pu, pu, sizex );
131 70000 : ( *normu ) = sqrtf( *normu );
132 :
133 70000 : if ( ( *normu ) == 0.f )
134 : {
135 0 : pu[0] = SQRT2;
136 : }
137 : else
138 : {
139 70000 : float rcp = 1.f / ( *normu );
140 :
141 245000 : for ( i = 0; i < sizex; i++ )
142 : {
143 175000 : pu[i] *= rcp;
144 : }
145 70000 : if ( pu[0] >= 0.f )
146 : {
147 70000 : pu[0] += 1;
148 70000 : ( *normu ) = -( *normu );
149 : }
150 : else
151 : {
152 0 : pu[0] -= 1;
153 : }
154 :
155 70000 : rcp = inv_sqrt( fabsf( pu[0] ) );
156 :
157 245000 : for ( i = 0; i < sizex; i++ )
158 : {
159 175000 : pu[i] *= rcp;
160 : }
161 : }
162 :
163 70000 : return;
164 : }
165 :
166 :
167 17500 : static void house_qr(
168 : const float *pA,
169 : float *pQ,
170 : float *pR,
171 : const int16_t n )
172 : {
173 17500 : int16_t n_rows = FOA_CHANNELS;
174 : float A[FOA_CHANNELS * FOA_CHANNELS];
175 : float U[FOA_CHANNELS * FOA_CHANNELS];
176 : float pu[FOA_CHANNELS];
177 : float pa[FOA_CHANNELS];
178 : float pv[FOA_CHANNELS];
179 : int16_t i, j, k, c, s, r;
180 :
181 17500 : mvr2r( pA, A, FOA_CHANNELS * FOA_CHANNELS );
182 17500 : set_zero( U, FOA_CHANNELS * FOA_CHANNELS );
183 17500 : set_zero( pR, FOA_CHANNELS * FOA_CHANNELS );
184 :
185 87500 : for ( k = 0; k < n_rows; k++ )
186 : {
187 350000 : for ( i = 0; i < n_rows; i++ )
188 : {
189 280000 : pa[i] = A[i * n + k];
190 : }
191 :
192 70000 : house_refl( &pa[k], n_rows - k, pu, &pR[k * n_rows + k] );
193 70000 : U[k * n_rows + k] = pu[0];
194 175000 : for ( c = k + 1; c < n_rows; c++ )
195 : {
196 525000 : for ( i = 0; i < n_rows; i++ )
197 : {
198 420000 : pa[i] = A[i * n + c];
199 : }
200 105000 : pv[c - k - 1] = dotp( pu, &pa[k], n_rows - k );
201 105000 : U[c * n_rows + k] = pu[c - k];
202 : }
203 :
204 70000 : i = 0;
205 245000 : for ( s = k; s < n_rows; s++ )
206 : {
207 175000 : j = 0;
208 525000 : for ( r = k + 1; r < n_rows; r++ )
209 : {
210 350000 : A[s * n_rows + r] -= ( pu[i] * pv[j] );
211 350000 : j++;
212 : }
213 175000 : i++;
214 : }
215 :
216 175000 : for ( r = k + 1; r < n_rows; r++ )
217 : {
218 105000 : pR[k * n_rows + r] = A[k * n_rows + r];
219 : }
220 : }
221 :
222 17500 : eye_matrix( pQ, FOA_CHANNELS, 1.0f );
223 :
224 87500 : for ( k = FOA_CHANNELS - 1; k >= 0; k-- )
225 : {
226 350000 : for ( i = 0; i < n_rows; i++ )
227 : {
228 280000 : pu[i] = U[i * n + k];
229 : }
230 :
231 245000 : for ( s = k; s < n_rows; s++ )
232 : {
233 875000 : for ( i = 0; i < n_rows; i++ )
234 : {
235 700000 : pa[i] = pQ[i * n + s];
236 : }
237 175000 : pv[s - k] = dotp( &pu[k], &pa[k], n_rows - k );
238 : }
239 245000 : for ( s = k; s < n_rows; s++ )
240 : {
241 700000 : for ( r = k; r < n_rows; r++ )
242 : {
243 525000 : pQ[s * n_rows + r] -= U[s * n_rows + k] * pv[r - k];
244 : }
245 : }
246 : }
247 :
248 17500 : return;
249 : }
250 :
251 :
252 : /*---------------------------------------------------------------------*
253 : * eig_qr()
254 : *
255 : * Compute eigenvalue decomposition by QR method
256 : *---------------------------------------------------------------------*/
257 :
258 1750 : void eig_qr(
259 : const float *A,
260 : const int16_t num_iter,
261 : float *EV,
262 : float *Vals,
263 : const int16_t n )
264 : {
265 : int16_t i;
266 : int16_t breakcnd, iter_num;
267 : float Ak[FOA_CHANNELS * FOA_CHANNELS], Qk[FOA_CHANNELS * FOA_CHANNELS], Rk[FOA_CHANNELS * FOA_CHANNELS], D[FOA_CHANNELS * FOA_CHANNELS], d;
268 :
269 1750 : assert( n <= FOA_CHANNELS );
270 1750 : breakcnd = 1;
271 1750 : iter_num = 0;
272 :
273 : /* check zero matrix */
274 1750 : d = dotp( A, A, n * n );
275 1750 : if ( d != 0. )
276 : {
277 1750 : breakcnd = 0;
278 : }
279 :
280 : /* duplicate */
281 1750 : mvr2r( A, Ak, n * n );
282 :
283 : /* identity */
284 1750 : set_zero( EV, n * n );
285 8750 : for ( i = 0; i < n; i++ )
286 : {
287 7000 : EV[i * n + i] = 1.0f;
288 : }
289 :
290 1750 : set_zero( Vals, n );
291 :
292 19250 : while ( breakcnd == 0 )
293 : {
294 17500 : iter_num++;
295 17500 : mvr2r( Ak, D, n * n );
296 :
297 : /* set diagonal */
298 87500 : for ( i = 0; i < n; i++ )
299 : {
300 70000 : D[i * n + i] = Vals[i];
301 : }
302 :
303 : /* stop condition */
304 17500 : d = dotp( D, D, n * n );
305 17500 : d = sqrtf( d ); /* Frobenius norm */
306 :
307 17500 : if ( ( d < 1e-10f ) || ( iter_num >= num_iter ) )
308 : {
309 1750 : breakcnd = 1;
310 : }
311 :
312 17500 : house_qr( Ak, Qk, Rk, n );
313 :
314 17500 : matrix_product( Qk, n, n, 0, Rk, n, n, 0, Ak );
315 17500 : matrix_product( Qk, n, n, 0, EV, n, n, 0, D );
316 :
317 17500 : mvr2r( D, EV, n * n );
318 : }
319 :
320 : /* get diagonal */
321 8750 : for ( i = 0; i < n; i++ )
322 : {
323 7000 : Vals[i] = Ak[i * n + i];
324 : }
325 :
326 1750 : return;
327 : }
328 :
329 :
330 : /*---------------------------------------------------------------------*
331 : * exhst_4x4()
332 : *
333 : * Find optimal permutation of eigenvectors
334 : *---------------------------------------------------------------------*/
335 :
336 6 : void exhst_4x4(
337 : float *cost_mtx,
338 : int16_t *path,
339 : const int16_t maximize )
340 : {
341 : int16_t i, j, k, l;
342 : float opt_val, cost;
343 :
344 6 : if ( maximize == 0 )
345 : {
346 0 : opt_val = 1e30f;
347 : }
348 : else
349 : {
350 6 : opt_val = -1e30f;
351 : }
352 :
353 12 : for ( i = 0; i < 1; i++ )
354 : {
355 30 : for ( j = 0; j < 4; j++ )
356 : {
357 24 : if ( j != i )
358 : {
359 90 : for ( k = 0; k < 4; k++ )
360 : {
361 72 : if ( ( k != i ) && ( k != j ) )
362 : {
363 180 : for ( l = 0; l < 4; l++ )
364 : {
365 144 : if ( ( l != i ) && ( l != j ) && ( l != k ) )
366 : {
367 36 : cost = cost_mtx[0 * 4 + i] + cost_mtx[1 * 4 + j] + cost_mtx[2 * 4 + k] + cost_mtx[3 * 4 + l];
368 36 : if ( ( ( maximize == 0 ) && ( opt_val > cost ) ) || ( ( maximize != 0 ) && ( opt_val < cost ) ) )
369 : {
370 9 : opt_val = cost;
371 9 : path[0] = i;
372 9 : path[1] = j;
373 9 : path[2] = k;
374 9 : path[3] = l;
375 : }
376 : }
377 : }
378 : }
379 : }
380 : }
381 : }
382 : }
383 :
384 6 : return;
385 : }
386 :
387 :
388 : /*---------------------------------------------------------------------*
389 : * mat2dquat()
390 : *
391 : * Convert 4D matrix -> double quaternion
392 : *---------------------------------------------------------------------*/
393 :
394 1750 : void mat2dquat(
395 : const float *a,
396 : float *ql,
397 : float *qr )
398 : {
399 : float AM[16], M2[16], tmp_l, tmp_r, temp, ql_max, qr_max;
400 : int16_t i, j, k, i_ql, i_qr;
401 :
402 : /* compute associate matrix */
403 1750 : AM[0 * 4 + 0] = ( a[0 * 4 + 0] + a[1 * 4 + 1] + a[2 * 4 + 2] + a[3 * 4 + 3] ) / 4.0f;
404 1750 : AM[1 * 4 + 0] = ( a[1 * 4 + 0] - a[0 * 4 + 1] + a[3 * 4 + 2] - a[2 * 4 + 3] ) / 4.0f;
405 1750 : AM[2 * 4 + 0] = ( a[2 * 4 + 0] - a[3 * 4 + 1] - a[0 * 4 + 2] + a[1 * 4 + 3] ) / 4.0f;
406 1750 : AM[3 * 4 + 0] = ( a[3 * 4 + 0] + a[2 * 4 + 1] - a[1 * 4 + 2] - a[0 * 4 + 3] ) / 4.0f;
407 1750 : AM[0 * 4 + 1] = ( a[1 * 4 + 0] - a[0 * 4 + 1] - a[3 * 4 + 2] + a[2 * 4 + 3] ) / 4.0f;
408 1750 : AM[1 * 4 + 1] = ( -a[0 * 4 + 0] - a[1 * 4 + 1] + a[2 * 4 + 2] + a[3 * 4 + 3] ) / 4.0f;
409 1750 : AM[2 * 4 + 1] = ( -a[3 * 4 + 0] - a[2 * 4 + 1] - a[1 * 4 + 2] - a[0 * 4 + 3] ) / 4.0f;
410 1750 : AM[3 * 4 + 1] = ( a[2 * 4 + 0] - a[3 * 4 + 1] + a[0 * 4 + 2] - a[1 * 4 + 3] ) / 4.0f;
411 1750 : AM[0 * 4 + 2] = ( a[2 * 4 + 0] + a[3 * 4 + 1] - a[0 * 4 + 2] - a[1 * 4 + 3] ) / 4.0f;
412 1750 : AM[1 * 4 + 2] = ( a[3 * 4 + 0] - a[2 * 4 + 1] - a[1 * 4 + 2] + a[0 * 4 + 3] ) / 4.0f;
413 1750 : AM[2 * 4 + 2] = ( -a[0 * 4 + 0] + a[1 * 4 + 1] - a[2 * 4 + 2] + a[3 * 4 + 3] ) / 4.0f;
414 1750 : AM[3 * 4 + 2] = ( -a[1 * 4 + 0] - a[0 * 4 + 1] - a[3 * 4 + 2] - a[2 * 4 + 3] ) / 4.0f;
415 1750 : AM[0 * 4 + 3] = ( a[3 * 4 + 0] - a[2 * 4 + 1] + a[1 * 4 + 2] - a[0 * 4 + 3] ) / 4.0f;
416 1750 : AM[1 * 4 + 3] = ( -a[2 * 4 + 0] - a[3 * 4 + 1] - a[0 * 4 + 2] - a[1 * 4 + 3] ) / 4.0f;
417 1750 : AM[2 * 4 + 3] = ( a[1 * 4 + 0] + a[0 * 4 + 1] - a[3 * 4 + 2] - a[2 * 4 + 3] ) / 4.0f;
418 1750 : AM[3 * 4 + 3] = ( -a[0 * 4 + 0] + a[1 * 4 + 1] + a[2 * 4 + 2] - a[3 * 4 + 3] ) / 4.0f;
419 :
420 : /* compute squared matrix */
421 29750 : for ( i = 0; i < 16; i++ )
422 : {
423 28000 : M2[i] = AM[i] * AM[i];
424 : }
425 :
426 : /* determine (absolute) left and right terms */
427 1750 : i_ql = 0;
428 1750 : i_qr = 0;
429 1750 : ql_max = -1;
430 1750 : qr_max = -1;
431 :
432 8750 : for ( i = 0; i < 4; i++ )
433 : {
434 7000 : tmp_l = 0.f;
435 7000 : tmp_r = 0.f;
436 35000 : for ( j = 0; j < 4; j++ )
437 : {
438 : /* sum over row */
439 28000 : tmp_l += M2[i * 4 + j];
440 : /* sum over column */
441 28000 : tmp_r += M2[j * 4 + i];
442 : }
443 :
444 7000 : ql[i] = sqrtf( tmp_l );
445 7000 : if ( ql[i] > ql_max )
446 : {
447 1900 : ql_max = ql[i];
448 1900 : i_ql = i;
449 : }
450 :
451 7000 : qr[i] = sqrtf( tmp_r );
452 7000 : if ( qr[i] > qr_max )
453 : {
454 1917 : qr_max = qr[i];
455 1917 : i_qr = i;
456 : }
457 : }
458 :
459 : /* set signs */
460 8750 : for ( k = 0; k < 4; k++ )
461 : {
462 7000 : if ( AM[i_ql * 4 + k] < 0 )
463 : {
464 2227 : qr[k] = -qr[k];
465 : }
466 : }
467 :
468 8750 : for ( k = 0; k < 4; k++ )
469 : {
470 7000 : temp = AM[k * 4 + i_qr] * ( ql[k] * qr[i_qr] );
471 7000 : if ( temp < 0 )
472 : {
473 3436 : ql[k] = -ql[k];
474 : }
475 : }
476 :
477 1750 : return;
478 : }
479 :
480 :
481 : /*---------------------------------------------------------------------*
482 : * dquat2mat()
483 : *
484 : * Convert double quaternion -> 4D matrix
485 : *---------------------------------------------------------------------*/
486 :
487 145960 : void dquat2mat(
488 : const float *ql,
489 : const float *qr,
490 : float *m )
491 : {
492 : float a, b, c, d;
493 : float w, x, y, z;
494 : float aw, ax, ay, az;
495 : float bw, bx, by, bz;
496 : float cw, cx, cy, cz;
497 : float dw, dx, dy, dz;
498 :
499 145960 : a = ql[0];
500 145960 : b = ql[1];
501 145960 : c = ql[2];
502 145960 : d = ql[3];
503 145960 : w = qr[0];
504 145960 : x = qr[1];
505 145960 : y = qr[2];
506 145960 : z = qr[3];
507 145960 : aw = a * w;
508 145960 : ax = a * x;
509 145960 : ay = a * y;
510 145960 : az = a * z;
511 145960 : bw = b * w;
512 145960 : bx = b * x;
513 145960 : by = b * y;
514 145960 : bz = b * z;
515 145960 : cw = c * w;
516 145960 : cx = c * x;
517 145960 : cy = c * y;
518 145960 : cz = c * z;
519 145960 : dw = d * w;
520 145960 : dx = d * x;
521 145960 : dy = d * y;
522 145960 : dz = d * z;
523 145960 : m[0] = aw - bx - cy - dz;
524 145960 : m[1] = -ax - bw + cz - dy;
525 145960 : m[2] = -ay - bz - cw + dx;
526 145960 : m[3] = -az + by - cx - dw;
527 145960 : m[4] = bw + ax - dy + cz;
528 145960 : m[5] = -bx + aw + dz + cy;
529 145960 : m[6] = -by + az - dw - cx;
530 145960 : m[7] = -bz - ay - dx + cw;
531 145960 : m[8] = cw + dx + ay - bz;
532 145960 : m[9] = -cx + dw - az - by;
533 145960 : m[10] = -cy + dz + aw + bx;
534 145960 : m[11] = -cz - dy + ax - bw;
535 145960 : m[12] = dw - cx + by + az;
536 145960 : m[13] = -dx - cw - bz + ay;
537 145960 : m[14] = -dy - cz + bw - ax;
538 145960 : m[15] = -dz + cy + bx + aw;
539 :
540 145960 : return;
541 : }
542 :
543 :
544 : /*---------------------------------------------------------------------*
545 : * quat_shortestpath()
546 : *
547 : * Shortest path verification (prior to quaternion interpolation)
548 : *---------------------------------------------------------------------*/
549 :
550 3649 : void quat_shortestpath(
551 : const float *q00,
552 : float *q01,
553 : const float *q10,
554 : float *q11 )
555 : {
556 : float d0, d1;
557 : int16_t res, i;
558 :
559 3649 : d0 = dotp( q00, q01, IVAS_PCA_INTERP );
560 3649 : d1 = dotp( q10, q11, IVAS_PCA_INTERP );
561 :
562 3649 : res = 0;
563 3649 : if ( ( d0 < 0. ) && ( d1 < 0. ) )
564 : {
565 0 : res = 1;
566 : }
567 : else
568 : {
569 3649 : if ( d0 < 0. )
570 : {
571 0 : if ( ( -d0 ) > d1 )
572 : {
573 0 : res = 1;
574 : }
575 : }
576 : else
577 : {
578 3649 : if ( d1 < 0. )
579 : {
580 0 : if ( ( -d1 ) > d0 )
581 : {
582 0 : res = 1;
583 : }
584 : }
585 : }
586 : }
587 :
588 3649 : if ( res )
589 : {
590 0 : for ( i = 0; i < IVAS_PCA_INTERP; i++ )
591 : {
592 0 : q01[i] = -q01[i];
593 0 : q11[i] = -q11[i];
594 : }
595 : }
596 :
597 3649 : return;
598 : }
599 :
600 :
601 : /*---------------------------------------------------------------------*
602 : * mat_det4()
603 : *
604 : * Compute determinant of 4D matrix - brute-force version
605 : *---------------------------------------------------------------------*/
606 :
607 1750 : float mat_det4(
608 : const float *m )
609 : {
610 : float d;
611 :
612 : #define a11 m[0]
613 : #define a12 m[1]
614 : #define a13 m[2]
615 : #define a14 m[3]
616 : #define a21 m[4]
617 : #define a22 m[5]
618 : #define a23 m[6]
619 : #define a24 m[7]
620 : #define a31 m[8]
621 : #define a32 m[9]
622 : #define a33 m[10]
623 : #define a34 m[11]
624 : #define a41 m[12]
625 : #define a42 m[13]
626 : #define a43 m[14]
627 : #define a44 m[15]
628 :
629 : /* 4! = 24 terms -> complexity = 24x(4 INDIRECT + 3 MULT) + 23 ADD */
630 1750 : d = a11 * a22 * a33 * a44 + a11 * a24 * a32 * a43 + a11 * a23 * a34 * a42 - a11 * a24 * a33 * a42 - a11 * a22 * a34 * a43 -
631 1750 : a11 * a23 * a32 * a44 - a12 * a21 * a33 * a44 - a12 * a23 * a34 * a41 - a12 * a24 * a31 * a43 + a12 * a24 * a33 * a41 +
632 1750 : a12 * a21 * a34 * a43 + a12 * a23 * a31 * a44 + a13 * a21 * a32 * a44 + a13 * a22 * a34 * a41 + a13 * a24 * a31 * a42 -
633 1750 : a13 * a24 * a32 * a41 - a13 * a21 * a34 * a42 - a13 * a22 * a31 * a44 - a14 * a21 * a32 * a43 - a14 * a22 * a33 * a41 -
634 1750 : a14 * a23 * a31 * a42 + a14 * a23 * a32 * a41 + a14 * a21 * a33 * a42 + a14 * a22 * a31 * a43;
635 :
636 1750 : return d;
637 : }
638 :
639 :
640 291920 : static void norm_quat(
641 : float *q )
642 : {
643 : float norm_q;
644 : int16_t i;
645 :
646 291920 : norm_q = dotp( q, q, IVAS_PCA_INTERP );
647 :
648 291920 : norm_q = inv_sqrt( norm_q ); // TODO: possible division by 0
649 :
650 1459600 : for ( i = 0; i < IVAS_PCA_INTERP; i++ )
651 : {
652 1167680 : q[i] *= norm_q;
653 : }
654 :
655 291920 : return;
656 : }
657 :
658 :
659 291920 : static void quat_nlerp_preproc(
660 : const float *q0,
661 : const float *q1,
662 : const float alpha,
663 : float *q_slerp )
664 : {
665 : int16_t i;
666 :
667 1459600 : for ( i = 0; i < IVAS_PCA_INTERP; i++ )
668 : {
669 1167680 : q_slerp[i] = alpha * q0[i] + ( 1.0f - alpha ) * q1[i];
670 : }
671 :
672 291920 : norm_quat( q_slerp );
673 :
674 291920 : return;
675 : }
676 :
677 :
678 3649 : void pca_interp_preproc(
679 : const float *prev_ql,
680 : const float *prev_qr,
681 : const float *ql,
682 : const float *qr,
683 : const int16_t len,
684 : float *ql_interp,
685 : float *qr_interp )
686 : {
687 : float alpha;
688 : int16_t j;
689 :
690 149609 : for ( j = 0; j < len; j++ )
691 : {
692 145960 : alpha = (float) j / (float) ( len - 1 ); // the increment can be updated by simple delta
693 145960 : alpha = 0.5f * ( 1.f - cosf( EVS_PI * alpha ) );
694 145960 : alpha = 1 - alpha;
695 145960 : quat_nlerp_preproc( prev_ql, ql, alpha, &ql_interp[j * IVAS_PCA_INTERP] );
696 145960 : quat_nlerp_preproc( prev_qr, qr, alpha, &qr_interp[j * IVAS_PCA_INTERP] );
697 : }
698 :
699 3649 : return;
700 : }
701 :
702 :
703 36 : static float acos_clip(
704 : float v )
705 : {
706 : float ph;
707 :
708 36 : v = check_bounds( v, -1.0f, 1.0f );
709 :
710 36 : ph = acosf( v );
711 :
712 36 : return ph;
713 : }
714 :
715 :
716 84 : static void sp2cart(
717 : const float ph1,
718 : const float ph2,
719 : const float ph3,
720 : float *q )
721 : {
722 : float s1, s2, s1s2;
723 :
724 84 : s1 = sinf( ph1 );
725 84 : s2 = sinf( ph2 );
726 84 : s1s2 = s1 * s2;
727 84 : q[3] = sinf( ph3 ) * s1s2;
728 84 : q[2] = cosf( ph3 ) * s1s2;
729 84 : q[1] = cosf( ph2 ) * s1;
730 84 : q[0] = cosf( ph1 );
731 :
732 84 : return;
733 : }
734 :
735 60 : static int16_t calc_n2(
736 : const float ph1 )
737 : {
738 : int16_t n2;
739 :
740 60 : n2 = (int16_t) roundf( 90.f * sinf( ph1 ) );
741 :
742 60 : if ( n2 % 2 == 0 )
743 : {
744 19 : n2++;
745 : }
746 :
747 60 : return n2;
748 : }
749 :
750 :
751 84 : static int16_t calc_n3(
752 : const float ph1,
753 : const float ph2 )
754 : {
755 : int16_t n3;
756 :
757 84 : n3 = (int16_t) roundf( 180.f * sinf( ph1 ) * sinf( ph2 ) );
758 :
759 84 : if ( n3 == 0 )
760 : {
761 0 : n3 = 1;
762 : }
763 : else
764 : {
765 84 : if ( ( n3 % 2 ) == 1 )
766 : {
767 72 : n3++;
768 : }
769 : }
770 :
771 84 : return n3;
772 : }
773 :
774 :
775 36 : static void q_ang_2surv(
776 : const float a,
777 : const int16_t N,
778 : float *a_q,
779 : int16_t *index )
780 : {
781 : float d, v;
782 : int16_t temp;
783 :
784 36 : if ( N == 1 )
785 : {
786 0 : a_q[0] = 0.f;
787 0 : a_q[1] = 0.f;
788 0 : index[0] = 0;
789 0 : index[1] = 0;
790 0 : return;
791 : }
792 36 : d = EVS_PI / ( N - 1 );
793 36 : if ( a >= EVS_PI )
794 : {
795 0 : index[0] = N - 1;
796 0 : index[1] = N - 2;
797 : }
798 : else
799 : {
800 36 : if ( a <= 0 )
801 : {
802 0 : index[0] = 0;
803 0 : index[1] = 1;
804 : }
805 : else
806 : {
807 36 : v = a / d;
808 36 : index[0] = (int16_t) floorf( v );
809 36 : index[1] = (int16_t) ceilf( v );
810 :
811 36 : if ( index[0] == index[1] )
812 : {
813 : /*printf("warning: identical indices\n"); */
814 : }
815 : else
816 : {
817 36 : if ( ( v - index[0] ) < ( index[1] - v ) )
818 : {
819 24 : temp = index[1];
820 24 : index[1] = index[0];
821 24 : index[0] = temp;
822 : }
823 : }
824 : }
825 : }
826 36 : a_q[0] = index[0] * d;
827 36 : a_q[1] = index[1] * d;
828 :
829 36 : return;
830 : }
831 :
832 :
833 48 : static void q_ang_circ(
834 : const float a,
835 : const int16_t N,
836 : float *a_q,
837 : int16_t *index )
838 : {
839 : float d;
840 48 : if ( N == 1 )
841 : {
842 0 : *a_q = 0.f;
843 0 : *index = 0;
844 :
845 0 : return;
846 : }
847 :
848 48 : d = PI2 / N;
849 :
850 48 : if ( a >= PI2 )
851 : {
852 0 : *index = N;
853 : }
854 : else
855 : {
856 48 : if ( a <= 0.f )
857 : {
858 0 : *index = 0;
859 : }
860 : else
861 : {
862 48 : *index = (int16_t) roundf( a / d );
863 : }
864 : }
865 :
866 48 : if ( *index == N )
867 : {
868 0 : *index = 0;
869 : }
870 :
871 48 : assert( ( *index >= 0 ) && ( *index <= ( N - 1 ) ) );
872 48 : *a_q = *index * d;
873 :
874 48 : return;
875 : }
876 :
877 :
878 12 : static int16_t sel_q(
879 : const float *q,
880 : const float *q_cand,
881 : const int16_t n )
882 : {
883 : int16_t i, i_min, j;
884 : float d, d_min, temp;
885 : const float *p;
886 :
887 12 : i_min = -1;
888 12 : d_min = 1e30f;
889 12 : p = q_cand;
890 :
891 60 : for ( i = 0; i < n; i++ )
892 : {
893 48 : d = 0.f;
894 240 : for ( j = 0; j < 4; j++ )
895 : {
896 192 : temp = q[j] - p[j];
897 192 : d += temp * temp;
898 : }
899 :
900 48 : if ( d < d_min )
901 : {
902 38 : d_min = d;
903 38 : i_min = i;
904 : }
905 48 : p += 4;
906 : }
907 12 : assert( i_min >= 0 );
908 :
909 12 : return i_min;
910 : }
911 :
912 :
913 960 : static int16_t get_pca_offset_n2(
914 : const int16_t index1 )
915 : {
916 : int16_t index2;
917 :
918 960 : if ( index1 <= IVAS_PCA_N1_EQ )
919 : {
920 960 : index2 = ivas_pca_offset_n2[index1];
921 : }
922 : else
923 : {
924 0 : index2 = ivas_pca_offset_n2[IVAS_PCA_N1 - 1 - index1];
925 : }
926 :
927 960 : return index2;
928 : }
929 :
930 :
931 : /*---------------------------------------------------------------------*
932 : * pca_enc_s3()
933 : *
934 : *
935 : *---------------------------------------------------------------------*/
936 :
937 12 : void pca_enc_s3(
938 : float *q,
939 : int32_t *index )
940 : {
941 : float ph1, ph2, ph3, r, v;
942 : float ph1_q[2], ph2_q[4], ph3_q[4];
943 : int16_t n1, n2[2], n3[4];
944 : int16_t ind1[2], ind2[4], ind3[4], index1, index2, index3;
945 : int16_t i, j;
946 : float q_cand[4 * 4];
947 :
948 12 : n1 = IVAS_PCA_N1;
949 12 : ph1 = acos_clip( q[0] );
950 12 : if ( fabsf( fabsf( q[0] ) - 1.0f ) < IVAS_PCA_QUAT_EPS )
951 : {
952 0 : if ( q[0] > 0 )
953 : {
954 0 : *ph1_q = 0.f;
955 0 : index1 = 0;
956 : }
957 : else
958 : {
959 0 : *ph1_q = EVS_PI;
960 0 : index1 = n1 - 1;
961 : }
962 :
963 0 : sp2cart( *ph1_q, 0.f, 0.f, q );
964 :
965 0 : *index = ivas_pca_offset_index1[index1];
966 :
967 0 : return;
968 : }
969 :
970 12 : q_ang_2surv( ph1, n1, ph1_q, ind1 );
971 :
972 12 : r = sqrtf( q[1] * q[1] + q[2] * q[2] + q[3] * q[3] );
973 :
974 12 : v = q[1] / r;
975 12 : ph2 = acos_clip( v );
976 :
977 12 : if ( fabs( fabs( v ) - 1.0f ) < IVAS_PCA_QUAT_EPS )
978 : {
979 0 : for ( i = 0; i < 2; i++ )
980 : {
981 0 : n2[i] = calc_n2( ph1_q[i] );
982 0 : if ( v > 0 )
983 : {
984 0 : ph2_q[i] = 0;
985 0 : ind2[i] = 0;
986 : }
987 : else
988 : {
989 0 : ph2_q[i] = EVS_PI;
990 0 : ind2[i] = n2[i] - 1;
991 : }
992 :
993 0 : sp2cart( ph1_q[i], ph2_q[i], 0, &q_cand[4 * i] );
994 : }
995 :
996 0 : j = sel_q( q, q_cand, 2 );
997 :
998 0 : mvr2r( q_cand + 4 * j, q, 4 );
999 0 : index1 = ind1[j];
1000 0 : index2 = ind2[j];
1001 :
1002 0 : index2 += get_pca_offset_n2( index1 );
1003 :
1004 0 : *index = ivas_pca_offset_index1[index1] + ivas_pca_offset_index2[index2];
1005 :
1006 0 : return;
1007 : }
1008 :
1009 36 : for ( i = 0; i < 2; i++ )
1010 : {
1011 24 : n2[i] = calc_n2( ph1_q[i] );
1012 24 : q_ang_2surv( ph2, n2[i], ph2_q + 2 * i, ind2 + 2 * i );
1013 : }
1014 12 : r = sqrtf( q[2] * q[2] + q[3] * q[3] );
1015 12 : v = q[2] / r;
1016 12 : ph3 = acos_clip( v );
1017 :
1018 12 : if ( q[3] < 0 )
1019 : {
1020 6 : ph3 = PI2 - ph3;
1021 : }
1022 :
1023 60 : for ( i = 0; i < 4; i++ )
1024 : {
1025 48 : n3[i] = calc_n3( ph1_q[i >> 1], ph2_q[i] );
1026 48 : q_ang_circ( ph3, n3[i], ph3_q + i, ind3 + i );
1027 48 : sp2cart( ph1_q[i >> 1], ph2_q[i], ph3_q[i], q_cand + ( 4 * i ) );
1028 : }
1029 :
1030 12 : j = sel_q( q, q_cand, 4 );
1031 :
1032 12 : mvr2r( q_cand + 4 * j, q, 4 );
1033 :
1034 12 : index1 = ind1[j >> 1];
1035 12 : index2 = ind2[j];
1036 12 : index3 = ind3[j];
1037 12 : index2 += get_pca_offset_n2( index1 );
1038 :
1039 12 : *index = ivas_pca_offset_index1[index1] + ivas_pca_offset_index2[index2] + index3;
1040 :
1041 12 : return;
1042 : }
1043 :
1044 : /*---------------------------------------------------------------------*
1045 : * pca_dec_s3()
1046 : *
1047 : *
1048 : *---------------------------------------------------------------------*/
1049 :
1050 36 : void pca_dec_s3(
1051 : const int32_t index,
1052 : float *q )
1053 : {
1054 : float ph1_q, ph2_q, ph3_q;
1055 : int32_t j;
1056 : int16_t i;
1057 : int16_t n1, n2, n3;
1058 : int16_t index1, index2, index3;
1059 : float d;
1060 :
1061 36 : j = index;
1062 36 : index1 = -1;
1063 645 : for ( i = 0; i < IVAS_PCA_N1; i++ )
1064 : {
1065 645 : if ( j < ivas_pca_offset_index1[i + 1] )
1066 : {
1067 36 : index1 = i;
1068 36 : break;
1069 : }
1070 : }
1071 :
1072 36 : assert( index1 > -1 );
1073 :
1074 36 : n1 = IVAS_PCA_N1;
1075 36 : d = EVS_PI / ( n1 - 1 );
1076 36 : ph1_q = index1 * d;
1077 36 : n2 = calc_n2( ph1_q );
1078 :
1079 36 : j -= ivas_pca_offset_index1[index1];
1080 36 : index2 = -1;
1081 :
1082 912 : for ( i = 0; i < n2; i++ )
1083 : {
1084 912 : if ( j < ivas_pca_offset_index2[i + get_pca_offset_n2( index1 ) + 1] )
1085 : {
1086 36 : index2 = i;
1087 36 : break;
1088 : }
1089 : }
1090 :
1091 36 : assert( index2 > -1 );
1092 :
1093 36 : if ( n2 == 1 )
1094 : {
1095 0 : ph2_q = 0;
1096 : }
1097 : else
1098 : {
1099 36 : d = EVS_PI / ( n2 - 1 );
1100 36 : ph2_q = index2 * d;
1101 : }
1102 :
1103 36 : j -= ivas_pca_offset_index2[index2 + get_pca_offset_n2( index1 )];
1104 :
1105 36 : index3 = (int16_t) j;
1106 :
1107 36 : n3 = calc_n3( ph1_q, ph2_q );
1108 :
1109 36 : if ( n3 == 1 )
1110 : {
1111 0 : ph3_q = 0;
1112 : }
1113 : else
1114 : {
1115 36 : d = PI2 / n3;
1116 36 : ph3_q = d * index3;
1117 : }
1118 36 : sp2cart( ph1_q, ph2_q, ph3_q, q );
1119 :
1120 36 : return;
1121 : }
|
