libs/ardour/dsp_filter.cc

   1 /*
   2  * Copyright (C) 2016 Robin Gareus <robin@gareus.org>
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software
  16  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  17  *
  18  */
  19
  20 #include <stdlib.h>
  21 #include <math.h>
  22 #include "ardour/dsp_filter.h"
  23
  24 #ifndef M_PI
  25 #define M_PI 3.14159265358979323846
  26 #endif
  27
  28 using namespace ARDOUR::DSP;
  29
  30 void
  31 ARDOUR::DSP::memset (float *data, const float val, const uint32_t n_samples) {
  32         for (uint32_t i = 0; i < n_samples; ++i) {
  33                 data[i] = val;
  34         }
  35 }
  36
  37 void
  38 ARDOUR::DSP::mmult (float *data, float *mult, const uint32_t n_samples) {
  39         for (uint32_t i = 0; i < n_samples; ++i) {
  40                 data[i] *= mult[i];
  41         }
  42 }
  43
  44
  45 LowPass::LowPass (double samplerate, float freq)
  46         : _rate (samplerate)
  47         , _z (0)
  48 {
  49         set_cutoff (freq);
  50 }
  51
  52 void
  53 LowPass::set_cutoff (float freq)
  54 {
  55         _a = 1.f - expf (-2.f * M_PI * freq / _rate);
  56 }
  57
  58 void
  59 LowPass::proc (float *data, const uint32_t n_samples)
  60 {
  61         // localize variables
  62         const float a = _a;
  63         float z = _z;
  64         for (uint32_t i = 0; i < n_samples; ++i) {
  65                 data[i] += a * (data[i] - z);
  66                 z = data[i];
  67         }
  68         _z = z;
  69 }
  70
  71 void
  72 LowPass::ctrl (float *data, const float val, const uint32_t n_samples)
  73 {
  74         // localize variables
  75         const float a = _a;
  76         float z = _z;
  77         for (uint32_t i = 0; i < n_samples; ++i) {
  78                 data[i] += a * (val - z);
  79                 z = data[i];
  80         }
  81         _z = z;
  82 }
  83
  84 ///////////////////////////////////////////////////////////////////////////////
  85
  86 BiQuad::BiQuad (double samplerate)
  87         : _rate (samplerate)
  88         , _z1 (0.0)
  89         , _z2 (0.0)
  90         , _a1 (0.0)
  91         , _a2 (0.0)
  92         , _b0 (1.0)
  93         , _b1 (0.0)
  94         , _b2 (0.0)
  95 {
  96 }
  97
  98 BiQuad::BiQuad (const BiQuad &other)
  99         : _rate (other._rate)
 100         , _z1 (0.0)
 101         , _z2 (0.0)
 102         , _a1 (other._a1)
 103         , _a2 (other._a2)
 104         , _b0 (other._b0)
 105         , _b1 (other._b1)
 106         , _b2 (other._b2)
 107 {
 108 }
 109
 110 void
 111 BiQuad::run (float *data, const uint32_t n_samples)
 112 {
 113         for (uint32_t i = 0; i < n_samples; ++i) {
 114                 const float xn = data[i];
 115                 const float z = _b0 * xn + _z1;
 116                 _z1           = _b1 * xn - _a1 * z + _z2;
 117                 _z2           = _b2 * xn - _a2 * z;
 118                 data[i] = z;
 119         }
 120 }
 121
 122 void
 123 BiQuad::compute (Type type, double freq, double Q, double gain)
 124 {
 125         /* Compute biquad filter settings.
 126          * Based on 'Cookbook formulae for audio EQ biquad filter coefficents'
 127          * by Robert Bristow-Johnson
 128          */
 129         const double     A = pow (10.0, (gain / 40.0));
 130         const double W0 = (2.0 * M_PI * freq) / _rate;
 131         const double sinW0  = sin (W0);
 132         const double cosW0  = cos (W0);
 133         const double alpha = sinW0 / (2.0 * Q);
 134         const double beta  = sqrt (A) / Q;
 135
 136         double _a0;
 137
 138         switch (type) {
 139                 case LowPass:
 140                         _b0 = (1.0 - cosW0) / 2.0;
 141                         _b1 =  1.0 - cosW0;
 142                         _b2 = (1.0 - cosW0) / 2.0;
 143                         _a0 =  1.0 + alpha;
 144                         _a1 = -2.0 * cosW0;
 145                         _a2 =  1.0 - alpha;
 146                         break;
 147
 148                 case HighPass:
 149                         _b0 =  (1.0 + cosW0) / 2.0;
 150                         _b1 = -(1.0 + cosW0);
 151                         _b2 =  (1.0 + cosW0) / 2.0;
 152                         _a0 =   1.0 + alpha;
 153                         _a1 =  -2.0 * cosW0;
 154                         _a2 =   1.0 - alpha;
 155                         break;
 156
 157                 case BandPassSkirt: /* Constant skirt gain, peak gain = Q */
 158                         _b0 =  sinW0 / 2.0;
 159                         _b1 =  0.0;
 160                         _b2 = -sinW0 / 2.0;
 161                         _a0 =  1.0 + alpha;
 162                         _a1 = -2.0 * cosW0;
 163                         _a2 =  1.0 - alpha;
 164                         break;
 165
 166                 case BandPass0dB: /* Constant 0 dB peak gain */
 167                         _b0 =  alpha;
 168                         _b1 =  0.0;
 169                         _b2 = -alpha;
 170                         _a0 =  1.0 + alpha;
 171                         _a1 = -2.0 * cosW0;
 172                         _a2 =  1.0 - alpha;
 173                         break;
 174
 175                 case Notch:
 176                         _b0 =  1.0;
 177                         _b1 = -2.0 * cosW0;
 178                         _b2 =  1.0;
 179                         _a0 =  1.0 + alpha;
 180                         _a1 = -2.0 * cosW0;
 181                         _a2 =  1.0 - alpha;
 182                         break;
 183
 184                 case AllPass:
 185                         _b0 =  1.0 - alpha;
 186                         _b1 = -2.0 * cosW0;
 187                         _b2 =  1.0 + alpha;
 188                         _a0 =  1.0 + alpha;
 189                         _a1 = -2.0 * cosW0;
 190                         _a2 =  1.0 - alpha;
 191                         break;
 192
 193                 case Peaking:
 194                         _b0 =  1.0 + (alpha * A);
 195                         _b1 = -2.0 * cosW0;
 196                         _b2 =  1.0 - (alpha * A);
 197                         _a0 =  1.0 + (alpha / A);
 198                         _a1 = -2.0 * cosW0;
 199                         _a2 =  1.0 - (alpha / A);
 200                         break;
 201
 202                 case LowShelf:
 203                         _b0 =         A * ((A + 1) - ((A - 1) * cosW0) + (beta * sinW0));
 204                         _b1 = (2.0 * A) * ((A - 1) - ((A + 1) * cosW0));
 205                         _b2 =         A * ((A + 1) - ((A - 1) * cosW0) - (beta * sinW0));
 206                         _a0 =              (A + 1) + ((A - 1) * cosW0) + (beta * sinW0);
 207                         _a1 =      -2.0 * ((A - 1) + ((A + 1) * cosW0));
 208                         _a2 =              (A + 1) + ((A - 1) * cosW0) - (beta * sinW0);
 209                         break;
 210
 211                 case HighShelf:
 212                         _b0 =          A * ((A + 1) + ((A - 1) * cosW0) + (beta * sinW0));
 213                         _b1 = -(2.0 * A) * ((A - 1) + ((A + 1) * cosW0));
 214                         _b2 =          A * ((A + 1) + ((A - 1) * cosW0) - (beta * sinW0));
 215                         _a0 =               (A + 1) - ((A - 1) * cosW0) + (beta * sinW0);
 216                         _a1 =        2.0 * ((A - 1) - ((A + 1) * cosW0));
 217                         _a2 =               (A + 1) - ((A - 1) * cosW0) - (beta * sinW0);
 218                         break;
 219                 default:
 220                         abort(); /*NOTREACHED*/
 221                         break;
 222         }
 223
 224         _b0 /= _a0;
 225         _b1 /= _a0;
 226         _b2 /= _a0;
 227         _a1 /= _a0;
 228         _a2 /= _a0;
 229 }