ffmpeg / libavcodec / iirfilter.c @ 2293b0b6
History  View  Annotate  Download (9.07 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 021101301 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 
static int butterworth_init_coeffs(void *avc, struct FFIIRFilterCoeffs *c, 
51 
enum IIRFilterMode filt_mode,

52 
int order, float cutoff_ratio, 
53 
float stopband)

54 
{ 
55 
int i, j;

56 
double wa;

57 
double p[MAXORDER + 1][2]; 
58  
59 
if (filt_mode != FF_FILTER_MODE_LOWPASS) {

60 
av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports "

61 
"lowpass filter mode\n");

62 
return 1; 
63 
} 
64 
if (order & 1) { 
65 
av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports "

66 
"even filter orders\n");

67 
return 1; 
68 
} 
69  
70 
wa = 2 * tan(M_PI * 0.5 * cutoff_ratio); 
71  
72 
c>cx[0] = 1; 
73 
for(i = 1; i < (order >> 1) + 1; i++) 
74 
c>cx[i] = c>cx[i  1] * (order  i + 1LL) / i; 
75  
76 
p[0][0] = 1.0; 
77 
p[0][1] = 0.0; 
78 
for(i = 1; i <= order; i++) 
79 
p[i][0] = p[i][1] = 0.0; 
80 
for(i = 0; i < order; i++){ 
81 
double zp[2]; 
82 
double th = (i + (order >> 1) + 0.5) * M_PI / order; 
83 
double a_re, a_im, c_re, c_im;

84 
zp[0] = cos(th) * wa;

85 
zp[1] = sin(th) * wa;

86 
a_re = zp[0] + 2.0; 
87 
c_re = zp[0]  2.0; 
88 
a_im = 
89 
c_im = zp[1];

90 
zp[0] = (a_re * c_re + a_im * c_im) / (c_re * c_re + c_im * c_im);

91 
zp[1] = (a_im * c_re  a_re * c_im) / (c_re * c_re + c_im * c_im);

92  
93 
for(j = order; j >= 1; j) 
94 
{ 
95 
a_re = p[j][0];

96 
a_im = p[j][1];

97 
p[j][0] = a_re*zp[0]  a_im*zp[1] + p[j1][0]; 
98 
p[j][1] = a_re*zp[1] + a_im*zp[0] + p[j1][1]; 
99 
} 
100 
a_re = p[0][0]*zp[0]  p[0][1]*zp[1]; 
101 
p[0][1] = p[0][0]*zp[1] + p[0][1]*zp[0]; 
102 
p[0][0] = a_re; 
103 
} 
104 
c>gain = p[order][0];

105 
for(i = 0; i < order; i++){ 
106 
c>gain += p[i][0];

107 
c>cy[i] = (p[i][0] * p[order][0] + p[i][1] * p[order][1]) / 
108 
(p[order][0] * p[order][0] + p[order][1] * p[order][1]); 
109 
} 
110 
c>gain /= 1 << order;

111  
112 
return 0; 
113 
} 
114  
115 
av_cold struct FFIIRFilterCoeffs* ff_iir_filter_init_coeffs(void *avc, 
116 
enum IIRFilterType filt_type,

117 
enum IIRFilterMode filt_mode,

118 
int order, float cutoff_ratio, 
119 
float stopband, float ripple) 
120 
{ 
121 
FFIIRFilterCoeffs *c; 
122  
123 
if (order <= 0  order > MAXORDER  cutoff_ratio >= 1.0) 
124 
return NULL; 
125  
126 
FF_ALLOCZ_OR_GOTO(avc, c, sizeof(FFIIRFilterCoeffs),

127 
init_fail); 
128 
FF_ALLOC_OR_GOTO (avc, c>cx, sizeof(c>cx[0]) * ((order >> 1) + 1), 
129 
init_fail); 
130 
FF_ALLOC_OR_GOTO (avc, c>cy, sizeof(c>cy[0]) * order, 
131 
init_fail); 
132 
c>order = order; 
133  
134 
if (filt_type == FF_FILTER_TYPE_BUTTERWORTH) {

135 
if (butterworth_init_coeffs(avc, c, filt_mode, order, cutoff_ratio,

136 
stopband)) { 
137 
goto init_fail;

138 
} 
139 
} else {

140 
av_log(avc, AV_LOG_ERROR, "filter type is not currently implemented\n");

141 
goto init_fail;

142 
} 
143  
144 
return c;

145  
146 
init_fail:

147 
ff_iir_filter_free_coeffs(c); 
148 
return NULL; 
149 
} 
150  
151 
av_cold struct FFIIRFilterState* ff_iir_filter_init_state(int order) 
152 
{ 
153 
FFIIRFilterState* s = av_mallocz(sizeof(FFIIRFilterState) + sizeof(s>x[0]) * (order  1)); 
154 
return s;

155 
} 
156  
157 
#define CONV_S16(dest, source) dest = av_clip_int16(lrintf(source));

158  
159 
#define CONV_FLT(dest, source) dest = source;

160  
161 
#define FILTER_BW_O4_1(i0, i1, i2, i3, fmt) \

162 
in = *src0 * c>gain \ 
163 
+ c>cy[0]*s>x[i0] + c>cy[1]*s>x[i1] \ 
164 
+ c>cy[2]*s>x[i2] + c>cy[3]*s>x[i3]; \ 
165 
res = (s>x[i0] + in )*1 \

166 
+ (s>x[i1] + s>x[i3])*4 \

167 
+ s>x[i2] *6; \

168 
CONV_##fmt(*dst0, res) \ 
169 
s>x[i0] = in; \ 
170 
src0 += sstep; \ 
171 
dst0 += dstep; 
172  
173 
#define FILTER_BW_O4(type, fmt) { \

174 
int i; \

175 
const type *src0 = src; \

176 
type *dst0 = dst; \ 
177 
for (i = 0; i < size; i += 4) { \ 
178 
float in, res; \

179 
FILTER_BW_O4_1(0, 1, 2, 3, fmt); \ 
180 
FILTER_BW_O4_1(1, 2, 3, 0, fmt); \ 
181 
FILTER_BW_O4_1(2, 3, 0, 1, fmt); \ 
182 
FILTER_BW_O4_1(3, 0, 1, 2, fmt); \ 
183 
} \ 
184 
} 
185  
186 
#define FILTER_DIRECT_FORM_II(type, fmt) { \

187 
int i; \

188 
const type *src0 = src; \

189 
type *dst0 = dst; \ 
190 
for (i = 0; i < size; i++) { \ 
191 
int j; \

192 
float in, res; \

193 
in = *src0 * c>gain; \ 
194 
for(j = 0; j < c>order; j++) \ 
195 
in += c>cy[j] * s>x[j]; \ 
196 
res = s>x[0] + in + s>x[c>order >> 1] * c>cx[c>order >> 1]; \ 
197 
for(j = 1; j < c>order >> 1; j++) \ 
198 
res += (s>x[j] + s>x[c>order  j]) * c>cx[j]; \ 
199 
for(j = 0; j < c>order  1; j++) \ 
200 
s>x[j] = s>x[j + 1]; \

201 
CONV_##fmt(*dst0, res) \ 
202 
s>x[c>order  1] = in; \

203 
src0 += sstep; \ 
204 
dst0 += dstep; \ 
205 
} \ 
206 
} 
207  
208 
void ff_iir_filter(const struct FFIIRFilterCoeffs *c, 
209 
struct FFIIRFilterState *s, int size, 
210 
const int16_t *src, int sstep, int16_t *dst, int dstep) 
211 
{ 
212 
if (c>order == 4) { 
213 
FILTER_BW_O4(int16_t, S16) 
214 
} else {

215 
FILTER_DIRECT_FORM_II(int16_t, S16) 
216 
} 
217 
} 
218  
219 
void ff_iir_filter_flt(const struct FFIIRFilterCoeffs *c, 
220 
struct FFIIRFilterState *s, int size, 
221 
const float *src, int sstep, void *dst, int dstep) 
222 
{ 
223 
if (c>order == 4) { 
224 
FILTER_BW_O4(float, FLT)

225 
} else {

226 
FILTER_DIRECT_FORM_II(float, FLT)

227 
} 
228 
} 
229  
230 
av_cold void ff_iir_filter_free_state(struct FFIIRFilterState *state) 
231 
{ 
232 
av_free(state); 
233 
} 
234  
235 
av_cold void ff_iir_filter_free_coeffs(struct FFIIRFilterCoeffs *coeffs) 
236 
{ 
237 
if(coeffs){

238 
av_free(coeffs>cx); 
239 
av_free(coeffs>cy); 
240 
} 
241 
av_free(coeffs); 
242 
} 
243  
244 
#ifdef TEST

245 
#define FILT_ORDER 4 
246 
#define SIZE 1024 
247 
int main(void) 
248 
{ 
249 
struct FFIIRFilterCoeffs *fcoeffs = NULL; 
250 
struct FFIIRFilterState *fstate = NULL; 
251 
float cutoff_coeff = 0.4; 
252 
int16_t x[SIZE], y[SIZE]; 
253 
int i;

254 
FILE* fd; 
255  
256 
fcoeffs = ff_iir_filter_init_coeffs(FF_FILTER_TYPE_BUTTERWORTH, 
257 
FF_FILTER_MODE_LOWPASS, FILT_ORDER, 
258 
cutoff_coeff, 0.0, 0.0); 
259 
fstate = ff_iir_filter_init_state(FILT_ORDER); 
260  
261 
for (i = 0; i < SIZE; i++) { 
262 
x[i] = lrint(0.75 * INT16_MAX * sin(0.5*M_PI*i*i/SIZE)); 
263 
} 
264  
265 
ff_iir_filter(fcoeffs, fstate, SIZE, x, 1, y, 1); 
266  
267 
fd = fopen("in.bin", "w"); 
268 
fwrite(x, sizeof(x[0]), SIZE, fd); 
269 
fclose(fd); 
270  
271 
fd = fopen("out.bin", "w"); 
272 
fwrite(y, sizeof(y[0]), SIZE, fd); 
273 
fclose(fd); 
274  
275 
ff_iir_filter_free_coeffs(fcoeffs); 
276 
ff_iir_filter_free_state(fstate); 
277 
return 0; 
278 
} 
279 
#endif /* TEST */ 