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 : /*====================================================================================
34 : EVS Codec 3GPP TS26.443 Nov 04, 2021. Version 12.14.0 / 13.10.0 / 14.6.0 / 15.4.0 / 16.3.0
35 : ====================================================================================*/
36 :
37 : #include <stdint.h>
38 : #include "options.h"
39 : #ifdef DEBUGGING
40 : #include "debug.h"
41 : #endif
42 : #include <math.h>
43 : #include "prot.h"
44 : #include "wmc_auto.h"
45 :
46 : /*------------------------------------------------------------------*
47 : * own_random()
48 : *
49 : * Random generator
50 : *------------------------------------------------------------------*/
51 :
52 : /*! r: output random value */
53 12452524638 : int16_t own_random(
54 : int16_t *seed /* i/o: random seed */
55 : )
56 : {
57 12452524638 : *seed = (int16_t) ( *seed * 31821L + 13849L );
58 :
59 12452524638 : return ( *seed );
60 : }
61 :
62 : /*---------------------------------------------------------------------
63 : * sign()
64 : *
65 : *---------------------------------------------------------------------*/
66 :
67 : /*! r: sign of x (+1/-1) */
68 24233414 : float sign(
69 : const float x /* i : input value of x */
70 : )
71 : {
72 24233414 : if ( x < 0.0f )
73 : {
74 10252887 : return -1.0f;
75 : }
76 : else
77 : {
78 13980527 : return 1.0f;
79 : }
80 : }
81 :
82 : /*---------------------------------------------------------------------
83 : * log2_f()
84 : *
85 : *---------------------------------------------------------------------*/
86 :
87 : /*! r: logarithm2 of x */
88 46437835 : float log2_f(
89 : const float x /* i : input value of x */
90 : )
91 : {
92 46437835 : return (float) ( log( x ) / log( 2.0f ) );
93 : }
94 :
95 8284228 : int16_t norm_ul( uint32_t UL_var1 )
96 : {
97 : int16_t var_out;
98 :
99 8284228 : if ( UL_var1 == 0 )
100 : {
101 0 : var_out = 0;
102 : }
103 : else
104 : {
105 120670785 : for ( var_out = 0; UL_var1 < (uint32_t) 0x80000000U; var_out++ )
106 : {
107 112386557 : UL_var1 <<= 1;
108 : }
109 : }
110 : BASOP_CHECK();
111 :
112 8284228 : return ( var_out );
113 : }
114 :
115 :
116 : /*---------------------------------------------------------------------
117 : * sum_s()
118 : * sum_l()
119 : * sum_f()
120 : *
121 : *---------------------------------------------------------------------*/
122 :
123 : /*! r: sum of all vector elements */
124 12128385 : int16_t sum_s(
125 : const int16_t *vec, /* i : input vector */
126 : const int16_t lvec /* i : length of input vector */
127 : )
128 : {
129 : int16_t i;
130 : int16_t tmp;
131 :
132 12128385 : tmp = 0;
133 108821007 : for ( i = 0; i < lvec; i++ )
134 : {
135 96692622 : tmp += vec[i];
136 : }
137 :
138 12128385 : return tmp;
139 : }
140 :
141 : #ifdef DEBUGGING
142 : /*! r: sum of all vector elements */
143 : int32_t sum_l(
144 : const int32_t *vec, /* i : input vector */
145 : const int16_t lvec /* i : length of input vector */
146 : )
147 : {
148 : int16_t i;
149 : int32_t tmpL;
150 :
151 : tmpL = 0;
152 : for ( i = 0; i < lvec; i++ )
153 : {
154 : tmpL += vec[i];
155 : }
156 :
157 : return tmpL;
158 : }
159 :
160 : #endif
161 : /*! r: sum of all vector elements */
162 72625213 : float sum_f(
163 : const float *vec, /* i : input vector */
164 : const int16_t lvec /* i : length of input vector */
165 : )
166 : {
167 : int16_t i;
168 : float tmp;
169 :
170 72625213 : tmp = 0.0f;
171 3599116226 : for ( i = 0; i < lvec; i++ )
172 : {
173 3526491013 : tmp += vec[i];
174 : }
175 :
176 72625213 : return tmp;
177 : }
178 :
179 : /*----------------------------------------------------------------------
180 : * sum2_f()
181 : *
182 : *---------------------------------------------------------------------*/
183 :
184 : /*! r: sum of all squared vector elements */
185 287922612 : float sum2_f(
186 : const float *vec, /* i : input vector */
187 : const int16_t lvec /* i : length of input vector */
188 : )
189 : {
190 : int16_t i;
191 : float tmp;
192 :
193 287922612 : tmp = 0.0f;
194 24627115094 : for ( i = 0; i < lvec; i++ )
195 : {
196 24339192482 : tmp += vec[i] * vec[i];
197 : }
198 :
199 287922612 : return tmp;
200 : }
201 :
202 : /*-------------------------------------------------------------------*
203 : * set_c()
204 : * set_s()
205 : * set_f()
206 : * set_l()
207 : * set_d()
208 : *
209 : * Set the vector elements to a value
210 : *-------------------------------------------------------------------*/
211 :
212 8870285 : void set_c(
213 : int8_t y[], /* i/o: Vector to set */
214 : const int8_t a, /* i : Value to set the vector to */
215 : const int32_t N /* i : Length of the vector */
216 : )
217 : {
218 : int16_t i;
219 :
220 6064197890 : for ( i = 0; i < N; i++ )
221 : {
222 6055327605 : y[i] = a;
223 : }
224 :
225 8870285 : return;
226 : }
227 :
228 :
229 379247005 : void set_s(
230 : int16_t y[], /* i/o: Vector to set */
231 : const int16_t a, /* i : Value to set the vector to */
232 : const int16_t N /* i : Length of the vector */
233 : )
234 : {
235 : int16_t i;
236 :
237 26947218308 : for ( i = 0; i < N; i++ )
238 : {
239 26567971303 : y[i] = a;
240 : }
241 :
242 379247005 : return;
243 : }
244 :
245 :
246 2013002 : void set_l(
247 : int32_t y[], /* i/o: Vector to set */
248 : const int32_t a, /* i : Value to set the vector to */
249 : const int16_t N /* i : Length of the vector */
250 : )
251 : {
252 : int16_t i;
253 :
254 44629638 : for ( i = 0; i < N; i++ )
255 : {
256 42616636 : y[i] = a;
257 : }
258 :
259 2013002 : return;
260 : }
261 :
262 4566198712 : void set_f(
263 : float y[], /* i/o: Vector to set */
264 : const float a, /* i : Value to set the vector to */
265 : const int16_t N /* i : Lenght of the vector */
266 : )
267 : {
268 : int16_t i;
269 :
270 >40863*10^7 : for ( i = 0; i < N; i++ )
271 : {
272 >40406*10^7 : y[i] = a;
273 : }
274 :
275 4566198712 : return;
276 : }
277 :
278 :
279 : /*---------------------------------------------------------------------*
280 : * set_zero()
281 : *
282 : * Set a vector vec[] of dimension lvec to zero
283 : *---------------------------------------------------------------------*/
284 :
285 6508137211 : void set_zero(
286 : float *vec, /* o : input vector */
287 : const int16_t lvec /* i : length of the vector */
288 : )
289 : {
290 : int16_t i;
291 :
292 >23536*10^7 : for ( i = 0; i < lvec; i++ )
293 : {
294 >22885*10^7 : *vec++ = 0.0f;
295 : }
296 :
297 6508137211 : return;
298 : }
299 :
300 :
301 : /*---------------------------------------------------------------------*
302 : * mvr2r()
303 : * mvs2s()
304 : * mvr2s()
305 : * mvs2r()
306 : * mvr2d()
307 : * mvd2r()
308 : *
309 : * Transfer the contents of vector x[] to vector y[]
310 : *---------------------------------------------------------------------*/
311 :
312 5812063437 : void mvr2r(
313 : const float x[], /* i : input vector */
314 : float y[], /* o : output vector */
315 : const int16_t n /* i : vector size */
316 : )
317 : {
318 : int16_t i;
319 :
320 5812063437 : if ( n <= 0 )
321 : {
322 : /* cannot transfer vectors with size 0 */
323 52746589 : return;
324 : }
325 :
326 5759316848 : if ( y < x )
327 : {
328 >67037*10^7 : for ( i = 0; i < n; i++ )
329 : {
330 >66755*10^7 : y[i] = x[i];
331 : }
332 : }
333 : else
334 : {
335 >61475*10^7 : for ( i = n - 1; i >= 0; i-- )
336 : {
337 >61181*10^7 : y[i] = x[i];
338 : }
339 : }
340 :
341 5759316848 : return;
342 : }
343 :
344 215993814 : void mvs2s(
345 : const int16_t x[], /* i : input vector */
346 : int16_t y[], /* o : output vector */
347 : const int16_t n /* i : vector size */
348 : )
349 : {
350 : int16_t i;
351 :
352 215993814 : if ( n <= 0 )
353 : {
354 : /* cannot transfer vectors with size 0 */
355 16546976 : return;
356 : }
357 :
358 199446838 : if ( y < x )
359 : {
360 586760276 : for ( i = 0; i < n; i++ )
361 : {
362 518265775 : y[i] = x[i];
363 : }
364 : }
365 : else
366 : {
367 2313119079 : for ( i = n - 1; i >= 0; i-- )
368 : {
369 2182166742 : y[i] = x[i];
370 : }
371 : }
372 :
373 199446838 : return;
374 : }
375 :
376 : #ifdef DEBUGGING
377 : /*! r: number of overload samples */
378 : uint32_t check_clipping(
379 : const float x[], /* i : input vector */
380 : const int16_t n, /* i : vector size */
381 : float *maxOverload, /* i/o: max overload value */
382 : float *minOverload /* i/o: max overload value */
383 : )
384 : {
385 : int16_t i;
386 : float temp;
387 : uint32_t noClipping = 0;
388 :
389 : for ( i = 0; i < n; i++ )
390 : {
391 : temp = x[i];
392 :
393 : if ( temp >= ( MAX16B_FLT + 0.5f ) )
394 : {
395 : noClipping++;
396 : if ( temp > *maxOverload )
397 : {
398 : *maxOverload = temp;
399 : }
400 : }
401 : else if ( temp <= ( MIN16B_FLT - 0.5f ) )
402 : {
403 : noClipping++;
404 : if ( temp < *minOverload )
405 : {
406 : *minOverload = temp;
407 : }
408 : }
409 : }
410 :
411 : return noClipping;
412 : }
413 :
414 : #endif
415 : /*! r: number of clipped samples */
416 37774101 : uint32_t mvr2s(
417 : const float x[], /* i : input vector */
418 : int16_t y[], /* o : output vector */
419 : const int16_t n /* i : vector size */
420 : )
421 : {
422 : int16_t i;
423 : float temp;
424 37774101 : uint32_t noClipping = 0;
425 :
426 37774101 : if ( n <= 0 )
427 : {
428 : /* cannot transfer vectors with size 0 */
429 1830 : return 0;
430 : }
431 :
432 37772271 : if ( (void *) y <= (const void *) x )
433 : {
434 29000164 : for ( i = 0; i < n; i++ )
435 : {
436 28967040 : temp = x[i];
437 28967040 : temp = (float) floor( temp + 0.5f );
438 :
439 28967040 : if ( temp > MAX16B_FLT )
440 : {
441 32 : temp = MAX16B_FLT;
442 32 : noClipping++;
443 : }
444 28967008 : else if ( temp < MIN16B_FLT )
445 : {
446 12 : temp = MIN16B_FLT;
447 12 : noClipping++;
448 : }
449 :
450 28967040 : y[i] = (int16_t) temp;
451 : }
452 : }
453 : else
454 : {
455 19773362598 : for ( i = n - 1; i >= 0; i-- )
456 : {
457 19735623451 : temp = x[i];
458 19735623451 : temp = (float) floor( temp + 0.5f );
459 :
460 19735623451 : if ( temp > MAX16B_FLT )
461 : {
462 18712 : temp = MAX16B_FLT;
463 18712 : noClipping++;
464 : }
465 19735604739 : else if ( temp < MIN16B_FLT )
466 : {
467 8408 : temp = MIN16B_FLT;
468 8408 : noClipping++;
469 : }
470 :
471 19735623451 : y[i] = (int16_t) temp;
472 : }
473 : }
474 :
475 37772271 : return noClipping;
476 : }
477 :
478 2541730 : void mvs2r(
479 : const int16_t x[], /* i : input vector */
480 : float y[], /* o : output vector */
481 : const int16_t n /* i : vector size */
482 : )
483 : {
484 : int16_t i;
485 :
486 2541730 : if ( n <= 0 )
487 : {
488 : /* cannot transfer vectors with size 0 */
489 0 : return;
490 : }
491 :
492 2541730 : if ( (void *) y < (const void *) x )
493 : {
494 18839881 : for ( i = 0; i < n; i++ )
495 : {
496 16298151 : y[i] = (float) x[i];
497 : }
498 : }
499 : else
500 : {
501 0 : for ( i = n - 1; i >= 0; i-- )
502 : {
503 0 : y[i] = (float) x[i];
504 : }
505 : }
506 :
507 2541730 : return;
508 : }
509 :
510 :
511 134286 : void mvl2l(
512 : const int32_t x[], /* i : input vector */
513 : int32_t y[], /* o : output vector */
514 : const int16_t n /* i : vector size */
515 : )
516 : {
517 : int16_t i;
518 :
519 134286 : if ( n <= 0 )
520 : {
521 : /* no need to transfer vectors with size 0 */
522 0 : return;
523 : }
524 :
525 134286 : if ( y < x )
526 : {
527 952461 : for ( i = 0; i < n; i++ )
528 : {
529 873719 : y[i] = x[i];
530 : }
531 : }
532 : else
533 : {
534 722072 : for ( i = n - 1; i >= 0; i-- )
535 : {
536 666528 : y[i] = x[i];
537 : }
538 : }
539 :
540 134286 : return;
541 : }
542 :
543 :
544 : /*---------------------------------------------------------------------*
545 : * maximum()
546 : *
547 : * Find index and value of the maximum in a vector
548 : *---------------------------------------------------------------------*/
549 :
550 : /*! r: index of the maximum value in the input vector */
551 150712323 : int16_t maximum(
552 : const float *vec, /* i : input vector */
553 : const int16_t lvec, /* i : length of input vector */
554 : float *max_val /* o : maximum value in the input vector */
555 : )
556 : {
557 : int16_t j, ind;
558 : float tmp;
559 :
560 150712323 : ind = 0;
561 150712323 : tmp = vec[0];
562 :
563 2405614731 : for ( j = 1; j < lvec; j++ )
564 : {
565 2254902408 : if ( vec[j] > tmp )
566 : {
567 411213277 : ind = j;
568 411213277 : tmp = vec[j];
569 : }
570 : }
571 :
572 150712323 : if ( max_val != NULL )
573 : {
574 32892240 : *max_val = tmp;
575 : }
576 :
577 150712323 : return ind;
578 : }
579 :
580 :
581 : /*! r: index of the maximum value in the input vector */
582 308047 : int16_t maximum_s(
583 : const int16_t *vec, /* i : input vector */
584 : const int16_t lvec, /* i : length of input vector */
585 : int16_t *max /* o : maximum value in the input vector */
586 : )
587 : {
588 : int16_t i, ind;
589 : int16_t tmp;
590 :
591 308047 : ind = 0;
592 308047 : tmp = vec[0];
593 :
594 2003943 : for ( i = 1; i < lvec; i++ )
595 : {
596 1695896 : if ( vec[i] > tmp )
597 : {
598 157662 : ind = i;
599 157662 : tmp = vec[i];
600 : }
601 : }
602 :
603 308047 : if ( max != NULL )
604 : {
605 308047 : *max = tmp;
606 : }
607 :
608 308047 : return ind;
609 : }
610 :
611 : /*---------------------------------------------------------------------*
612 : * maximumAbs()
613 : *
614 : * Find index and value of the maximum in a vector
615 : *---------------------------------------------------------------------*/
616 :
617 : /*! r: index of the maximum value in the input vector */
618 2911530 : int16_t maximumAbs(
619 : const float *vec, /* i : input vector */
620 : const int16_t lvec, /* i : length of input vector */
621 : float *max_val /* o : maximum value in the input vector */
622 : )
623 : {
624 : int16_t j, ind;
625 : float tmp;
626 :
627 2911530 : ind = 0;
628 2911530 : tmp = (float) fabs( vec[0] );
629 :
630 127485410 : for ( j = 1; j < lvec; j++ )
631 : {
632 124573880 : if ( (float) fabs( vec[j] ) > tmp )
633 : {
634 9856095 : ind = j;
635 9856095 : tmp = (float) fabs( vec[j] );
636 : }
637 : }
638 :
639 2911530 : if ( max_val != NULL )
640 : {
641 2820078 : *max_val = tmp;
642 : }
643 :
644 2911530 : return ind;
645 : }
646 :
647 : /*---------------------------------------------------------------------*
648 : * minimum()
649 : *
650 : * Find index of a minimum in a vector
651 : *---------------------------------------------------------------------*/
652 :
653 : /*! r: index of the minimum value in the input vector */
654 4810763 : int16_t minimum(
655 : const float *vec, /* i : input vector */
656 : const int16_t lvec, /* i : length of input vector */
657 : float *min_val /* o : minimum value in the input vector */
658 : )
659 : {
660 : int16_t j, ind;
661 : float tmp;
662 :
663 4810763 : ind = 0;
664 4810763 : tmp = vec[0];
665 :
666 52928853 : for ( j = 1; j < lvec; j++ )
667 : {
668 48118090 : if ( vec[j] < tmp )
669 : {
670 10615337 : ind = j;
671 10615337 : tmp = vec[j];
672 : }
673 : }
674 :
675 4810763 : if ( min_val != NULL )
676 : {
677 4584852 : *min_val = tmp;
678 : }
679 :
680 4810763 : return ind;
681 : }
682 :
683 : /*-------------------------------------------------------------------*
684 : * minimum_s()
685 : *
686 : * Finds minimum 16-bit signed integer value in the array and returns it.
687 : *-------------------------------------------------------------------*/
688 :
689 : /*! r: index of the minimum value in the input vector */
690 116363 : int16_t minimum_s(
691 : const int16_t *vec, /* i : Input vector */
692 : const int16_t lvec, /* i : Vector length */
693 : int16_t *min_val /* o : minimum value in the input vector */
694 : )
695 : {
696 : int16_t i, ind, tmp;
697 :
698 116363 : ind = 0;
699 116363 : tmp = vec[0];
700 :
701 1255509 : for ( i = 1; i < lvec; i++ )
702 : {
703 1139146 : if ( vec[i] < tmp )
704 : {
705 53545 : ind = i;
706 53545 : tmp = vec[i];
707 : }
708 : }
709 :
710 116363 : if ( min_val != NULL )
711 : {
712 116363 : *min_val = tmp;
713 : }
714 :
715 116363 : return ind;
716 : }
717 :
718 :
719 : /*---------------------------------------------------------------------*
720 : * emaximum()
721 : *
722 : * Find index of a maximum energy in a vector
723 : *---------------------------------------------------------------------*/
724 :
725 : /*! r: return index with max energy value in vector */
726 55119878 : int16_t emaximum(
727 : const float *vec, /* i : input vector */
728 : const int16_t lvec, /* i : length of input vector */
729 : float *ener_max /* o : maximum energy value */
730 : )
731 : {
732 : int16_t j, ind;
733 : float temp;
734 :
735 55119878 : *ener_max = 0.0f;
736 55119878 : ind = 0;
737 :
738 1949720548 : for ( j = 0; j < lvec; j++ )
739 : {
740 1894600670 : temp = vec[j] * vec[j];
741 :
742 1894600670 : if ( temp > *ener_max )
743 : {
744 291414620 : ind = j;
745 291414620 : *ener_max = temp;
746 : }
747 : }
748 :
749 55119878 : return ind;
750 : }
751 :
752 :
753 : /*---------------------------------------------------------------------*
754 : * mean()
755 : *
756 : * Find the mean of the vector
757 : *---------------------------------------------------------------------*/
758 :
759 : /*! r: mean of vector */
760 40626325 : float mean(
761 : const float *vec, /* i : input vector */
762 : const int16_t lvec /* i : length of input vector */
763 : )
764 : {
765 : float tmp;
766 :
767 40626325 : tmp = sum_f( vec, lvec ) / (float) lvec;
768 :
769 40626325 : return tmp;
770 : }
771 :
772 : /*---------------------------------------------------------------------*
773 : * dotp()
774 : *
775 : * Dot product of vector x[] and vector y[]
776 : *---------------------------------------------------------------------*/
777 :
778 : /*! r: dot product of x[] and y[] */
779 1293312778 : float dotp(
780 : const float x[], /* i : vector x[] */
781 : const float y[], /* i : vector y[] */
782 : const int16_t n /* i : vector length */
783 : )
784 : {
785 : int16_t i;
786 : float suma;
787 :
788 1293312778 : suma = x[0] * y[0];
789 :
790 90190294942 : for ( i = 1; i < n; i++ )
791 : {
792 88896982164 : suma += x[i] * y[i];
793 : }
794 :
795 1293312778 : return suma;
796 : }
797 :
798 :
799 : /*---------------------------------------------------------------------*
800 : * inv_sqrt()
801 : *
802 : * Find the inverse square root of the input value
803 : *---------------------------------------------------------------------*/
804 :
805 : /*! r: inverse square root of input value */
806 478435677 : float inv_sqrt(
807 : const float x /* i : input value */
808 : )
809 : {
810 478435677 : return (float) ( 1.0 / sqrt( x ) );
811 : }
812 :
813 : /*---------------------------------------------------------------------*
814 : * inv_sqrtf()
815 : *
816 : * Find the inverse square root of the input value (float)
817 : *---------------------------------------------------------------------*/
818 :
819 : /*! r: inverse square root of input value (float) */
820 5646183 : float inv_sqrtf(
821 : const float x /* i : input value */
822 : )
823 : {
824 5646183 : return ( 1.0f / sqrtf( x ) );
825 : }
826 :
827 : /*-------------------------------------------------------------------*
828 : * conv()
829 : *
830 : * Convolution between vectors x[] and h[] written to y[]
831 : * All vectors are of length L. Only the first L samples of the
832 : * convolution are considered.
833 : *-------------------------------------------------------------------*/
834 :
835 3520195 : void conv(
836 : const float x[], /* i : input vector */
837 : const float h[], /* i : impulse response (or second input vector) */
838 : float y[], /* o : output vetor (result of convolution) */
839 : const int16_t L /* i : vector size */
840 : )
841 : {
842 : float temp;
843 : int16_t i, n;
844 234581595 : for ( n = 0; n < L; n++ )
845 : {
846 231061400 : temp = x[0] * h[n];
847 8190095364 : for ( i = 1; i <= n; i++ )
848 : {
849 7959033964 : temp += x[i] * h[n - i];
850 : }
851 231061400 : y[n] = temp;
852 : }
853 :
854 3520195 : return;
855 : }
856 :
857 : /*-------------------------------------------------------------------*
858 : * fir()
859 : *
860 : * FIR filtering of vector x[] with filter having impulse response h[]
861 : * written to y[]
862 : * The input vector has length L and the FIR filter has an order of K, i.e.
863 : * K+1 coefficients. The memory of the input signal is provided in the vector mem[]
864 : * which has K values
865 : * The maximum length of the input signal is L_FRAME32k and the maximum order
866 : * of the FIR filter is L_FILT_MAX
867 : *-------------------------------------------------------------------*/
868 :
869 1505398 : void fir(
870 : const float x[], /* i : input vector */
871 : const float h[], /* i : impulse response of the FIR filter */
872 : float y[], /* o : output vector (result of filtering) */
873 : float mem[], /* i/o: memory of the input signal (L samples) */
874 : const int16_t L, /* i : input vector size */
875 : const int16_t K, /* i : order of the FIR filter (K+1 coefs.) */
876 : const int16_t upd /* i : 1 = update the memory, 0 = not */
877 : )
878 : {
879 : float buf_in[L_FRAME48k + 60], buf_out[L_FRAME48k], s;
880 : int16_t i, j;
881 :
882 : /* prepare the input buffer (copy and update memory) */
883 1505398 : mvr2r( mem, buf_in, K );
884 1505398 : mvr2r( x, buf_in + K, L );
885 :
886 1505398 : if ( upd )
887 : {
888 3622 : mvr2r( buf_in + L, mem, K );
889 : }
890 :
891 : /* do the filtering */
892 415083154 : for ( i = 0; i < L; i++ )
893 : {
894 413577756 : s = buf_in[K + i] * h[0];
895 :
896 2069080228 : for ( j = 1; j <= K; j++ )
897 : {
898 1655502472 : s += h[j] * buf_in[K + i - j];
899 : }
900 :
901 413577756 : buf_out[i] = s;
902 : }
903 :
904 : /* copy to the output buffer */
905 1505398 : mvr2r( buf_out, y, L );
906 :
907 1505398 : return;
908 : }
909 :
910 : /*-------------------------------------------------------------------*
911 : * v_add()
912 : *
913 : * Addition of two vectors sample by sample
914 : *-------------------------------------------------------------------*/
915 :
916 7769520255 : void v_add(
917 : const float x1[], /* i : Input vector 1 */
918 : const float x2[], /* i : Input vector 2 */
919 : float y[], /* o : Output vector that contains vector 1 + vector 2 */
920 : const int16_t N /* i : Vector length */
921 : )
922 : {
923 : int16_t i;
924 :
925 >10268*10^7 : for ( i = 0; i < N; i++ )
926 : {
927 94918621013 : y[i] = x1[i] + x2[i];
928 : }
929 :
930 7769520255 : return;
931 : }
932 :
933 :
934 : /*-------------------------------------------------------------------*
935 : * v_sub()
936 : *
937 : * Subtraction of two vectors sample by sample
938 : *-------------------------------------------------------------------*/
939 :
940 7573750557 : void v_sub(
941 : const float x1[], /* i : Input vector 1 */
942 : const float x2[], /* i : Input vector 2 */
943 : float y[], /* o : Output vector that contains vector 1 - vector 2 */
944 : const int16_t N /* i : Vector length */
945 : )
946 : {
947 : int16_t i;
948 :
949 39805845773 : for ( i = 0; i < N; i++ )
950 : {
951 32232095216 : y[i] = x1[i] - x2[i];
952 : }
953 :
954 7573750557 : return;
955 : }
956 :
957 : /*-------------------------------------------------------------------*
958 : * v_mult()
959 : *
960 : * Multiplication of two vectors
961 : *-------------------------------------------------------------------*/
962 :
963 720566020 : void v_mult(
964 : const float x1[], /* i : Input vector 1 */
965 : const float x2[], /* i : Input vector 2 */
966 : float y[], /* o : Output vector that contains vector 1 .* vector 2 */
967 : const int16_t N /* i : Vector length */
968 : )
969 : {
970 : int16_t i;
971 :
972 27177622890 : for ( i = 0; i < N; i++ )
973 : {
974 26457056870 : y[i] = x1[i] * x2[i];
975 : }
976 :
977 720566020 : return;
978 : }
979 :
980 : /*-------------------------------------------------------------------*
981 : * v_multc()
982 : *
983 : * Multiplication of vector by constant
984 : *-------------------------------------------------------------------*/
985 :
986 997314543 : void v_multc(
987 : const float x[], /* i : Input vector */
988 : const float c, /* i : Constant */
989 : float y[], /* o : Output vector that contains c*x */
990 : const int16_t N /* i : Vector length */
991 : )
992 : {
993 : int16_t i;
994 :
995 87580206807 : for ( i = 0; i < N; i++ )
996 : {
997 86582892264 : y[i] = c * x[i];
998 : }
999 :
1000 997314543 : return;
1001 : }
1002 :
1003 :
1004 : /*-------------------------------------------------------------------*
1005 : * squant()
1006 : *
1007 : * Scalar quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
1008 : *
1009 : * Searches a given codebook to find the nearest neighbour in Euclidean space.
1010 : * Index of the winning codeword and the winning codeword itself are returned.
1011 : *-------------------------------------------------------------------*/
1012 :
1013 : /*! r: index of the winning codeword */
1014 16019605 : int16_t squant(
1015 : const float x, /* i : scalar value to quantize */
1016 : float *xq, /* o : quantized value */
1017 : const float cb[], /* i : codebook */
1018 : const int16_t cbsize /* i : codebook size */
1019 : )
1020 : {
1021 : float dist, mindist, tmp;
1022 : int16_t c, idx;
1023 :
1024 16019605 : idx = 0;
1025 16019605 : mindist = 1e16f;
1026 :
1027 74366269 : for ( c = 0; c < cbsize; c++ )
1028 : {
1029 58346664 : dist = 0.0f;
1030 58346664 : tmp = x - cb[c];
1031 58346664 : dist += tmp * tmp;
1032 58346664 : if ( dist < mindist )
1033 : {
1034 30322668 : mindist = dist;
1035 30322668 : idx = c;
1036 : }
1037 : }
1038 :
1039 16019605 : *xq = cb[idx];
1040 :
1041 16019605 : return idx;
1042 : }
1043 :
1044 : /*! r: index of the winning codeword */
1045 1180544 : int16_t squant_int(
1046 : uint8_t x, /* i : scalar value to quantize */
1047 : uint8_t *xq, /* o : quantized value */
1048 : const uint8_t *cb, /* i : codebook */
1049 : const int16_t cbsize /* i : codebook size */
1050 : )
1051 : {
1052 : int16_t i, idx;
1053 : float mindist, d;
1054 :
1055 1180544 : idx = 0;
1056 1180544 : mindist = 10000000.0f;
1057 9294417 : for ( i = 0; i < cbsize; i++ )
1058 : {
1059 8113873 : d = (float) ( x - cb[i] ) * ( x - cb[i] );
1060 8113873 : if ( d < mindist )
1061 : {
1062 1962509 : mindist = d;
1063 1962509 : idx = i;
1064 : }
1065 : }
1066 1180544 : *xq = cb[idx];
1067 :
1068 1180544 : return idx;
1069 : }
1070 :
1071 :
1072 : /*-------------------------------------------------------------------*
1073 : * usquant()
1074 : *
1075 : * Uniform scalar quantizer according to MMSE criterion
1076 : * (nearest neighbour in Euclidean space)
1077 : *
1078 : * Applies quantization based on scale and round operations.
1079 : * Index of the winning codeword and the winning codeword itself are returned.
1080 : *-------------------------------------------------------------------*/
1081 :
1082 : /*! r: index of the winning codeword */
1083 5721579 : int16_t usquant(
1084 : const float x, /* i : scalar value to quantize */
1085 : float *xq, /* o : quantized value */
1086 : const float qlow, /* i : lowest codebook entry (index 0) */
1087 : const float delta, /* i : quantization step */
1088 : const int16_t cbsize /* i : codebook size */
1089 : )
1090 : {
1091 : int16_t idx;
1092 :
1093 5721579 : idx = (int16_t) max( 0.f, min( cbsize - 1, ( ( x - qlow ) / delta + 0.5f ) ) );
1094 5721579 : *xq = idx * delta + qlow;
1095 :
1096 5721579 : return idx;
1097 : }
1098 :
1099 :
1100 : /*-------------------------------------------------------------------*
1101 : * usdequant()
1102 : *
1103 : * Uniform scalar de-quantizer routine
1104 : *
1105 : * Applies de-quantization based on scale and round operations.
1106 : *-------------------------------------------------------------------*/
1107 :
1108 6788594 : float usdequant(
1109 : const int16_t idx, /* i : quantizer index */
1110 : const float qlow, /* i : lowest codebook entry (index 0) */
1111 : const float delta /* i : quantization step */
1112 : )
1113 : {
1114 : float g;
1115 :
1116 6788594 : g = idx * delta + qlow;
1117 :
1118 6788594 : return ( g );
1119 : }
1120 :
1121 :
1122 : /*-------------------------------------------------------------------*
1123 : * vquant()
1124 : *
1125 : * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
1126 : *
1127 : * Searches a given codebook to find the nearest neighbour in Euclidean space.
1128 : * Index of the winning codevector and the winning vector itself are returned.
1129 : *-------------------------------------------------------------------*/
1130 :
1131 : /*! r: index of the winning codevector */
1132 434630 : int16_t vquant(
1133 : float x[], /* i : vector to quantize */
1134 : const float x_mean[], /* i : vector mean to subtract (0 if none) */
1135 : float xq[], /* o : quantized vector */
1136 : const float cb[], /* i : codebook */
1137 : const int16_t dim, /* i : dimension of codebook vectors */
1138 : const int16_t cbsize /* i : codebook size */
1139 : )
1140 : {
1141 : float dist, mindist, tmp;
1142 : int16_t c, d, idx, j;
1143 :
1144 434630 : idx = 0;
1145 434630 : mindist = 1e16f;
1146 :
1147 434630 : if ( x_mean != 0 )
1148 : {
1149 815847 : for ( d = 0; d < dim; d++ )
1150 : {
1151 602353 : x[d] -= x_mean[d];
1152 : }
1153 : }
1154 :
1155 434630 : j = 0;
1156 15589880 : for ( c = 0; c < cbsize; c++ )
1157 : {
1158 15155250 : dist = 0.0f;
1159 66325594 : for ( d = 0; d < dim; d++ )
1160 : {
1161 51170344 : tmp = x[d] - cb[j++];
1162 51170344 : dist += tmp * tmp;
1163 : }
1164 :
1165 15155250 : if ( dist < mindist )
1166 : {
1167 1727345 : mindist = dist;
1168 1727345 : idx = c;
1169 : }
1170 : }
1171 :
1172 434630 : if ( xq == 0 )
1173 : {
1174 0 : return idx;
1175 : }
1176 :
1177 434630 : j = idx * dim;
1178 1921527 : for ( d = 0; d < dim; d++ )
1179 : {
1180 1486897 : xq[d] = cb[j++];
1181 : }
1182 :
1183 434630 : if ( x_mean != 0 )
1184 : {
1185 815847 : for ( d = 0; d < dim; d++ )
1186 : {
1187 602353 : xq[d] += x_mean[d];
1188 : }
1189 : }
1190 :
1191 434630 : return idx;
1192 : }
1193 :
1194 : /*-------------------------------------------------------------------*
1195 : * w_vquant()
1196 : *
1197 : * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
1198 : *
1199 : * Searches a given codebook to find the nearest neighbour in Euclidean space.
1200 : * Weights are put on the error for each vector element.
1201 : * Index of the winning codevector and the winning vector itself are returned.
1202 : *-------------------------------------------------------------------*/
1203 :
1204 : /*! r: index of the winning codevector */
1205 50920 : int16_t w_vquant(
1206 : float x[], /* i : vector to quantize */
1207 : const float x_mean[], /* i : vector mean to subtract (0 if none) */
1208 : const int16_t weights[], /* i : error weights */
1209 : float xq[], /* o : quantized vector */
1210 : const float cb[], /* i : codebook */
1211 : const int16_t dim, /* i : dimension of codebook vectors */
1212 : const int16_t cbsize, /* i : codebook size */
1213 : const int16_t rev_vect /* i : reverse codebook vectors */
1214 : )
1215 : {
1216 : float dist, mindist, tmp;
1217 : int16_t c, d, idx, j, k;
1218 :
1219 50920 : idx = 0;
1220 50920 : mindist = 1e16f;
1221 :
1222 50920 : if ( x_mean != 0 )
1223 : {
1224 0 : for ( d = 0; d < dim; d++ )
1225 : {
1226 0 : x[d] -= x_mean[d];
1227 : }
1228 : }
1229 :
1230 50920 : j = 0;
1231 50920 : if ( rev_vect )
1232 : {
1233 12551 : k = dim - 1;
1234 1891531 : for ( c = 0; c < cbsize; c++ )
1235 : {
1236 1878980 : dist = 0.0f;
1237 :
1238 9394900 : for ( d = k; d >= 0; d-- )
1239 : {
1240 7515920 : tmp = x[d] - cb[j++];
1241 7515920 : dist += weights[d] * ( tmp * tmp );
1242 : }
1243 :
1244 1878980 : if ( dist < mindist )
1245 : {
1246 134448 : mindist = dist;
1247 134448 : idx = c;
1248 : }
1249 : }
1250 :
1251 12551 : if ( xq == 0 )
1252 : {
1253 0 : return idx;
1254 : }
1255 :
1256 12551 : j = idx * dim;
1257 62755 : for ( d = k; d >= 0; d-- )
1258 : {
1259 50204 : xq[d] = cb[j++];
1260 : }
1261 : }
1262 : else
1263 : {
1264 2074994 : for ( c = 0; c < cbsize; c++ )
1265 : {
1266 2036625 : dist = 0.0f;
1267 :
1268 10183125 : for ( d = 0; d < dim; d++ )
1269 : {
1270 8146500 : tmp = x[d] - cb[j++];
1271 8146500 : dist += weights[d] * ( tmp * tmp );
1272 : }
1273 :
1274 2036625 : if ( dist < mindist )
1275 : {
1276 199050 : mindist = dist;
1277 199050 : idx = c;
1278 : }
1279 : }
1280 :
1281 38369 : if ( xq == 0 )
1282 : {
1283 25460 : return idx;
1284 : }
1285 :
1286 12909 : j = idx * dim;
1287 64545 : for ( d = 0; d < dim; d++ )
1288 : {
1289 51636 : xq[d] = cb[j++];
1290 : }
1291 : }
1292 :
1293 25460 : if ( x_mean != 0 )
1294 : {
1295 0 : for ( d = 0; d < dim; d++ )
1296 : {
1297 0 : xq[d] += x_mean[d];
1298 : }
1299 : }
1300 :
1301 25460 : return idx;
1302 : }
1303 :
1304 :
1305 : /*----------------------------------------------------------------------------------*
1306 : * v_sort()
1307 : *
1308 : * Sorting of vectors. This is very fast with almost ordered vectors.
1309 : *----------------------------------------------------------------------------------*/
1310 :
1311 7306030 : void v_sort(
1312 : float *r, /* i/o: Vector to be sorted in place */
1313 : const int16_t lo, /* i : Low limit of sorting range */
1314 : const int16_t up /* I : High limit of sorting range */
1315 : )
1316 : {
1317 : int16_t i, j;
1318 : float tempr;
1319 :
1320 116077196 : for ( i = up - 1; i >= lo; i-- )
1321 : {
1322 108771166 : tempr = r[i];
1323 135511079 : for ( j = i + 1; j <= up && ( tempr > r[j] ); j++ )
1324 : {
1325 26739913 : r[j - 1] = r[j];
1326 : }
1327 :
1328 108771166 : r[j - 1] = tempr;
1329 : }
1330 :
1331 7306030 : return;
1332 : }
1333 :
1334 37827 : void sort(
1335 : uint16_t *x, /* i/o: Vector to be sorted */
1336 : uint16_t len /* i/o: vector length */
1337 : )
1338 : {
1339 : int16_t i;
1340 : uint16_t j, tempr;
1341 :
1342 310410 : for ( i = len - 2; i >= 0; i-- )
1343 : {
1344 272583 : tempr = x[i];
1345 425882 : for ( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
1346 : {
1347 153299 : x[j - 1] = x[j];
1348 : }
1349 272583 : x[j - 1] = tempr;
1350 : }
1351 :
1352 37827 : return;
1353 : }
1354 :
1355 :
1356 : /*---------------------------------------------------------------------*
1357 : * var()
1358 : *
1359 : * Calculate the variance of a vector
1360 : *---------------------------------------------------------------------*/
1361 :
1362 : /*! r: variance of vector */
1363 6711213 : float var(
1364 : const float *x, /* i : input vector */
1365 : const int16_t len /* i : length of inputvector */
1366 : )
1367 : {
1368 : float m;
1369 : float v;
1370 : int16_t i;
1371 :
1372 6711213 : m = mean( x, len );
1373 :
1374 6711213 : v = 0.0f;
1375 38504088 : for ( i = 0; i < len; i++ )
1376 : {
1377 31792875 : v += ( x[i] - m ) * ( x[i] - m );
1378 : }
1379 6711213 : v /= len;
1380 :
1381 6711213 : return v;
1382 : }
1383 :
1384 :
1385 : /*---------------------------------------------------------------------*
1386 : * std_dev()
1387 : *
1388 : * Calculate the standard deviation of a vector
1389 : *---------------------------------------------------------------------*/
1390 :
1391 : /*! r: standard deviation */
1392 26370 : float std_dev(
1393 : const float *x, /* i : input vector */
1394 : const int16_t len /* i : length of the input vector */
1395 : )
1396 : {
1397 : int16_t i;
1398 : float std;
1399 :
1400 26370 : std = 1e-16f;
1401 237330 : for ( i = 0; i < len; i++ )
1402 : {
1403 210960 : std += x[i] * x[i];
1404 : }
1405 :
1406 26370 : std = (float) sqrt( std / len );
1407 :
1408 26370 : return std;
1409 : }
1410 :
1411 :
1412 : /*---------------------------------------------------------------------*
1413 : * dot_product_mat()
1414 : *
1415 : * Calculates dot product of type x'*A*x, where x is column vector of size m,
1416 : * and A is square matrix of size m*m
1417 : *---------------------------------------------------------------------*/
1418 :
1419 : /*! r: the dot product x'*A*x */
1420 271980 : float dot_product_mat(
1421 : const float *x, /* i : vector x */
1422 : const float *A, /* i : matrix A */
1423 : const int16_t m /* i : vector & matrix size */
1424 : )
1425 : {
1426 : int16_t i, j;
1427 : float suma, tmp_sum;
1428 : const float *pt_x, *pt_A;
1429 :
1430 271980 : pt_A = A;
1431 271980 : suma = 0;
1432 :
1433 3535740 : for ( i = 0; i < m; i++ )
1434 : {
1435 3263760 : tmp_sum = 0;
1436 3263760 : pt_x = x;
1437 42428880 : for ( j = 0; j < m; j++ )
1438 : {
1439 39165120 : tmp_sum += *pt_x++ * *pt_A++;
1440 : }
1441 :
1442 3263760 : suma += x[i] * tmp_sum;
1443 : }
1444 :
1445 271980 : return suma;
1446 : }
1447 :
1448 :
1449 : /*--------------------------------------------------------------------------------*
1450 : * polezero_filter()
1451 : *
1452 : * Y(Z)=X(Z)(b[0]+b[1]z^(-1)+..+b[L]z^(-L))/(a[0]+a[1]z^(-1)+..+a[M]z^(-M))
1453 : * mem[n]=x[n]+cp[0]mem[n-1]+..+cp[M-1]mem[n-M], where cp[i]=-a[i+1]/a[0]
1454 : * y[n]=cz[0]mem[n]+cz[1]mem[n-1]+..+cz[L]mem[n-L], where cz[i]=b[i]/a[0]
1455 : * mem={mem[n-K] mem[n-K+1] . . . . mem[n-2] mem[n-1]}, where K=max(L,M)
1456 : *
1457 : * a[0] must be equal to 1.0f!
1458 : *---------------------------------------------------------------------------------*/
1459 :
1460 456 : void polezero_filter(
1461 : const float *in, /* i : input vector */
1462 : float *out, /* o : output vector */
1463 : const int16_t N, /* i : input vector size */
1464 : const float *b, /* i : numerator coefficients */
1465 : const float *a, /* i : denominator coefficients */
1466 : const int16_t order, /* i : filter order */
1467 : float *mem /* i/o: filter memory */
1468 : )
1469 : {
1470 : int16_t i, j, k;
1471 :
1472 :
1473 4488 : for ( i = 0; i < order; i++ )
1474 : {
1475 4032 : out[i] = in[i] * b[0];
1476 21012 : for ( j = 0; j < i; j++ )
1477 : {
1478 16980 : out[i] += in[i - 1 - j] * b[j + 1] - out[i - 1 - j] * a[j + 1];
1479 : }
1480 :
1481 25044 : for ( k = order - 1; j < order; j++, k-- )
1482 : {
1483 21012 : out[i] += mem[k] * b[j + 1] - mem[k + order] * a[j + 1];
1484 : }
1485 : }
1486 :
1487 113160 : for ( ; i < N; i++ )
1488 : {
1489 112704 : out[i] = in[i] * b[0];
1490 1106904 : for ( j = 0; j < order; j++ )
1491 : {
1492 994200 : out[i] += in[i - 1 - j] * b[j + 1] - out[i - 1 - j] * a[j + 1];
1493 : }
1494 : }
1495 :
1496 4488 : for ( i = 0; i < order; i++ )
1497 : {
1498 4032 : mem[i] = in[N - order + i];
1499 4032 : mem[i + order] = out[N - order + i];
1500 : }
1501 :
1502 456 : return;
1503 : }
1504 :
1505 : #define WMC_TOOL_SKIP
1506 6404687 : static float fleft_shift( float input, const int16_t shift )
1507 : {
1508 6404687 : return ( input * (float) pow( 2.0, (double) shift ) );
1509 : }
1510 :
1511 1110536 : static float fright_shift( float input, const int16_t shift )
1512 : {
1513 1110536 : return ( input * (float) pow( 0.5, (double) shift ) );
1514 : }
1515 : #undef WMC_TOOL_SKIP
1516 :
1517 :
1518 : /*--------------------------------------------------------------------------------*
1519 : * root_a()
1520 : *
1521 : * Implements a quadratic approximation to sqrt(a)
1522 : * Firstly, a is normalized to lie between 0.25 & 1.0
1523 : * by shifting the input left or right by an even number of
1524 : * shifts. Even shifts represent powers of 4 which, after
1525 : * the sqrt, can easily be converted to powers of 2 and are
1526 : * easily dealt with.
1527 : * At the heart of the algorithm is a quadratic
1528 : * approximation of the curve sqrt(a) for 0.25 <= a <= 1.0.
1529 : * Sqrt(a) approx = 0.27 + 1.0127 * a - 0.2864 * a^2
1530 : *
1531 : *---------------------------------------------------------------------------------*/
1532 :
1533 1110536 : float root_a(
1534 : float a )
1535 : {
1536 : int16_t shift_a;
1537 : float mod_a;
1538 : float approx;
1539 :
1540 1110536 : if ( a <= 0.0f )
1541 : {
1542 0 : return 0.0;
1543 : }
1544 :
1545 : #define WMC_TOOL_SKIP
1546 : /* This next piece of code implements a "norm" function */
1547 : /* and returns the shift needed to scale "a" to have a */
1548 : /* 1 in the (MSB-1) position. This is equivalent to */
1549 : /* giving a value between 0.5 & 1.0. */
1550 1110536 : mod_a = a;
1551 :
1552 1110536 : shift_a = 0;
1553 1110536 : while ( mod_a > 1.0 )
1554 : {
1555 0 : mod_a /= 2.0;
1556 0 : shift_a--;
1557 : }
1558 :
1559 4446216 : while ( mod_a < 0.5 )
1560 : {
1561 3335680 : mod_a *= 2.0;
1562 3335680 : shift_a++;
1563 : }
1564 : #undef WMC_TOOL_SKIP
1565 :
1566 1110536 : shift_a &= 0xfffe;
1567 1110536 : mod_a = fleft_shift( a, shift_a );
1568 :
1569 1110536 : approx = 0.27f + 1.0127f * mod_a - 0.2864f * mod_a * mod_a;
1570 :
1571 1110536 : approx = fright_shift( approx, ( shift_a >> 1 ) );
1572 :
1573 1110536 : return ( approx );
1574 : }
1575 :
1576 : /*--------------------------------------------------------------------------------*
1577 : * root_a_over_b()
1578 : *
1579 : * Implements an approximation to sqrt(a/b)
1580 : * Firstly a & b are normalized to lie between 0.25 & 1.0
1581 : * by shifting the inputs left or right by an even number
1582 : * of shifts.
1583 : * Even shifts represent powers of 4 which, after the sqrt,
1584 : * become powers of 2 and are easily dealt with.
1585 : * At the heart of the algorithm is an approximation of the
1586 : * curve sqrt(a/b) for 0.25 <= a <= 1.0 & 0.25 <= b <= 1.0.
1587 : * Given the value of b, the 2nd order coefficients p0, p1
1588 : * & p2 can be determined so that...
1589 : * Sqrt(a/b) approx = p0 + p1 * a + p2 * a^2
1590 : * where p0 approx = 0.7176 - 0.8815 * b + 0.4429 * b^2
1591 : * p1 approx = 2.6908 - 3.3056 * b + 1.6608 * b^2
1592 : * p2 approx = -0.7609 + 0.9346 * b - 0.4695 * b^2
1593 : *
1594 : *---------------------------------------------------------------------------------*/
1595 :
1596 1767778 : float root_a_over_b(
1597 : float a,
1598 : float b )
1599 : {
1600 : int16_t shift_a, shift_b, shift;
1601 : float mod_a, mod_b;
1602 1767778 : float p2 = -0.7609f;
1603 1767778 : float p1 = 2.6908f;
1604 1767778 : float p0 = 0.7176f;
1605 : float b_sqr;
1606 : float approx;
1607 :
1608 1767778 : if ( ( a <= 0.0f ) || ( b <= 0.0f ) )
1609 : {
1610 3061 : return 0.0;
1611 : }
1612 : #define WMC_TOOL_SKIP
1613 1764717 : if ( isinf( a ) )
1614 : #undef WMC_TOOL_SKIP
1615 : {
1616 0 : return FLT_MAX;
1617 : }
1618 : #define WMC_TOOL_SKIP
1619 1764717 : if ( isinf( b ) )
1620 : #undef WMC_TOOL_SKIP
1621 : {
1622 0 : return 0.f;
1623 : }
1624 :
1625 1764717 : a += 0x00000001;
1626 1764717 : b += 0x00000001;
1627 :
1628 : #define WMC_TOOL_SKIP
1629 : /* This next piece of code implements a "norm" function */
1630 : /* and returns the shift needed to scale "a" to have a */
1631 : /* 1 in the (MSB-1) position. This is equivalent to */
1632 : /* giving a value between 0.5 & 1.0. */
1633 1764717 : mod_a = a;
1634 :
1635 1764717 : shift_a = 0;
1636 17165477 : while ( mod_a > 1.0 )
1637 : {
1638 15400760 : mod_a /= 2.0;
1639 15400760 : shift_a--;
1640 : }
1641 :
1642 1764717 : while ( mod_a < 0.5 )
1643 : {
1644 0 : mod_a *= 2.0;
1645 0 : shift_a++;
1646 : }
1647 : #undef WMC_TOOL_SKIP
1648 :
1649 1764717 : shift_a &= 0xfffe;
1650 1764717 : mod_a = fleft_shift( a, shift_a );
1651 :
1652 : #define WMC_TOOL_SKIP
1653 : /* This next piece of code implements a "norm" function */
1654 : /* and returns the shift needed to scale "b" to have a */
1655 : /* 1 in the (MSB-1) position. This is equivalent to */
1656 : /* giving a value between 0.5 & 1.0. */
1657 1764717 : mod_b = b;
1658 :
1659 1764717 : shift_b = 0;
1660 31164959 : while ( mod_b > 1.0 )
1661 : {
1662 29400242 : mod_b /= 2.0;
1663 29400242 : shift_b--;
1664 : }
1665 :
1666 1764717 : while ( mod_b < 0.5 )
1667 : {
1668 0 : mod_b *= 2.0;
1669 0 : shift_b++;
1670 : }
1671 : #undef WMC_TOOL_SKIP
1672 :
1673 1764717 : shift_b &= 0xfffe;
1674 1764717 : mod_b = fleft_shift( b, shift_b );
1675 :
1676 1764717 : shift = ( shift_b - shift_a ) >> 1;
1677 :
1678 1764717 : b_sqr = mod_b * mod_b;
1679 :
1680 1764717 : p2 += 0.9346f * mod_b + -0.4695f * b_sqr;
1681 1764717 : p1 += -3.3056f * mod_b + 1.6608f * b_sqr;
1682 1764717 : p0 += -0.8815f * mod_b + 0.4429f * b_sqr;
1683 :
1684 1764717 : approx = p0 + p1 * mod_a + p2 * mod_a * mod_a;
1685 :
1686 1764717 : approx = fleft_shift( approx, shift );
1687 :
1688 1764717 : return ( approx );
1689 : }
1690 :
1691 : /*--------------------------------------------------------------------------------*
1692 : * rint_new()
1693 : *
1694 : * Round to the nearest integer with mid-point exception
1695 : *---------------------------------------------------------------------------------*/
1696 :
1697 23396 : double rint_new(
1698 : double x )
1699 : {
1700 : int16_t a;
1701 :
1702 : /* middle value point test */
1703 23396 : if ( ceil( x + 0.5 ) == floor( x + 0.5 ) )
1704 : {
1705 106 : a = (int16_t) ceil( x );
1706 :
1707 106 : if ( a % 2 == 0 )
1708 : {
1709 54 : return ceil( x );
1710 : }
1711 : else
1712 : {
1713 52 : return floor( x );
1714 : }
1715 : }
1716 : else
1717 : {
1718 23290 : return floor( x + 0.5 );
1719 : }
1720 : }
1721 :
1722 :
1723 : /*-------------------------------------------------------------------*
1724 : * anint()
1725 : *
1726 : * Round to the nearest integer.
1727 : *-------------------------------------------------------------------*/
1728 :
1729 807835 : double anint(
1730 : double x )
1731 : {
1732 807835 : return ( x ) >= 0 ? (int32_t) ( ( x ) + 0.5 ) : (int32_t) ( (x) -0.5 );
1733 : }
1734 :
1735 : /*-------------------------------------------------------------------*
1736 : * is_numeric_float()
1737 : *
1738 : * Returns 0 for all NaN and Inf values defined according to IEEE 754
1739 : * floating point number's definition. Returns 1 for numeric values.
1740 : *-------------------------------------------------------------------*/
1741 :
1742 216285 : int16_t is_numeric_float(
1743 : float x )
1744 : {
1745 : int16_t retval;
1746 : #define WMC_TOOL_SKIP
1747 216285 : retval = (int16_t) ( !isnan( x ) && !isinf( x ) );
1748 : #undef WMC_TOOL_SKIP
1749 216285 : return retval;
1750 : }
1751 :
1752 : /*-------------------------------------------------------------------*
1753 : * delay_signal()
1754 : *
1755 : * Delay buffer by defined number of samples
1756 : *-------------------------------------------------------------------*/
1757 :
1758 57081342 : void delay_signal(
1759 : float x[], /* i/o: signal to be delayed */
1760 : const int16_t len, /* i : length of the input signal */
1761 : float mem[], /* i/o: synchronization memory */
1762 : const int16_t delay /* i : delay in samples */
1763 : )
1764 : {
1765 : float tmp_buffer[L_FRAME48k];
1766 :
1767 57081342 : mvr2r( mem, tmp_buffer, delay );
1768 57081342 : mvr2r( x + len - delay, mem, delay );
1769 57081342 : mvr2r( x, x + delay, len - delay );
1770 57081342 : mvr2r( tmp_buffer, x, delay );
1771 :
1772 57081342 : return;
1773 : }
|
