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/dB.h"
  23 #include "ardour/dsp_filter.h"
  24
  25 #ifndef M_PI
  26 #define M_PI 3.14159265358979323846
  27 #endif
  28
  29 using namespace ARDOUR::DSP;
  30
  31 void
  32 ARDOUR::DSP::memset (float *data, const float val, const uint32_t n_samples) {
  33         for (uint32_t i = 0; i < n_samples; ++i) {
  34                 data[i] = val;
  35         }
  36 }
  37
  38 void
  39 ARDOUR::DSP::mmult (float *data, float *mult, const uint32_t n_samples) {
  40         for (uint32_t i = 0; i < n_samples; ++i) {
  41                 data[i] *= mult[i];
  42         }
  43 }
  44
  45 float
  46 ARDOUR::DSP::log_meter (float power) {
  47         // compare to gtk2_ardour/logmeter.h
  48         static const float lower_db = -192.f;
  49         static const float upper_db = 0.f;
  50         static const float non_linearity = 8.0;
  51         return (power < lower_db ? 0.0 : powf ((power - lower_db) / (upper_db - lower_db), non_linearity));
  52 }
  53
  54 float
  55 ARDOUR::DSP::log_meter_coeff (float coeff) {
  56         if (coeff <= 0) return 0;
  57         return log_meter (fast_coefficient_to_dB (coeff));
  58 }
  59
  60 void
  61 ARDOUR::DSP::peaks (float *data, float &min, float &max, uint32_t n_samples) {
  62         for (uint32_t i = 0; i < n_samples; ++i) {
  63                 if (data[i] < min) min = data[i];
  64                 if (data[i] > max) max = data[i];
  65         }
  66 }
  67
  68 LowPass::LowPass (double samplerate, float freq)
  69         : _rate (samplerate)
  70         , _z (0)
  71 {
  72         set_cutoff (freq);
  73 }
  74
  75 void
  76 LowPass::set_cutoff (float freq)
  77 {
  78         _a = 1.f - expf (-2.f * M_PI * freq / _rate);
  79 }
  80
  81 void
  82 LowPass::proc (float *data, const uint32_t n_samples)
  83 {
  84         // localize variables
  85         const float a = _a;
  86         float z = _z;
  87         for (uint32_t i = 0; i < n_samples; ++i) {
  88                 data[i] += a * (data[i] - z);
  89                 z = data[i];
  90         }
  91         _z = z;
  92 }
  93
  94 void
  95 LowPass::ctrl (float *data, const float val, const uint32_t n_samples)
  96 {
  97         // localize variables
  98         const float a = _a;
  99         float z = _z;
 100         for (uint32_t i = 0; i < n_samples; ++i) {
 101                 data[i] += a * (val - z);
 102                 z = data[i];
 103         }
 104         _z = z;
 105 }
 106
 107 ///////////////////////////////////////////////////////////////////////////////
 108
 109 BiQuad::BiQuad (double samplerate)
 110         : _rate (samplerate)
 111         , _z1 (0.0)
 112         , _z2 (0.0)
 113         , _a1 (0.0)
 114         , _a2 (0.0)
 115         , _b0 (1.0)
 116         , _b1 (0.0)
 117         , _b2 (0.0)
 118 {
 119 }
 120
 121 BiQuad::BiQuad (const BiQuad &other)
 122         : _rate (other._rate)
 123         , _z1 (0.0)
 124         , _z2 (0.0)
 125         , _a1 (other._a1)
 126         , _a2 (other._a2)
 127         , _b0 (other._b0)
 128         , _b1 (other._b1)
 129         , _b2 (other._b2)
 130 {
 131 }
 132
 133 void
 134 BiQuad::run (float *data, const uint32_t n_samples)
 135 {
 136         for (uint32_t i = 0; i < n_samples; ++i) {
 137                 const float xn = data[i];
 138                 const float z = _b0 * xn + _z1;
 139                 _z1           = _b1 * xn - _a1 * z + _z2;
 140                 _z2           = _b2 * xn - _a2 * z;
 141                 data[i] = z;
 142         }
 143 }
 144
 145 void
 146 BiQuad::compute (Type type, double freq, double Q, double gain)
 147 {
 148         /* Compute biquad filter settings.
 149          * Based on 'Cookbook formulae for audio EQ biquad filter coefficents'
 150          * by Robert Bristow-Johnson
 151          */
 152         const double     A = pow (10.0, (gain / 40.0));
 153         const double W0 = (2.0 * M_PI * freq) / _rate;
 154         const double sinW0  = sin (W0);
 155         const double cosW0  = cos (W0);
 156         const double alpha = sinW0 / (2.0 * Q);
 157         const double beta  = sqrt (A) / Q;
 158
 159         double _a0;
 160
 161         switch (type) {
 162                 case LowPass:
 163                         _b0 = (1.0 - cosW0) / 2.0;
 164                         _b1 =  1.0 - cosW0;
 165                         _b2 = (1.0 - cosW0) / 2.0;
 166                         _a0 =  1.0 + alpha;
 167                         _a1 = -2.0 * cosW0;
 168                         _a2 =  1.0 - alpha;
 169                         break;
 170
 171                 case HighPass:
 172                         _b0 =  (1.0 + cosW0) / 2.0;
 173                         _b1 = -(1.0 + cosW0);
 174                         _b2 =  (1.0 + cosW0) / 2.0;
 175                         _a0 =   1.0 + alpha;
 176                         _a1 =  -2.0 * cosW0;
 177                         _a2 =   1.0 - alpha;
 178                         break;
 179
 180                 case BandPassSkirt: /* Constant skirt gain, peak gain = Q */
 181                         _b0 =  sinW0 / 2.0;
 182                         _b1 =  0.0;
 183                         _b2 = -sinW0 / 2.0;
 184                         _a0 =  1.0 + alpha;
 185                         _a1 = -2.0 * cosW0;
 186                         _a2 =  1.0 - alpha;
 187                         break;
 188
 189                 case BandPass0dB: /* Constant 0 dB peak gain */
 190                         _b0 =  alpha;
 191                         _b1 =  0.0;
 192                         _b2 = -alpha;
 193                         _a0 =  1.0 + alpha;
 194                         _a1 = -2.0 * cosW0;
 195                         _a2 =  1.0 - alpha;
 196                         break;
 197
 198                 case Notch:
 199                         _b0 =  1.0;
 200                         _b1 = -2.0 * cosW0;
 201                         _b2 =  1.0;
 202                         _a0 =  1.0 + alpha;
 203                         _a1 = -2.0 * cosW0;
 204                         _a2 =  1.0 - alpha;
 205                         break;
 206
 207                 case AllPass:
 208                         _b0 =  1.0 - alpha;
 209                         _b1 = -2.0 * cosW0;
 210                         _b2 =  1.0 + alpha;
 211                         _a0 =  1.0 + alpha;
 212                         _a1 = -2.0 * cosW0;
 213                         _a2 =  1.0 - alpha;
 214                         break;
 215
 216                 case Peaking:
 217                         _b0 =  1.0 + (alpha * A);
 218                         _b1 = -2.0 * cosW0;
 219                         _b2 =  1.0 - (alpha * A);
 220                         _a0 =  1.0 + (alpha / A);
 221                         _a1 = -2.0 * cosW0;
 222                         _a2 =  1.0 - (alpha / A);
 223                         break;
 224
 225                 case LowShelf:
 226                         _b0 =         A * ((A + 1) - ((A - 1) * cosW0) + (beta * sinW0));
 227                         _b1 = (2.0 * A) * ((A - 1) - ((A + 1) * cosW0));
 228                         _b2 =         A * ((A + 1) - ((A - 1) * cosW0) - (beta * sinW0));
 229                         _a0 =              (A + 1) + ((A - 1) * cosW0) + (beta * sinW0);
 230                         _a1 =      -2.0 * ((A - 1) + ((A + 1) * cosW0));
 231                         _a2 =              (A + 1) + ((A - 1) * cosW0) - (beta * sinW0);
 232                         break;
 233
 234                 case HighShelf:
 235                         _b0 =          A * ((A + 1) + ((A - 1) * cosW0) + (beta * sinW0));
 236                         _b1 = -(2.0 * A) * ((A - 1) + ((A + 1) * cosW0));
 237                         _b2 =          A * ((A + 1) + ((A - 1) * cosW0) - (beta * sinW0));
 238                         _a0 =               (A + 1) - ((A - 1) * cosW0) + (beta * sinW0);
 239                         _a1 =        2.0 * ((A - 1) - ((A + 1) * cosW0));
 240                         _a2 =               (A + 1) - ((A - 1) * cosW0) - (beta * sinW0);
 241                         break;
 242                 default:
 243                         abort(); /*NOTREACHED*/
 244                         break;
 245         }
 246
 247         _b0 /= _a0;
 248         _b1 /= _a0;
 249         _b2 /= _a0;
 250         _a1 /= _a0;
 251         _a2 /= _a0;
 252 }