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 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 
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[j1][0]; 
102 
p[j][1] = a_re*zp[1] + a_im*zp[0] + p[j1][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 */ 