core/filters/biquad.cpp

   1
   2 #include "config.h"
   3
   4 #include "biquad.h"
   5
   6 #include <algorithm>
   7 #include <cassert>
   8 #include <cmath>
   9
  10 #include "alnumbers.h"
  11 #include "opthelpers.h"
  12
  13
  14 template<typename Real>
  15 void BiquadFilterR<Real>::setParams(BiquadType type, Real f0norm, Real gain, Real rcpQ)
  16 {
  17     /* HACK: Limit gain to -100dB. This shouldn't ever happen, all callers
  18      * already clamp to minimum of 0.001, or have a limited range of values
  19      * that don't go below 0.126. But it seems to with some callers. This needs
  20      * to be investigated.
  21      */
  22     gain = std::max(gain, Real(0.00001));
  23
  24     const Real w0{al::numbers::pi_v<Real>*2.0f * f0norm};
  25     const Real sin_w0{std::sin(w0)};
  26     const Real cos_w0{std::cos(w0)};
  27     const Real alpha{sin_w0/2.0f * rcpQ};
  28
  29     Real sqrtgain_alpha_2;
  30     Real a[3]{ 1.0f, 0.0f, 0.0f };
  31     Real b[3]{ 1.0f, 0.0f, 0.0f };
  32
  33     /* Calculate filter coefficients depending on filter type */
  34     switch(type)
  35     {
  36         case BiquadType::HighShelf:
  37             sqrtgain_alpha_2 = 2.0f * std::sqrt(gain) * alpha;
  38             b[0] =       gain*((gain+1.0f) + (gain-1.0f)*cos_w0 + sqrtgain_alpha_2);
  39             b[1] = -2.0f*gain*((gain-1.0f) + (gain+1.0f)*cos_w0                   );
  40             b[2] =       gain*((gain+1.0f) + (gain-1.0f)*cos_w0 - sqrtgain_alpha_2);
  41             a[0] =             (gain+1.0f) - (gain-1.0f)*cos_w0 + sqrtgain_alpha_2;
  42             a[1] =  2.0f*     ((gain-1.0f) - (gain+1.0f)*cos_w0                   );
  43             a[2] =             (gain+1.0f) - (gain-1.0f)*cos_w0 - sqrtgain_alpha_2;
  44             break;
  45         case BiquadType::LowShelf:
  46             sqrtgain_alpha_2 = 2.0f * std::sqrt(gain) * alpha;
  47             b[0] =       gain*((gain+1.0f) - (gain-1.0f)*cos_w0 + sqrtgain_alpha_2);
  48             b[1] =  2.0f*gain*((gain-1.0f) - (gain+1.0f)*cos_w0                   );
  49             b[2] =       gain*((gain+1.0f) - (gain-1.0f)*cos_w0 - sqrtgain_alpha_2);
  50             a[0] =             (gain+1.0f) + (gain-1.0f)*cos_w0 + sqrtgain_alpha_2;
  51             a[1] = -2.0f*     ((gain-1.0f) + (gain+1.0f)*cos_w0                   );
  52             a[2] =             (gain+1.0f) + (gain-1.0f)*cos_w0 - sqrtgain_alpha_2;
  53             break;
  54         case BiquadType::Peaking:
  55             b[0] =  1.0f + alpha * gain;
  56             b[1] = -2.0f * cos_w0;
  57             b[2] =  1.0f - alpha * gain;
  58             a[0] =  1.0f + alpha / gain;
  59             a[1] = -2.0f * cos_w0;
  60             a[2] =  1.0f - alpha / gain;
  61             break;
  62
  63         case BiquadType::LowPass:
  64             b[0] = (1.0f - cos_w0) / 2.0f;
  65             b[1] =  1.0f - cos_w0;
  66             b[2] = (1.0f - cos_w0) / 2.0f;
  67             a[0] =  1.0f + alpha;
  68             a[1] = -2.0f * cos_w0;
  69             a[2] =  1.0f - alpha;
  70             break;
  71         case BiquadType::HighPass:
  72             b[0] =  (1.0f + cos_w0) / 2.0f;
  73             b[1] = -(1.0f + cos_w0);
  74             b[2] =  (1.0f + cos_w0) / 2.0f;
  75             a[0] =   1.0f + alpha;
  76             a[1] =  -2.0f * cos_w0;
  77             a[2] =   1.0f - alpha;
  78             break;
  79         case BiquadType::BandPass:
  80             b[0] =  alpha;
  81             b[1] =  0.0f;
  82             b[2] = -alpha;
  83             a[0] =  1.0f + alpha;
  84             a[1] = -2.0f * cos_w0;
  85             a[2] =  1.0f - alpha;
  86             break;
  87     }
  88
  89     mA1 = a[1] / a[0];
  90     mA2 = a[2] / a[0];
  91     mB0 = b[0] / a[0];
  92     mB1 = b[1] / a[0];
  93     mB2 = b[2] / a[0];
  94 }
  95
  96 template<typename Real>
  97 void BiquadFilterR<Real>::process(const al::span<const Real> src, Real *dst)
  98 {
  99     const Real b0{mB0};
 100     const Real b1{mB1};
 101     const Real b2{mB2};
 102     const Real a1{mA1};
 103     const Real a2{mA2};
 104     Real z1{mZ1};
 105     Real z2{mZ2};
 106
 107     /* Processing loop is Transposed Direct Form II. This requires less storage
 108      * compared to Direct Form I (only two delay components, instead of a four-
 109      * sample history; the last two inputs and outputs), and works better for
 110      * floating-point which favors summing similarly-sized values while being
 111      * less bothered by overflow.
 112      *
 113      * See: http://www.earlevel.com/main/2003/02/28/biquads/
 114      */
 115     auto proc_sample = [b0,b1,b2,a1,a2,&z1,&z2](Real input) noexcept -> Real
 116     {
 117         const Real output{input*b0 + z1};
 118         z1 = input*b1 - output*a1 + z2;
 119         z2 = input*b2 - output*a2;
 120         return output;
 121     };
 122     std::transform(src.cbegin(), src.cend(), dst, proc_sample);
 123
 124     mZ1 = z1;
 125     mZ2 = z2;
 126 }
 127
 128 template<typename Real>
 129 void BiquadFilterR<Real>::dualProcess(BiquadFilterR &other, const al::span<const Real> src,
 130     Real *dst)
 131 {
 132     const Real b00{mB0};
 133     const Real b01{mB1};
 134     const Real b02{mB2};
 135     const Real a01{mA1};
 136     const Real a02{mA2};
 137     const Real b10{other.mB0};
 138     const Real b11{other.mB1};
 139     const Real b12{other.mB2};
 140     const Real a11{other.mA1};
 141     const Real a12{other.mA2};
 142     Real z01{mZ1};
 143     Real z02{mZ2};
 144     Real z11{other.mZ1};
 145     Real z12{other.mZ2};
 146
 147     auto proc_sample = [b00,b01,b02,a01,a02,b10,b11,b12,a11,a12,&z01,&z02,&z11,&z12](Real input) noexcept -> Real
 148     {
 149         const Real tmpout{input*b00 + z01};
 150         z01 = input*b01 - tmpout*a01 + z02;
 151         z02 = input*b02 - tmpout*a02;
 152         input = tmpout;
 153
 154         const Real output{input*b10 + z11};
 155         z11 = input*b11 - output*a11 + z12;
 156         z12 = input*b12 - output*a12;
 157         return output;
 158     };
 159     std::transform(src.cbegin(), src.cend(), dst, proc_sample);
 160
 161     mZ1 = z01;
 162     mZ2 = z02;
 163     other.mZ1 = z11;
 164     other.mZ2 = z12;
 165 }
 166
 167 template class BiquadFilterR<float>;
 168 template class BiquadFilterR<double>;