Alc/filters/filter.cpp

   1
   2 #include "config.h"
   3
   4 #include <cmath>
   5
   6 #include "AL/alc.h"
   7 #include "AL/al.h"
   8
   9 #include "alMain.h"
  10 #include "defs.h"
  11
  12
  13 void BiquadFilter_setParams(BiquadFilter *filter, BiquadType type, ALfloat gain, ALfloat f0norm, ALfloat rcpQ)
  14 {
  15     ALfloat alpha, sqrtgain_alpha_2;
  16     ALfloat w0, sin_w0, cos_w0;
  17     ALfloat a[3] = { 1.0f, 0.0f, 0.0f };
  18     ALfloat b[3] = { 1.0f, 0.0f, 0.0f };
  19
  20     // Limit gain to -100dB
  21     assert(gain > 0.00001f);
  22
  23     w0 = F_TAU * f0norm;
  24     sin_w0 = std::sin(w0);
  25     cos_w0 = std::cos(w0);
  26     alpha = sin_w0/2.0f * rcpQ;
  27
  28     /* Calculate filter coefficients depending on filter type */
  29     switch(type)
  30     {
  31         case BiquadType::HighShelf:
  32             sqrtgain_alpha_2 = 2.0f * std::sqrt(gain) * alpha;
  33             b[0] =       gain*((gain+1.0f) + (gain-1.0f)*cos_w0 + sqrtgain_alpha_2);
  34             b[1] = -2.0f*gain*((gain-1.0f) + (gain+1.0f)*cos_w0                   );
  35             b[2] =       gain*((gain+1.0f) + (gain-1.0f)*cos_w0 - sqrtgain_alpha_2);
  36             a[0] =             (gain+1.0f) - (gain-1.0f)*cos_w0 + sqrtgain_alpha_2;
  37             a[1] =  2.0f*     ((gain-1.0f) - (gain+1.0f)*cos_w0                   );
  38             a[2] =             (gain+1.0f) - (gain-1.0f)*cos_w0 - sqrtgain_alpha_2;
  39             break;
  40         case BiquadType::LowShelf:
  41             sqrtgain_alpha_2 = 2.0f * std::sqrt(gain) * alpha;
  42             b[0] =       gain*((gain+1.0f) - (gain-1.0f)*cos_w0 + sqrtgain_alpha_2);
  43             b[1] =  2.0f*gain*((gain-1.0f) - (gain+1.0f)*cos_w0                   );
  44             b[2] =       gain*((gain+1.0f) - (gain-1.0f)*cos_w0 - sqrtgain_alpha_2);
  45             a[0] =             (gain+1.0f) + (gain-1.0f)*cos_w0 + sqrtgain_alpha_2;
  46             a[1] = -2.0f*     ((gain-1.0f) + (gain+1.0f)*cos_w0                   );
  47             a[2] =             (gain+1.0f) + (gain-1.0f)*cos_w0 - sqrtgain_alpha_2;
  48             break;
  49         case BiquadType::Peaking:
  50             gain = std::sqrt(gain);
  51             b[0] =  1.0f + alpha * gain;
  52             b[1] = -2.0f * cos_w0;
  53             b[2] =  1.0f - alpha * gain;
  54             a[0] =  1.0f + alpha / gain;
  55             a[1] = -2.0f * cos_w0;
  56             a[2] =  1.0f - alpha / gain;
  57             break;
  58
  59         case BiquadType::LowPass:
  60             b[0] = (1.0f - cos_w0) / 2.0f;
  61             b[1] =  1.0f - cos_w0;
  62             b[2] = (1.0f - cos_w0) / 2.0f;
  63             a[0] =  1.0f + alpha;
  64             a[1] = -2.0f * cos_w0;
  65             a[2] =  1.0f - alpha;
  66             break;
  67         case BiquadType::HighPass:
  68             b[0] =  (1.0f + cos_w0) / 2.0f;
  69             b[1] = -(1.0f + cos_w0);
  70             b[2] =  (1.0f + cos_w0) / 2.0f;
  71             a[0] =   1.0f + alpha;
  72             a[1] =  -2.0f * cos_w0;
  73             a[2] =   1.0f - alpha;
  74             break;
  75         case BiquadType::BandPass:
  76             b[0] =  alpha;
  77             b[1] =  0;
  78             b[2] = -alpha;
  79             a[0] =  1.0f + alpha;
  80             a[1] = -2.0f * cos_w0;
  81             a[2] =  1.0f - alpha;
  82             break;
  83     }
  84
  85     filter->a1 = a[1] / a[0];
  86     filter->a2 = a[2] / a[0];
  87     filter->b0 = b[0] / a[0];
  88     filter->b1 = b[1] / a[0];
  89     filter->b2 = b[2] / a[0];
  90 }
  91
  92
  93 void BiquadFilter_processC(BiquadFilter *filter, ALfloat *RESTRICT dst, const ALfloat *RESTRICT src, ALsizei numsamples)
  94 {
  95     const ALfloat a1 = filter->a1;
  96     const ALfloat a2 = filter->a2;
  97     const ALfloat b0 = filter->b0;
  98     const ALfloat b1 = filter->b1;
  99     const ALfloat b2 = filter->b2;
 100     ALfloat z1 = filter->z1;
 101     ALfloat z2 = filter->z2;
 102     ALsizei i;
 103
 104     ASSUME(numsamples > 0);
 105
 106     /* Processing loop is Transposed Direct Form II. This requires less storage
 107      * compared to Direct Form I (only two delay components, instead of a four-
 108      * sample history; the last two inputs and outputs), and works better for
 109      * floating-point which favors summing similarly-sized values while being
 110      * less bothered by overflow.
 111      *
 112      * See: http://www.earlevel.com/main/2003/02/28/biquads/
 113      */
 114     for(i = 0;i < numsamples;i++)
 115     {
 116         ALfloat input = src[i];
 117         ALfloat output = input*b0 + z1;
 118         z1 = input*b1 - output*a1 + z2;
 119         z2 = input*b2 - output*a2;
 120         dst[i] = output;
 121     }
 122
 123     filter->z1 = z1;
 124     filter->z2 = z2;
 125 }