1 | /* |
---|
2 | Copyright (C) 2003 Paul Brossier |
---|
3 | |
---|
4 | This program is free software; you can redistribute it and/or modify |
---|
5 | it under the terms of the GNU General Public License as published by |
---|
6 | the Free Software Foundation; either version 2 of the License, or |
---|
7 | (at your option) any later version. |
---|
8 | |
---|
9 | This program is distributed in the hope that it will be useful, |
---|
10 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
---|
12 | GNU General Public License for more details. |
---|
13 | |
---|
14 | You should have received a copy of the GNU General Public License |
---|
15 | along with this program; if not, write to the Free Software |
---|
16 | Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. |
---|
17 | |
---|
18 | */ |
---|
19 | |
---|
20 | #include "aubio_priv.h" |
---|
21 | #include "fvec.h" |
---|
22 | #include "mathutils.h" |
---|
23 | #include "temporal/biquad.h" |
---|
24 | |
---|
25 | /** \note this file needs to be in double or more less precision would lead to large |
---|
26 | * errors in the output |
---|
27 | */ |
---|
28 | |
---|
29 | struct _aubio_biquad_t { |
---|
30 | lsmp_t a2; |
---|
31 | lsmp_t a3; |
---|
32 | lsmp_t b1; |
---|
33 | lsmp_t b2; |
---|
34 | lsmp_t b3; |
---|
35 | lsmp_t o1; |
---|
36 | lsmp_t o2; |
---|
37 | lsmp_t i1; |
---|
38 | lsmp_t i2; |
---|
39 | }; |
---|
40 | |
---|
41 | void aubio_biquad_do(aubio_biquad_t * b, fvec_t * in) { |
---|
42 | uint_t i,j; |
---|
43 | lsmp_t i1 = b->i1; |
---|
44 | lsmp_t i2 = b->i2; |
---|
45 | lsmp_t o1 = b->o1; |
---|
46 | lsmp_t o2 = b->o2; |
---|
47 | lsmp_t a2 = b->a2; |
---|
48 | lsmp_t a3 = b->a3; |
---|
49 | lsmp_t b1 = b->b1; |
---|
50 | lsmp_t b2 = b->b2; |
---|
51 | lsmp_t b3 = b->b3; |
---|
52 | |
---|
53 | i=0; // works in mono only !!! |
---|
54 | //for (i=0;i<in->channels;i++) { |
---|
55 | for (j = 0; j < in->length; j++) { |
---|
56 | lsmp_t i0 = in->data[i][j]; |
---|
57 | lsmp_t o0 = b1 * i0 + b2 * i1 + b3 * i2 |
---|
58 | - a2 * o1 - a3 * o2;// + 1e-37; |
---|
59 | in->data[i][j] = o0; |
---|
60 | i2 = i1; |
---|
61 | i1 = i0; |
---|
62 | o2 = o1; |
---|
63 | o1 = o0; |
---|
64 | } |
---|
65 | b->i2 = i2; |
---|
66 | b->i1 = i1; |
---|
67 | b->o2 = o2; |
---|
68 | b->o1 = o1; |
---|
69 | //} |
---|
70 | } |
---|
71 | |
---|
72 | void aubio_biquad_do_filtfilt(aubio_biquad_t * b, fvec_t * in, fvec_t * tmp) { |
---|
73 | uint_t j,i=0; |
---|
74 | uint_t length = in->length; |
---|
75 | lsmp_t mir; |
---|
76 | /* mirroring */ |
---|
77 | mir = 2*in->data[i][0]; |
---|
78 | b->i1 = mir - in->data[i][2]; |
---|
79 | b->i2 = mir - in->data[i][1]; |
---|
80 | /* apply filtering */ |
---|
81 | aubio_biquad_do(b,in); |
---|
82 | /* invert */ |
---|
83 | for (j = 0; j < length; j++) |
---|
84 | tmp->data[i][length-j-1] = in->data[i][j]; |
---|
85 | /* mirror again */ |
---|
86 | mir = 2*tmp->data[i][0]; |
---|
87 | b->i1 = mir - tmp->data[i][2]; |
---|
88 | b->i2 = mir - tmp->data[i][1]; |
---|
89 | /* apply filtering */ |
---|
90 | aubio_biquad_do(b,tmp); |
---|
91 | /* invert back */ |
---|
92 | for (j = 0; j < length; j++) |
---|
93 | in->data[i][j] = tmp->data[i][length-j-1]; |
---|
94 | } |
---|
95 | |
---|
96 | aubio_biquad_t * new_aubio_biquad( |
---|
97 | lsmp_t b1, lsmp_t b2, lsmp_t b3, |
---|
98 | lsmp_t a2, lsmp_t a3) { |
---|
99 | aubio_biquad_t * b = AUBIO_NEW(aubio_biquad_t); |
---|
100 | b->a2 = a2; |
---|
101 | b->a3 = a3; |
---|
102 | b->b1 = b1; |
---|
103 | b->b2 = b2; |
---|
104 | b->b3 = b3; |
---|
105 | b->i1 = 0.; |
---|
106 | b->i2 = 0.; |
---|
107 | b->o1 = 0.; |
---|
108 | b->o2 = 0.; |
---|
109 | return b; |
---|
110 | } |
---|
111 | |
---|
112 | void del_aubio_biquad(aubio_biquad_t * b) { |
---|
113 | AUBIO_FREE(b); |
---|
114 | } |
---|