1 /**
2 Basic IIR 1-pole and 2-pole filters through biquads.
3
4 Copyright: Guillaume Piolat 2015-2017.
5 License: $(LINK2 http://www.boost.org/LICENSE_1_0.txt, Boost License 1.0)
6 */
7 module dplug.dsp.iir;
8
9 import std.math: SQRT1_2, PI, pow, sin, cos, sqrt;
10 import std.complex: Complex,
11 complexAbs = abs,
12 complexExp = exp,
13 complexsqAbs = sqAbs,
14 complexFromPolar = fromPolar;
15 import dplug.core.math;
16 import inteli.emmintrin;
17
18 // TODO: function to make biquads from z-plane poles and zeroes
19
20 public
21 {
22
23 /// Type which hold the biquad coefficients.
24 /// Important: Coefficients are considered always normalized by a0.
25 /// Note: coeff[0] is b0,
26 /// coeff[1] is b1,
27 /// coeff[2] is b2,
28 /// coeff[3] is a1,
29 /// coeff[4] is a2 in the litterature.
30 alias BiquadCoeff = double[5];
31
32 /// Maintain state for a biquad state.
33 /// A biquad is a realization that can model two poles and two zeros.
34 struct BiquadDelay
35 {
36 enum order = 2;
37
38 double _x0;
39 double _x1;
40 double _y0;
41 double _y1;
42
43 void initialize() nothrow @nogc
44 {
45 _x0 = 0;
46 _x1 = 0;
47 _y0 = 0;
48 _y1 = 0;
49 }
50
51 static if (order == 2)
52 {
53 /// Process a single sample through the biquad filter.
54 /// This is rather inefficient, in general you'll prefer to use the `nextBuffer`
55 /// functions.
56 float nextSample(float input, const(BiquadCoeff) coeff) nothrow @nogc
57 {
58 double x1 = _x0,
59 x2 = _x1,
60 y1 = _y0,
61 y2 = _y1;
62
63 double a0 = coeff[0],
64 a1 = coeff[1],
65 a2 = coeff[2],
66 a3 = coeff[3],
67 a4 = coeff[4];
68
69 double current = a0 * input + a1 * x1 + a2 * x2 - a3 * y1 - a4 * y2;
70
71 _x0 = input;
72 _x1 = x1;
73 _y0 = current;
74 _y1 = y1;
75 return current;
76 }
77 ///ditto
78 double nextSample(double input, const(BiquadCoeff) coeff) nothrow @nogc
79 {
80 double x1 = _x0,
81 x2 = _x1,
82 y1 = _y0,
83 y2 = _y1;
84
85 double a0 = coeff[0],
86 a1 = coeff[1],
87 a2 = coeff[2],
88 a3 = coeff[3],
89 a4 = coeff[4];
90
91 double current = a0 * input + a1 * x1 + a2 * x2 - a3 * y1 - a4 * y2;
92
93 _x0 = input;
94 _x1 = x1;
95 _y0 = current;
96 _y1 = y1;
97 return current;
98 }
99
100 void nextBuffer(const(float)* input, float* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc
101 {
102 static if (D_InlineAsm_Any)
103 {
104 double x0 = _x0,
105 x1 = _x1,
106 y0 = _y0,
107 y1 = _y1;
108
109 double a0 = coeff[0],
110 a1 = coeff[1],
111 a2 = coeff[2],
112 a3 = coeff[3],
113 a4 = coeff[4];
114
115 version(LDC)
116 {
117 import inteli.emmintrin;
118
119 __m128d XMM0 = _mm_set_pd(x1, x0);
120 __m128d XMM1 = _mm_set_pd(y1, y0);
121 __m128d XMM2 = _mm_set_pd(a2, a1);
122 __m128d XMM3 = _mm_set_pd(a4, a3);
123 __m128d XMM4 = _mm_set_sd(a0);
124
125 __m128d XMM6 = _mm_undefined_pd();
126 __m128d XMM7 = _mm_undefined_pd();
127 __m128d XMM5 = _mm_undefined_pd();
128 for (int n = 0; n < frames; ++n)
129 {
130 __m128 INPUT = _mm_load_ss(input + n);
131 XMM5 = _mm_setzero_pd();
132 XMM5 = _mm_cvtss_sd(XMM5,INPUT);
133
134 XMM6 = XMM0;
135 XMM7 = XMM1;
136 XMM5 = _mm_mul_pd(XMM5, XMM4); // input[i]*a0
137 XMM6 = _mm_mul_pd(XMM6, XMM2); // x1*a2 x0*a1
138 XMM7 = _mm_mul_pd(XMM7, XMM3); // y1*a4 y0*a3
139 XMM5 = _mm_add_pd(XMM5, XMM6);
140 XMM5 = _mm_sub_pd(XMM5, XMM7); // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3
141 XMM6 = XMM5;
142
143 XMM0 = cast(double2) _mm_slli_si128!8(cast(__m128i) XMM0);
144 XMM6 = cast(double2) _mm_srli_si128!8(cast(__m128i) XMM6);
145
146 XMM0 = _mm_cvtss_sd(XMM0, INPUT);
147 XMM5 = _mm_add_pd(XMM5, XMM6);
148 XMM7 = cast(double2) _mm_cvtsd_ss(_mm_undefined_ps(), XMM5);
149 XMM5 = _mm_unpacklo_pd(XMM5, XMM1);
150 XMM1 = XMM5;
151 _mm_store_ss(output + n, cast(__m128) XMM7);
152 }
153 _mm_storel_pd(&x0, XMM0);
154 _mm_storeh_pd(&x1, XMM0);
155 _mm_storel_pd(&y0, XMM1);
156 _mm_storeh_pd(&y1, XMM1);
157 }
158 else version(D_InlineAsm_X86)
159 {
160 asm nothrow @nogc
161 {
162 mov EAX, input;
163 mov EDX, output;
164 mov ECX, frames;
165
166 movlpd XMM0, qword ptr x0; // XMM0 = x1 x0
167 movhpd XMM0, qword ptr x1;
168 movlpd XMM1, qword ptr y0; // XMM1 = y1 y0
169 movhpd XMM1, qword ptr y1;
170
171 movlpd XMM2, qword ptr a1; // XMM2 = a2 a1
172 movhpd XMM2, qword ptr a2;
173 movlpd XMM3, qword ptr a3; // XMM3 = a4 a3
174 movhpd XMM3, qword ptr a4;
175
176 movq XMM4, qword ptr a0; // XMM4 = 0 a0
177
178 align 16;
179 loop:
180 pxor XMM5, XMM5;
181 cvtss2sd XMM5, dword ptr [EAX];
182
183 movapd XMM6, XMM0;
184 movapd XMM7, XMM1;
185
186 mulpd XMM5, XMM4; // input[i]*a0
187 mulpd XMM6, XMM2; // x1*a2 x0*a1
188 mulpd XMM7, XMM3; // y1*a4 y0*a3
189
190 addpd XMM5, XMM6;
191 subpd XMM5, XMM7; // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3
192
193 movapd XMM6, XMM5;
194 pslldq XMM0, 8;
195 psrldq XMM6, 8;
196
197 cvtss2sd XMM0, dword ptr [EAX]; // XMM0 <- x0 input[i]
198 addpd XMM5, XMM6; // garbage | input[i]*a0 + x0*a1 - y0*a3 + x1*a2 - y1*a4
199
200 cvtsd2ss XMM7, XMM5;
201 punpcklqdq XMM5, XMM1; // XMM5 <- y0 current
202 add EAX, 4;
203 movd dword ptr [EDX], XMM7;
204 add EDX, 4;
205 movapd XMM1, XMM5;
206
207 sub ECX, 1;
208 jnz short loop;
209
210 movlpd qword ptr x0, XMM0;
211 movhpd qword ptr x1, XMM0;
212 movlpd qword ptr y0, XMM1;
213 movhpd qword ptr y1, XMM1;
214 }
215 }
216 else version(D_InlineAsm_X86_64)
217 {
218 ubyte[16] storage0;
219 ubyte[16] storage1;
220 asm nothrow @nogc
221 {
222 movups storage0, XMM6;
223 movups storage1, XMM7;
224
225 mov RAX, input;
226 mov RDX, output;
227 mov ECX, frames;
228
229 movlpd XMM0, qword ptr x0; // XMM0 = x1 x0
230 movhpd XMM0, qword ptr x1;
231 movlpd XMM1, qword ptr y0; // XMM1 = y1 y0
232 movhpd XMM1, qword ptr y1;
233
234 movlpd XMM2, qword ptr a1; // XMM2 = a2 a1
235 movhpd XMM2, qword ptr a2;
236 movlpd XMM3, qword ptr a3; // XMM3 = a4 a3
237 movhpd XMM3, qword ptr a4;
238
239 movq XMM4, qword ptr a0; // XMM4 = 0 a0
240
241 align 16;
242 loop:
243 pxor XMM5, XMM5;
244 cvtss2sd XMM5, dword ptr [RAX];
245
246 movapd XMM6, XMM0;
247 movapd XMM7, XMM1;
248
249 mulpd XMM5, XMM4; // input[i]*a0
250 mulpd XMM6, XMM2; // x1*a2 x0*a1
251 mulpd XMM7, XMM3; // y1*a4 y0*a3
252
253 addpd XMM5, XMM6;
254 subpd XMM5, XMM7; // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3
255
256 movapd XMM6, XMM5;
257 pslldq XMM0, 8;
258 psrldq XMM6, 8;
259
260 addpd XMM5, XMM6; // garbage | input[i]*a0 + x0*a1 - y0*a3 + x1*a2 - y1*a4
261 cvtss2sd XMM0, dword ptr [RAX]; // XMM0 <- x0 input[i]
262 cvtsd2ss XMM7, XMM5;
263 punpcklqdq XMM5, XMM1; // XMM5 <- y0 current
264 add RAX, 4;
265 movd dword ptr [RDX], XMM7;
266 add RDX, 4;
267 movapd XMM1, XMM5;
268
269 sub ECX, 1;
270 jnz short loop;
271
272 movlpd qword ptr x0, XMM0;
273 movhpd qword ptr x1, XMM0;
274 movlpd qword ptr y0, XMM1;
275 movhpd qword ptr y1, XMM1;
276
277 movups XMM6, storage0; // XMMx with x >= 6 registers need to be preserved
278 movups XMM7, storage1;
279 }
280 }
281 else
282 static assert(false, "Not implemented for this platform.");
283
284 _x0 = x0;
285 _x1 = x1;
286 _y0 = y0;
287 _y1 = y1;
288 }
289 else
290 {
291 double x0 = _x0,
292 x1 = _x1,
293 y0 = _y0,
294 y1 = _y1;
295
296 double a0 = coeff[0],
297 a1 = coeff[1],
298 a2 = coeff[2],
299 a3 = coeff[3],
300 a4 = coeff[4];
301
302 for(int i = 0; i < frames; ++i)
303 {
304 double current = a0 * input[i] + a1 * x0 + a2 * x1 - a3 * y0 - a4 * y1;
305
306 x1 = x0;
307 x0 = input[i];
308 y1 = y0;
309 y0 = current;
310 output[i] = current;
311 }
312
313 _x0 = x0;
314 _x1 = x1;
315 _y0 = y0;
316 _y1 = y1;
317 }
318 }
319 ///ditto
320 void nextBuffer(const(double)* input, double* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc
321 {
322 double x0 = _x0,
323 x1 = _x1,
324 y0 = _y0,
325 y1 = _y1;
326
327 double a0 = coeff[0],
328 a1 = coeff[1],
329 a2 = coeff[2],
330 a3 = coeff[3],
331 a4 = coeff[4];
332
333 __m128d XMM0 = _mm_set_pd(x1, x0);
334 __m128d XMM1 = _mm_set_pd(y1, y0);
335 __m128d XMM2 = _mm_set_pd(a2, a1);
336 __m128d XMM3 = _mm_set_pd(a4, a3);
337 __m128d XMM4 = _mm_set_sd(a0);
338
339 __m128d XMM6 = _mm_undefined_pd();
340 __m128d XMM7 = _mm_undefined_pd();
341 __m128d XMM5 = _mm_undefined_pd();
342 for (int n = 0; n < frames; ++n)
343 {
344 __m128d INPUT = _mm_load_sd(input + n);
345 XMM5 = INPUT;
346
347 XMM6 = XMM0;
348 XMM7 = XMM1;
349 XMM5 = _mm_mul_pd(XMM5, XMM4); // input[i]*a0
350 XMM6 = _mm_mul_pd(XMM6, XMM2); // x1*a2 x0*a1
351 XMM7 = _mm_mul_pd(XMM7, XMM3); // y1*a4 y0*a3
352 XMM5 = _mm_add_pd(XMM5, XMM6);
353 XMM5 = _mm_sub_pd(XMM5, XMM7); // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3
354 XMM6 = XMM5;
355
356 XMM0 = cast(double2) _mm_slli_si128!8(cast(__m128i) XMM0);
357 XMM6 = cast(double2) _mm_srli_si128!8(cast(__m128i) XMM6);
358 XMM0.ptr[0] = INPUT.array[0];
359 XMM5 = _mm_add_pd(XMM5, XMM6);
360 XMM7 = XMM5;
361 XMM5 = _mm_unpacklo_pd(XMM5, XMM1);
362 XMM1 = XMM5;
363 _mm_store_sd(output + n, XMM7);
364 }
365 _mm_storel_pd(&x0, XMM0);
366 _mm_storeh_pd(&x1, XMM0);
367 _mm_storel_pd(&y0, XMM1);
368 _mm_storeh_pd(&y1, XMM1);
369 }
370
371 /// Special version of biquad processing, for a constant DC input.
372 void nextBuffer(float input, float* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc
373 {
374 // Note: this naive version performs better than an intel-intrinsics one
375 double x0 = _x0,
376 x1 = _x1,
377 y0 = _y0,
378 y1 = _y1;
379
380 double a0 = coeff[0],
381 a1 = coeff[1],
382 a2 = coeff[2],
383 a3 = coeff[3],
384 a4 = coeff[4];
385
386 for(int i = 0; i < frames; ++i)
387 {
388 double current = a0 * input + a1 * x0 + a2 * x1 - a3 * y0 - a4 * y1;
389
390 x1 = x0;
391 x0 = input;
392 y1 = y0;
393 y0 = current;
394 output[i] = current;
395 }
396
397 _x0 = x0;
398 _x1 = x1;
399 _y0 = y0;
400 _y1 = y1;
401 }
402 ///ditto
403 void nextBuffer(double input, double* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc
404 {
405 double x0 = _x0,
406 x1 = _x1,
407 y0 = _y0,
408 y1 = _y1;
409
410 double a0 = coeff[0],
411 a1 = coeff[1],
412 a2 = coeff[2],
413 a3 = coeff[3],
414 a4 = coeff[4];
415
416 for(int i = 0; i < frames; ++i)
417 {
418 double current = a0 * input + a1 * x0 + a2 * x1 - a3 * y0 - a4 * y1;
419
420 x1 = x0;
421 x0 = input;
422 y1 = y0;
423 y0 = current;
424 output[i] = current;
425 }
426
427 _x0 = x0;
428 _x1 = x1;
429 _y0 = y0;
430 _y1 = y1;
431 }
432 }
433 }
434
435 /// 1-pole low-pass filter.
436 /// Note: the cutoff frequency can be >= nyquist, in which case it asymptotically approaches a bypass.
437 /// the cutoff frequency can be below 0 Hz, in which case it is equal to zero.
438 /// This filter is normalized on DC.
439 /// You always have -3 dB at cutoff in the valid range.
440 ///
441 /// See_also: http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/
442 BiquadCoeff biquadOnePoleLowPass(double frequency, double sampleRate) nothrow @nogc
443 {
444 double fc = frequency / sampleRate;
445 if (fc < 0.0f)
446 fc = 0.0f;
447 double t2 = fast_exp(-2.0 * PI * fc);
448 BiquadCoeff result;
449 result[0] = 1 - t2;
450 result[1] = 0;
451 result[2] = 0;
452 result[3] = -t2;
453 result[4] = 0;
454 return result;
455 }
456
457 /// 1-pole high-pass filter.
458 /// Note: Like the corresponding one-pole lowpass, this is normalized for DC.
459 /// The cutoff frequency can be <= 0 Hz, in which case it is a bypass.
460 /// Going in very high frequency does NOT give zero.
461 /// You always have -3 dB at cutoff in the valid range.
462 ///
463 /// See_also: https://www.dspguide.com/ch19/2.html
464 BiquadCoeff biquadOnePoleHighPass(double frequency, double sampleRate) nothrow @nogc
465 {
466 double fc = frequency / sampleRate;
467 if (fc < 0.0f)
468 fc = 0.0f;
469 double t2 = fast_exp(-2.0 * PI * fc);
470 BiquadCoeff result;
471 result[0] = (1 + t2) * 0.5;
472 result[1] = -(1 + t2) * 0.5;
473 result[2] = 0;
474 result[3] = -t2;
475 result[4] = 0;
476 return result;
477 }
478
479 /// 1-pole low-pass filter, frequency mapping is not precise.
480 /// Not accurate across sample rates, but coefficient computation is cheap. Not advised.
481 BiquadCoeff biquadOnePoleLowPassImprecise(double frequency, double samplerate) nothrow @nogc
482 {
483 double t0 = frequency / samplerate;
484 if (t0 > 0.5f)
485 t0 = 0.5f;
486
487 double t1 = (1 - 2 * t0);
488 double t2 = t1 * t1;
489
490 BiquadCoeff result;
491 result[0] = cast(float)(1 - t2);
492 result[1] = 0;
493 result[2] = 0;
494 result[3] = cast(float)(-t2);
495 result[4] = 0;
496 return result;
497 }
498
499 /// 1-pole high-pass filter, frequency mapping is not precise.
500 /// Not accurate across sample rates, but coefficient computation is cheap. Not advised.
501 BiquadCoeff biquadOnePoleHighPassImprecise(double frequency, double samplerate) nothrow @nogc
502 {
503 double t0 = frequency / samplerate;
504 if (t0 > 0.5f)
505 t0 = 0.5f;
506
507 double t1 = (2 * t0);
508 double t2 = t1 * t1;
509
510 BiquadCoeff result;
511 result[0] = cast(float)(1 - t2);
512 result[1] = 0;
513 result[2] = 0;
514 result[3] = cast(float)(t2);
515 result[4] = 0;
516 return result;
517 }
518
519 // Cookbook formulae for audio EQ biquad filter coefficients
520 // by Robert Bristow-Johnson
521
522 /// Low-pass filter 12 dB/oct as described by Robert Bristow-Johnson.
523 BiquadCoeff biquadRBJLowPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc
524 {
525 return generateBiquad(BiquadType.LOW_PASS_FILTER, frequency, samplerate, 0, Q);
526 }
527
528 /// High-pass filter 12 dB/oct as described by Robert Bristow-Johnson.
529 BiquadCoeff biquadRBJHighPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc
530 {
531 return generateBiquad(BiquadType.HIGH_PASS_FILTER, frequency, samplerate, 0, Q);
532 }
533
534 /// Band-pass filter as described by Robert Bristow-Johnson.
535 BiquadCoeff biquadRBJBandPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc
536 {
537 return generateBiquad(BiquadType.BAND_PASS_FILTER, frequency, samplerate, 0, Q);
538 }
539
540 /// Notch filter as described by Robert Bristow-Johnson.
541 BiquadCoeff biquadRBJNotch(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc
542 {
543 return generateBiquad(BiquadType.NOTCH_FILTER, frequency, samplerate, 0, Q);
544 }
545
546 /// Peak filter as described by Robert Bristow-Johnson.
547 BiquadCoeff biquadRBJPeak(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc
548 {
549 return generateBiquad(BiquadType.PEAK_FILTER, frequency, samplerate, gain, Q);
550 }
551
552 /// Low-shelf filter as described by Robert Bristow-Johnson.
553 BiquadCoeff biquadRBJLowShelf(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc
554 {
555 return generateBiquad(BiquadType.LOW_SHELF, frequency, samplerate, gain, Q);
556 }
557
558 /// High-shelf filter as described by Robert Bristow-Johnson.
559 BiquadCoeff biquadRBJHighShelf(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc
560 {
561 return generateBiquad(BiquadType.HIGH_SHELF, frequency, samplerate, gain, Q);
562 }
563
564 /// 2nd order All-pass filter as described by Robert Bristow-Johnson.
565 /// This is helpful to introduce the exact same phase response as the RBJ low-pass, but doesn't affect magnitude.
566 BiquadCoeff biquadRBJAllPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc
567 {
568 return generateBiquad(BiquadType.ALL_PASS_FILTER, frequency, samplerate, 0, Q);
569 }
570
571 /// Identity biquad, pass signal unchanged.
572 BiquadCoeff biquadBypass() nothrow @nogc
573 {
574 BiquadCoeff result;
575 result[0] = 1;
576 result[1] = 0;
577 result[2] = 0;
578 result[3] = 0;
579 result[4] = 0;
580 return result;
581 }
582
583 /// Zero biquad, gives zero output.
584 BiquadCoeff biquadZero() nothrow @nogc
585 {
586 BiquadCoeff result;
587 result[0] = 0;
588 result[1] = 0;
589 result[2] = 0;
590 result[3] = 0;
591 result[4] = 0;
592 return result;
593 }
594
595 /// Bessel 2-pole lowpass.
596 BiquadCoeff biquadBesselLowPass(double frequency, double sampleRate) nothrow @nogc
597 {
598 double normalGain = 1;
599 double normalW = 0;
600 Complex!double pole0 = Complex!double(-1.5 , 0.8660);
601 Complex!double pole1 = Complex!double(-1.5 , -0.8660);
602 double fc = frequency / sampleRate;
603 double T = fc * 2 * PI;
604 pole0 = complexExp(pole0 * T); // matched Z transform
605 pole1 = complexExp(pole1 * T);
606 Complex!double zero01 = Complex!double(-1.0, 0.0);
607 BiquadCoeff coeff = biquad2Poles(pole0, zero01, pole1, zero01);
608 double scaleFactor = 1.0 / complexAbs( biquadResponse(coeff, 0 ));
609 return biquadApplyScale(coeff, scaleFactor);
610 }
611 }
612
613 private
614 {
615 enum BiquadType
616 {
617 LOW_PASS_FILTER,
618 HIGH_PASS_FILTER,
619 BAND_PASS_FILTER,
620 NOTCH_FILTER,
621 PEAK_FILTER,
622 LOW_SHELF,
623 HIGH_SHELF,
624 ALL_PASS_FILTER
625 }
626
627 // generates RBJ biquad coefficients
628 BiquadCoeff generateBiquad(BiquadType type, double frequency, double samplerate, double gaindB, double Q) nothrow @nogc
629 {
630 // regardless of the output precision, always compute coefficients in double precision
631
632 double A = pow(10.0, gaindB / 40.0);
633 double w0 = (2.0 * PI) * frequency / samplerate;
634 double sin_w0 = sin(w0);
635 double cos_w0 = cos(w0);
636
637 double alpha = sin_w0 / (2 * Q);
638
639 // = sin(w0)*sinh( ln(2)/2 * BW * w0/sin(w0) ) (case: BW)
640 // = sin(w0)/2 * sqrt( (A + 1/A)*(1/S - 1) + 2 ) (case: S)
641
642 double b0, b1, b2, a0, a1, a2;
643
644 final switch(type)
645 {
646 case BiquadType.LOW_PASS_FILTER:
647 b0 = (1 - cos_w0) / 2;
648 b1 = 1 - cos_w0;
649 b2 = (1 - cos_w0) / 2;
650 a0 = 1 + alpha;
651 a1 = -2 * cos_w0;
652 a2 = 1 - alpha;
653 break;
654
655 case BiquadType.HIGH_PASS_FILTER:
656 b0 = (1 + cos_w0) / 2;
657 b1 = -(1 + cos_w0);
658 b2 = (1 + cos_w0) / 2;
659 a0 = 1 + alpha;
660 a1 = -2 * cos_w0;
661 a2 = 1 - alpha;
662 break;
663
664 case BiquadType.BAND_PASS_FILTER:
665 b0 = alpha;
666 b1 = 0;
667 b2 = -alpha;
668 a0 = 1 + alpha;
669 a1 = -2 * cos_w0;
670 a2 = 1 - alpha;
671 break;
672
673 case BiquadType.NOTCH_FILTER:
674 b0 = 1;
675 b1 = -2*cos_w0;
676 b2 = 1;
677 a0 = 1 + alpha;
678 a1 = -2*cos(w0);
679 a2 = 1 - alpha;
680 break;
681
682 case BiquadType.PEAK_FILTER:
683 b0 = 1 + alpha * A;
684 b1 = -2 * cos_w0;
685 b2 = 1 - alpha * A;
686 a0 = 1 + alpha / A;
687 a1 = -2 * cos_w0;
688 a2 = 1 - alpha / A;
689 break;
690
691 case BiquadType.LOW_SHELF:
692 {
693 double ap1 = A + 1;
694 double am1 = A - 1;
695 double M = 2 * sqrt(A) * alpha;
696 b0 = A * (ap1 - am1 * cos_w0 + M);
697 b1 = 2 * A * (am1 - ap1 * cos_w0);
698 b2 = A * (ap1 - am1 * cos_w0 - M);
699 a0 = ap1 + am1 * cos_w0 + M;
700 a1 = -2 * (am1 + ap1 * cos_w0);
701 a2 = ap1 + am1 * cos_w0 - M;
702 }
703 break;
704
705 case BiquadType.HIGH_SHELF:
706 {
707 double ap1 = A + 1;
708 double am1 = A - 1;
709 double M = 2 * sqrt(A) * alpha;
710 b0 = A * (ap1 + am1 * cos_w0 + M);
711 b1 = -2 * A * (am1 + ap1 * cos_w0);
712 b2 = A * (ap1 + am1 * cos_w0 - M);
713 a0 = ap1 - am1 * cos_w0 + M;
714 a1 = 2 * (am1 - ap1 * cos_w0);
715 a2 = ap1 - am1 * cos_w0 - M;
716 }
717 break;
718
719 case BiquadType.ALL_PASS_FILTER:
720 {
721 b0 = 1 - alpha;
722 b1 = -2 * cos_w0;
723 b2 = 1 + alpha;
724 a0 = 1 + alpha;
725 a1 = -2 * cos_w0;
726 a2 = 1 - alpha;
727 }
728 break;
729 }
730
731 BiquadCoeff result;
732 result[0] = cast(float)(b0 / a0); // FUTURE: this sounds useless and harmful to cast to float???
733 result[1] = cast(float)(b1 / a0);
734 result[2] = cast(float)(b2 / a0);
735 result[3] = cast(float)(a1 / a0);
736 result[4] = cast(float)(a2 / a0);
737 return result;
738 }
739 }
740
741
742 // TODO: deprecated this assembly, sounds useless vs inteli
743 version(D_InlineAsm_X86)
744 private enum D_InlineAsm_Any = true;
745 else version(D_InlineAsm_X86_64)
746 private enum D_InlineAsm_Any = true;
747 else
748 private enum D_InlineAsm_Any = false;
749
750
751 private:
752
753
754
755 BiquadCoeff biquad2Poles(Complex!double pole1, Complex!double zero1, Complex!double pole2, Complex!double zero2) nothrow @nogc
756 {
757 // Note: either it's a double pole, or two pole on the real axis.
758 // Same for zeroes
759
760 assert(complexAbs(pole1) <= 1);
761 assert(complexAbs(pole2) <= 1);
762
763 double a1;
764 double a2;
765 double epsilon = 0;
766
767 if (pole1.im != 0)
768 {
769 assert(pole1.re == pole2.re);
770 assert(pole1.im == -pole2.im);
771 a1 = -2 * pole1.re;
772 a2 = complexsqAbs(pole1);
773 }
774 else
775 {
776 assert(pole2.im == 0);
777 a1 = -(pole1.re + pole2.re);
778 a2 = pole1.re * pole2.re;
779 }
780
781 const double b0 = 1;
782 double b1;
783 double b2;
784
785 if (zero1.im != 0)
786 {
787 assert(zero2.re == zero2.re);
788 assert(zero2.im == -zero2.im);
789 b1 = -2 * zero1.re;
790 b2 = complexsqAbs(zero1);
791 }
792 else
793 {
794 assert(zero2.im == 0);
795 b1 = -(zero1.re + zero2.re);
796 b2 = zero1.re * zero2.re;
797 }
798
799 return [b0, b1, b2, a1, a2];
800 }
801
802 BiquadCoeff biquadApplyScale(BiquadCoeff biquad, double scale) nothrow @nogc
803 {
804 biquad[0] *= scale;
805 biquad[1] *= scale;
806 biquad[2] *= scale;
807 return biquad;
808 }
809
810 // Calculate filter response at the given normalized frequency.
811 Complex!double biquadResponse(BiquadCoeff coeff, double normalizedFrequency) nothrow @nogc
812 {
813 static Complex!double addmul(Complex!double c, double v, Complex!double c1)
814 {
815 return Complex!double(c.re + v * c1.re, c.im + v * c1.im);
816 }
817
818 double w = 2 * PI * normalizedFrequency;
819 Complex!double czn1 = complexFromPolar (1., -w);
820 Complex!double czn2 = complexFromPolar (1., -2 * w);
821 Complex!double ch = 1.0;
822 Complex!double cbot = 1.0;
823
824 Complex!double cb = 1.0;
825 Complex!double ct = coeff[0]; // b0
826 ct = addmul (ct, coeff[1], czn1); // b1
827 ct = addmul (ct, coeff[2], czn2); // b2
828 cb = addmul (cb, coeff[3], czn1); // a1
829 cb = addmul (cb, coeff[4], czn2); // a2
830 ch *= ct;
831 cbot *= cb;
832 return ch / cbot;
833 }
834
835 nothrow @nogc unittest
836 {
837 BiquadCoeff c = biquadBesselLowPass(20000, 44100);
838 }