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 3362623458 : int16_t own_random(
54 : int16_t *seed /* i/o: random seed */
55 : )
56 : {
57 3362623458 : *seed = (int16_t) ( *seed * 31821L + 13849L );
58 :
59 3362623458 : return ( *seed );
60 : }
61 :
62 : /*---------------------------------------------------------------------
63 : * sign()
64 : *
65 : *---------------------------------------------------------------------*/
66 :
67 : /*! r: sign of x (+1/-1) */
68 10316033 : float sign(
69 : const float x /* i : input value of x */
70 : )
71 : {
72 10316033 : if ( x < 0.0f )
73 : {
74 4379189 : return -1.0f;
75 : }
76 : else
77 : {
78 5936844 : return 1.0f;
79 : }
80 : }
81 :
82 : /*---------------------------------------------------------------------
83 : * log2_f()
84 : *
85 : *---------------------------------------------------------------------*/
86 :
87 : /*! r: logarithm2 of x */
88 22057291 : float log2_f(
89 : const float x /* i : input value of x */
90 : )
91 : {
92 22057291 : return (float) ( log( x ) / log( 2.0f ) );
93 : }
94 :
95 4486336 : int16_t norm_ul( uint32_t UL_var1 )
96 : {
97 : int16_t var_out;
98 :
99 4486336 : if ( UL_var1 == 0 )
100 : {
101 0 : var_out = 0;
102 : }
103 : else
104 : {
105 60515416 : for ( var_out = 0; UL_var1 < (uint32_t) 0x80000000U; var_out++ )
106 : {
107 56029080 : UL_var1 <<= 1;
108 : }
109 : }
110 : BASOP_CHECK();
111 :
112 4486336 : 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 2992029 : 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 2992029 : tmp = 0;
133 28865210 : for ( i = 0; i < lvec; i++ )
134 : {
135 25873181 : tmp += vec[i];
136 : }
137 :
138 2992029 : 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 26294337 : 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 26294337 : tmp = 0.0f;
171 983815861 : for ( i = 0; i < lvec; i++ )
172 : {
173 957521524 : tmp += vec[i];
174 : }
175 :
176 26294337 : return tmp;
177 : }
178 :
179 : /*----------------------------------------------------------------------
180 : * sum2_f()
181 : *
182 : *---------------------------------------------------------------------*/
183 :
184 : /*! r: sum of all squared vector elements */
185 64013643 : 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 64013643 : tmp = 0.0f;
194 6672977610 : for ( i = 0; i < lvec; i++ )
195 : {
196 6608963967 : tmp += vec[i] * vec[i];
197 : }
198 :
199 64013643 : 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 2252893 : 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 1455307315 : for ( i = 0; i < N; i++ )
221 : {
222 1453054422 : y[i] = a;
223 : }
224 :
225 2252893 : return;
226 : }
227 :
228 :
229 83263209 : 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 6036170255 : for ( i = 0; i < N; i++ )
238 : {
239 5952907046 : y[i] = a;
240 : }
241 :
242 83263209 : return;
243 : }
244 :
245 :
246 23234 : 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 716330 : for ( i = 0; i < N; i++ )
255 : {
256 693096 : y[i] = a;
257 : }
258 :
259 23234 : return;
260 : }
261 :
262 756396403 : 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 74191329575 : for ( i = 0; i < N; i++ )
271 : {
272 73434933172 : y[i] = a;
273 : }
274 :
275 756396403 : return;
276 : }
277 :
278 :
279 : /*---------------------------------------------------------------------*
280 : * set_zero()
281 : *
282 : * Set a vector vec[] of dimension lvec to zero
283 : *---------------------------------------------------------------------*/
284 :
285 1240881874 : 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 50970553211 : for ( i = 0; i < lvec; i++ )
293 : {
294 49729671337 : *vec++ = 0.0f;
295 : }
296 :
297 1240881874 : 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 997264767 : 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 997264767 : if ( n <= 0 )
321 : {
322 : /* cannot transfer vectors with size 0 */
323 11028500 : return;
324 : }
325 :
326 986236267 : if ( y < x )
327 : {
328 >12965*10^7 : for ( i = 0; i < n; i++ )
329 : {
330 >12915*10^7 : y[i] = x[i];
331 : }
332 : }
333 : else
334 : {
335 >11890*10^7 : for ( i = n - 1; i >= 0; i-- )
336 : {
337 >11841*10^7 : y[i] = x[i];
338 : }
339 : }
340 :
341 986236267 : return;
342 : }
343 :
344 46029470 : 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 46029470 : if ( n <= 0 )
353 : {
354 : /* cannot transfer vectors with size 0 */
355 4569164 : return;
356 : }
357 :
358 41460306 : if ( y < x )
359 : {
360 111980146 : for ( i = 0; i < n; i++ )
361 : {
362 99008698 : y[i] = x[i];
363 : }
364 : }
365 : else
366 : {
367 383436625 : for ( i = n - 1; i >= 0; i-- )
368 : {
369 354947767 : y[i] = x[i];
370 : }
371 : }
372 :
373 41460306 : 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 11410133 : 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 11410133 : uint32_t noClipping = 0;
425 :
426 11410133 : if ( n <= 0 )
427 : {
428 : /* cannot transfer vectors with size 0 */
429 552 : return 0;
430 : }
431 :
432 11409581 : if ( (void *) y <= (const void *) x )
433 : {
434 1682100 : for ( i = 0; i < n; i++ )
435 : {
436 1680000 : temp = x[i];
437 1680000 : temp = (float) floor( temp + 0.5f );
438 :
439 1680000 : if ( temp > MAX16B_FLT )
440 : {
441 0 : temp = MAX16B_FLT;
442 0 : noClipping++;
443 : }
444 1680000 : else if ( temp < MIN16B_FLT )
445 : {
446 0 : temp = MIN16B_FLT;
447 0 : noClipping++;
448 : }
449 :
450 1680000 : y[i] = (int16_t) temp;
451 : }
452 : }
453 : else
454 : {
455 3788173328 : for ( i = n - 1; i >= 0; i-- )
456 : {
457 3776765847 : temp = x[i];
458 3776765847 : temp = (float) floor( temp + 0.5f );
459 :
460 3776765847 : if ( temp > MAX16B_FLT )
461 : {
462 6921 : temp = MAX16B_FLT;
463 6921 : noClipping++;
464 : }
465 3776758926 : else if ( temp < MIN16B_FLT )
466 : {
467 6335 : temp = MIN16B_FLT;
468 6335 : noClipping++;
469 : }
470 :
471 3776765847 : y[i] = (int16_t) temp;
472 : }
473 : }
474 :
475 11409581 : return noClipping;
476 : }
477 :
478 1269219 : 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 1269219 : if ( n <= 0 )
487 : {
488 : /* cannot transfer vectors with size 0 */
489 0 : return;
490 : }
491 :
492 1269219 : if ( (void *) y < (const void *) x )
493 : {
494 3225246 : for ( i = 0; i < n; i++ )
495 : {
496 1956027 : 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 1269219 : return;
508 : }
509 :
510 :
511 18384 : 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 18384 : if ( n <= 0 )
520 : {
521 : /* no need to transfer vectors with size 0 */
522 0 : return;
523 : }
524 :
525 18384 : if ( y < x )
526 : {
527 108785 : for ( i = 0; i < n; i++ )
528 : {
529 99881 : y[i] = x[i];
530 : }
531 : }
532 : else
533 : {
534 123240 : for ( i = n - 1; i >= 0; i-- )
535 : {
536 113760 : y[i] = x[i];
537 : }
538 : }
539 :
540 18384 : 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 58755738 : 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 58755738 : ind = 0;
561 58755738 : tmp = vec[0];
562 :
563 956693648 : for ( j = 1; j < lvec; j++ )
564 : {
565 897937910 : if ( vec[j] > tmp )
566 : {
567 155388449 : ind = j;
568 155388449 : tmp = vec[j];
569 : }
570 : }
571 :
572 58755738 : if ( max_val != NULL )
573 : {
574 13224837 : *max_val = tmp;
575 : }
576 :
577 58755738 : return ind;
578 : }
579 :
580 :
581 : /*! r: index of the maximum value in the input vector */
582 55988 : 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 55988 : ind = 0;
592 55988 : tmp = vec[0];
593 :
594 484117 : for ( i = 1; i < lvec; i++ )
595 : {
596 428129 : if ( vec[i] > tmp )
597 : {
598 36437 : ind = i;
599 36437 : tmp = vec[i];
600 : }
601 : }
602 :
603 55988 : if ( max != NULL )
604 : {
605 55988 : *max = tmp;
606 : }
607 :
608 55988 : 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 1624053 : 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 1624053 : ind = 0;
628 1624053 : tmp = (float) fabs( vec[0] );
629 :
630 60819951 : for ( j = 1; j < lvec; j++ )
631 : {
632 59195898 : if ( (float) fabs( vec[j] ) > tmp )
633 : {
634 5539360 : ind = j;
635 5539360 : tmp = (float) fabs( vec[j] );
636 : }
637 : }
638 :
639 1624053 : if ( max_val != NULL )
640 : {
641 1619052 : *max_val = tmp;
642 : }
643 :
644 1624053 : 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 1365416 : 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 1365416 : ind = 0;
664 1365416 : tmp = vec[0];
665 :
666 12048881 : for ( j = 1; j < lvec; j++ )
667 : {
668 10683465 : if ( vec[j] < tmp )
669 : {
670 2504547 : ind = j;
671 2504547 : tmp = vec[j];
672 : }
673 : }
674 :
675 1365416 : if ( min_val != NULL )
676 : {
677 1232327 : *min_val = tmp;
678 : }
679 :
680 1365416 : 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 42732 : 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 42732 : ind = 0;
699 42732 : tmp = vec[0];
700 :
701 458791 : for ( i = 1; i < lvec; i++ )
702 : {
703 416059 : if ( vec[i] < tmp )
704 : {
705 19671 : ind = i;
706 19671 : tmp = vec[i];
707 : }
708 : }
709 :
710 42732 : if ( min_val != NULL )
711 : {
712 42732 : *min_val = tmp;
713 : }
714 :
715 42732 : 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 20838739 : 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 20838739 : *ener_max = 0.0f;
736 20838739 : ind = 0;
737 :
738 711916478 : for ( j = 0; j < lvec; j++ )
739 : {
740 691077739 : temp = vec[j] * vec[j];
741 :
742 691077739 : if ( temp > *ener_max )
743 : {
744 104417793 : ind = j;
745 104417793 : *ener_max = temp;
746 : }
747 : }
748 :
749 20838739 : return ind;
750 : }
751 :
752 :
753 : /*---------------------------------------------------------------------*
754 : * mean()
755 : *
756 : * Find the mean of the vector
757 : *---------------------------------------------------------------------*/
758 :
759 : /*! r: mean of vector */
760 14136448 : 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 14136448 : tmp = sum_f( vec, lvec ) / (float) lvec;
768 :
769 14136448 : 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 321275148 : 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 321275148 : suma = x[0] * y[0];
789 :
790 28396144004 : for ( i = 1; i < n; i++ )
791 : {
792 28074868856 : suma += x[i] * y[i];
793 : }
794 :
795 321275148 : 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 168544945 : float inv_sqrt(
807 : const float x /* i : input value */
808 : )
809 : {
810 168544945 : 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 1675798 : float inv_sqrtf(
821 : const float x /* i : input value */
822 : )
823 : {
824 1675798 : 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 1804292 : 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 120617380 : for ( n = 0; n < L; n++ )
845 : {
846 118813088 : temp = x[0] * h[n];
847 4297247984 : for ( i = 1; i <= n; i++ )
848 : {
849 4178434896 : temp += x[i] * h[n - i];
850 : }
851 118813088 : y[n] = temp;
852 : }
853 :
854 1804292 : 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 504583 : 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 504583 : mvr2r( mem, buf_in, K );
884 504583 : mvr2r( x, buf_in + K, L );
885 :
886 504583 : if ( upd )
887 : {
888 97 : mvr2r( buf_in + L, mem, K );
889 : }
890 :
891 : /* do the filtering */
892 155681223 : for ( i = 0; i < L; i++ )
893 : {
894 155176640 : s = buf_in[K + i] * h[0];
895 :
896 804225280 : for ( j = 1; j <= K; j++ )
897 : {
898 649048640 : s += h[j] * buf_in[K + i - j];
899 : }
900 :
901 155176640 : buf_out[i] = s;
902 : }
903 :
904 : /* copy to the output buffer */
905 504583 : mvr2r( buf_out, y, L );
906 :
907 504583 : return;
908 : }
909 :
910 : /*-------------------------------------------------------------------*
911 : * v_add()
912 : *
913 : * Addition of two vectors sample by sample
914 : *-------------------------------------------------------------------*/
915 :
916 1584204048 : 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 20523045363 : for ( i = 0; i < N; i++ )
926 : {
927 18938841315 : y[i] = x1[i] + x2[i];
928 : }
929 :
930 1584204048 : return;
931 : }
932 :
933 :
934 : /*-------------------------------------------------------------------*
935 : * v_sub()
936 : *
937 : * Subtraction of two vectors sample by sample
938 : *-------------------------------------------------------------------*/
939 :
940 1621016460 : 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 8681124114 : for ( i = 0; i < N; i++ )
950 : {
951 7060107654 : y[i] = x1[i] - x2[i];
952 : }
953 :
954 1621016460 : return;
955 : }
956 :
957 : /*-------------------------------------------------------------------*
958 : * v_mult()
959 : *
960 : * Multiplication of two vectors
961 : *-------------------------------------------------------------------*/
962 :
963 82210692 : 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 3692459224 : for ( i = 0; i < N; i++ )
973 : {
974 3610248532 : y[i] = x1[i] * x2[i];
975 : }
976 :
977 82210692 : return;
978 : }
979 :
980 : /*-------------------------------------------------------------------*
981 : * v_multc()
982 : *
983 : * Multiplication of vector by constant
984 : *-------------------------------------------------------------------*/
985 :
986 181250032 : 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 19920861161 : for ( i = 0; i < N; i++ )
996 : {
997 19739611129 : y[i] = c * x[i];
998 : }
999 :
1000 181250032 : 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 5054901 : 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 5054901 : idx = 0;
1025 5054901 : mindist = 1e16f;
1026 :
1027 25217892 : for ( c = 0; c < cbsize; c++ )
1028 : {
1029 20162991 : dist = 0.0f;
1030 20162991 : tmp = x - cb[c];
1031 20162991 : dist += tmp * tmp;
1032 20162991 : if ( dist < mindist )
1033 : {
1034 11545267 : mindist = dist;
1035 11545267 : idx = c;
1036 : }
1037 : }
1038 :
1039 5054901 : *xq = cb[idx];
1040 :
1041 5054901 : return idx;
1042 : }
1043 :
1044 : /*! r: index of the winning codeword */
1045 401123 : 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 401123 : idx = 0;
1056 401123 : mindist = 10000000.0f;
1057 3347997 : for ( i = 0; i < cbsize; i++ )
1058 : {
1059 2946874 : d = (float) ( x - cb[i] ) * ( x - cb[i] );
1060 2946874 : if ( d < mindist )
1061 : {
1062 723791 : mindist = d;
1063 723791 : idx = i;
1064 : }
1065 : }
1066 401123 : *xq = cb[idx];
1067 :
1068 401123 : 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 1677272 : 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 1677272 : idx = (int16_t) max( 0.f, min( cbsize - 1, ( ( x - qlow ) / delta + 0.5f ) ) );
1094 1677272 : *xq = idx * delta + qlow;
1095 :
1096 1677272 : 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 1857835 : 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 1857835 : g = idx * delta + qlow;
1117 :
1118 1857835 : 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 243089 : 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 243089 : idx = 0;
1145 243089 : mindist = 1e16f;
1146 :
1147 243089 : if ( x_mean != 0 )
1148 : {
1149 481596 : for ( d = 0; d < dim; d++ )
1150 : {
1151 356958 : x[d] -= x_mean[d];
1152 : }
1153 : }
1154 :
1155 243089 : j = 0;
1156 8906161 : for ( c = 0; c < cbsize; c++ )
1157 : {
1158 8663072 : dist = 0.0f;
1159 37766320 : for ( d = 0; d < dim; d++ )
1160 : {
1161 29103248 : tmp = x[d] - cb[j++];
1162 29103248 : dist += tmp * tmp;
1163 : }
1164 :
1165 8663072 : if ( dist < mindist )
1166 : {
1167 955071 : mindist = dist;
1168 955071 : idx = c;
1169 : }
1170 : }
1171 :
1172 243089 : if ( xq == 0 )
1173 : {
1174 0 : return idx;
1175 : }
1176 :
1177 243089 : j = idx * dim;
1178 1073851 : for ( d = 0; d < dim; d++ )
1179 : {
1180 830762 : xq[d] = cb[j++];
1181 : }
1182 :
1183 243089 : if ( x_mean != 0 )
1184 : {
1185 481596 : for ( d = 0; d < dim; d++ )
1186 : {
1187 356958 : xq[d] += x_mean[d];
1188 : }
1189 : }
1190 :
1191 243089 : 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 45432 : 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 45432 : idx = 0;
1220 45432 : mindist = 1e16f;
1221 :
1222 45432 : 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 45432 : j = 0;
1231 45432 : if ( rev_vect )
1232 : {
1233 11227 : k = dim - 1;
1234 1679031 : for ( c = 0; c < cbsize; c++ )
1235 : {
1236 1667804 : dist = 0.0f;
1237 :
1238 8339020 : for ( d = k; d >= 0; d-- )
1239 : {
1240 6671216 : tmp = x[d] - cb[j++];
1241 6671216 : dist += weights[d] * ( tmp * tmp );
1242 : }
1243 :
1244 1667804 : if ( dist < mindist )
1245 : {
1246 119810 : mindist = dist;
1247 119810 : idx = c;
1248 : }
1249 : }
1250 :
1251 11227 : if ( xq == 0 )
1252 : {
1253 0 : return idx;
1254 : }
1255 :
1256 11227 : j = idx * dim;
1257 56135 : for ( d = k; d >= 0; d-- )
1258 : {
1259 44908 : xq[d] = cb[j++];
1260 : }
1261 : }
1262 : else
1263 : {
1264 1832890 : for ( c = 0; c < cbsize; c++ )
1265 : {
1266 1798685 : dist = 0.0f;
1267 :
1268 8993425 : for ( d = 0; d < dim; d++ )
1269 : {
1270 7194740 : tmp = x[d] - cb[j++];
1271 7194740 : dist += weights[d] * ( tmp * tmp );
1272 : }
1273 :
1274 1798685 : if ( dist < mindist )
1275 : {
1276 176394 : mindist = dist;
1277 176394 : idx = c;
1278 : }
1279 : }
1280 :
1281 34205 : if ( xq == 0 )
1282 : {
1283 22716 : return idx;
1284 : }
1285 :
1286 11489 : j = idx * dim;
1287 57445 : for ( d = 0; d < dim; d++ )
1288 : {
1289 45956 : xq[d] = cb[j++];
1290 : }
1291 : }
1292 :
1293 22716 : 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 22716 : 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 2305978 : 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 36399522 : for ( i = up - 1; i >= lo; i-- )
1321 : {
1322 34093544 : tempr = r[i];
1323 40893574 : for ( j = i + 1; j <= up && ( tempr > r[j] ); j++ )
1324 : {
1325 6800030 : r[j - 1] = r[j];
1326 : }
1327 :
1328 34093544 : r[j - 1] = tempr;
1329 : }
1330 :
1331 2305978 : return;
1332 : }
1333 :
1334 5662 : 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 43849 : for ( i = len - 2; i >= 0; i-- )
1343 : {
1344 38187 : tempr = x[i];
1345 64607 : for ( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
1346 : {
1347 26420 : x[j - 1] = x[j];
1348 : }
1349 38187 : x[j - 1] = tempr;
1350 : }
1351 :
1352 5662 : return;
1353 : }
1354 :
1355 :
1356 : /*---------------------------------------------------------------------*
1357 : * var()
1358 : *
1359 : * Calculate the variance of a vector
1360 : *---------------------------------------------------------------------*/
1361 :
1362 : /*! r: variance of vector */
1363 1028707 : 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 1028707 : m = mean( x, len );
1373 :
1374 1028707 : v = 0.0f;
1375 7766578 : for ( i = 0; i < len; i++ )
1376 : {
1377 6737871 : v += ( x[i] - m ) * ( x[i] - m );
1378 : }
1379 1028707 : v /= len;
1380 :
1381 1028707 : 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 6150 : 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 6150 : std = 1e-16f;
1401 55350 : for ( i = 0; i < len; i++ )
1402 : {
1403 49200 : std += x[i] * x[i];
1404 : }
1405 :
1406 6150 : std = (float) sqrt( std / len );
1407 :
1408 6150 : 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 55800 : 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 55800 : pt_A = A;
1431 55800 : suma = 0;
1432 :
1433 725400 : for ( i = 0; i < m; i++ )
1434 : {
1435 669600 : tmp_sum = 0;
1436 669600 : pt_x = x;
1437 8704800 : for ( j = 0; j < m; j++ )
1438 : {
1439 8035200 : tmp_sum += *pt_x++ * *pt_A++;
1440 : }
1441 :
1442 669600 : suma += x[i] * tmp_sum;
1443 : }
1444 :
1445 55800 : 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 0 : 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 0 : for ( i = 0; i < order; i++ )
1474 : {
1475 0 : out[i] = in[i] * b[0];
1476 0 : for ( j = 0; j < i; j++ )
1477 : {
1478 0 : out[i] += in[i - 1 - j] * b[j + 1] - out[i - 1 - j] * a[j + 1];
1479 : }
1480 :
1481 0 : for ( k = order - 1; j < order; j++, k-- )
1482 : {
1483 0 : out[i] += mem[k] * b[j + 1] - mem[k + order] * a[j + 1];
1484 : }
1485 : }
1486 :
1487 0 : for ( ; i < N; i++ )
1488 : {
1489 0 : out[i] = in[i] * b[0];
1490 0 : for ( j = 0; j < order; j++ )
1491 : {
1492 0 : out[i] += in[i - 1 - j] * b[j + 1] - out[i - 1 - j] * a[j + 1];
1493 : }
1494 : }
1495 :
1496 0 : for ( i = 0; i < order; i++ )
1497 : {
1498 0 : mem[i] = in[N - order + i];
1499 0 : mem[i + order] = out[N - order + i];
1500 : }
1501 :
1502 0 : return;
1503 : }
1504 :
1505 : #define WMC_TOOL_SKIP
1506 1513074 : static float fleft_shift( float input, const int16_t shift )
1507 : {
1508 1513074 : return ( input * (float) pow( 2.0, (double) shift ) );
1509 : }
1510 :
1511 101928 : static float fright_shift( float input, const int16_t shift )
1512 : {
1513 101928 : 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 101928 : float root_a(
1534 : float a )
1535 : {
1536 : int16_t shift_a;
1537 : float mod_a;
1538 : float approx;
1539 :
1540 101928 : 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 101928 : mod_a = a;
1551 :
1552 101928 : shift_a = 0;
1553 101928 : while ( mod_a > 1.0 )
1554 : {
1555 0 : mod_a /= 2.0;
1556 0 : shift_a--;
1557 : }
1558 :
1559 393263 : while ( mod_a < 0.5 )
1560 : {
1561 291335 : mod_a *= 2.0;
1562 291335 : shift_a++;
1563 : }
1564 : #undef WMC_TOOL_SKIP
1565 :
1566 101928 : shift_a &= 0xfffe;
1567 101928 : mod_a = fleft_shift( a, shift_a );
1568 :
1569 101928 : approx = 0.27f + 1.0127f * mod_a - 0.2864f * mod_a * mod_a;
1570 :
1571 101928 : approx = fright_shift( approx, ( shift_a >> 1 ) );
1572 :
1573 101928 : 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 470382 : 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 470382 : float p2 = -0.7609f;
1603 470382 : float p1 = 2.6908f;
1604 470382 : float p0 = 0.7176f;
1605 : float b_sqr;
1606 : float approx;
1607 :
1608 470382 : if ( ( a <= 0.0f ) || ( b <= 0.0f ) )
1609 : {
1610 0 : return 0.0;
1611 : }
1612 : #define WMC_TOOL_SKIP
1613 470382 : if ( isinf( a ) )
1614 : #undef WMC_TOOL_SKIP
1615 : {
1616 0 : return FLT_MAX;
1617 : }
1618 : #define WMC_TOOL_SKIP
1619 470382 : if ( isinf( b ) )
1620 : #undef WMC_TOOL_SKIP
1621 : {
1622 0 : return 0.f;
1623 : }
1624 :
1625 470382 : a += 0x00000001;
1626 470382 : 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 470382 : mod_a = a;
1634 :
1635 470382 : shift_a = 0;
1636 1972827 : while ( mod_a > 1.0 )
1637 : {
1638 1502445 : mod_a /= 2.0;
1639 1502445 : shift_a--;
1640 : }
1641 :
1642 470382 : while ( mod_a < 0.5 )
1643 : {
1644 0 : mod_a *= 2.0;
1645 0 : shift_a++;
1646 : }
1647 : #undef WMC_TOOL_SKIP
1648 :
1649 470382 : shift_a &= 0xfffe;
1650 470382 : 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 470382 : mod_b = b;
1658 :
1659 470382 : shift_b = 0;
1660 3258087 : while ( mod_b > 1.0 )
1661 : {
1662 2787705 : mod_b /= 2.0;
1663 2787705 : shift_b--;
1664 : }
1665 :
1666 470382 : while ( mod_b < 0.5 )
1667 : {
1668 0 : mod_b *= 2.0;
1669 0 : shift_b++;
1670 : }
1671 : #undef WMC_TOOL_SKIP
1672 :
1673 470382 : shift_b &= 0xfffe;
1674 470382 : mod_b = fleft_shift( b, shift_b );
1675 :
1676 470382 : shift = ( shift_b - shift_a ) >> 1;
1677 :
1678 470382 : b_sqr = mod_b * mod_b;
1679 :
1680 470382 : p2 += 0.9346f * mod_b + -0.4695f * b_sqr;
1681 470382 : p1 += -3.3056f * mod_b + 1.6608f * b_sqr;
1682 470382 : p0 += -0.8815f * mod_b + 0.4429f * b_sqr;
1683 :
1684 470382 : approx = p0 + p1 * mod_a + p2 * mod_a * mod_a;
1685 :
1686 470382 : approx = fleft_shift( approx, shift );
1687 :
1688 470382 : return ( approx );
1689 : }
1690 :
1691 : /*--------------------------------------------------------------------------------*
1692 : * rint_new()
1693 : *
1694 : * Round to the nearest integer with mid-point exception
1695 : *---------------------------------------------------------------------------------*/
1696 :
1697 0 : double rint_new(
1698 : double x )
1699 : {
1700 : int16_t a;
1701 :
1702 : /* middle value point test */
1703 0 : if ( ceil( x + 0.5 ) == floor( x + 0.5 ) )
1704 : {
1705 0 : a = (int16_t) ceil( x );
1706 :
1707 0 : if ( a % 2 == 0 )
1708 : {
1709 0 : return ceil( x );
1710 : }
1711 : else
1712 : {
1713 0 : return floor( x );
1714 : }
1715 : }
1716 : else
1717 : {
1718 0 : return floor( x + 0.5 );
1719 : }
1720 : }
1721 :
1722 :
1723 : /*-------------------------------------------------------------------*
1724 : * anint()
1725 : *
1726 : * Round to the nearest integer.
1727 : *-------------------------------------------------------------------*/
1728 :
1729 47500 : double anint(
1730 : double x )
1731 : {
1732 47500 : 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 116934 : 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 116934 : float_int.float_val = x;
1756 :
1757 116934 : 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 10899996 : 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 10899996 : mvr2r( mem, tmp_buffer, delay );
1776 10899996 : mvr2r( x + len - delay, mem, delay );
1777 10899996 : mvr2r( x, x + delay, len - delay );
1778 10899996 : mvr2r( tmp_buffer, x, delay );
1779 :
1780 10899996 : return;
1781 : }
|
