18 // For SSE, we set the denormals-as-zero flag instead.
19 #define early_undenormalise(sample)
21 #else // !defined(__SSE__)
27 #define early_undenormalise(sample) { \
29 uf.f = float(sample); \
30 if ((uf.i&0x60000000)==0) sample=0.0f; \
33 #endif // !_defined(__SSE__)
47 uses coefficients grabbed from
48 RBJs audio eq cookbook:
49 http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
53 float cutoff_freq = omega;
54 cutoff_freq = min(cutoff_freq, (float)M_PI);
55 cutoff_freq = max(cutoff_freq, 0.001f);
56 calcSinCos(cutoff_freq, &sn, &cs);
57 if (resonance <= 0) resonance = 0.001f;
60 // Faster version of real_resonance = resonance ^ (1 / order).
61 // pow(), at least on current GCC, is pretty slow.
62 float real_resonance = resonance;
63 switch (filter_order) {
68 real_resonance = sqrt(real_resonance);
71 real_resonance = sqrt(real_resonance);
74 real_resonance = cbrt(real_resonance);
80 float real_resonance = pow(resonance, 1.0f / filter_order);
83 float alpha = float(sn / (2 * real_resonance));
91 a1 = a2 = b1 = b2 = 0.0; //identity filter
103 b0 = (1 + cs) * 0.5f;
135 //unknown filter type
140 const float invA0 = 1.0f / a0;
151 // Feed this to gnuplot to get a graph of the frequency response.
152 const float Fs2 = OUTPUT_FREQUENCY * 0.5f;
153 printf("set xrange [2:%f]; ", Fs2);
154 printf("set yrange [-80:20]; ");
155 printf("set log x; ");
156 printf("phasor(x) = cos(x*pi/%f)*{1,0} + sin(x*pi/%f)*{0,1}; ", Fs2, Fs2);
157 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); ");
158 printf("db(x) = 20*log10(x); ");
159 printf("plot db(abs(tfunc(x, %f, %f, %f, %f, %f, %f))) title \"\"\n", b0, b1, b2, 1.0f, a1, a2);
163 void Filter::init(FilterType type, int order)
166 filter_order = order;
167 if (filtertype == FILTER_NONE) filter_order = 0;
168 if (filter_order == 0) filtertype = FILTER_NONE;
170 //reset feedback buffer
171 for (unsigned i = 0; i < filter_order; i++) {
172 feedback[i].d0 = feedback[i].d1 = 0.0f;
177 void Filter::render_chunk(float *inout_buf, unsigned int n_samples)
179 void Filter::render_chunk(float *inout_buf, unsigned int n_samples, unsigned stride)
183 const unsigned stride = 1;
184 unsigned old_denormals_mode = _MM_GET_FLUSH_ZERO_MODE();
185 _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
187 assert((n_samples & 3) == 0); // make sure n_samples is divisible by 4
189 // Apply the filter FILTER_ORDER times.
190 for (unsigned j = 0; j < filter_order; j++) {
191 float d0 = feedback[j].d0;
192 float d1 = feedback[j].d1;
193 float *inout_ptr = inout_buf;
195 // Render n_samples mono samples. Unrolling manually by a
196 // factor four seemingly helps a lot, perhaps because it
197 // lets the CPU overlap arithmetic and memory operations
198 // better, or perhaps simply because the loop overhead is
200 for (unsigned i = n_samples >> 2; i; i--) {
206 d0 = b1*in - a1*out + d1;
213 d0 = b1*in - a1*out + d1;
220 d0 = b1*in - a1*out + d1;
227 d0 = b1*in - a1*out + d1;
231 early_undenormalise(d0); //do denormalization step
232 early_undenormalise(d1);
238 _MM_SET_FLUSH_ZERO_MODE(old_denormals_mode);
242 void Filter::render(float *inout_buf, unsigned int buf_size, float cutoff, float resonance)
244 //render buf_size mono samples
246 assert(buf_size % 4 == 0);
248 if (filter_order == 0)
251 this->set_linear_cutoff(cutoff);
252 this->set_resonance(resonance);
254 this->render_chunk(inout_buf, buf_size);
257 void StereoFilter::init(FilterType type, int new_order)
260 parm_filter.init(type, new_order);
261 memset(feedback, 0, sizeof(feedback));
263 for (unsigned i = 0; i < 2; ++i) {
264 filters[i].init(type, 0, new_order, 0);
269 void StereoFilter::render(float *inout_left_ptr, unsigned n_samples, float cutoff, float resonance)
272 if (parm_filter.filtertype == FILTER_NONE || parm_filter.filter_order == 0)
275 unsigned old_denormals_mode = _MM_GET_FLUSH_ZERO_MODE();
276 _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
278 parm_filter.set_linear_cutoff(cutoff);
279 parm_filter.set_resonance(resonance);
280 parm_filter.update();
282 __m128 b0 = _mm_set1_ps(parm_filter.b0);
283 __m128 b1 = _mm_set1_ps(parm_filter.b1);
284 __m128 b2 = _mm_set1_ps(parm_filter.b2);
285 __m128 a1 = _mm_set1_ps(parm_filter.a1);
286 __m128 a2 = _mm_set1_ps(parm_filter.a2);
288 // Apply the filter FILTER_ORDER times.
289 for (unsigned j = 0; j < parm_filter.filter_order; j++) {
290 __m128 d0 = feedback[j].d0;
291 __m128 d1 = feedback[j].d1;
292 __m64 *inout_ptr = (__m64 *)inout_left_ptr;
294 __m128 in = _mm_set1_ps(0.0f), out;
295 for (unsigned i = n_samples; i; i--) {
296 in = _mm_loadl_pi(in, inout_ptr);
297 out = _mm_add_ps(_mm_mul_ps(b0, in), d0);
298 _mm_storel_pi(inout_ptr, out);
299 d0 = _mm_add_ps(_mm_sub_ps(_mm_mul_ps(b1, in), _mm_mul_ps(a1, out)), d1);
300 d1 = _mm_sub_ps(_mm_mul_ps(b2, in), _mm_mul_ps(a2, out));
307 _MM_SET_FLUSH_ZERO_MODE(old_denormals_mode);
309 if (filters[0].filtertype == FILTER_NONE || filters[0].filter_order == 0)
312 for (unsigned i = 0; i < 2; ++i) {
313 filters[i].set_linear_cutoff(cutoff);
314 filters[i].set_resonance(resonance);
316 filters[i].render_chunk(inout_left_ptr, n_samples, 2);
325 Find the transfer function for an IIR biquad. This is relatively basic signal
326 processing, but for completeness, here's the rationale for the function:
328 The basic system of an IIR biquad looks like this, for input x[n], output y[n]
329 and constant filter coefficients [ab][0-2]:
331 a2 y[n-2] + a1 y[n-1] + a0 y[n] = b2 x[n-2] + b1 x[n-1] + b0 x[n]
333 Taking the discrete Fourier transform (DFT) of both sides (denoting by convention
334 DFT{x[n]} by X[w], where w is the angular frequency, going from 0 to 2pi), yields,
335 due to the linearity and shift properties of the DFT:
337 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]
339 Simple factorization and reorganization yields
341 Y[w] / X[w] = (b1 e^2jw + b1 e^jw + b0) / (a2 e^2jw + a1 e^jw + a0)
343 and Y[w] / X[w] is by definition the filter's _transfer function_
344 (customarily denoted by H(w)), ie. the complex factor it applies to the
345 frequency component w. The absolute value of the transfer function is
346 the frequency response, ie. how much frequency w is boosted or weakened.
348 (This derivation usually goes via the Z-transform and not the DFT, but the
349 idea is exactly the same; the Z-transform is just a bit more general.)
351 Sending a signal through first one filter and then through another one
352 will naturally be equivalent to a filter with the transfer function equal
353 to the pointwise multiplication of the two filters, so for N-order filters
354 we need to raise the answer to the Nth power.
357 complex<double> Filter::evaluate_transfer_function(float omega)
359 complex<float> z = exp(complex<float>(0.0f, omega));
360 complex<float> z2 = z * z;
361 return pow((b0 * z2 + b1 * z + b2) / (1.0f * z2 + a1 * z + a2), filter_order);