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 28723033125 : int16_t own_random(
54 : int16_t *seed /* i/o: random seed */
55 : )
56 : {
57 28723033125 : *seed = (int16_t) ( *seed * 31821L + 13849L );
58 :
59 28723033125 : return ( *seed );
60 : }
61 :
62 : /*---------------------------------------------------------------------
63 : * sign()
64 : *
65 : *---------------------------------------------------------------------*/
66 :
67 : /*! r: sign of x (+1/-1) */
68 173961415 : float sign(
69 : const float x /* i : input value of x */
70 : )
71 : {
72 173961415 : if ( x < 0.0f )
73 : {
74 76216536 : return -1.0f;
75 : }
76 : else
77 : {
78 97744879 : return 1.0f;
79 : }
80 : }
81 :
82 : /*---------------------------------------------------------------------
83 : * log2_f()
84 : *
85 : *---------------------------------------------------------------------*/
86 :
87 : /*! r: logarithm2 of x */
88 315721451 : float log2_f(
89 : const float x /* i : input value of x */
90 : )
91 : {
92 315721451 : return (float) ( log( x ) / log( 2.0f ) );
93 : }
94 :
95 31960178 : int16_t norm_ul( uint32_t UL_var1 )
96 : {
97 : int16_t var_out;
98 :
99 31960178 : if ( UL_var1 == 0 )
100 : {
101 0 : var_out = 0;
102 : }
103 : else
104 : {
105 404401369 : for ( var_out = 0; UL_var1 < (uint32_t) 0x80000000U; var_out++ )
106 : {
107 372441191 : UL_var1 <<= 1;
108 : }
109 : }
110 : BASOP_CHECK();
111 :
112 31960178 : 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 28231972 : 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 28231972 : tmp = 0;
133 314258122 : for ( i = 0; i < lvec; i++ )
134 : {
135 286026150 : tmp += vec[i];
136 : }
137 :
138 28231972 : 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 335186559 : 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 335186559 : tmp = 0.0f;
171 14225251607 : for ( i = 0; i < lvec; i++ )
172 : {
173 13890065048 : tmp += vec[i];
174 : }
175 :
176 335186559 : return tmp;
177 : }
178 :
179 : /*----------------------------------------------------------------------
180 : * sum2_f()
181 : *
182 : *---------------------------------------------------------------------*/
183 :
184 : /*! r: sum of all squared vector elements */
185 684769906 : 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 684769906 : tmp = 0.0f;
194 71925663641 : for ( i = 0; i < lvec; i++ )
195 : {
196 71240893735 : tmp += vec[i] * vec[i];
197 : }
198 :
199 684769906 : 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 23090505 : 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 12896988323 : for ( i = 0; i < N; i++ )
221 : {
222 12873897818 : y[i] = a;
223 : }
224 :
225 23090505 : return;
226 : }
227 :
228 :
229 913377134 : 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 65670264075 : for ( i = 0; i < N; i++ )
238 : {
239 64756886941 : y[i] = a;
240 : }
241 :
242 913377134 : return;
243 : }
244 :
245 :
246 201650 : 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 8393474 : for ( i = 0; i < N; i++ )
255 : {
256 8191824 : y[i] = a;
257 : }
258 :
259 201650 : return;
260 : }
261 :
262 7644851863 : 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 >83958*10^7 : for ( i = 0; i < N; i++ )
271 : {
272 >83193*10^7 : y[i] = a;
273 : }
274 :
275 7644851863 : return;
276 : }
277 :
278 :
279 : /*---------------------------------------------------------------------*
280 : * set_zero()
281 : *
282 : * Set a vector vec[] of dimension lvec to zero
283 : *---------------------------------------------------------------------*/
284 :
285 14393999378 : 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 >44501*10^7 : for ( i = 0; i < lvec; i++ )
293 : {
294 >43062*10^7 : *vec++ = 0.0f;
295 : }
296 :
297 14393999378 : 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 11053198592 : 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 11053198592 : if ( n <= 0 )
321 : {
322 : /* cannot transfer vectors with size 0 */
323 94429949 : return;
324 : }
325 :
326 10958768643 : if ( y < x )
327 : {
328 >14123*10^8 : for ( i = 0; i < n; i++ )
329 : {
330 >14070*10^8 : y[i] = x[i];
331 : }
332 : }
333 : else
334 : {
335 >13237*10^8 : for ( i = n - 1; i >= 0; i-- )
336 : {
337 >13180*10^8 : y[i] = x[i];
338 : }
339 : }
340 :
341 10958768643 : return;
342 : }
343 :
344 621304561 : 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 621304561 : if ( n <= 0 )
353 : {
354 : /* cannot transfer vectors with size 0 */
355 80089027 : return;
356 : }
357 :
358 541215534 : if ( y < x )
359 : {
360 1290082756 : for ( i = 0; i < n; i++ )
361 : {
362 1124394411 : y[i] = x[i];
363 : }
364 : }
365 : else
366 : {
367 4125094363 : for ( i = n - 1; i >= 0; i-- )
368 : {
369 3749567174 : y[i] = x[i];
370 : }
371 : }
372 :
373 541215534 : 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 93636901 : 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 93636901 : uint32_t noClipping = 0;
425 :
426 93636901 : if ( n <= 0 )
427 : {
428 : /* cannot transfer vectors with size 0 */
429 1884 : return 0;
430 : }
431 :
432 93635017 : if ( (void *) y <= (const void *) x )
433 : {
434 21204450 : for ( i = 0; i < n; i++ )
435 : {
436 21176000 : temp = x[i];
437 21176000 : temp = (float) floor( temp + 0.5f );
438 :
439 21176000 : if ( temp > MAX16B_FLT )
440 : {
441 11413 : temp = MAX16B_FLT;
442 11413 : noClipping++;
443 : }
444 21164587 : else if ( temp < MIN16B_FLT )
445 : {
446 8594 : temp = MIN16B_FLT;
447 8594 : noClipping++;
448 : }
449 :
450 21176000 : y[i] = (int16_t) temp;
451 : }
452 : }
453 : else
454 : {
455 41530492194 : for ( i = n - 1; i >= 0; i-- )
456 : {
457 41436885627 : temp = x[i];
458 41436885627 : temp = (float) floor( temp + 0.5f );
459 :
460 41436885627 : if ( temp > MAX16B_FLT )
461 : {
462 14262 : temp = MAX16B_FLT;
463 14262 : noClipping++;
464 : }
465 41436871365 : else if ( temp < MIN16B_FLT )
466 : {
467 13487 : temp = MIN16B_FLT;
468 13487 : noClipping++;
469 : }
470 :
471 41436885627 : y[i] = (int16_t) temp;
472 : }
473 : }
474 :
475 93635017 : return noClipping;
476 : }
477 :
478 9657054 : 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 9657054 : if ( n <= 0 )
487 : {
488 : /* cannot transfer vectors with size 0 */
489 0 : return;
490 : }
491 :
492 9657054 : if ( (void *) y < (const void *) x )
493 : {
494 35701301 : for ( i = 0; i < n; i++ )
495 : {
496 26044247 : 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 9657054 : return;
508 : }
509 :
510 :
511 559540 : 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 559540 : if ( n <= 0 )
520 : {
521 : /* no need to transfer vectors with size 0 */
522 0 : return;
523 : }
524 :
525 559540 : if ( y < x )
526 : {
527 4792951 : for ( i = 0; i < n; i++ )
528 : {
529 4408208 : y[i] = x[i];
530 : }
531 : }
532 : else
533 : {
534 2274089 : for ( i = n - 1; i >= 0; i-- )
535 : {
536 2099292 : y[i] = x[i];
537 : }
538 : }
539 :
540 559540 : 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 793548125 : 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 793548125 : ind = 0;
561 793548125 : tmp = vec[0];
562 :
563 12936341645 : for ( j = 1; j < lvec; j++ )
564 : {
565 12142793520 : if ( vec[j] > tmp )
566 : {
567 2027065967 : ind = j;
568 2027065967 : tmp = vec[j];
569 : }
570 : }
571 :
572 793548125 : if ( max_val != NULL )
573 : {
574 151452567 : *max_val = tmp;
575 : }
576 :
577 793548125 : return ind;
578 : }
579 :
580 :
581 : /*! r: index of the maximum value in the input vector */
582 994242 : 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 994242 : ind = 0;
592 994242 : tmp = vec[0];
593 :
594 11112095 : for ( i = 1; i < lvec; i++ )
595 : {
596 10117853 : if ( vec[i] > tmp )
597 : {
598 729281 : ind = i;
599 729281 : tmp = vec[i];
600 : }
601 : }
602 :
603 994242 : if ( max != NULL )
604 : {
605 994242 : *max = tmp;
606 : }
607 :
608 994242 : 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 22142129 : 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 22142129 : ind = 0;
628 22142129 : tmp = (float) fabs( vec[0] );
629 :
630 818033303 : for ( j = 1; j < lvec; j++ )
631 : {
632 795891174 : if ( (float) fabs( vec[j] ) > tmp )
633 : {
634 76866717 : ind = j;
635 76866717 : tmp = (float) fabs( vec[j] );
636 : }
637 : }
638 :
639 22142129 : if ( max_val != NULL )
640 : {
641 22006978 : *max_val = tmp;
642 : }
643 :
644 22142129 : 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 12998688 : 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 12998688 : ind = 0;
664 12998688 : tmp = vec[0];
665 :
666 102263718 : for ( j = 1; j < lvec; j++ )
667 : {
668 89265030 : if ( vec[j] < tmp )
669 : {
670 20696873 : ind = j;
671 20696873 : tmp = vec[j];
672 : }
673 : }
674 :
675 12998688 : if ( min_val != NULL )
676 : {
677 11050374 : *min_val = tmp;
678 : }
679 :
680 12998688 : 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 943012 : 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 943012 : ind = 0;
699 943012 : tmp = vec[0];
700 :
701 10542572 : for ( i = 1; i < lvec; i++ )
702 : {
703 9599560 : if ( vec[i] < tmp )
704 : {
705 520935 : ind = i;
706 520935 : tmp = vec[i];
707 : }
708 : }
709 :
710 943012 : if ( min_val != NULL )
711 : {
712 943012 : *min_val = tmp;
713 : }
714 :
715 943012 : 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 285239381 : 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 285239381 : *ener_max = 0.0f;
736 285239381 : ind = 0;
737 :
738 9692666152 : for ( j = 0; j < lvec; j++ )
739 : {
740 9407426771 : temp = vec[j] * vec[j];
741 :
742 9407426771 : if ( temp > *ener_max )
743 : {
744 1320522397 : ind = j;
745 1320522397 : *ener_max = temp;
746 : }
747 : }
748 :
749 285239381 : return ind;
750 : }
751 :
752 :
753 : /*---------------------------------------------------------------------*
754 : * mean()
755 : *
756 : * Find the mean of the vector
757 : *---------------------------------------------------------------------*/
758 :
759 : /*! r: mean of vector */
760 183329701 : 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 183329701 : tmp = sum_f( vec, lvec ) / (float) lvec;
768 :
769 183329701 : 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 4421257060 : 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 4421257060 : suma = x[0] * y[0];
789 :
790 >38927*10^7 : for ( i = 1; i < n; i++ )
791 : {
792 >38485*10^7 : suma += x[i] * y[i];
793 : }
794 :
795 4421257060 : 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 1771632493 : float inv_sqrt(
807 : const float x /* i : input value */
808 : )
809 : {
810 1771632493 : 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 21118618 : float inv_sqrtf(
821 : const float x /* i : input value */
822 : )
823 : {
824 21118618 : 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 17037905 : 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 1128806257 : for ( n = 0; n < L; n++ )
845 : {
846 1111768352 : temp = x[0] * h[n];
847 38599079344 : for ( i = 1; i <= n; i++ )
848 : {
849 37487310992 : temp += x[i] * h[n - i];
850 : }
851 1111768352 : y[n] = temp;
852 : }
853 :
854 17037905 : 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 3539290 : 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 3539290 : mvr2r( mem, buf_in, K );
884 3539290 : mvr2r( x, buf_in + K, L );
885 :
886 3539290 : if ( upd )
887 : {
888 3264 : mvr2r( buf_in + L, mem, K );
889 : }
890 :
891 : /* do the filtering */
892 1042601138 : for ( i = 0; i < L; i++ )
893 : {
894 1039061848 : s = buf_in[K + i] * h[0];
895 :
896 5356628680 : for ( j = 1; j <= K; j++ )
897 : {
898 4317566832 : s += h[j] * buf_in[K + i - j];
899 : }
900 :
901 1039061848 : buf_out[i] = s;
902 : }
903 :
904 : /* copy to the output buffer */
905 3539290 : mvr2r( buf_out, y, L );
906 :
907 3539290 : return;
908 : }
909 :
910 : /*-------------------------------------------------------------------*
911 : * v_add()
912 : *
913 : * Addition of two vectors sample by sample
914 : *-------------------------------------------------------------------*/
915 :
916 12521774456 : 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 >13234*10^7 : for ( i = 0; i < N; i++ )
926 : {
927 >11982*10^7 : y[i] = x1[i] + x2[i];
928 : }
929 :
930 12521774456 : return;
931 : }
932 :
933 :
934 : /*-------------------------------------------------------------------*
935 : * v_sub()
936 : *
937 : * Subtraction of two vectors sample by sample
938 : *-------------------------------------------------------------------*/
939 :
940 12217868442 : 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 65577595056 : for ( i = 0; i < N; i++ )
950 : {
951 53359726614 : y[i] = x1[i] - x2[i];
952 : }
953 :
954 12217868442 : return;
955 : }
956 :
957 : /*-------------------------------------------------------------------*
958 : * v_mult()
959 : *
960 : * Multiplication of two vectors
961 : *-------------------------------------------------------------------*/
962 :
963 935569385 : 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 37727832214 : for ( i = 0; i < N; i++ )
973 : {
974 36792262829 : y[i] = x1[i] * x2[i];
975 : }
976 :
977 935569385 : return;
978 : }
979 :
980 : /*-------------------------------------------------------------------*
981 : * v_multc()
982 : *
983 : * Multiplication of vector by constant
984 : *-------------------------------------------------------------------*/
985 :
986 1575547434 : 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 >16303*10^7 : for ( i = 0; i < N; i++ )
996 : {
997 >16145*10^7 : y[i] = c * x[i];
998 : }
999 :
1000 1575547434 : 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 78286477 : 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 78286477 : idx = 0;
1025 78286477 : mindist = 1e16f;
1026 :
1027 368778519 : for ( c = 0; c < cbsize; c++ )
1028 : {
1029 290492042 : dist = 0.0f;
1030 290492042 : tmp = x - cb[c];
1031 290492042 : dist += tmp * tmp;
1032 290492042 : if ( dist < mindist )
1033 : {
1034 157433667 : mindist = dist;
1035 157433667 : idx = c;
1036 : }
1037 : }
1038 :
1039 78286477 : *xq = cb[idx];
1040 :
1041 78286477 : return idx;
1042 : }
1043 :
1044 : /*! r: index of the winning codeword */
1045 8102521 : 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 8102521 : idx = 0;
1056 8102521 : mindist = 10000000.0f;
1057 66995346 : for ( i = 0; i < cbsize; i++ )
1058 : {
1059 58892825 : d = (float) ( x - cb[i] ) * ( x - cb[i] );
1060 58892825 : if ( d < mindist )
1061 : {
1062 18756520 : mindist = d;
1063 18756520 : idx = i;
1064 : }
1065 : }
1066 8102521 : *xq = cb[idx];
1067 :
1068 8102521 : 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 18300657 : 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 18300657 : idx = (int16_t) max( 0.f, min( cbsize - 1, ( ( x - qlow ) / delta + 0.5f ) ) );
1094 18300657 : *xq = idx * delta + qlow;
1095 :
1096 18300657 : 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 12371958 : 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 12371958 : g = idx * delta + qlow;
1117 :
1118 12371958 : 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 3050813 : 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 3050813 : idx = 0;
1145 3050813 : mindist = 1e16f;
1146 :
1147 3050813 : if ( x_mean != 0 )
1148 : {
1149 7491049 : for ( d = 0; d < dim; d++ )
1150 : {
1151 5617827 : x[d] -= x_mean[d];
1152 : }
1153 : }
1154 :
1155 3050813 : j = 0;
1156 111404029 : for ( c = 0; c < cbsize; c++ )
1157 : {
1158 108353216 : dist = 0.0f;
1159 455960768 : for ( d = 0; d < dim; d++ )
1160 : {
1161 347607552 : tmp = x[d] - cb[j++];
1162 347607552 : dist += tmp * tmp;
1163 : }
1164 :
1165 108353216 : if ( dist < mindist )
1166 : {
1167 12145267 : mindist = dist;
1168 12145267 : idx = c;
1169 : }
1170 : }
1171 :
1172 3050813 : if ( xq == 0 )
1173 : {
1174 0 : return idx;
1175 : }
1176 :
1177 3050813 : j = idx * dim;
1178 13379004 : for ( d = 0; d < dim; d++ )
1179 : {
1180 10328191 : xq[d] = cb[j++];
1181 : }
1182 :
1183 3050813 : if ( x_mean != 0 )
1184 : {
1185 7491049 : for ( d = 0; d < dim; d++ )
1186 : {
1187 5617827 : xq[d] += x_mean[d];
1188 : }
1189 : }
1190 :
1191 3050813 : 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 351626 : 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 351626 : idx = 0;
1220 351626 : mindist = 1e16f;
1221 :
1222 351626 : 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 351626 : j = 0;
1231 351626 : if ( rev_vect )
1232 : {
1233 85559 : k = dim - 1;
1234 13254027 : for ( c = 0; c < cbsize; c++ )
1235 : {
1236 13168468 : dist = 0.0f;
1237 :
1238 65842340 : for ( d = k; d >= 0; d-- )
1239 : {
1240 52673872 : tmp = x[d] - cb[j++];
1241 52673872 : dist += weights[d] * ( tmp * tmp );
1242 : }
1243 :
1244 13168468 : if ( dist < mindist )
1245 : {
1246 932936 : mindist = dist;
1247 932936 : idx = c;
1248 : }
1249 : }
1250 :
1251 85559 : if ( xq == 0 )
1252 : {
1253 0 : return idx;
1254 : }
1255 :
1256 85559 : j = idx * dim;
1257 427795 : for ( d = k; d >= 0; d-- )
1258 : {
1259 342236 : xq[d] = cb[j++];
1260 : }
1261 : }
1262 : else
1263 : {
1264 14887785 : for ( c = 0; c < cbsize; c++ )
1265 : {
1266 14621718 : dist = 0.0f;
1267 :
1268 73108590 : for ( d = 0; d < dim; d++ )
1269 : {
1270 58486872 : tmp = x[d] - cb[j++];
1271 58486872 : dist += weights[d] * ( tmp * tmp );
1272 : }
1273 :
1274 14621718 : if ( dist < mindist )
1275 : {
1276 1403788 : mindist = dist;
1277 1403788 : idx = c;
1278 : }
1279 : }
1280 :
1281 266067 : if ( xq == 0 )
1282 : {
1283 175813 : return idx;
1284 : }
1285 :
1286 90254 : j = idx * dim;
1287 451270 : for ( d = 0; d < dim; d++ )
1288 : {
1289 361016 : xq[d] = cb[j++];
1290 : }
1291 : }
1292 :
1293 175813 : 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 175813 : 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 22544841 : 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 357797927 : for ( i = up - 1; i >= lo; i-- )
1321 : {
1322 335253086 : tempr = r[i];
1323 480847565 : for ( j = i + 1; j <= up && ( tempr > r[j] ); j++ )
1324 : {
1325 145594479 : r[j - 1] = r[j];
1326 : }
1327 :
1328 335253086 : r[j - 1] = tempr;
1329 : }
1330 :
1331 22544841 : return;
1332 : }
1333 :
1334 107427 : 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 906538 : for ( i = len - 2; i >= 0; i-- )
1343 : {
1344 799111 : tempr = x[i];
1345 1239026 : for ( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
1346 : {
1347 439915 : x[j - 1] = x[j];
1348 : }
1349 799111 : x[j - 1] = tempr;
1350 : }
1351 :
1352 107427 : return;
1353 : }
1354 :
1355 :
1356 : /*---------------------------------------------------------------------*
1357 : * var()
1358 : *
1359 : * Calculate the variance of a vector
1360 : *---------------------------------------------------------------------*/
1361 :
1362 : /*! r: variance of vector */
1363 15342626 : 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 15342626 : m = mean( x, len );
1373 :
1374 15342626 : v = 0.0f;
1375 107666999 : for ( i = 0; i < len; i++ )
1376 : {
1377 92324373 : v += ( x[i] - m ) * ( x[i] - m );
1378 : }
1379 15342626 : v /= len;
1380 :
1381 15342626 : 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 181635 : 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 181635 : std = 1e-16f;
1401 1634715 : for ( i = 0; i < len; i++ )
1402 : {
1403 1453080 : std += x[i] * x[i];
1404 : }
1405 :
1406 181635 : std = (float) sqrt( std / len );
1407 :
1408 181635 : 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 1744524 : 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 1744524 : pt_A = A;
1431 1744524 : suma = 0;
1432 :
1433 22678812 : for ( i = 0; i < m; i++ )
1434 : {
1435 20934288 : tmp_sum = 0;
1436 20934288 : pt_x = x;
1437 272145744 : for ( j = 0; j < m; j++ )
1438 : {
1439 251211456 : tmp_sum += *pt_x++ * *pt_A++;
1440 : }
1441 :
1442 20934288 : suma += x[i] * tmp_sum;
1443 : }
1444 :
1445 1744524 : 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 2152 : 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 20840 : for ( i = 0; i < order; i++ )
1474 : {
1475 18688 : out[i] = in[i] * b[0];
1476 95644 : for ( j = 0; j < i; j++ )
1477 : {
1478 76956 : out[i] += in[i - 1 - j] * b[j + 1] - out[i - 1 - j] * a[j + 1];
1479 : }
1480 :
1481 114332 : for ( k = order - 1; j < order; j++, k-- )
1482 : {
1483 95644 : out[i] += mem[k] * b[j + 1] - mem[k + order] * a[j + 1];
1484 : }
1485 : }
1486 :
1487 534376 : for ( ; i < N; i++ )
1488 : {
1489 532224 : out[i] = in[i] * b[0];
1490 5143752 : for ( j = 0; j < order; j++ )
1491 : {
1492 4611528 : out[i] += in[i - 1 - j] * b[j + 1] - out[i - 1 - j] * a[j + 1];
1493 : }
1494 : }
1495 :
1496 20840 : for ( i = 0; i < order; i++ )
1497 : {
1498 18688 : mem[i] = in[N - order + i];
1499 18688 : mem[i + order] = out[N - order + i];
1500 : }
1501 :
1502 2152 : return;
1503 : }
1504 :
1505 : #define WMC_TOOL_SKIP
1506 15722743 : static float fleft_shift( float input, const int16_t shift )
1507 : {
1508 15722743 : return ( input * (float) pow( 2.0, (double) shift ) );
1509 : }
1510 :
1511 1524088 : static float fright_shift( float input, const int16_t shift )
1512 : {
1513 1524088 : 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 1524088 : float root_a(
1534 : float a )
1535 : {
1536 : int16_t shift_a;
1537 : float mod_a;
1538 : float approx;
1539 :
1540 1524088 : 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 1524088 : mod_a = a;
1551 :
1552 1524088 : shift_a = 0;
1553 1524088 : while ( mod_a > 1.0 )
1554 : {
1555 0 : mod_a /= 2.0;
1556 0 : shift_a--;
1557 : }
1558 :
1559 6012747 : while ( mod_a < 0.5 )
1560 : {
1561 4488659 : mod_a *= 2.0;
1562 4488659 : shift_a++;
1563 : }
1564 : #undef WMC_TOOL_SKIP
1565 :
1566 1524088 : shift_a &= 0xfffe;
1567 1524088 : mod_a = fleft_shift( a, shift_a );
1568 :
1569 1524088 : approx = 0.27f + 1.0127f * mod_a - 0.2864f * mod_a * mod_a;
1570 :
1571 1524088 : approx = fright_shift( approx, ( shift_a >> 1 ) );
1572 :
1573 1524088 : 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 4736328 : 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 4736328 : float p2 = -0.7609f;
1603 4736328 : float p1 = 2.6908f;
1604 4736328 : float p0 = 0.7176f;
1605 : float b_sqr;
1606 : float approx;
1607 :
1608 4736328 : if ( ( a <= 0.0f ) || ( b <= 0.0f ) )
1609 : {
1610 3443 : return 0.0;
1611 : }
1612 : #define WMC_TOOL_SKIP
1613 4732885 : if ( isinf( a ) )
1614 : #undef WMC_TOOL_SKIP
1615 : {
1616 0 : return FLT_MAX;
1617 : }
1618 : #define WMC_TOOL_SKIP
1619 4732885 : if ( isinf( b ) )
1620 : #undef WMC_TOOL_SKIP
1621 : {
1622 0 : return 0.f;
1623 : }
1624 :
1625 4732885 : a += 0x00000001;
1626 4732885 : 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 4732885 : mod_a = a;
1634 :
1635 4732885 : shift_a = 0;
1636 26710964 : while ( mod_a > 1.0 )
1637 : {
1638 21978079 : mod_a /= 2.0;
1639 21978079 : shift_a--;
1640 : }
1641 :
1642 4732885 : while ( mod_a < 0.5 )
1643 : {
1644 0 : mod_a *= 2.0;
1645 0 : shift_a++;
1646 : }
1647 : #undef WMC_TOOL_SKIP
1648 :
1649 4732885 : shift_a &= 0xfffe;
1650 4732885 : 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 4732885 : mod_b = b;
1658 :
1659 4732885 : shift_b = 0;
1660 45891984 : while ( mod_b > 1.0 )
1661 : {
1662 41159099 : mod_b /= 2.0;
1663 41159099 : shift_b--;
1664 : }
1665 :
1666 4732885 : while ( mod_b < 0.5 )
1667 : {
1668 0 : mod_b *= 2.0;
1669 0 : shift_b++;
1670 : }
1671 : #undef WMC_TOOL_SKIP
1672 :
1673 4732885 : shift_b &= 0xfffe;
1674 4732885 : mod_b = fleft_shift( b, shift_b );
1675 :
1676 4732885 : shift = ( shift_b - shift_a ) >> 1;
1677 :
1678 4732885 : b_sqr = mod_b * mod_b;
1679 :
1680 4732885 : p2 += 0.9346f * mod_b + -0.4695f * b_sqr;
1681 4732885 : p1 += -3.3056f * mod_b + 1.6608f * b_sqr;
1682 4732885 : p0 += -0.8815f * mod_b + 0.4429f * b_sqr;
1683 :
1684 4732885 : approx = p0 + p1 * mod_a + p2 * mod_a * mod_a;
1685 :
1686 4732885 : approx = fleft_shift( approx, shift );
1687 :
1688 4732885 : return ( approx );
1689 : }
1690 :
1691 : /*--------------------------------------------------------------------------------*
1692 : * rint_new()
1693 : *
1694 : * Round to the nearest integer with mid-point exception
1695 : *---------------------------------------------------------------------------------*/
1696 :
1697 110838 : double rint_new(
1698 : double x )
1699 : {
1700 : int16_t a;
1701 :
1702 : /* middle value point test */
1703 110838 : if ( ceil( x + 0.5 ) == floor( x + 0.5 ) )
1704 : {
1705 389 : a = (int16_t) ceil( x );
1706 :
1707 389 : if ( a % 2 == 0 )
1708 : {
1709 207 : return ceil( x );
1710 : }
1711 : else
1712 : {
1713 182 : return floor( x );
1714 : }
1715 : }
1716 : else
1717 : {
1718 110449 : return floor( x + 0.5 );
1719 : }
1720 : }
1721 :
1722 :
1723 : /*-------------------------------------------------------------------*
1724 : * anint()
1725 : *
1726 : * Round to the nearest integer.
1727 : *-------------------------------------------------------------------*/
1728 :
1729 692747 : double anint(
1730 : double x )
1731 : {
1732 692747 : 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 1058910 : int16_t is_numeric_float(
1743 : float x )
1744 : {
1745 : #ifndef BASOP_NOGLOB
1746 : union float_int
1747 : #else /* BASOP_NOGLOB */
1748 : union float_int
1749 : #endif /* BASOP_NOGLOB */
1750 : {
1751 : float float_val;
1752 : int32_t int_val;
1753 : } float_int;
1754 :
1755 1058910 : float_int.float_val = x;
1756 :
1757 1058910 : return ( ( float_int.int_val & 0x7f800000 ) != 0x7f800000 );
1758 : }
1759 :
1760 : /*-------------------------------------------------------------------*
1761 : * delay_signal()
1762 : *
1763 : * Delay buffer by defined number of samples
1764 : *-------------------------------------------------------------------*/
1765 :
1766 116916842 : void delay_signal(
1767 : float x[], /* i/o: signal to be delayed */
1768 : const int16_t len, /* i : length of the input signal */
1769 : float mem[], /* i/o: synchronization memory */
1770 : const int16_t delay /* i : delay in samples */
1771 : )
1772 : {
1773 : float tmp_buffer[L_FRAME48k];
1774 :
1775 116916842 : mvr2r( mem, tmp_buffer, delay );
1776 116916842 : mvr2r( x + len - delay, mem, delay );
1777 116916842 : mvr2r( x, x + delay, len - delay );
1778 116916842 : mvr2r( tmp_buffer, x, delay );
1779 :
1780 116916842 : return;
1781 : }
|
