9 #include "shared/shared_defs.h"
21 // For SSE, we set the denormals-as-zero flag instead.
22 #define early_undenormalise(sample)
24 #else // !defined(__SSE__)
30 #define early_undenormalise(sample) { \
32 uf.f = float(sample); \
33 if ((uf.i&0x60000000)==0) sample=0.0f; \
36 #endif // !_defined(__SSE__)
51 uses coefficients grabbed from
52 RBJs audio eq cookbook:
53 http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
57 float cutoff_freq = omega;
58 cutoff_freq = min(cutoff_freq, (float)M_PI);
59 cutoff_freq = max(cutoff_freq, 0.001f);
60 calcSinCos(cutoff_freq, &sn, &cs);
61 if (resonance <= 0) resonance = 0.001f;
64 // Faster version of real_resonance = resonance ^ (1 / order).
65 // pow(), at least on current GCC, is pretty slow.
66 float real_resonance = resonance;
67 switch (filter_order) {
72 real_resonance = sqrt(real_resonance);
75 real_resonance = sqrt(real_resonance);
78 real_resonance = cbrt(real_resonance);
84 float real_resonance = pow(resonance, 1.0f / filter_order);
87 float alpha = float(sn / (2 * real_resonance));
95 a1 = a2 = b1 = b2 = 0.0; //identity filter
107 b0 = (1 + cs) * 0.5f;
138 case FILTER_PEAKING_EQ:
147 case FILTER_LOW_SHELF:
148 b0 = A * ((A + 1) - (A - 1)*cs + 2 * sqrt(A) * alpha);
149 b1 = 2 * A * ((A - 1) - (A + 1)*cs );
150 b2 = A * ((A + 1) - (A - 1)*cs - 2 * sqrt(A) * alpha);
151 a0 = (A + 1) + (A - 1)*cs + 2 * sqrt(A) * alpha ;
152 a1 = -2 * ((A - 1) + (A + 1)*cs );
153 a2 = (A + 1) + (A - 1)*cs - 2 * sqrt(A) * alpha ;
156 case FILTER_HIGH_SHELF:
157 b0 = A * ((A + 1) + (A - 1)*cs + 2 * sqrt(A) * alpha);
158 b1 = -2 * A * ((A - 1) + (A + 1)*cs );
159 b2 = A * ((A + 1) + (A - 1)*cs - 2 * sqrt(A) * alpha);
160 a0 = (A + 1) - (A - 1)*cs + 2 * sqrt(A) * alpha ;
161 a1 = 2 * ((A - 1) - (A + 1)*cs );
162 a2 = (A + 1) - (A - 1)*cs - 2 * sqrt(A) * alpha ;
166 //unknown filter type
171 const float invA0 = 1.0f / a0;
182 // Feed this to gnuplot to get a graph of the frequency response.
183 const float Fs2 = OUTPUT_FREQUENCY * 0.5f;
184 printf("set xrange [2:%f]; ", Fs2);
185 printf("set yrange [-80:20]; ");
186 printf("set log x; ");
187 printf("phasor(x) = cos(x*pi/%f)*{1,0} + sin(x*pi/%f)*{0,1}; ", Fs2, Fs2);
188 printf("tfunc(x, b0, b1, b2, a0, a1, a2) = (b0 * phasor(x)**2 + b1 * phasor(x) + b2) / (a0 * phasor(x)**2 + a1 * phasor(x) + a2); ");
189 printf("db(x) = 20*log10(x); ");
190 printf("plot db(abs(tfunc(x, %f, %f, %f, %f, %f, %f))) title \"\"\n", b0, b1, b2, 1.0f, a1, a2);
194 void Filter::init(FilterType type, int order)
197 filter_order = order;
198 if (filtertype == FILTER_NONE) filter_order = 0;
199 if (filter_order == 0) filtertype = FILTER_NONE;
201 //reset feedback buffer
202 for (unsigned i = 0; i < filter_order; i++) {
203 feedback[i].d0 = feedback[i].d1 = 0.0f;
208 void Filter::render_chunk(float *inout_buf, unsigned int n_samples)
210 void Filter::render_chunk(float *inout_buf, unsigned int n_samples, unsigned stride)
214 const unsigned stride = 1;
215 unsigned old_denormals_mode = _MM_GET_FLUSH_ZERO_MODE();
216 _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
218 assert((n_samples & 3) == 0); // make sure n_samples is divisible by 4
220 // Apply the filter FILTER_ORDER times.
221 for (unsigned j = 0; j < filter_order; j++) {
222 float d0 = feedback[j].d0;
223 float d1 = feedback[j].d1;
224 float *inout_ptr = inout_buf;
226 // Render n_samples mono samples. Unrolling manually by a
227 // factor four seemingly helps a lot, perhaps because it
228 // lets the CPU overlap arithmetic and memory operations
229 // better, or perhaps simply because the loop overhead is
231 for (unsigned i = n_samples >> 2; i; i--) {
237 d0 = b1*in - a1*out + d1;
244 d0 = b1*in - a1*out + d1;
251 d0 = b1*in - a1*out + d1;
258 d0 = b1*in - a1*out + d1;
262 early_undenormalise(d0); //do denormalization step
263 early_undenormalise(d1);
269 _MM_SET_FLUSH_ZERO_MODE(old_denormals_mode);
273 void Filter::render(float *inout_buf, unsigned int buf_size, float cutoff, float resonance)
275 //render buf_size mono samples
277 assert(buf_size % 4 == 0);
279 if (filter_order == 0)
282 this->set_linear_cutoff(cutoff);
283 this->set_resonance(resonance);
285 this->render_chunk(inout_buf, buf_size);
288 void StereoFilter::init(FilterType type, int new_order)
291 parm_filter.init(type, new_order);
292 memset(feedback, 0, sizeof(feedback));
294 for (unsigned i = 0; i < 2; ++i) {
295 filters[i].init(type, new_order);
300 void StereoFilter::render(float *inout_left_ptr, unsigned n_samples, float cutoff, float resonance, float dbgain_normalized)
303 if (parm_filter.filtertype == FILTER_NONE || parm_filter.filter_order == 0)
306 unsigned old_denormals_mode = _MM_GET_FLUSH_ZERO_MODE();
307 _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
309 parm_filter.set_linear_cutoff(cutoff);
310 parm_filter.set_resonance(resonance);
311 parm_filter.set_dbgain_normalized(dbgain_normalized);
312 parm_filter.update();
314 __m128 b0 = _mm_set1_ps(parm_filter.b0);
315 __m128 b1 = _mm_set1_ps(parm_filter.b1);
316 __m128 b2 = _mm_set1_ps(parm_filter.b2);
317 __m128 a1 = _mm_set1_ps(parm_filter.a1);
318 __m128 a2 = _mm_set1_ps(parm_filter.a2);
320 // Apply the filter FILTER_ORDER times.
321 for (unsigned j = 0; j < parm_filter.filter_order; j++) {
322 __m128 d0 = feedback[j].d0;
323 __m128 d1 = feedback[j].d1;
324 __m64 *inout_ptr = (__m64 *)inout_left_ptr;
326 __m128 in = _mm_set1_ps(0.0f), out;
327 for (unsigned i = n_samples; i; i--) {
328 in = _mm_loadl_pi(in, inout_ptr);
329 out = _mm_add_ps(_mm_mul_ps(b0, in), d0);
330 _mm_storel_pi(inout_ptr, out);
331 d0 = _mm_add_ps(_mm_sub_ps(_mm_mul_ps(b1, in), _mm_mul_ps(a1, out)), d1);
332 d1 = _mm_sub_ps(_mm_mul_ps(b2, in), _mm_mul_ps(a2, out));
339 _MM_SET_FLUSH_ZERO_MODE(old_denormals_mode);
341 if (filters[0].filtertype == FILTER_NONE || filters[0].filter_order == 0)
344 for (unsigned i = 0; i < 2; ++i) {
345 filters[i].set_linear_cutoff(cutoff);
346 filters[i].set_resonance(resonance);
348 filters[i].render_chunk(inout_left_ptr, n_samples, 2);
357 Find the transfer function for an IIR biquad. This is relatively basic signal
358 processing, but for completeness, here's the rationale for the function:
360 The basic system of an IIR biquad looks like this, for input x[n], output y[n]
361 and constant filter coefficients [ab][0-2]:
363 a2 y[n-2] + a1 y[n-1] + a0 y[n] = b2 x[n-2] + b1 x[n-1] + b0 x[n]
365 Taking the discrete Fourier transform (DFT) of both sides (denoting by convention
366 DFT{x[n]} by X[w], where w is the angular frequency, going from 0 to 2pi), yields,
367 due to the linearity and shift properties of the DFT:
369 a2 e^2jw Y[w] + a1 e^jw Y[w] + a0 Y[w] = b2 e^2jw X[w] + b1 e^jw X[w] + b0 Y[w]
371 Simple factorization and reorganization yields
373 Y[w] / X[w] = (b1 e^2jw + b1 e^jw + b0) / (a2 e^2jw + a1 e^jw + a0)
375 and Y[w] / X[w] is by definition the filter's _transfer function_
376 (customarily denoted by H(w)), ie. the complex factor it applies to the
377 frequency component w. The absolute value of the transfer function is
378 the frequency response, ie. how much frequency w is boosted or weakened.
380 (This derivation usually goes via the Z-transform and not the DFT, but the
381 idea is exactly the same; the Z-transform is just a bit more general.)
383 Sending a signal through first one filter and then through another one
384 will naturally be equivalent to a filter with the transfer function equal
385 to the pointwise multiplication of the two filters, so for N-order filters
386 we need to raise the answer to the Nth power.
389 complex<double> Filter::evaluate_transfer_function(float omega)
391 complex<float> z = exp(complex<float>(0.0f, omega));
392 complex<float> z2 = z * z;
393 return pow((b0 * z2 + b1 * z + b2) / (1.0f * z2 + a1 * z + a2), filter_order);