20 // For SSE, we set the denormals-as-zero flag instead.
21 #define early_undenormalise(sample)
23 #else // !defined(__SSE__)
29 #define early_undenormalise(sample) { \
31 uf.f = float(sample); \
32 if ((uf.i&0x60000000)==0) sample=0.0f; \
35 #endif // !_defined(__SSE__)
50 uses coefficients grabbed from
51 RBJs audio eq cookbook:
52 http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
56 float cutoff_freq = omega;
57 cutoff_freq = min(cutoff_freq, (float)M_PI);
58 cutoff_freq = max(cutoff_freq, 0.001f);
59 calcSinCos(cutoff_freq, &sn, &cs);
60 if (resonance <= 0) resonance = 0.001f;
63 // Faster version of real_resonance = resonance ^ (1 / order).
64 // pow(), at least on current GCC, is pretty slow.
65 float real_resonance = resonance;
66 switch (filter_order) {
71 real_resonance = sqrt(real_resonance);
74 real_resonance = sqrt(real_resonance);
77 real_resonance = cbrt(real_resonance);
83 float real_resonance = pow(resonance, 1.0f / filter_order);
86 float alpha = float(sn / (2 * real_resonance));
94 a1 = a2 = b1 = b2 = 0.0; //identity filter
106 b0 = (1 + cs) * 0.5f;
137 case FILTER_PEAKING_EQ:
146 case FILTER_LOW_SHELF:
147 b0 = A * ((A + 1) - (A - 1)*cs + 2 * sqrt(A) * alpha);
148 b1 = 2 * A * ((A - 1) - (A + 1)*cs );
149 b2 = A * ((A + 1) - (A - 1)*cs - 2 * sqrt(A) * alpha);
150 a0 = (A + 1) + (A - 1)*cs + 2 * sqrt(A) * alpha ;
151 a1 = -2 * ((A - 1) + (A + 1)*cs );
152 a2 = (A + 1) + (A - 1)*cs - 2 * sqrt(A) * alpha ;
155 case FILTER_HIGH_SHELF:
156 b0 = A * ((A + 1) + (A - 1)*cs + 2 * sqrt(A) * alpha);
157 b1 = -2 * A * ((A - 1) + (A + 1)*cs );
158 b2 = A * ((A + 1) + (A - 1)*cs - 2 * sqrt(A) * alpha);
159 a0 = (A + 1) - (A - 1)*cs + 2 * sqrt(A) * alpha ;
160 a1 = 2 * ((A - 1) - (A + 1)*cs );
161 a2 = (A + 1) - (A - 1)*cs - 2 * sqrt(A) * alpha ;
165 //unknown filter type
170 const float invA0 = 1.0f / a0;
181 // Feed this to gnuplot to get a graph of the frequency response.
182 const float Fs2 = OUTPUT_FREQUENCY * 0.5f;
183 printf("set xrange [2:%f]; ", Fs2);
184 printf("set yrange [-80:20]; ");
185 printf("set log x; ");
186 printf("phasor(x) = cos(x*pi/%f)*{1,0} + sin(x*pi/%f)*{0,1}; ", Fs2, Fs2);
187 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); ");
188 printf("db(x) = 20*log10(x); ");
189 printf("plot db(abs(tfunc(x, %f, %f, %f, %f, %f, %f))) title \"\"\n", b0, b1, b2, 1.0f, a1, a2);
193 void Filter::init(FilterType type, int order)
196 filter_order = order;
197 if (filtertype == FILTER_NONE) filter_order = 0;
198 if (filter_order == 0) filtertype = FILTER_NONE;
200 //reset feedback buffer
201 for (unsigned i = 0; i < filter_order; i++) {
202 feedback[i].d0 = feedback[i].d1 = 0.0f;
207 void Filter::render_chunk(float *inout_buf, unsigned int n_samples)
209 void Filter::render_chunk(float *inout_buf, unsigned int n_samples, unsigned stride)
213 const unsigned stride = 1;
214 unsigned old_denormals_mode = _MM_GET_FLUSH_ZERO_MODE();
215 _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
217 assert((n_samples & 3) == 0); // make sure n_samples is divisible by 4
219 // Apply the filter FILTER_ORDER times.
220 for (unsigned j = 0; j < filter_order; j++) {
221 float d0 = feedback[j].d0;
222 float d1 = feedback[j].d1;
223 float *inout_ptr = inout_buf;
225 // Render n_samples mono samples. Unrolling manually by a
226 // factor four seemingly helps a lot, perhaps because it
227 // lets the CPU overlap arithmetic and memory operations
228 // better, or perhaps simply because the loop overhead is
230 for (unsigned i = n_samples >> 2; i; i--) {
236 d0 = b1*in - a1*out + d1;
243 d0 = b1*in - a1*out + d1;
250 d0 = b1*in - a1*out + d1;
257 d0 = b1*in - a1*out + d1;
261 early_undenormalise(d0); //do denormalization step
262 early_undenormalise(d1);
268 _MM_SET_FLUSH_ZERO_MODE(old_denormals_mode);
272 void Filter::render(float *inout_buf, unsigned int buf_size, float cutoff, float resonance)
274 //render buf_size mono samples
276 assert(buf_size % 4 == 0);
278 if (filter_order == 0)
281 this->set_linear_cutoff(cutoff);
282 this->set_resonance(resonance);
284 this->render_chunk(inout_buf, buf_size);
287 void StereoFilter::init(FilterType type, int new_order)
290 parm_filter.init(type, new_order);
291 memset(feedback, 0, sizeof(feedback));
293 for (unsigned i = 0; i < 2; ++i) {
294 filters[i].init(type, new_order);
299 void StereoFilter::render(float *inout_left_ptr, unsigned n_samples, float cutoff, float resonance, float dbgain_normalized)
302 if (parm_filter.filtertype == FILTER_NONE || parm_filter.filter_order == 0)
305 unsigned old_denormals_mode = _MM_GET_FLUSH_ZERO_MODE();
306 _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
308 parm_filter.set_linear_cutoff(cutoff);
309 parm_filter.set_resonance(resonance);
310 parm_filter.set_dbgain_normalized(dbgain_normalized);
311 parm_filter.update();
313 __m128 b0 = _mm_set1_ps(parm_filter.b0);
314 __m128 b1 = _mm_set1_ps(parm_filter.b1);
315 __m128 b2 = _mm_set1_ps(parm_filter.b2);
316 __m128 a1 = _mm_set1_ps(parm_filter.a1);
317 __m128 a2 = _mm_set1_ps(parm_filter.a2);
319 // Apply the filter FILTER_ORDER times.
320 for (unsigned j = 0; j < parm_filter.filter_order; j++) {
321 __m128 d0 = feedback[j].d0;
322 __m128 d1 = feedback[j].d1;
323 __m64 *inout_ptr = (__m64 *)inout_left_ptr;
325 __m128 in = _mm_set1_ps(0.0f), out;
326 for (unsigned i = n_samples; i; i--) {
327 in = _mm_loadl_pi(in, inout_ptr);
328 out = _mm_add_ps(_mm_mul_ps(b0, in), d0);
329 _mm_storel_pi(inout_ptr, out);
330 d0 = _mm_add_ps(_mm_sub_ps(_mm_mul_ps(b1, in), _mm_mul_ps(a1, out)), d1);
331 d1 = _mm_sub_ps(_mm_mul_ps(b2, in), _mm_mul_ps(a2, out));
338 _MM_SET_FLUSH_ZERO_MODE(old_denormals_mode);
340 if (filters[0].filtertype == FILTER_NONE || filters[0].filter_order == 0)
343 for (unsigned i = 0; i < 2; ++i) {
344 filters[i].set_linear_cutoff(cutoff);
345 filters[i].set_resonance(resonance);
347 filters[i].render_chunk(inout_left_ptr, n_samples, 2);
356 Find the transfer function for an IIR biquad. This is relatively basic signal
357 processing, but for completeness, here's the rationale for the function:
359 The basic system of an IIR biquad looks like this, for input x[n], output y[n]
360 and constant filter coefficients [ab][0-2]:
362 a2 y[n-2] + a1 y[n-1] + a0 y[n] = b2 x[n-2] + b1 x[n-1] + b0 x[n]
364 Taking the discrete Fourier transform (DFT) of both sides (denoting by convention
365 DFT{x[n]} by X[w], where w is the angular frequency, going from 0 to 2pi), yields,
366 due to the linearity and shift properties of the DFT:
368 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]
370 Simple factorization and reorganization yields
372 Y[w] / X[w] = (b1 e^2jw + b1 e^jw + b0) / (a2 e^2jw + a1 e^jw + a0)
374 and Y[w] / X[w] is by definition the filter's _transfer function_
375 (customarily denoted by H(w)), ie. the complex factor it applies to the
376 frequency component w. The absolute value of the transfer function is
377 the frequency response, ie. how much frequency w is boosted or weakened.
379 (This derivation usually goes via the Z-transform and not the DFT, but the
380 idea is exactly the same; the Z-transform is just a bit more general.)
382 Sending a signal through first one filter and then through another one
383 will naturally be equivalent to a filter with the transfer function equal
384 to the pointwise multiplication of the two filters, so for N-order filters
385 we need to raise the answer to the Nth power.
388 complex<double> Filter::evaluate_transfer_function(float omega)
390 complex<float> z = exp(complex<float>(0.0f, omega));
391 complex<float> z2 = z * z;
392 return pow((b0 * z2 + b1 * z + b2) / (1.0f * z2 + a1 * z + a2), filter_order);