ffmpeg / libavcodec / iirfilter.c @ d42dc217
History | View | Annotate | Download (8.27 KB)
1 |
/*
|
---|---|
2 |
* IIR filter
|
3 |
* Copyright (c) 2008 Konstantin Shishkov
|
4 |
*
|
5 |
* This file is part of FFmpeg.
|
6 |
*
|
7 |
* FFmpeg is free software; you can redistribute it and/or
|
8 |
* modify it under the terms of the GNU Lesser General Public
|
9 |
* License as published by the Free Software Foundation; either
|
10 |
* version 2.1 of the License, or (at your option) any later version.
|
11 |
*
|
12 |
* FFmpeg is distributed in the hope that it will be useful,
|
13 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
14 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
15 |
* Lesser General Public License for more details.
|
16 |
*
|
17 |
* You should have received a copy of the GNU Lesser General Public
|
18 |
* License along with FFmpeg; if not, write to the Free Software
|
19 |
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
20 |
*/
|
21 |
|
22 |
/**
|
23 |
* @file
|
24 |
* different IIR filters implementation
|
25 |
*/
|
26 |
|
27 |
#include "iirfilter.h" |
28 |
#include <math.h> |
29 |
|
30 |
/**
|
31 |
* IIR filter global parameters
|
32 |
*/
|
33 |
typedef struct FFIIRFilterCoeffs{ |
34 |
int order;
|
35 |
float gain;
|
36 |
int *cx;
|
37 |
float *cy;
|
38 |
}FFIIRFilterCoeffs; |
39 |
|
40 |
/**
|
41 |
* IIR filter state
|
42 |
*/
|
43 |
typedef struct FFIIRFilterState{ |
44 |
float x[1]; |
45 |
}FFIIRFilterState; |
46 |
|
47 |
/// maximum supported filter order
|
48 |
#define MAXORDER 30 |
49 |
|
50 |
av_cold struct FFIIRFilterCoeffs* ff_iir_filter_init_coeffs(void *avc, |
51 |
enum IIRFilterType filt_type,
|
52 |
enum IIRFilterMode filt_mode,
|
53 |
int order, float cutoff_ratio, |
54 |
float stopband, float ripple) |
55 |
{ |
56 |
int i, j;
|
57 |
FFIIRFilterCoeffs *c; |
58 |
double wa;
|
59 |
double p[MAXORDER + 1][2]; |
60 |
|
61 |
if(filt_type != FF_FILTER_TYPE_BUTTERWORTH || filt_mode != FF_FILTER_MODE_LOWPASS)
|
62 |
return NULL; |
63 |
if(order <= 1 || (order & 1) || order > MAXORDER || cutoff_ratio >= 1.0) |
64 |
return NULL; |
65 |
|
66 |
FF_ALLOCZ_OR_GOTO(avc, c, sizeof(FFIIRFilterCoeffs),
|
67 |
init_fail); |
68 |
FF_ALLOC_OR_GOTO (avc, c->cx, sizeof(c->cx[0]) * ((order >> 1) + 1), |
69 |
init_fail); |
70 |
FF_ALLOC_OR_GOTO (avc, c->cy, sizeof(c->cy[0]) * order, |
71 |
init_fail); |
72 |
c->order = order; |
73 |
|
74 |
wa = 2 * tan(M_PI * 0.5 * cutoff_ratio); |
75 |
|
76 |
c->cx[0] = 1; |
77 |
for(i = 1; i < (order >> 1) + 1; i++) |
78 |
c->cx[i] = c->cx[i - 1] * (order - i + 1LL) / i; |
79 |
|
80 |
p[0][0] = 1.0; |
81 |
p[0][1] = 0.0; |
82 |
for(i = 1; i <= order; i++) |
83 |
p[i][0] = p[i][1] = 0.0; |
84 |
for(i = 0; i < order; i++){ |
85 |
double zp[2]; |
86 |
double th = (i + (order >> 1) + 0.5) * M_PI / order; |
87 |
double a_re, a_im, c_re, c_im;
|
88 |
zp[0] = cos(th) * wa;
|
89 |
zp[1] = sin(th) * wa;
|
90 |
a_re = zp[0] + 2.0; |
91 |
c_re = zp[0] - 2.0; |
92 |
a_im = |
93 |
c_im = zp[1];
|
94 |
zp[0] = (a_re * c_re + a_im * c_im) / (c_re * c_re + c_im * c_im);
|
95 |
zp[1] = (a_im * c_re - a_re * c_im) / (c_re * c_re + c_im * c_im);
|
96 |
|
97 |
for(j = order; j >= 1; j--) |
98 |
{ |
99 |
a_re = p[j][0];
|
100 |
a_im = p[j][1];
|
101 |
p[j][0] = a_re*zp[0] - a_im*zp[1] + p[j-1][0]; |
102 |
p[j][1] = a_re*zp[1] + a_im*zp[0] + p[j-1][1]; |
103 |
} |
104 |
a_re = p[0][0]*zp[0] - p[0][1]*zp[1]; |
105 |
p[0][1] = p[0][0]*zp[1] + p[0][1]*zp[0]; |
106 |
p[0][0] = a_re; |
107 |
} |
108 |
c->gain = p[order][0];
|
109 |
for(i = 0; i < order; i++){ |
110 |
c->gain += p[i][0];
|
111 |
c->cy[i] = (-p[i][0] * p[order][0] + -p[i][1] * p[order][1]) / |
112 |
(p[order][0] * p[order][0] + p[order][1] * p[order][1]); |
113 |
} |
114 |
c->gain /= 1 << order;
|
115 |
|
116 |
return c;
|
117 |
|
118 |
init_fail:
|
119 |
ff_iir_filter_free_coeffs(c); |
120 |
return NULL; |
121 |
} |
122 |
|
123 |
av_cold struct FFIIRFilterState* ff_iir_filter_init_state(int order) |
124 |
{ |
125 |
FFIIRFilterState* s = av_mallocz(sizeof(FFIIRFilterState) + sizeof(s->x[0]) * (order - 1)); |
126 |
return s;
|
127 |
} |
128 |
|
129 |
#define CONV_S16(dest, source) dest = av_clip_int16(lrintf(source));
|
130 |
|
131 |
#define CONV_FLT(dest, source) dest = source;
|
132 |
|
133 |
#define FILTER_BW_O4_1(i0, i1, i2, i3, fmt) \
|
134 |
in = *src0 * c->gain \ |
135 |
+ c->cy[0]*s->x[i0] + c->cy[1]*s->x[i1] \ |
136 |
+ c->cy[2]*s->x[i2] + c->cy[3]*s->x[i3]; \ |
137 |
res = (s->x[i0] + in )*1 \
|
138 |
+ (s->x[i1] + s->x[i3])*4 \
|
139 |
+ s->x[i2] *6; \
|
140 |
CONV_##fmt(*dst0, res) \ |
141 |
s->x[i0] = in; \ |
142 |
src0 += sstep; \ |
143 |
dst0 += dstep; |
144 |
|
145 |
#define FILTER_BW_O4(type, fmt) { \
|
146 |
int i; \
|
147 |
const type *src0 = src; \
|
148 |
type *dst0 = dst; \ |
149 |
for (i = 0; i < size; i += 4) { \ |
150 |
float in, res; \
|
151 |
FILTER_BW_O4_1(0, 1, 2, 3, fmt); \ |
152 |
FILTER_BW_O4_1(1, 2, 3, 0, fmt); \ |
153 |
FILTER_BW_O4_1(2, 3, 0, 1, fmt); \ |
154 |
FILTER_BW_O4_1(3, 0, 1, 2, fmt); \ |
155 |
} \ |
156 |
} |
157 |
|
158 |
#define FILTER_DIRECT_FORM_II(type, fmt) { \
|
159 |
int i; \
|
160 |
const type *src0 = src; \
|
161 |
type *dst0 = dst; \ |
162 |
for (i = 0; i < size; i++) { \ |
163 |
int j; \
|
164 |
float in, res; \
|
165 |
in = *src0 * c->gain; \ |
166 |
for(j = 0; j < c->order; j++) \ |
167 |
in += c->cy[j] * s->x[j]; \ |
168 |
res = s->x[0] + in + s->x[c->order >> 1] * c->cx[c->order >> 1]; \ |
169 |
for(j = 1; j < c->order >> 1; j++) \ |
170 |
res += (s->x[j] + s->x[c->order - j]) * c->cx[j]; \ |
171 |
for(j = 0; j < c->order - 1; j++) \ |
172 |
s->x[j] = s->x[j + 1]; \
|
173 |
CONV_##fmt(*dst0, res) \ |
174 |
s->x[c->order - 1] = in; \
|
175 |
src0 += sstep; \ |
176 |
dst0 += dstep; \ |
177 |
} \ |
178 |
} |
179 |
|
180 |
void ff_iir_filter(const struct FFIIRFilterCoeffs *c, |
181 |
struct FFIIRFilterState *s, int size, |
182 |
const int16_t *src, int sstep, int16_t *dst, int dstep) |
183 |
{ |
184 |
if (c->order == 4) { |
185 |
FILTER_BW_O4(int16_t, S16) |
186 |
} else {
|
187 |
FILTER_DIRECT_FORM_II(int16_t, S16) |
188 |
} |
189 |
} |
190 |
|
191 |
void ff_iir_filter_flt(const struct FFIIRFilterCoeffs *c, |
192 |
struct FFIIRFilterState *s, int size, |
193 |
const float *src, int sstep, void *dst, int dstep) |
194 |
{ |
195 |
if (c->order == 4) { |
196 |
FILTER_BW_O4(float, FLT)
|
197 |
} else {
|
198 |
FILTER_DIRECT_FORM_II(float, FLT)
|
199 |
} |
200 |
} |
201 |
|
202 |
av_cold void ff_iir_filter_free_state(struct FFIIRFilterState *state) |
203 |
{ |
204 |
av_free(state); |
205 |
} |
206 |
|
207 |
av_cold void ff_iir_filter_free_coeffs(struct FFIIRFilterCoeffs *coeffs) |
208 |
{ |
209 |
if(coeffs){
|
210 |
av_free(coeffs->cx); |
211 |
av_free(coeffs->cy); |
212 |
} |
213 |
av_free(coeffs); |
214 |
} |
215 |
|
216 |
#ifdef TEST
|
217 |
#define FILT_ORDER 4 |
218 |
#define SIZE 1024 |
219 |
int main(void) |
220 |
{ |
221 |
struct FFIIRFilterCoeffs *fcoeffs = NULL; |
222 |
struct FFIIRFilterState *fstate = NULL; |
223 |
float cutoff_coeff = 0.4; |
224 |
int16_t x[SIZE], y[SIZE]; |
225 |
int i;
|
226 |
FILE* fd; |
227 |
|
228 |
fcoeffs = ff_iir_filter_init_coeffs(FF_FILTER_TYPE_BUTTERWORTH, |
229 |
FF_FILTER_MODE_LOWPASS, FILT_ORDER, |
230 |
cutoff_coeff, 0.0, 0.0); |
231 |
fstate = ff_iir_filter_init_state(FILT_ORDER); |
232 |
|
233 |
for (i = 0; i < SIZE; i++) { |
234 |
x[i] = lrint(0.75 * INT16_MAX * sin(0.5*M_PI*i*i/SIZE)); |
235 |
} |
236 |
|
237 |
ff_iir_filter(fcoeffs, fstate, SIZE, x, 1, y, 1); |
238 |
|
239 |
fd = fopen("in.bin", "w"); |
240 |
fwrite(x, sizeof(x[0]), SIZE, fd); |
241 |
fclose(fd); |
242 |
|
243 |
fd = fopen("out.bin", "w"); |
244 |
fwrite(y, sizeof(y[0]), SIZE, fd); |
245 |
fclose(fd); |
246 |
|
247 |
ff_iir_filter_free_coeffs(fcoeffs); |
248 |
ff_iir_filter_free_state(fstate); |
249 |
return 0; |
250 |
} |
251 |
#endif /* TEST */ |