src/calf/biquad.h

   1 /* Calf DSP Library
   2  * Biquad filters
   3  * Copyright (C) 2001-2007 Krzysztof Foltman
   4  *
   5  * Most of code in this file is based on freely
   6  * available other work of other people (filter equations).
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU Lesser General Public
  10  * License as published by the Free Software Foundation; either
  11  * version 2 of the License, or (at your option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  * Lesser General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU Lesser General
  19  * Public License along with this program; if not, write to the
  20  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  21  * Boston, MA  02110-1301  USA
  22  */
  23 #ifndef __CALF_BIQUAD_H
  24 #define __CALF_BIQUAD_H
  25
  26 #include <complex>
  27 #include "primitives.h"
  28
  29 namespace dsp {
  30
  31 /**
  32  * Coefficients for two-pole two-zero filter, for floating point values,
  33  * plus a bunch of functions to set them to typical values.
  34  *
  35  * Coefficient calculation is based on famous Robert Bristow-Johnson's equations,
  36  * except where it's not.
  37  * The coefficient calculation is NOT mine, the only exception is the lossy
  38  * optimization in Zoelzer and rbj HP filter code.
  39  *
  40  * See http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt for reference.
  41  *
  42  * don't use this for integers because it won't work
  43  */
  44 class biquad_coeffs
  45 {
  46 public:
  47     // filter coefficients
  48     double a0, a1, a2, b1, b2;
  49     typedef std::complex<double> cfloat;
  50
  51     biquad_coeffs()
  52     {
  53         set_null();
  54     }
  55
  56     inline void set_null()
  57     {
  58         a0 = 1.0;
  59         b1 = b2 = a1 = a2 = 0.f;
  60     }
  61
  62     /** Lowpass filter based on Robert Bristow-Johnson's equations
  63      * Perhaps every synth code that doesn't use SVF uses these
  64      * equations :)
  65      * @param fc     resonant frequency
  66      * @param q      resonance (gain at fc)
  67      * @param sr     sample rate
  68      * @param gain   amplification (gain at 0Hz)
  69      */
  70     inline void set_lp_rbj(float fc, float q, float sr, float gain = 1.0)
  71     {
  72         double omega=(2.0*M_PI*fc/sr);
  73         double sn=sin(omega);
  74         double cs=cos(omega);
  75         double alpha=(sn/(2*q));
  76         double inv=(1.0/(1.0+alpha));
  77
  78         a2 = a0 =  (gain*inv*(1.0 - cs)*0.5);
  79         a1 =  a0 + a0;
  80         b1 =  (-2.0*cs*inv);
  81         b2 =  ((1.0 - alpha)*inv);
  82     }
  83
  84     // different lowpass filter, based on Zoelzer's equations, modified by
  85     // me (kfoltman) to use polynomials to approximate tangent function
  86     // not very accurate, but perhaps good enough for synth work :)
  87     // odsr is "one divided by samplerate"
  88     // from how it looks, it perhaps uses bilinear transform - but who knows :)
  89     inline void set_lp_zoelzer(float fc, float q, float odsr, float gain=1.0)
  90     {
  91         double omega=(M_PI*fc*odsr);
  92         double omega2=omega*omega;
  93         double K=omega*(1+omega2*omega2*(1.0/1.45));
  94         double KK=K*K;
  95         double QK=q*(KK+1.f);
  96         double iQK=1.0f/(QK+K);
  97         double inv=q*iQK;
  98         b2 =  (iQK*(QK-K));
  99         b1 =  (2.*(KK-1.f)*inv);
 100         a2 = a0 =  (inv*gain*KK);
 101         a1 =  a0 + a0;
 102     }
 103
 104     /** Highpass filter based on Robert Bristow-Johnson's equations
 105      * @param fc     resonant frequency
 106      * @param q      resonance (gain at fc)
 107      * @param sr     sample rate
 108      * @param gain   amplification (gain at sr/2)
 109      */
 110     inline void set_hp_rbj(float fc, float q, float esr, float gain=1.0)
 111     {
 112         double omega=(double)(2*M_PI*fc/esr);
 113         double sn=sin(omega);
 114         double cs=cos(omega);
 115         double alpha=(double)(sn/(2*q));
 116
 117         double inv=(double)(1.0/(1.0+alpha));
 118
 119         a0 =  (gain*inv*(1 + cs)/2);
 120         a1 =  -2.f * a0;
 121         a2 =  a0;
 122         b1 =  (-2*cs*inv);
 123         b2 =  ((1 - alpha)*inv);
 124     }
 125
 126     // this replaces sin/cos with polynomial approximation
 127     inline void set_hp_rbj_optimized(float fc, float q, float esr, float gain=1.0)
 128     {
 129         double omega=(double)(2*M_PI*fc/esr);
 130         double sn=omega+omega*omega*omega*(1.0/6.0)+omega*omega*omega*omega*omega*(1.0/120);
 131         double cs=1-omega*omega*(1.0/2.0)+omega*omega*omega*omega*(1.0/24);
 132         double alpha=(double)(sn/(2*q));
 133
 134         double inv=(double)(1.0/(1.0+alpha));
 135
 136         a0 =  (gain*inv*(1 + cs)*(1.0/2.0));
 137         a1 =  -2.f * a0;
 138         a2 =  a0;
 139         b1 =  (-2.*cs*inv);
 140         b2 =  ((1. - alpha)*inv);
 141     }
 142
 143     /** Bandpass filter based on Robert Bristow-Johnson's equations (normalized to 1.0 at center frequency)
 144      * @param fc     center frequency (gain at fc = 1.0)
 145      * @param q      =~ fc/bandwidth (not quite, but close)  - 1/Q = 2*sinh(ln(2)/2*BW*w0/sin(w0))
 146      * @param sr     sample rate
 147      * @param gain   amplification (gain at sr/2)
 148      */
 149     inline void set_bp_rbj(double fc, double q, double esr, double gain=1.0)
 150     {
 151         double omega=(double)(2*M_PI*fc/esr);
 152         double sn=sin(omega);
 153         double cs=cos(omega);
 154         double alpha=(double)(sn/(2*q));
 155
 156         double inv=(double)(1.0/(1.0+alpha));
 157
 158         a0 =  (double)(gain*inv*alpha);
 159         a1 =  0.f;
 160         a2 =  (double)(-gain*inv*alpha);
 161         b1 =  (double)(-2*cs*inv);
 162         b2 =  (double)((1 - alpha)*inv);
 163     }
 164
 165     // rbj's bandreject
 166     inline void set_br_rbj(double fc, double q, double esr, double gain=1.0)
 167     {
 168         double omega=(double)(2*M_PI*fc/esr);
 169         double sn=sin(omega);
 170         double cs=cos(omega);
 171         double alpha=(double)(sn/(2*q));
 172
 173         double inv=(double)(1.0/(1.0+alpha));
 174
 175         a0 =  (gain*inv);
 176         a1 =  (-gain*inv*2.*cs);
 177         a2 =  (gain*inv);
 178         b1 =  (-2.*cs*inv);
 179         b2 =  ((1. - alpha)*inv);
 180     }
 181     // this is mine (and, I guess, it sucks/doesn't work)
 182     void set_allpass(float freq, float pole_r, float sr)
 183     {
 184         double a=prewarp(freq, sr);
 185         double q=pole_r;
 186         set_bilinear(a*a+q*q, -2.0f*a, 1, a*a+q*q, 2.0f*a, 1);
 187     }
 188     /// prewarping for bilinear transform, maps given digital frequency to analog counterpart for analog filter design
 189     static inline double prewarp(float freq, float sr)
 190     {
 191         if (freq>sr*0.49) freq=(float)(sr*0.49);
 192         return (double)(tan(M_PI*freq/sr));
 193     }
 194     /// convert analog angular frequency value to digital
 195     static inline double unwarp(float omega, float sr)
 196     {
 197         double T = 1.0 / sr;
 198         return (2 / T) * atan(omega * T / 2);
 199     }
 200     /// convert analog filter time constant to digital counterpart
 201     static inline double unwarpf(float t, float sr)
 202     {
 203         // this is most likely broken and works by pure accident!
 204         double omega = 1.0 / t;
 205         omega = unwarp(omega, sr);
 206         // I really don't know why does it have to be M_PI and not 2 * M_PI!
 207         double f = M_PI / omega;
 208         return f / sr;
 209     }
 210     /// set digital filter parameters based on given analog filter parameters
 211     void set_bilinear(double aa0, double aa1, double aa2, double ab0, double ab1, double ab2)
 212     {
 213         double q=(double)(1.0/(ab0+ab1+ab2));
 214         a0 = (aa0+aa1+aa2)*q;
 215         a1 = 2*(aa0-aa2)*q;
 216         a2 = (aa0-aa1+aa2)*q;
 217         b1 = 2*(ab0-ab2)*q;
 218         b2 = (ab0-ab1+ab2)*q;
 219     }
 220
 221     /// set digital filter parameters directly
 222     void set_bilinear_direct(double aa0, double aa1, double aa2, double ab1, double ab2)
 223     {
 224         a0 = aa0;
 225         a1 = aa1;
 226         a2 = aa2;
 227         b1 = ab1;
 228         b2 = ab2;
 229     }
 230
 231
 232     /// RBJ peaking EQ
 233     /// @param freq   peak frequency
 234     /// @param q      q (correlated to freq/bandwidth, @see set_bp_rbj)
 235     /// @param peak   peak gain (1.0 means no peak, >1.0 means a peak, less than 1.0 is a dip)
 236     inline void set_peakeq_rbj(double freq, double q, double peak, double sr)
 237     {
 238         double A = sqrt(peak);
 239         double w0 = freq * 2 * M_PI * (1.0 / sr);
 240         double alpha = sin(w0) / (2 * q);
 241         double ib0 = 1.0 / (1 + alpha/A);
 242         a1 = b1 = -2*cos(w0) * ib0;
 243         a0 = ib0 * (1 + alpha*A);
 244         a2 = ib0 * (1 - alpha*A);
 245         b2 = ib0 * (1 - alpha/A);
 246     }
 247
 248     /// RBJ low shelf EQ - amplitication of 'peak' at 0 Hz and of 1.0 (0dB) at sr/2 Hz
 249     /// @param freq   corner frequency (gain at freq is sqrt(peak))
 250     /// @param q      q (relates bandwidth and peak frequency), the higher q, the louder the resonant peak (situated below fc) is
 251     /// @param peak   shelf gain (1.0 means no peak, >1.0 means a peak, less than 1.0 is a dip)
 252     inline void set_lowshelf_rbj(float freq, float q, float peak, float sr)
 253     {
 254         double A = sqrt(peak);
 255         double w0 = freq * 2 * M_PI * (1.0 / sr);
 256         double alpha = sin(w0) / (2 * q);
 257         double cw0 = cos(w0);
 258         double tmp = 2 * sqrt(A) * alpha;
 259         double b0 = 0.f, ib0 = 0.f;
 260
 261         a0 =    A*( (A+1) - (A-1)*cw0 + tmp);
 262         a1 =  2*A*( (A-1) - (A+1)*cw0);
 263         a2 =    A*( (A+1) - (A-1)*cw0 - tmp);
 264         b0 =        (A+1) + (A-1)*cw0 + tmp;
 265         b1 =   -2*( (A-1) + (A+1)*cw0);
 266         b2 =        (A+1) + (A-1)*cw0 - tmp;
 267
 268         ib0 = 1.0 / b0;
 269         b1 *= ib0;
 270         b2 *= ib0;
 271         a0 *= ib0;
 272         a1 *= ib0;
 273         a2 *= ib0;
 274     }
 275
 276     /// RBJ high shelf EQ - amplitication of 0dB at 0 Hz and of peak at sr/2 Hz
 277     /// @param freq   corner frequency (gain at freq is sqrt(peak))
 278     /// @param q      q (relates bandwidth and peak frequency), the higher q, the louder the resonant peak (situated above fc) is
 279     /// @param peak   shelf gain (1.0 means no peak, >1.0 means a peak, less than 1.0 is a dip)
 280     inline void set_highshelf_rbj(float freq, float q, float peak, float sr)
 281     {
 282         double A = sqrt(peak);
 283         double w0 = freq * 2 * M_PI * (1.0 / sr);
 284         double alpha = sin(w0) / (2 * q);
 285         double cw0 = cos(w0);
 286         double tmp = 2 * sqrt(A) * alpha;
 287         double b0 = 0.f, ib0 = 0.f;
 288
 289         a0 =    A*( (A+1) + (A-1)*cw0 + tmp);
 290         a1 = -2*A*( (A-1) + (A+1)*cw0);
 291         a2 =    A*( (A+1) + (A-1)*cw0 - tmp);
 292         b0 =        (A+1) - (A-1)*cw0 + tmp;
 293         b1 =    2*( (A-1) - (A+1)*cw0);
 294         b2 =        (A+1) - (A-1)*cw0 - tmp;
 295
 296         ib0 = 1.0 / b0;
 297         b1 *= ib0;
 298         b2 *= ib0;
 299         a0 *= ib0;
 300         a1 *= ib0;
 301         a2 *= ib0;
 302     }
 303
 304     /// copy coefficients from another biquad
 305     inline void copy_coeffs(const biquad_coeffs &src)
 306     {
 307         a0 = src.a0;
 308         a1 = src.a1;
 309         a2 = src.a2;
 310         b1 = src.b1;
 311         b2 = src.b2;
 312     }
 313
 314     /// Return the filter's gain at frequency freq
 315     /// @param freq   Frequency to look up
 316     /// @param sr     Filter sample rate (used to convert frequency to angular frequency)
 317     float freq_gain(float freq, float sr) const
 318     {
 319         typedef std::complex<double> cfloat;
 320         freq *= 2.0 * M_PI / sr;
 321         cfloat z = 1.0 / exp(cfloat(0.0, freq));
 322
 323         return std::abs(h_z(z));
 324     }
 325
 326     /// Return H(z) the filter's gain at frequency freq
 327     /// @param z   Z variable (e^jw)
 328     cfloat h_z(const cfloat &z) const
 329     {
 330
 331         return (cfloat(a0) + double(a1) * z + double(a2) * z*z) / (cfloat(1.0) + double(b1) * z + double(b2) * z*z);
 332     }
 333
 334 };
 335
 336 /**
 337  * Two-pole two-zero filter, for floating point values.
 338  * Uses "traditional" Direct I form (separate FIR and IIR halves).
 339  * don't use this for integers because it won't work
 340  */
 341 struct biquad_d1: public biquad_coeffs
 342 {
 343     /// input[n-1]
 344     double x1;
 345     /// input[n-2]
 346     double x2;
 347     /// output[n-1]
 348     double y1;
 349     /// output[n-2]
 350     double y2;
 351     /// Constructor (initializes state to all zeros)
 352     biquad_d1()
 353     {
 354         reset();
 355     }
 356     /// direct I form with four state variables
 357     inline double process(double in)
 358     {
 359         double out = in * a0 + x1 * a1 + x2 * a2 - y1 * b1 - y2 * b2;
 360         x2 = x1;
 361         y2 = y1;
 362         x1 = in;
 363         y1 = out;
 364         return out;
 365     }
 366
 367     /// direct I form with zero input
 368     inline double process_zeroin()
 369     {
 370         double out = - y1 * b1 - y2 * b2;
 371         y2 = y1;
 372         y1 = out;
 373         return out;
 374     }
 375
 376     /// simplified version for lowpass case with two zeros at -1
 377     inline double process_lp(double in)
 378     {
 379         double out = a0*(in + x1 + x1 + x2) - y1 * b1 - y2 * b2;
 380         x2 = x1;
 381         y2 = y1;
 382         x1 = in;
 383         y1 = out;
 384         return out;
 385     }
 386     /// simplified version for highpass case with two zeros at 1
 387     inline double process_hp(double in)
 388     {
 389         double out = a0*(in - x1 - x1 + x2) - y1 * b1 - y2 * b2;
 390         x2 = x1;
 391         y2 = y1;
 392         x1 = in;
 393         y1 = out;
 394         return out;
 395     }
 396     /// Sanitize (set to 0 if potentially denormal) filter state
 397     inline void sanitize()
 398     {
 399         dsp::sanitize(x1);
 400         dsp::sanitize(y1);
 401         dsp::sanitize(x2);
 402         dsp::sanitize(y2);
 403     }
 404     /// Reset state variables
 405     inline void reset()
 406     {
 407         dsp::zero(x1);
 408         dsp::zero(y1);
 409         dsp::zero(x2);
 410         dsp::zero(y2);
 411     }
 412     inline bool empty() const {
 413         return (y1 == 0. && y2 == 0.);
 414     }
 415
 416 };
 417
 418 /**
 419  * Two-pole two-zero filter, for floating point values.
 420  * Uses slightly faster Direct II form (combined FIR and IIR halves).
 421  * However, when used with wildly varying coefficients, it may
 422  * make more zipper noise than Direct I form, so it's better to
 423  * use it when filter coefficients are not changed mid-stream.
 424  */
 425 struct biquad_d2: public biquad_coeffs
 426 {
 427     /// state[n-1]
 428     double w1;
 429     /// state[n-2]
 430     double w2;
 431     /// Constructor (initializes state to all zeros)
 432     biquad_d2()
 433     {
 434         reset();
 435     }
 436     /// direct II form with two state variables
 437     inline double process(double in)
 438     {
 439         double n = in;
 440         dsp::sanitize_denormal(n);
 441         dsp::sanitize(n);
 442         dsp::sanitize(w1);
 443         dsp::sanitize(w2);
 444
 445         double tmp = n - w1 * b1 - w2 * b2;
 446         double out = tmp * a0 + w1 * a1 + w2 * a2;
 447         w2 = w1;
 448         w1 = tmp;
 449         return out;
 450     }
 451
 452     // direct II form with two state variables, lowpass version
 453     // interesting fact: this is actually slower than the general version!
 454     inline double process_lp(double in)
 455     {
 456         double tmp = in - w1 * b1 - w2 * b2;
 457         double out = (tmp  + w2 + w1* 2) * a0;
 458         w2 = w1;
 459         w1 = tmp;
 460         return out;
 461     }
 462
 463     /// Is the filter state completely silent? (i.e. set to 0 by sanitize function)
 464     inline bool empty() const {
 465         return (w1 == 0.f && w2 == 0.f);
 466     }
 467
 468
 469     /// Sanitize (set to 0 if potentially denormal) filter state
 470     inline void sanitize()
 471     {
 472         dsp::sanitize(w1);
 473         dsp::sanitize(w2);
 474     }
 475
 476     /// Reset state variables
 477     inline void reset()
 478     {
 479         dsp::zero(w1);
 480         dsp::zero(w2);
 481     }
 482 };
 483
 484 /**
 485  * Two-pole two-zero filter, for floating point values.
 486  * Uses "traditional" Direct I form (separate FIR and IIR halves).
 487  * don't use this for integers because it won't work
 488  */
 489 struct biquad_d1_lerp: public biquad_coeffs
 490 {
 491     double a0cur, a1cur, a2cur, b1cur, b2cur;
 492     double a0delta, a1delta, a2delta, b1delta, b2delta;
 493     /// input[n-1]
 494     double  x1;
 495     /// input[n-2]
 496     double  x2;
 497     /// output[n-1]
 498     double  y1;
 499     /// output[n-2]
 500     double  y2;
 501     /// Constructor (initializes state to all zeros)
 502     biquad_d1_lerp()
 503     {
 504         reset();
 505     }
 506     #define _DO_COEFF(coeff) coeff##delta = (coeff - coeff##cur) * (frac)
 507     void big_step(double frac)
 508     {
 509         _DO_COEFF(a0);
 510         _DO_COEFF(a1);
 511         _DO_COEFF(a2);
 512         _DO_COEFF(b1);
 513         _DO_COEFF(b2);
 514     }
 515     #undef _DO_COEFF
 516     /// direct I form with four state variables
 517     inline double process(double in)
 518     {
 519         double out = in * a0cur + x1 * a1cur + x2 * a2cur - y1 * b1cur - y2 * b2cur;
 520         x2 = x1;
 521         y2 = y1;
 522         x1 = in;
 523         y1 = out;
 524         a0cur += a0delta;
 525         a1cur += a1delta;
 526         a2cur += a2delta;
 527         b1cur += b1delta;
 528         b2cur += b2delta;
 529         return out;
 530     }
 531
 532     /// direct I form with zero input
 533     inline double process_zeroin()
 534     {
 535         double  out = - y1 * b1 - y2 * b2;
 536         y2 = y1;
 537         y1 = out;
 538         b1cur += b1delta;
 539         b2cur += b2delta;
 540         return out;
 541     }
 542
 543     /// simplified version for lowpass case with two zeros at -1
 544     inline double process_lp(double in)
 545     {
 546         double out = a0*(in + x1 + x1 + x2) - y1 * b1 - y2 * b2;
 547         x2 = x1;
 548         y2 = y1;
 549         x1 = in;
 550         y1 = out;
 551         return out;
 552     }
 553     /// Sanitize (set to 0 if potentially denormal) filter state
 554     inline void sanitize()
 555     {
 556         dsp::sanitize(x1);
 557         dsp::sanitize(y1);
 558         dsp::sanitize(x2);
 559         dsp::sanitize(y2);
 560         dsp::sanitize(a0cur);
 561         dsp::sanitize(a1cur);
 562         dsp::sanitize(a2cur);
 563         dsp::sanitize(b1cur);
 564         dsp::sanitize(b2cur);
 565     }
 566     /// Reset state variables
 567     inline void reset()
 568     {
 569         dsp::zero(x1);
 570         dsp::zero(y1);
 571         dsp::zero(x2);
 572         dsp::zero(y2);
 573         dsp::zero(a0cur);
 574         dsp::zero(a1cur);
 575         dsp::zero(a2cur);
 576         dsp::zero(b1cur);
 577         dsp::zero(b2cur);
 578     }
 579     inline bool empty() {
 580         return (y1 == 0. && y2 == 0.);
 581     }
 582
 583 };
 584
 585 /// Compose two filters in series
 586 template<class F1, class F2>
 587 class filter_compose {
 588 public:
 589     typedef std::complex<float> cfloat;
 590     F1 f1;
 591     F2 f2;
 592 public:
 593     float process(float value) {
 594         return f2.process(f1.process(value));
 595     }
 596
 597     inline cfloat h_z(const cfloat &z) const {
 598         return f1.h_z(z) * f2.h_z(z);
 599     }
 600
 601     /// Return the filter's gain at frequency freq
 602     /// @param freq   Frequency to look up
 603     /// @param sr     Filter sample rate (used to convert frequency to angular frequency)
 604     float freq_gain(float freq, float sr) const
 605     {
 606         typedef std::complex<double> cfloat;
 607         freq *= 2.0 * M_PI / sr;
 608         cfloat z = 1.0 / exp(cfloat(0.0, freq));
 609
 610         return std::abs(h_z(z));
 611     }
 612
 613     void sanitize() {
 614         f1.sanitize();
 615         f2.sanitize();
 616     }
 617 };
 618
 619 /// Compose two filters in parallel
 620 template<class F1, class F2>
 621 class filter_sum {
 622 public:
 623     typedef std::complex<double> cfloat;
 624     F1 f1;
 625     F2 f2;
 626 public:
 627     double process(double value) {
 628         return f2.process(value) + f1.process(value);
 629     }
 630
 631     inline cfloat h_z(const cfloat &z) const {
 632         return f1.h_z(z) + f2.h_z(z);
 633     }
 634
 635     /// Return the filter's gain at frequency freq
 636     /// @param freq   Frequency to look up
 637     /// @param sr     Filter sample rate (used to convert frequency to angular frequency)
 638     float freq_gain(float freq, float sr) const
 639     {
 640         typedef std::complex<double> cfloat;
 641         freq *= 2.0 * M_PI / sr;
 642         cfloat z = 1.0 / exp(cfloat(0.0, freq));
 643
 644         return std::abs(h_z(z));
 645     }
 646
 647     void sanitize() {
 648         f1.sanitize();
 649         f2.sanitize();
 650     }
 651 };
 652
 653 };
 654
 655 #endif