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 }