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_cnst.h"
36 : #include <math.h>
37 : #include <assert.h>
38 : #include "ivas_rom_com.h"
39 : #include "wmc_auto.h"
40 : #include "prot_fx.h"
41 : #include "ivas_prot_fx.h"
42 :
43 : /*---------------------------------------------------------------------*
44 : * eye_matrix()
45 : *
46 : *---------------------------------------------------------------------*/
47 :
48 848080 : static Word16 check_bound_fx( Word32 tmp )
49 : {
50 848080 : IF( GT_32( tmp, MAX16B ) )
51 : {
52 65 : return MAX16B;
53 : }
54 848015 : ELSE IF( LT_32( tmp, MIN16B ) )
55 : {
56 0 : return MIN16B;
57 : }
58 : ELSE
59 : {
60 848015 : return extract_l( tmp );
61 : }
62 : }
63 :
64 607668 : void eye_matrix_fx(
65 : Word16 *mat, // Q
66 : const Word16 n,
67 : const Word16 d // Q
68 : )
69 : {
70 : Word16 i;
71 :
72 10330356 : FOR( i = 0; i < n * n; i++ )
73 : {
74 9722688 : mat[i] = 0;
75 9722688 : move16();
76 : }
77 3038340 : FOR( i = 0; i < n; i++ )
78 : {
79 2430672 : mat[i * n + i] = d; // Q
80 2430672 : move16();
81 : }
82 :
83 607668 : return;
84 : }
85 :
86 17500 : void eye_matrix_fx32(
87 : Word32 *mat, // Q
88 : const Word16 n,
89 : const Word32 d // Q
90 : )
91 : {
92 : Word16 i;
93 :
94 297500 : FOR( i = 0; i < n * n; i++ )
95 : {
96 280000 : mat[i] = 0;
97 280000 : move32();
98 : }
99 87500 : FOR( i = 0; i < n; i++ )
100 : {
101 70000 : mat[i * n + i] = d; // Q
102 70000 : move32();
103 : }
104 :
105 17500 : return;
106 : }
107 : /*---------------------------------------------------------------------*
108 : * cov()
109 : *
110 : * Compute covariance of input channels
111 : *---------------------------------------------------------------------*/
112 :
113 3500 : void cov_subfr_fx(
114 : Word32 **ptr_sig_fx, // Q11
115 : Word64 *r_fx_64, // Q11
116 : const Word16 n_channels,
117 : const Word16 len )
118 : {
119 : Word64 s_fx;
120 : Word16 j, k, l;
121 : Word32 *t_fx, *tt_fx;
122 :
123 : /* Compute diagonal r[k][k] */
124 17500 : FOR( k = 0; k < n_channels; k++ )
125 : {
126 14000 : t_fx = &ptr_sig_fx[k][0];
127 14000 : s_fx = W_shr( W_mult0_32_32( t_fx[0], t_fx[0] ), 11 ); // Q11 + Q11 - Q11 -> Q11
128 5760000 : FOR( j = 1; j < len; j++ )
129 : {
130 5746000 : if ( s_fx != 0 )
131 : {
132 5745771 : t_fx[j] = t_fx[j];
133 5745771 : move32();
134 : }
135 5746000 : s_fx = W_add( s_fx, W_shr( W_mult0_32_32( t_fx[j], t_fx[j] ), 11 ) ); // Q11 + Q11 - Q11 -> Q11
136 : }
137 14000 : r_fx_64[k * n_channels + k] = s_fx; // Q11
138 14000 : move64();
139 : }
140 :
141 : /* Compute off-diagonal r[k][l] = r[l][k] */
142 14000 : FOR( k = 0; k < ( n_channels - 1 ); k++ )
143 : {
144 10500 : t_fx = &ptr_sig_fx[k][0];
145 31500 : FOR( l = k + 1; l < n_channels; l++ )
146 : {
147 21000 : tt_fx = &ptr_sig_fx[l][0];
148 21000 : s_fx = W_shr( W_mult0_32_32( t_fx[0], tt_fx[0] ), 11 ); // Q11 + Q11 - Q11 -> Q11
149 8640000 : FOR( j = 1; j < len; j++ )
150 : {
151 8619000 : s_fx = W_add( s_fx, W_shr( W_mult0_32_32( t_fx[j], tt_fx[j] ), 11 ) );
152 : }
153 21000 : r_fx_64[k * n_channels + l] = s_fx; // Q11
154 21000 : r_fx_64[l * n_channels + k] = s_fx; // Q11
155 21000 : move64();
156 21000 : move64();
157 : }
158 : }
159 :
160 3500 : return;
161 : }
162 :
163 70000 : static void house_refl_fx(
164 : const Word32 *px_fx, // Q: px_q
165 : const Word16 px_q,
166 : const Word16 sizex,
167 : Word32 *pu_fx, // Q: pu_q
168 : Word16 *pu_fx_q,
169 : Word32 *normu_fx, // Q: q_normu
170 : Word16 *q_normu )
171 : {
172 : Word16 i;
173 : Word16 exp, tmp, normu_q;
174 : Word32 L_tmp;
175 :
176 : Word16 pu_e[FOA_CHANNELS];
177 70000 : Copy32( px_fx, pu_fx, sizex );
178 70000 : set16_fx( pu_e, sub( 31, px_q ), sizex );
179 70000 : Word64 L64_sum = 1;
180 70000 : move64();
181 245000 : FOR( Word16 k = 0; k < sizex; k++ )
182 : {
183 175000 : L64_sum = W_mac_32_32( L64_sum, pu_fx[k], pu_fx[k] ); // pu_q
184 : }
185 70000 : Word16 norm = W_norm( L64_sum );
186 70000 : L64_sum = W_shl( L64_sum, norm );
187 70000 : *normu_fx = W_extract_h( L64_sum ); // ener_side_q
188 70000 : normu_q = sub( 31, sub( 31, sub( add( add( shl( px_q, 1 ), 1 ), norm ), 32 ) ) );
189 70000 : move32();
190 :
191 70000 : exp = sub( Q31, normu_q );
192 70000 : ( *normu_fx ) = Sqrt32( *normu_fx, &exp );
193 70000 : move32();
194 70000 : normu_q = sub( Q31, exp );
195 :
196 70000 : IF( ( *normu_fx ) == 0 )
197 : {
198 0 : pu_fx[0] = SQRT2_FIXED; // same q as other elements -> Q30
199 0 : pu_e[0] = 1;
200 0 : move32();
201 0 : move16();
202 : }
203 : ELSE
204 : {
205 70000 : tmp = BASOP_Util_Divide3232_Scale( ONE_IN_Q30, *normu_fx, &exp ); // Q30 + px_q, 1073741824 ->1 in Q30
206 70000 : exp = add( exp, sub( 1, sub( 31, normu_q ) ) );
207 :
208 70000 : Word32 rcp_fx = L_deposit_h( tmp ); // rcp_q
209 70000 : Word16 rcp_q = sub( Q31, exp );
210 :
211 245000 : FOR( i = 0; i < sizex; i++ )
212 : {
213 175000 : norm = norm_l( pu_fx[i] );
214 175000 : L_tmp = L_shl( pu_fx[i], norm );
215 175000 : pu_fx[i] = Mpy_32_32( L_tmp, rcp_fx ); // ( px_q + norm ) + rcp_q - 31 -> exp: 31 + 31 - (px_q + norm + rcp_q)
216 175000 : pu_e[i] = sub( 62, add( add( px_q, norm ), rcp_q ) );
217 175000 : move32();
218 175000 : move16();
219 : }
220 :
221 70000 : IF( pu_fx[0] >= 0 )
222 : {
223 70000 : pu_fx[0] = BASOP_Util_Add_Mant32Exp( pu_fx[0], pu_e[0], ONE_IN_Q30, 1, &pu_e[0] );
224 70000 : ( *normu_fx ) = L_negate( *normu_fx );
225 :
226 70000 : move32();
227 70000 : move32();
228 : }
229 : ELSE
230 : {
231 0 : pu_fx[0] = BASOP_Util_Add_Mant32Exp( pu_fx[0], pu_e[0], -ONE_IN_Q30, 1, &pu_e[0] );
232 0 : move32();
233 : }
234 :
235 70000 : Word16 exp2, exp1 = pu_e[0];
236 70000 : move16();
237 70000 : L_tmp = Sqrt32( L_abs( pu_fx[0] ), &exp1 );
238 70000 : tmp = BASOP_Util_Divide3232_Scale( 2147483647, L_add( L_tmp, EPSILON_FX ), &exp ); // 1 in Q31
239 70000 : exp = add( exp, sub( 0, exp1 ) );
240 70000 : L_tmp = L_deposit_h( tmp );
241 70000 : rcp_fx = BASOP_Util_Add_Mant32Exp( 0, 0, L_tmp, exp, &exp2 );
242 245000 : FOR( i = 0; i < sizex; i++ )
243 : {
244 175000 : pu_fx[i] = Mpy_32_32( pu_fx[i], rcp_fx ); // pu_e[i] + exp2
245 175000 : move32();
246 175000 : pu_e[i] = add( pu_e[i], exp2 );
247 175000 : move32();
248 : }
249 :
250 70000 : Word16 max_e = pu_e[0];
251 70000 : move16();
252 245000 : FOR( i = 0; i < sizex; i++ )
253 : {
254 175000 : IF( GT_16( pu_e[i], max_e ) )
255 : {
256 6685 : max_e = pu_e[i];
257 6685 : move16();
258 : }
259 : }
260 :
261 245000 : FOR( i = 0; i < sizex; i++ )
262 : {
263 175000 : pu_fx[i] = L_shr( pu_fx[i], sub( max_e, pu_e[i] ) ); // max_e
264 175000 : move32();
265 : }
266 70000 : *pu_fx_q = sub( 31, max_e );
267 70000 : *q_normu = normu_q;
268 70000 : move32();
269 70000 : move16();
270 : }
271 :
272 70000 : return;
273 : }
274 :
275 17500 : static void house_qr_fx(
276 : const Word32 *pA_fx, // Q: pA_q
277 : Word16 pA_q,
278 : Word32 *pQ_fx, // Q: Q31
279 : Word32 *pR_fx, // Q: pA_q
280 : const Word16 n )
281 : {
282 17500 : Word16 n_rows = FOA_CHANNELS;
283 17500 : move16();
284 17500 : Word16 pu_fx_q = 0, pR_fx_q, exp2;
285 17500 : move16();
286 : Word16 i, j, k, c, s, r;
287 : Word32 A_fx[FOA_CHANNELS * FOA_CHANNELS];
288 : Word32 U_fx[FOA_CHANNELS * FOA_CHANNELS];
289 : Word16 U_e[FOA_CHANNELS * FOA_CHANNELS];
290 : Word32 pu_fx[FOA_CHANNELS];
291 : Word16 tmp_pu_e[FOA_CHANNELS];
292 : Word16 pv_exp[FOA_CHANNELS];
293 : Word32 pa_fx[FOA_CHANNELS];
294 : Word16 tmp_e;
295 : Word32 pv_fx[FOA_CHANNELS], L_tmp1;
296 :
297 :
298 17500 : Copy32( pA_fx, A_fx, FOA_CHANNELS * FOA_CHANNELS );
299 17500 : set_zero_fx( U_fx, FOA_CHANNELS * FOA_CHANNELS );
300 17500 : set16_fx( U_e, 0, FOA_CHANNELS * FOA_CHANNELS );
301 17500 : set_zero_fx( pR_fx, FOA_CHANNELS * FOA_CHANNELS );
302 87500 : FOR( k = 0; k < n_rows; k++ )
303 : {
304 350000 : FOR( i = 0; i < n_rows; i++ )
305 : {
306 280000 : pa_fx[i] = A_fx[i * n + k]; // pA_q
307 280000 : move32();
308 : }
309 :
310 70000 : house_refl_fx( &pa_fx[k], pA_q, sub( n_rows, k ), pu_fx, &pu_fx_q, &pR_fx[k * n_rows + k], &pR_fx_q );
311 70000 : pR_fx[k * n_rows + k] = L_shr( pR_fx[k * n_rows + k], sub( pR_fx_q, pA_q ) ); // pA_q
312 70000 : move32();
313 :
314 :
315 70000 : U_fx[k * n_rows + k] = pu_fx[0]; // pu_fx_q
316 70000 : U_e[k * n_rows + k] = sub( 31, pu_fx_q ); // pu_fx_q
317 70000 : move32();
318 70000 : move16();
319 175000 : FOR( c = k + 1; c < n_rows; c++ )
320 : {
321 525000 : FOR( i = 0; i < n_rows; i++ )
322 : {
323 420000 : pa_fx[i] = A_fx[i * n + c]; // pA_q
324 420000 : move32();
325 : }
326 :
327 105000 : Word64 L64_sum = 1;
328 105000 : move64();
329 : Word16 norm;
330 455000 : FOR( Word16 l = 0; l < n_rows - k; l++ )
331 : {
332 350000 : L64_sum = W_mac_32_32( L64_sum, pa_fx[k + l], pu_fx[l] ); // pA_q + pu_fx_q
333 : }
334 105000 : norm = W_norm( L64_sum );
335 105000 : L64_sum = W_shl( L64_sum, norm );
336 105000 : pv_fx[c - k - 1] = W_extract_h( L64_sum ); // pv_exp
337 105000 : pv_exp[c - k - 1] = sub( 31, sub( add( add( add( pu_fx_q, pA_q ), 1 ), norm ), 32 ) );
338 105000 : U_fx[c * n_rows + k] = pu_fx[c - k]; // pu_fx_q
339 105000 : U_e[c * n_rows + k] = sub( 31, pu_fx_q ); // pu_fx_q
340 105000 : move32();
341 105000 : move32();
342 105000 : move16();
343 105000 : move16();
344 : }
345 :
346 70000 : i = 0;
347 70000 : move16();
348 245000 : FOR( s = k; s < n_rows; s++ )
349 : {
350 175000 : j = 0;
351 175000 : move16();
352 525000 : FOR( r = k + 1; r < n_rows; r++ )
353 : {
354 :
355 350000 : A_fx[s * n_rows + r] = BASOP_Util_Add_Mant32Exp( A_fx[s * n_rows + r], sub( 31, pA_q ), L_negate( Mpy_32_32( pv_fx[j], pu_fx[i] ) ), add( sub( 31, pu_fx_q ), pv_exp[j] ), &exp2 ); // exp2
356 350000 : A_fx[s * n_rows + r] = L_shr_sat( A_fx[s * n_rows + r], sub( sub( 31, exp2 ), pA_q ) ); // pA_q
357 350000 : move32();
358 350000 : j = add( j, 1 );
359 : }
360 175000 : i = add( i, 1 );
361 : }
362 :
363 175000 : FOR( r = k + 1; r < n_rows; r++ )
364 : {
365 105000 : pR_fx[k * n_rows + r] = A_fx[k * n_rows + r]; // pA_q
366 105000 : move32();
367 : }
368 : }
369 :
370 17500 : eye_matrix_fx32( pQ_fx, FOA_CHANNELS, ONE_IN_Q31 ); // 1 in Q31
371 :
372 87500 : FOR( k = FOA_CHANNELS - 1; k >= 0; k-- )
373 : {
374 350000 : FOR( i = 0; i < n_rows; i++ )
375 : {
376 280000 : pu_fx[i] = U_fx[i * n + k];
377 280000 : move32();
378 280000 : tmp_pu_e[i] = U_e[i * n + k];
379 280000 : move16();
380 : }
381 :
382 245000 : FOR( s = k; s < n_rows; s++ )
383 : {
384 875000 : FOR( i = 0; i < n_rows; i++ )
385 : {
386 700000 : pa_fx[i] = pQ_fx[i * n + s]; // Q31
387 700000 : move32();
388 : }
389 :
390 :
391 175000 : pv_fx[s - k] = dotp_fixed( &pu_fx[k], &pa_fx[k], sub( n_rows, k ) ); // exp: tmp_pu_e[k]
392 175000 : pv_exp[s - k] = tmp_pu_e[k];
393 175000 : move32();
394 175000 : move16();
395 : }
396 245000 : FOR( s = k; s < n_rows; s++ )
397 : {
398 700000 : FOR( r = k; r < n_rows; r++ )
399 : {
400 525000 : L_tmp1 = BASOP_Util_Add_Mant32Exp( pQ_fx[s * n_rows + r], 0, L_negate( Mpy_32_32( pv_fx[r - k], U_fx[s * n_rows + k] ) ), pv_exp[r - k] + U_e[s * n_rows + k], &tmp_e ); // Q31
401 525000 : IF( tmp_e > 0 )
402 : {
403 24969 : pQ_fx[s * n_rows + r] = check_bounds_l( L_tmp1, L_negate( L_shr( MAX_32, tmp_e ) ), L_shr( MAX_32, tmp_e ) ); // Q31
404 24969 : pQ_fx[s * n_rows + r] = L_shl( pQ_fx[s * n_rows + r], tmp_e ); // Q31
405 24969 : move32();
406 24969 : move32();
407 : }
408 : ELSE
409 : {
410 500031 : pQ_fx[s * n_rows + r] = L_shl( L_tmp1, tmp_e ); // Q31
411 500031 : move32();
412 : }
413 : }
414 : }
415 : }
416 :
417 17500 : return;
418 : }
419 :
420 : /*---------------------------------------------------------------------*
421 : * eig_qr()
422 : *
423 : * Compute eigenvalue decomposition by QR method
424 : *---------------------------------------------------------------------*/
425 :
426 1750 : void eig_qr_fx(
427 : const Word32 *A_fx, // A_q
428 : Word16 A_q,
429 : const Word16 num_iter,
430 : Word32 *EV_fx, // Q31
431 : Word32 *Vals_fx, // A_q
432 : const Word16 n )
433 : {
434 : Word16 i;
435 : Word16 breakcnd, iter_num;
436 :
437 : Word32 Ak_fx[FOA_CHANNELS * FOA_CHANNELS], Qk_fx[FOA_CHANNELS * FOA_CHANNELS], Rk_fx[FOA_CHANNELS * FOA_CHANNELS], D_fx[FOA_CHANNELS * FOA_CHANNELS], d_fx;
438 : Word16 d_q, exp;
439 :
440 1750 : assert( n <= FOA_CHANNELS );
441 1750 : breakcnd = 1;
442 1750 : iter_num = 0;
443 1750 : move16();
444 1750 : move16();
445 :
446 : /* check zero matrix */
447 1750 : d_fx = dotp_fixed( A_fx, A_fx, n * n ); // A_q + A_q - Q31 -> d_q
448 1750 : d_q = sub( add( A_q, A_q ), 31 );
449 :
450 1750 : if ( d_fx != 0 )
451 : {
452 1750 : breakcnd = 0;
453 1750 : move16();
454 : }
455 : /* duplicate */
456 1750 : Copy32( A_fx, Ak_fx, n * n ); // A_q
457 :
458 : /* identity */
459 1750 : set_zero_fx( EV_fx, n * n );
460 :
461 8750 : FOR( i = 0; i < n; i++ )
462 : {
463 7000 : EV_fx[i * n + i] = ONE_IN_Q31; // 1 in Q31
464 7000 : move32();
465 : }
466 1750 : set_zero_fx( Vals_fx, n );
467 :
468 19250 : WHILE( breakcnd == 0 )
469 : {
470 17500 : iter_num = add( iter_num, 1 );
471 :
472 17500 : Copy32( Ak_fx, D_fx, n * n );
473 :
474 : /* set diagonal */
475 :
476 87500 : FOR( i = 0; i < n; i++ )
477 : {
478 70000 : D_fx[i * n + i] = Vals_fx[i]; // A_q
479 70000 : move32();
480 : }
481 :
482 : /* stop condition */
483 17500 : d_fx = dotp_fixed_guarded( D_fx, D_fx, n * n ); // ( A_q + A_q - 31 ) - find_guarded_bits_fx( n * n )
484 17500 : d_q = sub( sub( add( A_q, A_q ), 31 ), find_guarded_bits_fx( n * n ) );
485 17500 : exp = sub( 31, d_q );
486 17500 : d_fx = Sqrt32( d_fx, &exp );
487 :
488 17500 : test();
489 17500 : if ( ( d_fx < 0 ) || GE_16( iter_num, num_iter ) )
490 : {
491 1750 : breakcnd = 1;
492 1750 : move16();
493 : }
494 :
495 17500 : house_qr_fx( Ak_fx, A_q, Qk_fx, Rk_fx, n );
496 :
497 17500 : matrix_product_fx( Qk_fx, n, n, 0, Rk_fx, n, n, 0, Ak_fx ); // A_q + Q31 - Q31 -> A_q
498 17500 : matrix_product_mant_exp_fx( Qk_fx, 0, n, n, 0, EV_fx, 0, n, n, 0, D_fx, &exp ); // Q31 + Q31 - Q31 -> Q31
499 17500 : Scale_sig32( D_fx, imult1616( n, n ), exp ); // Q31
500 :
501 17500 : Copy32( D_fx, EV_fx, imult1616( n, n ) ); // Q31
502 : }
503 :
504 :
505 : /* get diagonal */
506 8750 : FOR( i = 0; i < n; i++ )
507 : {
508 7000 : Vals_fx[i] = Ak_fx[i * n + i];
509 7000 : move32();
510 : }
511 1750 : return;
512 : }
513 :
514 : /*---------------------------------------------------------------------*
515 : * exhst_4x4()
516 : *
517 : * Find optimal permutation of eigenvectors
518 : *---------------------------------------------------------------------*/
519 :
520 0 : void exhst_4x4_fx(
521 : Word16 *cost_mtx, // Q
522 : Word16 *path,
523 : const Word16 maximize )
524 : {
525 : Word16 i, j, k, l;
526 : Word16 opt_val, cost;
527 :
528 0 : IF( maximize == 0 )
529 : {
530 0 : opt_val = MAX_16;
531 0 : move16();
532 : }
533 : ELSE
534 : {
535 0 : opt_val = -MAX_16;
536 0 : move16();
537 : }
538 :
539 0 : FOR( i = 0; i < 1; i++ )
540 : {
541 0 : FOR( j = 0; j < 4; j++ )
542 : {
543 0 : IF( NE_16( j, i ) )
544 : {
545 0 : FOR( k = 0; k < 4; k++ )
546 : {
547 0 : test();
548 0 : IF( NE_16( k, i ) && NE_16( k, j ) )
549 : {
550 0 : FOR( l = 0; l < 4; l++ )
551 : {
552 0 : test();
553 0 : test();
554 0 : IF( NE_16( l, i ) && NE_16( l, j ) && NE_16( l, k ) )
555 : {
556 0 : cost = add( add( cost_mtx[0 * 4 + i], cost_mtx[1 * 4 + j] ), add( cost_mtx[2 * 4 + k], cost_mtx[3 * 4 + l] ) );
557 0 : test();
558 0 : test();
559 0 : test();
560 0 : IF( ( ( maximize == 0 ) && GT_16( opt_val, cost ) ) || ( ( maximize != 0 ) && LT_16( opt_val, cost ) ) )
561 : {
562 0 : opt_val = cost;
563 0 : path[0] = i;
564 0 : path[1] = j;
565 0 : path[2] = k;
566 0 : path[3] = l;
567 0 : move16();
568 0 : move16();
569 0 : move16();
570 0 : move16();
571 0 : move16();
572 : }
573 : }
574 : }
575 : }
576 : }
577 : }
578 : }
579 : }
580 :
581 0 : return;
582 : }
583 :
584 :
585 : /*---------------------------------------------------------------------*
586 : * mat2dquat()
587 : *
588 : * Convert 4D matrix -> double quaternion
589 : *---------------------------------------------------------------------*/
590 :
591 1750 : void mat2dquat_fx(
592 : const Word16 *a, // Q15
593 : Word16 *ql, // Q15
594 : Word16 *qr // Q15
595 : )
596 : {
597 : Word16 AM[16], tmp_l, tmp_r, temp, ql_max, qr_max;
598 : Word16 i, j, k, i_ql, i_qr, tmp_l_e, tmp_r_e;
599 : Word32 M2_fx[16];
600 :
601 :
602 : /* compute associate matrix */
603 :
604 : /* Q: Q15*/
605 1750 : AM[0 * 4 + 0] = check_bound_fx( L_shr( L_add( L_add( L_add( a[0 * 4 + 0], a[1 * 4 + 1] ), a[2 * 4 + 2] ), a[3 * 4 + 3] ), 2 ) );
606 1750 : AM[1 * 4 + 0] = check_bound_fx( L_shr( L_sub( L_add( L_sub( a[1 * 4 + 0], a[0 * 4 + 1] ), a[3 * 4 + 2] ), a[2 * 4 + 3] ), 2 ) );
607 1750 : AM[2 * 4 + 0] = check_bound_fx( L_shr( L_add( L_sub( L_sub( a[2 * 4 + 0], a[3 * 4 + 1] ), a[0 * 4 + 2] ), a[1 * 4 + 3] ), 2 ) );
608 1750 : AM[3 * 4 + 0] = check_bound_fx( L_shr( L_sub( L_sub( L_add( a[3 * 4 + 0], a[2 * 4 + 1] ), a[1 * 4 + 2] ), a[0 * 4 + 3] ), 2 ) );
609 1750 : AM[0 * 4 + 1] = check_bound_fx( L_shr( L_add( L_sub( L_sub( a[1 * 4 + 0], a[0 * 4 + 1] ), a[3 * 4 + 2] ), a[2 * 4 + 3] ), 2 ) );
610 1750 : AM[1 * 4 + 1] = check_bound_fx( L_shr( L_add( L_add( L_sub( -a[0 * 4 + 0], a[1 * 4 + 1] ), a[2 * 4 + 2] ), a[3 * 4 + 3] ), 2 ) );
611 1750 : AM[2 * 4 + 1] = check_bound_fx( L_shr( L_sub( L_sub( L_sub( -a[3 * 4 + 0], a[2 * 4 + 1] ), a[1 * 4 + 2] ), a[0 * 4 + 3] ), 2 ) );
612 1750 : AM[3 * 4 + 1] = check_bound_fx( L_shr( L_sub( L_add( L_sub( a[2 * 4 + 0], a[3 * 4 + 1] ), a[0 * 4 + 2] ), a[1 * 4 + 3] ), 2 ) );
613 1750 : AM[0 * 4 + 2] = check_bound_fx( L_shr( L_sub( L_sub( L_add( a[2 * 4 + 0], a[3 * 4 + 1] ), a[0 * 4 + 2] ), a[1 * 4 + 3] ), 2 ) );
614 1750 : AM[1 * 4 + 2] = check_bound_fx( L_shr( L_add( L_sub( L_sub( a[3 * 4 + 0], a[2 * 4 + 1] ), a[1 * 4 + 2] ), a[0 * 4 + 3] ), 2 ) );
615 1750 : AM[2 * 4 + 2] = check_bound_fx( L_shr( L_add( L_sub( L_add( -a[0 * 4 + 0], a[1 * 4 + 1] ), a[2 * 4 + 2] ), a[3 * 4 + 3] ), 2 ) );
616 1750 : AM[3 * 4 + 2] = check_bound_fx( L_shr( L_sub( L_sub( L_sub( -a[1 * 4 + 0], a[0 * 4 + 1] ), a[3 * 4 + 2] ), a[2 * 4 + 3] ), 2 ) );
617 1750 : AM[0 * 4 + 3] = check_bound_fx( L_shr( L_sub( L_add( L_sub( a[3 * 4 + 0], a[2 * 4 + 1] ), a[1 * 4 + 2] ), a[0 * 4 + 3] ), 2 ) );
618 1750 : AM[1 * 4 + 3] = check_bound_fx( L_shr( L_sub( L_sub( L_sub( -a[2 * 4 + 0], a[3 * 4 + 1] ), a[0 * 4 + 2] ), a[1 * 4 + 3] ), 2 ) );
619 1750 : AM[2 * 4 + 3] = check_bound_fx( L_shr( L_sub( L_sub( L_add( a[1 * 4 + 0], a[0 * 4 + 1] ), a[3 * 4 + 2] ), a[2 * 4 + 3] ), 2 ) );
620 1750 : AM[3 * 4 + 3] = check_bound_fx( L_shr( L_sub( L_add( L_add( -a[0 * 4 + 0], a[1 * 4 + 1] ), a[2 * 4 + 2] ), a[3 * 4 + 3] ), 2 ) );
621 :
622 1750 : move16();
623 1750 : move16();
624 1750 : move16();
625 1750 : move16();
626 1750 : move16();
627 1750 : move16();
628 1750 : move16();
629 1750 : move16();
630 1750 : move16();
631 1750 : move16();
632 1750 : move16();
633 1750 : move16();
634 1750 : move16();
635 1750 : move16();
636 1750 : move16();
637 1750 : move16();
638 1750 : move16();
639 :
640 : /* compute squared matrix */
641 29750 : FOR( i = 0; i < 16; i++ )
642 : {
643 28000 : M2_fx[i] = L_shr( L_mult0( AM[i], AM[i] ), 15 ); // Q15
644 28000 : move16();
645 : }
646 :
647 : /* determine (absolute) left and right terms */
648 1750 : i_ql = 0;
649 1750 : i_qr = 0;
650 1750 : ql_max = MIN_16; // Q15
651 1750 : qr_max = MIN_16; // Q15
652 1750 : move16();
653 1750 : move16();
654 1750 : move16();
655 1750 : move16();
656 :
657 8750 : FOR( i = 0; i < 4; i++ )
658 : {
659 7000 : tmp_l = 0;
660 7000 : tmp_r = 0;
661 7000 : move16();
662 7000 : move16();
663 7000 : tmp_r_e = 0;
664 7000 : tmp_l_e = 0;
665 7000 : move16();
666 7000 : move16();
667 35000 : FOR( j = 0; j < 4; j++ )
668 : {
669 : /* sum over row */
670 28000 : tmp_l_e = BASOP_Util_Add_MantExp( tmp_l, tmp_l_e, check_bound_fx( M2_fx[i * 4 + j] ), 0, &tmp_l );
671 : /* sum over column */
672 28000 : tmp_r_e = BASOP_Util_Add_MantExp( tmp_r, tmp_r_e, check_bound_fx( M2_fx[j * 4 + i] ), 0, &tmp_r );
673 : }
674 :
675 :
676 7000 : ql[i] = Sqrt16( tmp_l, &tmp_l_e ); // tmp_l_e
677 7000 : ql[i] = check_bound_fx( L_shl( ql[i], tmp_l_e ) ); // Q15
678 7000 : move16();
679 7000 : move16();
680 7000 : IF( GT_16( ql[i], ql_max ) )
681 : {
682 3626 : ql_max = ql[i];
683 3626 : i_ql = i;
684 3626 : move16();
685 3626 : move16();
686 : }
687 :
688 7000 : qr[i] = Sqrt16( tmp_r, &tmp_r_e ); // tmp_r_e
689 7000 : qr[i] = check_bound_fx( L_shl( qr[i], tmp_r_e ) ); // Q15
690 7000 : move16();
691 7000 : move16();
692 7000 : IF( GT_16( qr[i], qr_max ) )
693 : {
694 3623 : qr_max = qr[i]; // Q15
695 3623 : i_qr = i;
696 3623 : move16();
697 3623 : move16();
698 : }
699 : }
700 :
701 : /* set signs */
702 8750 : FOR( k = 0; k < 4; k++ )
703 : {
704 7000 : IF( AM[i_ql * 4 + k] < 0 )
705 : {
706 3681 : qr[k] = negate( qr[k] ); // Q15
707 3681 : move16();
708 : }
709 : }
710 :
711 8750 : FOR( k = 0; k < 4; k++ )
712 : {
713 7000 : temp = mult( AM[k * 4 + i_qr], mult( ql[k], qr[i_qr] ) ); // Q15
714 7000 : IF( temp < 0 )
715 : {
716 2497 : ql[k] = negate( ql[k] ); // Q15
717 2497 : move16();
718 : }
719 : }
720 :
721 1750 : return;
722 : }
723 :
724 :
725 : /*---------------------------------------------------------------------*
726 : * dquat2mat()
727 : *
728 : * Convert double quaternion -> 4D matrix
729 : *---------------------------------------------------------------------*/
730 :
731 46880 : void dquat2mat_fx(
732 : const Word16 *ql, // Q15
733 : const Word16 *qr, // Q15
734 : Word16 *m // Q15
735 : )
736 : {
737 : Word16 a, b, c, d;
738 : Word16 w, x, y, z;
739 : Word16 aw, ax, ay, az;
740 : Word16 bw, bx, by, bz;
741 : Word16 cw, cx, cy, cz;
742 : Word16 dw, dx, dy, dz;
743 :
744 46880 : a = ql[0];
745 46880 : move16();
746 46880 : b = ql[1];
747 46880 : move16();
748 46880 : c = ql[2];
749 46880 : move16();
750 46880 : d = ql[3];
751 46880 : move16();
752 46880 : w = qr[0];
753 46880 : move16();
754 46880 : x = qr[1];
755 46880 : move16();
756 46880 : y = qr[2];
757 46880 : move16();
758 46880 : z = qr[3];
759 46880 : move16();
760 46880 : aw = mult( a, w ); // Ql + Qr - 15
761 46880 : ax = mult( a, x );
762 46880 : ay = mult( a, y );
763 46880 : az = mult( a, z );
764 46880 : bw = mult( b, w );
765 46880 : bx = mult( b, x );
766 46880 : by = mult( b, y );
767 46880 : bz = mult( b, z );
768 46880 : cw = mult( c, w );
769 46880 : cx = mult( c, x );
770 46880 : cy = mult( c, y );
771 46880 : cz = mult( c, z );
772 46880 : dw = mult( d, w );
773 46880 : dx = mult( d, x );
774 46880 : dy = mult( d, y );
775 46880 : dz = mult( d, z );
776 : // Q15
777 46880 : m[0] = check_bound_fx( L_sub( L_sub( aw, bx ), L_add( cy, dz ) ) );
778 46880 : move16();
779 46880 : m[1] = check_bound_fx( L_sub( L_sub( cz, dy ), L_add( ax, bw ) ) );
780 46880 : move16();
781 46880 : m[2] = check_bound_fx( L_add( L_sub( L_sub( negate( ay ), bz ), cw ), dx ) );
782 46880 : move16();
783 46880 : m[3] = check_bound_fx( L_sub( L_sub( by, az ), L_add( cx, dw ) ) );
784 46880 : move16();
785 46880 : m[4] = check_bound_fx( L_add( L_sub( L_add( bw, ax ), dy ), cz ) );
786 46880 : move16();
787 46880 : m[5] = check_bound_fx( L_add( L_add( L_add( negate( bx ), aw ), dz ), cy ) );
788 46880 : move16();
789 46880 : m[6] = check_bound_fx( L_sub( L_sub( L_add( negate( by ), az ), dw ), cx ) );
790 46880 : move16();
791 46880 : m[7] = check_bound_fx( L_add( L_sub( L_sub( negate( bz ), ay ), dx ), cw ) );
792 46880 : move16();
793 46880 : m[8] = check_bound_fx( L_sub( L_add( L_add( cw, dx ), ay ), bz ) );
794 46880 : move16();
795 46880 : m[9] = check_bound_fx( L_sub( L_sub( L_add( negate( cx ), dw ), az ), by ) );
796 46880 : move16();
797 46880 : m[10] = check_bound_fx( L_add( L_add( L_add( negate( cy ), dz ), aw ), bx ) );
798 46880 : move16();
799 46880 : m[11] = check_bound_fx( L_sub( L_add( L_sub( negate( cz ), dy ), ax ), bw ) );
800 46880 : move16();
801 46880 : m[12] = check_bound_fx( L_add( L_add( L_sub( dw, cx ), by ), az ) );
802 46880 : move16();
803 46880 : m[13] = check_bound_fx( L_add( L_sub( L_sub( negate( dx ), cw ), bz ), ay ) );
804 46880 : move16();
805 46880 : m[14] = check_bound_fx( L_sub( L_add( L_sub( negate( dy ), cz ), bw ), ax ) );
806 46880 : move16();
807 46880 : m[15] = check_bound_fx( L_add( L_add( L_add( negate( dz ), cy ), bx ), aw ) );
808 46880 : move16();
809 :
810 46880 : return;
811 : }
812 : /*---------------------------------------------------------------------*
813 : * quat_shortestpath()
814 : *
815 : * Shortest path verification (prior to quaternion interpolation)
816 : *---------------------------------------------------------------------*/
817 :
818 : // Not tested
819 1172 : void quat_shortestpath_fx(
820 : const Word16 *q00,
821 : Word16 *q01,
822 : const Word16 *q10,
823 : Word16 *q11 )
824 : {
825 : Word32 d0, d1;
826 : Word16 res, i;
827 : Word16 exp1, exp2;
828 1172 : exp1 = 0;
829 1172 : move16();
830 1172 : exp2 = 0;
831 1172 : move16();
832 :
833 1172 : d0 = dotp_fx( q00, q01, IVAS_PCA_INTERP, &exp1 );
834 1172 : d1 = dotp_fx( q10, q11, IVAS_PCA_INTERP, &exp2 );
835 :
836 1172 : res = 0;
837 1172 : move16();
838 1172 : test();
839 1172 : IF( d0 < 0 && d1 < 0 )
840 : {
841 0 : res = 1;
842 0 : move16();
843 : }
844 : ELSE
845 : {
846 1172 : IF( d0 < 0 )
847 : {
848 0 : if ( GT_32( L_negate( d0 ), d1 ) )
849 : {
850 0 : res = 1;
851 0 : move16();
852 : }
853 : }
854 : ELSE
855 : {
856 1172 : IF( d1 < 0 )
857 : {
858 0 : if ( GT_32( L_negate( d1 ), d0 ) )
859 : {
860 0 : res = 1;
861 0 : move16();
862 : }
863 : }
864 : }
865 : }
866 :
867 1172 : IF( res )
868 : {
869 0 : FOR( i = 0; i < IVAS_PCA_INTERP; i++ )
870 : {
871 0 : q01[i] = negate( q01[i] );
872 0 : move16();
873 0 : q11[i] = negate( q11[i] );
874 0 : move16();
875 : }
876 : }
877 :
878 1172 : return;
879 : }
880 : /*---------------------------------------------------------------------*
881 : * mat_det4()
882 : *
883 : * Compute determinant of 4D matrix - brute-force version
884 : *---------------------------------------------------------------------*/
885 :
886 1750 : Word16 mat_det4_fx(
887 : const Word16 *m // Q15
888 : )
889 : {
890 : Word16 d;
891 :
892 : #define a11 m[0]
893 : #define a12 m[1]
894 : #define a13 m[2]
895 : #define a14 m[3]
896 : #define a21 m[4]
897 : #define a22 m[5]
898 : #define a23 m[6]
899 : #define a24 m[7]
900 : #define a31 m[8]
901 : #define a32 m[9]
902 : #define a33 m[10]
903 : #define a34 m[11]
904 : #define a41 m[12]
905 : #define a42 m[13]
906 : #define a43 m[14]
907 : #define a44 m[15]
908 :
909 : /* 4! = 24 terms -> complexity = 24x(4 INDIRECT + 3 MULT) + 23 ADD */
910 1750 : d = add( add( add( sub( sub( sub( sub( sub( sub( add( add( add( add( add( add( sub( sub( sub( sub( sub( sub( add( add( mult( mult( a11, a22 ), mult( a33, a44 ) ), mult( mult( a11, a24 ), mult( a32, a43 ) ) ), mult( mult( a11, a23 ), mult( a34, a42 ) ) ), mult( mult( a11, a24 ), mult( a33, a42 ) ) ), mult( mult( a11, a22 ), mult( a34, a43 ) ) ),
911 1750 : mult( mult( a11, a23 ), mult( a32, a44 ) ) ),
912 1750 : mult( mult( a12, a21 ), mult( a33, a44 ) ) ),
913 1750 : mult( mult( a12, a23 ), mult( a34, a41 ) ) ),
914 1750 : mult( mult( a12, a24 ), mult( a31, a43 ) ) ),
915 1750 : mult( mult( a12, a24 ), mult( a33, a41 ) ) ),
916 1750 : mult( mult( a12, a21 ), mult( a34, a43 ) ) ),
917 1750 : mult( mult( a12, a23 ), mult( a31, a44 ) ) ),
918 1750 : mult( mult( a13, a21 ), mult( a32, a44 ) ) ),
919 1750 : mult( mult( a13, a22 ), mult( a34, a41 ) ) ),
920 1750 : mult( mult( a13, a24 ), mult( a31, a42 ) ) ),
921 1750 : mult( mult( a13, a24 ), mult( a32, a41 ) ) ),
922 1750 : mult( mult( a13, a21 ), mult( a34, a42 ) ) ),
923 1750 : mult( mult( a13, a22 ), mult( a31, a44 ) ) ),
924 1750 : mult( mult( a14, a21 ), mult( a32, a43 ) ) ),
925 1750 : mult( mult( a14, a22 ), mult( a33, a41 ) ) ),
926 1750 : mult( mult( a14, a23 ), mult( a31, a42 ) ) ),
927 1750 : mult( mult( a14, a23 ), mult( a32, a41 ) ) ),
928 1750 : mult( mult( a14, a21 ), mult( a33, a42 ) ) ),
929 1750 : mult( mult( a14, a22 ), mult( a31, a43 ) ) );
930 :
931 1750 : return d; // Q15
932 : }
933 :
934 93760 : static Word32 dotp16_fixed_guarded_fx( const Word16 x[], /* i : vector x[] Qx */
935 : const Word16 y[], /* i : vector y[] Qy */
936 : const Word16 n /* i : vector length */ )
937 : {
938 : Word16 i;
939 : Word32 suma;
940 93760 : Word16 guarded_bits = find_guarded_bits_fx( n );
941 93760 : suma = L_shr( L_mult( x[0], y[0] ), guarded_bits ); // Qx + Qy - guarded_bits
942 :
943 375040 : FOR( i = 1; i < n; i++ )
944 : {
945 281280 : suma = L_add( suma, L_shr( L_mult( x[i], y[i] ), guarded_bits ) ); // Qx + Qy - guarded_bits
946 : }
947 :
948 93760 : return suma;
949 : }
950 :
951 93760 : static void norm_quat_fx(
952 : Word16 *q // Q15
953 : )
954 : {
955 : Word32 norm_q;
956 : Word16 i, exp1;
957 93760 : exp1 = 0;
958 93760 : move16();
959 93760 : norm_q = dotp16_fixed_guarded_fx( q, q, IVAS_PCA_INTERP ); // (Q15 + Q15 - Q15) - exp1
960 93760 : exp1 = find_guarded_bits_fx( IVAS_PCA_INTERP );
961 93760 : norm_q = ISqrt32( norm_q, &exp1 ); /*Q(31 - exp)*/
962 :
963 468800 : FOR( i = 0; i < IVAS_PCA_INTERP; i++ )
964 : {
965 375040 : q[i] = round_fx( L_shl( Mpy_32_16_1( norm_q, q[i] ), exp1 ) ); /* Q(15) */
966 375040 : move16();
967 : }
968 :
969 93760 : return;
970 : }
971 :
972 93760 : static void quat_nlerp_preproc_fx(
973 : const Word16 *q0, // Q15
974 : const Word16 *q1, // Q15
975 : const Word16 alpha, // Q15
976 : Word16 *q_slerp // Q15
977 : )
978 : {
979 : Word16 i;
980 : Word16 tmp1, tmp2;
981 :
982 468800 : FOR( i = 0; i < IVAS_PCA_INTERP; i++ )
983 : {
984 375040 : tmp1 = mult( sub( MAX_16, alpha ), q1[i] );
985 375040 : tmp2 = mult_r( alpha, q0[i] );
986 375040 : test();
987 375040 : if ( EQ_16( alpha, q0[i] ) && EQ_16( alpha, MAX_16 ) )
988 : {
989 2332 : tmp2 = MAX_16;
990 2332 : move16();
991 : }
992 375040 : q_slerp[i] = add( tmp1, tmp2 ); // Q15
993 375040 : move16();
994 : }
995 :
996 93760 : norm_quat_fx( q_slerp );
997 :
998 93760 : return;
999 : }
1000 :
1001 1172 : void pca_interp_preproc_fx(
1002 : const Word16 *prev_ql, // Q15
1003 : const Word16 *prev_qr, // Q15
1004 : const Word16 *ql, // Q15
1005 : const Word16 *qr, // Q15
1006 : const Word16 len,
1007 : Word16 *ql_interp, // Q15
1008 : Word16 *qr_interp // Q15
1009 : )
1010 : {
1011 : Word16 alpha;
1012 : Word16 j;
1013 : Word16 tmp, tmp2, tmp3, tmp_e;
1014 48052 : FOR( j = 0; j < len; j++ )
1015 : {
1016 46880 : tmp = sub( len, 1 );
1017 46880 : IF( j == 0 )
1018 : {
1019 1172 : alpha = 0;
1020 1172 : move16();
1021 : }
1022 : ELSE
1023 : {
1024 45708 : alpha = BASOP_Util_Divide1616_Scale( j, tmp, &tmp_e ); // the increment can be updated by simple delta
1025 45708 : alpha = shl_sat( alpha, tmp_e ); /* Q15 */
1026 : }
1027 46880 : tmp2 = mult( EVS_PI_FX, alpha ); /* Q13 */
1028 46880 : tmp3 = getCosWord16( tmp2 ); /* Q14 */
1029 46880 : alpha = sub_sat( ONE_IN_Q14, tmp3 ); /* Q15 */
1030 46880 : alpha = sub( MAX_16, alpha ); // Q15
1031 46880 : quat_nlerp_preproc_fx( prev_ql, ql, alpha, &ql_interp[j * IVAS_PCA_INTERP] );
1032 46880 : quat_nlerp_preproc_fx( prev_qr, qr, alpha, &qr_interp[j * IVAS_PCA_INTERP] );
1033 : }
1034 :
1035 1172 : return;
1036 : }
1037 :
1038 0 : static Word16 acos_clip_fx(
1039 : Word16 v_fx // Q15
1040 : )
1041 : {
1042 : Word16 ph_fx;
1043 :
1044 0 : v_fx = check_bounds_s_fx( v_fx, -32767, 32767 ); // Q15
1045 :
1046 : Word32 tmp1_fx, tmp2_fx;
1047 : Word16 tmp1_e, tmp2_e;
1048 0 : tmp1_fx = L_sub( ONE_IN_Q31, L_deposit_h( mult( v_fx, v_fx ) ) ); // Q31
1049 0 : tmp1_e = 0;
1050 0 : tmp2_e = 0;
1051 0 : move16();
1052 0 : move16();
1053 0 : tmp1_fx = Sqrt32( tmp1_fx, &tmp1_e ); // Q31 - tmp1_e
1054 0 : tmp2_fx = L_deposit_h( v_fx ); // Q31
1055 :
1056 0 : ph_fx = BASOP_util_atan2( tmp1_fx, tmp2_fx, sub( tmp1_e, tmp2_e ) ); // Q13
1057 :
1058 :
1059 0 : return ph_fx; // Q13
1060 : }
1061 :
1062 12 : static void sp2cart_fx(
1063 : const Word16 ph1, // Q12
1064 : const Word16 ph2, // Q12
1065 : const Word16 ph3, // Q12
1066 : Word16 *q // Q15
1067 : )
1068 : {
1069 : Word16 s1, s2, s1s2;
1070 : Word16 sin_ph3, cos_ph3;
1071 :
1072 12 : sin_ph3 = cos_ph3 = ph3; // Q12
1073 12 : move16();
1074 12 : move16();
1075 :
1076 12 : IF( GT_16( ph3, 12868 /* PI in Q12 */ ) )
1077 : {
1078 6 : sin_ph3 = sub( 12868, ph3 ); /* sin(x) = sin(PI - x) */
1079 6 : cos_ph3 = sub( 25736, ph3 ); /* cos(x) = cos(2*PI - x) */
1080 : }
1081 12 : sin_ph3 = shl( sin_ph3, 1 ); /* Q12 -> Q13 */
1082 12 : cos_ph3 = shl( cos_ph3, 1 ); /* Q12 -> Q13 */
1083 :
1084 12 : s1 = getSinWord16( ph1 ); /* Q15 */
1085 12 : s2 = getSinWord16( ph2 ); /* Q15 */
1086 12 : s1s2 = mult( s1, s2 ); /* Q15 */
1087 12 : q[3] = mult( getSinWord16( sin_ph3 ), s1s2 ); /* Q15 */
1088 12 : move16();
1089 12 : q[2] = shl_sat( mult( getCosWord16( cos_ph3 ), s1s2 ), 1 ); /* Q15 */
1090 12 : move16();
1091 12 : q[1] = shl_sat( mult( getCosWord16( ph2 ), s1 ), 1 ); /* Q15 */
1092 12 : move16();
1093 12 : q[0] = shl_sat( getCosWord16( ph1 ), 1 ); /* Q15 */
1094 12 : move16();
1095 :
1096 12 : return;
1097 : }
1098 :
1099 0 : static Word16 calc_n2_fx(
1100 : const Word16 ph1 // Q15
1101 : )
1102 : {
1103 : Word16 n2;
1104 :
1105 0 : n2 = extract_l( L_shr( L_mult0( 90, getSinWord16( ph1 ) ), 15 ) ); // Q15 + Q0 - Q15 -> Q0
1106 :
1107 0 : if ( n2 % 2 == 0 )
1108 : {
1109 0 : n2 = add( n2, 1 );
1110 : }
1111 :
1112 0 : return n2; // Q0
1113 : }
1114 :
1115 :
1116 12 : static Word16 calc_n3_fx(
1117 : const Word16 ph1,
1118 : const Word16 ph2 )
1119 : {
1120 : Word16 n3;
1121 12 : Word16 temp1 = mult( getSinWord16( ph2 ), getSinWord16( ph1 ) ); /* Q15 */
1122 12 : n3 = round_fx( L_mult( temp1, 23040 /* 180.0f in Q7 */ ) ); /* Q15 + Q7 + Q1 - Q16 -> Q7*/
1123 :
1124 12 : n3 = shr( n3, 7 );
1125 :
1126 12 : IF( n3 == 0 )
1127 : {
1128 0 : n3 = 1;
1129 0 : move16();
1130 : }
1131 : ELSE
1132 : {
1133 12 : IF( EQ_16( s_and( n3, 1 ), 1 ) )
1134 : {
1135 10 : n3 = add( n3, 1 );
1136 : }
1137 : }
1138 :
1139 12 : return n3;
1140 : }
1141 :
1142 0 : static void q_ang_2surv_fx(
1143 : const Word16 a_fx, // Q13
1144 : const Word16 N,
1145 : Word16 *a_q_fx, // Q15
1146 : Word16 *index )
1147 : {
1148 : Word16 d_fx, v_fx, d_e, v_e;
1149 : Word16 temp;
1150 :
1151 0 : IF( EQ_16( N, 1 ) )
1152 : {
1153 0 : index[0] = 0;
1154 0 : index[1] = 0;
1155 :
1156 0 : a_q_fx[0] = 0;
1157 0 : a_q_fx[1] = 0;
1158 0 : move16();
1159 0 : move16();
1160 0 : move16();
1161 0 : move16();
1162 0 : return;
1163 : }
1164 :
1165 0 : d_fx = BASOP_Util_Divide1616_Scale( EVS_PI_FX, sub( N, 1 ), &d_e );
1166 0 : d_e = add( d_e, ( 2 - 15 ) );
1167 0 : IF( GE_16( a_fx, EVS_PI_FX ) )
1168 : {
1169 0 : index[0] = sub( N, 1 );
1170 0 : index[1] = sub( N, 2 );
1171 0 : move16();
1172 0 : move16();
1173 : }
1174 : ELSE
1175 : {
1176 0 : IF( a_fx <= 0 )
1177 : {
1178 0 : index[0] = 0;
1179 0 : index[1] = 1;
1180 0 : move16();
1181 0 : move16();
1182 : }
1183 : ELSE
1184 : {
1185 0 : v_fx = BASOP_Util_Divide1616_Scale( a_fx, d_fx, &v_e );
1186 0 : v_e = add( v_e, sub( 2, d_e ) );
1187 :
1188 0 : index[0] = shr( v_fx, sub( Q15, v_e ) );
1189 0 : index[1] = add( shr( v_fx, sub( Q15, v_e ) ), 1 );
1190 0 : move16();
1191 0 : move16();
1192 :
1193 0 : IF( EQ_16( index[0], index[1] ) )
1194 : {
1195 : /*printf("warning: identical indices\n"); */
1196 : }
1197 : ELSE
1198 : {
1199 0 : IF( LT_16( sub( shr( v_fx, sub( Q15, v_e ) ), index[0] ), sub( index[1], shr( v_fx, sub( Q15, v_e ) ) ) ) )
1200 : {
1201 0 : temp = index[1];
1202 0 : index[1] = index[0];
1203 0 : index[0] = temp;
1204 0 : move16();
1205 0 : move16();
1206 0 : move16();
1207 : }
1208 : }
1209 : }
1210 : }
1211 :
1212 0 : a_q_fx[0] = extract_l( L_shr( L_mult0( index[0], d_fx ), 2 - d_e ) ); // Q13
1213 0 : a_q_fx[1] = extract_l( L_shr( L_mult0( index[1], d_fx ), 2 - d_e ) ); // Q13
1214 0 : move16();
1215 0 : move16();
1216 :
1217 0 : return;
1218 : }
1219 :
1220 0 : static void q_ang_circ_fx(
1221 : const Word16 a_fx, // Q13
1222 : const Word16 N,
1223 : Word16 *a_q_fx, // Q12
1224 : Word16 *index )
1225 : {
1226 : Word16 d_fx, d_e, scale;
1227 0 : IF( EQ_16( N, 1 ) )
1228 : {
1229 0 : *a_q_fx = 0;
1230 0 : *index = 0;
1231 0 : move16();
1232 0 : move16();
1233 0 : return;
1234 : }
1235 :
1236 0 : d_fx = BASOP_Util_Divide1616_Scale( EVS_PI_FX, N, &d_e ); // Q14
1237 0 : d_e = add( d_e, ( 3 - 15 ) );
1238 :
1239 0 : IF( GE_16( a_fx, EVS_PI_FX ) )
1240 : {
1241 0 : *index = N;
1242 0 : move16();
1243 : }
1244 : ELSE
1245 : {
1246 0 : IF( a_fx <= 0 )
1247 : {
1248 0 : *index = 0;
1249 0 : move16();
1250 : }
1251 : ELSE
1252 : {
1253 0 : *index = BASOP_Util_Divide1616_Scale( a_fx, d_fx, &scale );
1254 0 : scale = add( scale, sub( 3, d_e ) );
1255 0 : *index = shr( *index, sub( 15, scale ) );
1256 0 : move16();
1257 0 : move16();
1258 : }
1259 : }
1260 :
1261 0 : if ( EQ_16( *index, N ) )
1262 : {
1263 0 : *index = 0;
1264 0 : move16();
1265 : }
1266 :
1267 0 : assert( ( *index >= 0 ) && ( *index <= ( N - 1 ) ) );
1268 0 : d_fx = shl( d_fx, sub( 12, sub( 15, d_e ) ) ); // Q12
1269 0 : *a_q_fx = extract_l( L_mult0( *index, d_fx ) ); // Q12
1270 0 : move16();
1271 :
1272 0 : return;
1273 : }
1274 :
1275 0 : static Word16 sel_q_fx(
1276 : const Word16 *q, // Q12
1277 : const Word16 *q_cand, // Q12
1278 : const Word16 n )
1279 : {
1280 : Word16 i, i_min, j, temp;
1281 : Word32 d, d_min;
1282 : const Word16 *p;
1283 :
1284 0 : i_min = -1;
1285 0 : move16();
1286 0 : d_min = MAX_32;
1287 0 : move32();
1288 0 : p = q_cand;
1289 :
1290 0 : FOR( i = 0; i < n; i++ )
1291 : {
1292 0 : d = 0;
1293 0 : move16();
1294 0 : FOR( j = 0; j < 4; j++ )
1295 : {
1296 0 : temp = sub( q[j], p[j] );
1297 0 : d = L_add( d, L_mult0( temp, temp ) );
1298 : }
1299 :
1300 0 : IF( LT_32( d, d_min ) )
1301 : {
1302 0 : d_min = d;
1303 0 : i_min = i;
1304 0 : move16();
1305 0 : move16();
1306 : }
1307 0 : p += 4;
1308 : }
1309 0 : assert( i_min >= 0 );
1310 :
1311 0 : return i_min;
1312 : }
1313 :
1314 316 : static Word16 get_pca_offset_n2_fx(
1315 : const Word16 index1 )
1316 : {
1317 : Word16 index2;
1318 316 : IF( LE_16( index1, IVAS_PCA_N1_EQ ) )
1319 : {
1320 316 : index2 = ivas_pca_offset_n2[index1];
1321 316 : move16();
1322 : }
1323 : ELSE
1324 : {
1325 0 : index2 = ivas_pca_offset_n2[IVAS_PCA_N1 - 1 - index1];
1326 0 : move16();
1327 : }
1328 :
1329 316 : return index2;
1330 : }
1331 :
1332 : /*---------------------------------------------------------------------*
1333 : * pca_enc_s3()
1334 : *
1335 : *
1336 : *---------------------------------------------------------------------*/
1337 :
1338 0 : void pca_enc_s3_fx(
1339 : Word16 *q_fx, // Q15
1340 : Word32 *index )
1341 : {
1342 : Word16 n1, n2[2], n3[4];
1343 : Word16 ind1[2], ind2[4], ind3[4], index1, index2, index3;
1344 : Word16 i, j;
1345 : Word16 ph1_fx, ph2_fx, ph3_fx, r_fx, v_fx, tmp, r_e;
1346 : Word16 ph1_q_fx[2], ph2_q_fx[4], ph3_q_fx[4], v_e;
1347 : Word16 q_cand_fx[4 * 4];
1348 :
1349 0 : n1 = IVAS_PCA_N1;
1350 0 : move16();
1351 0 : ph1_fx = acos_clip_fx( q_fx[0] ); // Q13
1352 :
1353 0 : IF( LT_16( abs_s( sub( abs_s( q_fx[0] ), 32767 ) ), IVAS_PCA_QUAT_EPS_FX ) )
1354 : {
1355 0 : IF( q_fx[0] > 0 )
1356 : {
1357 0 : index1 = 0;
1358 0 : *ph1_q_fx = 0;
1359 0 : move16();
1360 0 : move16();
1361 : }
1362 : ELSE
1363 : {
1364 0 : index1 = sub( n1, 1 );
1365 0 : *ph1_q_fx = EVS_PI_FX; // Q13
1366 0 : move16();
1367 0 : move16();
1368 : }
1369 :
1370 0 : sp2cart_fx( *ph1_q_fx, 0, 0, q_fx );
1371 :
1372 0 : *index = ivas_pca_offset_index1[index1];
1373 0 : move32();
1374 :
1375 0 : return;
1376 : }
1377 :
1378 0 : q_ang_2surv_fx( ph1_fx, n1, ph1_q_fx, ind1 );
1379 :
1380 0 : tmp = add( add( mult( q_fx[1], q_fx[1] ), mult( q_fx[2], q_fx[2] ) ), mult( q_fx[3], q_fx[3] ) ); // Q15 + Q15 - Q15 -> Q15
1381 0 : r_e = 0;
1382 0 : move16();
1383 0 : r_fx = Sqrt16( tmp, &r_e );
1384 0 : v_fx = BASOP_Util_Divide1616_Scale( q_fx[1], r_fx, &v_e );
1385 0 : v_e = sub( v_e, sub( 0, r_e ) );
1386 0 : v_fx = shr( v_fx, sub( 0, v_e ) );
1387 0 : ph2_fx = acos_clip_fx( v_fx ); // Q13
1388 :
1389 0 : IF( LT_16( sub( abs_s( v_fx ), MAX_16 ), IVAS_PCA_QUAT_EPS_FX ) )
1390 : {
1391 0 : FOR( i = 0; i < 2; i++ )
1392 : {
1393 0 : IF( v_fx > 0 )
1394 : {
1395 0 : ph2_q_fx[i] = 0;
1396 0 : ind2[i] = 0;
1397 0 : move16();
1398 0 : move16();
1399 : }
1400 : ELSE
1401 : {
1402 0 : ph2_q_fx[i] = EVS_PI_FX;
1403 0 : ind2[i] = sub( n2[i], 1 );
1404 0 : move16();
1405 0 : move16();
1406 : }
1407 :
1408 0 : sp2cart_fx( ph1_q_fx[i], ph2_q_fx[i], 0, &q_cand_fx[4 * i] );
1409 : }
1410 :
1411 0 : j = sel_q_fx( q_fx, q_cand_fx, 2 );
1412 :
1413 0 : Copy( q_cand_fx + 4 * j, q_fx, 4 );
1414 0 : index1 = ind1[j];
1415 0 : index2 = ind2[j];
1416 0 : move16();
1417 0 : move16();
1418 :
1419 0 : index2 = add( index2, get_pca_offset_n2_fx( index1 ) );
1420 :
1421 0 : *index = L_add( ivas_pca_offset_index1[index1], ivas_pca_offset_index2[index2] );
1422 0 : move32();
1423 :
1424 0 : return;
1425 : }
1426 :
1427 0 : FOR( i = 0; i < 2; i++ )
1428 : {
1429 0 : n2[i] = calc_n2_fx( ph1_q_fx[i] );
1430 0 : move16();
1431 0 : q_ang_2surv_fx( ph2_fx, n2[i], ph2_q_fx + 2 * i, ind2 + 2 * i );
1432 : }
1433 :
1434 0 : r_fx = Sqrt16( add( mult( q_fx[2], q_fx[2] ), mult( q_fx[3], q_fx[3] ) ), &r_e );
1435 :
1436 :
1437 0 : v_fx = BASOP_Util_Divide1616_Scale( q_fx[2], r_fx, &v_e );
1438 0 : v_e = add( v_e, sub( 0, r_e ) );
1439 0 : v_fx = shr( v_fx, sub( 0, v_e ) ); // Q15
1440 0 : ph3_fx = acos_clip_fx( v_fx ); // Q13
1441 :
1442 0 : IF( q_fx[3] < 0 )
1443 : {
1444 0 : ph3_fx = sub( EVS_PI_FX, shr( ph3_fx, 1 ) ); // Q12
1445 : }
1446 :
1447 0 : FOR( i = 0; i < 4; i++ )
1448 : {
1449 0 : n3[i] = calc_n3_fx( ph1_q_fx[i >> 1], ph2_q_fx[i] );
1450 0 : move16();
1451 0 : q_ang_circ_fx( ph3_fx, n3[i], ph3_q_fx + i, ind3 + i );
1452 0 : sp2cart_fx( ph1_q_fx[i >> 1], ph2_q_fx[i], ph3_q_fx[i], q_cand_fx + ( 4 * i ) );
1453 : }
1454 :
1455 0 : j = sel_q_fx( q_fx, q_cand_fx, 4 );
1456 :
1457 0 : Copy( q_cand_fx + 4 * j, q_fx, 4 );
1458 :
1459 0 : index1 = ind1[j >> 1];
1460 0 : index2 = ind2[j];
1461 0 : index3 = ind3[j];
1462 0 : move16();
1463 0 : move16();
1464 0 : move16();
1465 0 : index2 = add( index2, get_pca_offset_n2_fx( index1 ) );
1466 :
1467 0 : *index = L_add( L_add( ivas_pca_offset_index1[index1], ivas_pca_offset_index2[index2] ), index3 );
1468 0 : move32();
1469 :
1470 0 : return;
1471 : }
1472 :
1473 : /*---------------------------------------------------------------------*
1474 : * pca_dec_s3()
1475 : *
1476 : *
1477 : *---------------------------------------------------------------------*/
1478 :
1479 :
1480 : // Not tested
1481 12 : void pca_dec_s3_fx(
1482 : const Word32 index,
1483 : Word16 *q_fx )
1484 : {
1485 : Word16 ph1_q_fx, ph2_q_fx, ph3_q_fx, num_fx;
1486 :
1487 : Word32 j;
1488 : Word16 i;
1489 : Word16 n2, n3;
1490 :
1491 : Word16 index1, index2, index3;
1492 : Word16 d_fx, exp;
1493 :
1494 12 : j = index;
1495 12 : move16();
1496 12 : index1 = -1;
1497 12 : move16();
1498 215 : FOR( i = 0; i < IVAS_PCA_N1; i++ )
1499 : {
1500 215 : IF( LT_32( j, ivas_pca_offset_index1[i + 1] ) )
1501 : {
1502 12 : index1 = i;
1503 12 : move16();
1504 12 : BREAK;
1505 : }
1506 : }
1507 :
1508 12 : assert( index1 > -1 );
1509 :
1510 12 : ph1_q_fx = ph1_q_n2_tbl[index1][0]; // Q13
1511 12 : move16();
1512 12 : n2 = ph1_q_n2_tbl[index1][1]; // Q0
1513 12 : move16();
1514 :
1515 12 : j = L_sub( j, ivas_pca_offset_index1[index1] );
1516 12 : index2 = -1;
1517 12 : move16();
1518 304 : FOR( i = 0; i < n2; i++ )
1519 : {
1520 304 : IF( LT_32( j, L_deposit_l( ivas_pca_offset_index2[add( add( i, get_pca_offset_n2_fx( index1 ) ), 1 )] ) ) )
1521 : {
1522 12 : index2 = i;
1523 12 : move16();
1524 12 : BREAK;
1525 : }
1526 : }
1527 :
1528 12 : assert( index2 > -1 );
1529 :
1530 12 : IF( EQ_16( n2, 1 ) )
1531 : {
1532 0 : ph2_q_fx = 0;
1533 0 : move16();
1534 : }
1535 : ELSE
1536 : {
1537 :
1538 12 : num_fx = 12868; // EVS_PI in Q12
1539 12 : move16();
1540 :
1541 12 : d_fx = BASOP_Util_Divide1616_Scale( num_fx, sub( n2, 1 ), &exp ); /* Q12 */
1542 12 : exp = add( 3 - 15, exp );
1543 12 : d_fx = shl( d_fx, sub( exp, 3 ) ); /* Q12 */
1544 :
1545 12 : ph2_q_fx = i_mult( index2, d_fx ); // Q12
1546 12 : ph2_q_fx = shl( ph2_q_fx, 1 ); /* Q12 -> Q13 */
1547 : }
1548 :
1549 12 : j = L_sub( j, ivas_pca_offset_index2[add( index2, get_pca_offset_n2_fx( index1 ) )] );
1550 :
1551 12 : index3 = extract_l( j );
1552 :
1553 12 : n3 = calc_n3_fx( ph1_q_fx, ph2_q_fx );
1554 :
1555 12 : IF( EQ_16( n3, 1 ) )
1556 : {
1557 0 : ph3_q_fx = 0;
1558 0 : move16();
1559 : }
1560 : ELSE
1561 : {
1562 12 : num_fx = 25736; /* PI2 in Q12 */
1563 12 : move16();
1564 12 : d_fx = BASOP_Util_Divide1616_Scale( num_fx, n3, &exp ); /* Q12 */
1565 12 : exp = add( 3 - 15, exp );
1566 12 : d_fx = shl( d_fx, sub( exp, 3 ) ); /* Q12 */
1567 12 : ph3_q_fx = i_mult( index3, d_fx ); /* Q12 */
1568 : }
1569 12 : sp2cart_fx( ph1_q_fx, ph2_q_fx, ph3_q_fx, q_fx );
1570 :
1571 12 : return;
1572 : }
|
