git/src/biquads.c

/* libSoX Biquad filter effects   (c) 2006-8 robs@users.sourceforge.net
 *
 * This library is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * This library is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
 * General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *
 * 2-pole filters designed by Robert Bristow-Johnson <rbj@audioimagination.com>
 *   see https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
 *
 * 1-pole filters based on code (c) 2000 Chris Bagwell <cbagwell@sprynet.com>
 *   Algorithms: Recursive single pole low/high pass filter
 *   Reference: The Scientist and Engineer's Guide to Digital Signal Processing
 *
 *   low-pass: output[N] = input[N] * A + output[N-1] * B
 *     X = exp(-2.0 * pi * Fc)
 *     A = 1 - X
 *     B = X
 *     Fc = cutoff freq / sample rate
 *
 *     Mimics an RC low-pass filter:
 *
 *     ---/\/\/\/\----------->
 *                   |
 *                  --- C
 *                  ---
 *                   |
 *                   |
 *                   V
 *
 *   high-pass: output[N] = A0 * input[N] + A1 * input[N-1] + B1 * output[N-1]
 *     X  = exp(-2.0 * pi * Fc)
 *     A0 = (1 + X) / 2
 *     A1 = -(1 + X) / 2
 *     B1 = X
 *     Fc = cutoff freq / sample rate
 *
 *     Mimics an RC high-pass filter:
 *
 *         || C
 *     ----||--------->
 *         ||    |
 *               <
 *               > R
 *               <
 *               |
 *               V
 */


#include "biquad.h"
#include <assert.h>
#include <string.h>

typedef biquad_t priv_t;


static int hilo1_getopts(sox_effect_t * effp, int argc, char **argv) {
  return lsx_biquad_getopts(effp, argc, argv, 1, 1, 0, 1, 2, "",
      *effp->handler.name == 'l'? filter_LPF_1 : filter_HPF_1);
}


static int hilo2_getopts(sox_effect_t * effp, int argc, char **argv) {
  priv_t * p = (priv_t *)effp->priv;
  if (argc > 1 && strcmp(argv[1], "-1") == 0)
    return hilo1_getopts(effp, argc - 1, argv + 1);
  if (argc > 1 && strcmp(argv[1], "-2") == 0)
    ++argv, --argc;
  p->width = sqrt(0.5); /* Default to Butterworth */
  return lsx_biquad_getopts(effp, argc, argv, 1, 2, 0, 1, 2, "qohk",
      *effp->handler.name == 'l'? filter_LPF : filter_HPF);
}


static int bandpass_getopts(sox_effect_t * effp, int argc, char **argv) {
  filter_t type = filter_BPF;
  if (argc > 1 && strcmp(argv[1], "-c") == 0)
    ++argv, --argc, type = filter_BPF_CSG;
  return lsx_biquad_getopts(effp, argc, argv, 2, 2, 0, 1, 2, "hkqob", type);
}


static int bandrej_getopts(sox_effect_t * effp, int argc, char **argv) {
  return lsx_biquad_getopts(effp, argc, argv, 2, 2, 0, 1, 2, "hkqob", filter_notch);
}


static int allpass_getopts(sox_effect_t * effp, int argc, char **argv) {
  filter_t type = filter_APF;
  int m;
  if (argc > 1 && strcmp(argv[1], "-1") == 0)
    ++argv, --argc, type = filter_AP1;
  else if (argc > 1 && strcmp(argv[1], "-2") == 0)
    ++argv, --argc, type = filter_AP2;
  m = 1 + (type == filter_APF);
  return lsx_biquad_getopts(effp, argc, argv, m, m, 0, 1, 2, "hkqo", type);
}


static int tone_getopts(sox_effect_t * effp, int argc, char **argv) {
  priv_t * p = (priv_t *)effp->priv;
  p->width = 0.5;
  p->fc = *effp->handler.name == 'b'? 100 : 3000;
  return lsx_biquad_getopts(effp, argc, argv, 1, 3, 1, 2, 0, "shkqo",
      *effp->handler.name == 'b'?  filter_lowShelf: filter_highShelf);
}


static int equalizer_getopts(sox_effect_t * effp, int argc, char **argv) {
  return lsx_biquad_getopts(effp, argc, argv, 3, 3, 0, 1, 2, "qohk", filter_peakingEQ);
}


static int band_getopts(sox_effect_t * effp, int argc, char **argv) {
  filter_t type = filter_BPF_SPK;
  if (argc > 1 && strcmp(argv[1], "-n") == 0)
    ++argv, --argc, type = filter_BPF_SPK_N;
  return lsx_biquad_getopts(effp, argc, argv, 1, 2, 0, 1, 2, "hkqo", type);
}


static int deemph_getopts(sox_effect_t * effp, int argc, char **argv) {
  return lsx_biquad_getopts(effp, argc, argv, 0, 0, 0, 1, 2, "s", filter_deemph);
}


static int riaa_getopts(sox_effect_t * effp, int argc, char **argv) {
  priv_t * p = (priv_t *)effp->priv;
  p->filter_type = filter_riaa;
  (void)argv;
  return --argc? lsx_usage(effp) : SOX_SUCCESS;
}


static void make_poly_from_roots(
    double const * roots, size_t num_roots, double * poly)
{
  size_t i, j;
  poly[0] = 1;
  poly[1] = -roots[0];
  memset(poly + 2, 0, (num_roots + 1 - 2) * sizeof(*poly));
  for (i = 1; i < num_roots; ++i)
    for (j = num_roots; j > 0; --j)
      poly[j] -= poly[j - 1] * roots[i];
}

static int start(sox_effect_t * effp)
{
  priv_t * p = (priv_t *)effp->priv;
  double w0, A, alpha, mult;

  if (p->filter_type == filter_deemph) { /* See deemph.plt for documentation */
    if (effp->in_signal.rate == 44100) {
      p->fc    = 5283;
      p->width = 0.4845;
      p->gain  = -9.477;
    }
    else if (effp->in_signal.rate == 48000) {
      p->fc    = 5356;
      p->width = 0.479;
      p->gain  = -9.62;
    }
    else {
      lsx_fail("sample rate must be 44100 (audio-CD) or 48000 (DAT)");
      return SOX_EOF;
    }
  }

  w0 = 2 * M_PI * p->fc / effp->in_signal.rate;
  A  = exp(p->gain / 40 * log(10.));
  alpha = 0, mult = dB_to_linear(max(p->gain, 0));

  if (w0 > M_PI) {
    lsx_fail("frequency must be less than half the sample-rate (Nyquist rate)");
    return SOX_EOF;
  }

  /* Set defaults: */
  p->b0 = p->b1 = p->b2 = p->a1 = p->a2 = 0;
  p->a0 = 1;

  if (p->width) switch (p->width_type) {
    case width_slope:
      alpha = sin(w0)/2 * sqrt((A + 1/A)*(1/p->width - 1) + 2);
      break;

    case width_Q:
      alpha = sin(w0)/(2*p->width);
      break;

    case width_bw_oct:
      alpha = sin(w0)*sinh(log(2.)/2 * p->width * w0/sin(w0));
      break;

    case width_bw_Hz:
      alpha = sin(w0)/(2*p->fc/p->width);
      break;

    case width_bw_kHz: assert(0); /* Shouldn't get here */

    case width_bw_old:
      alpha = tan(M_PI * p->width / effp->in_signal.rate);
      break;
  }
  switch (p->filter_type) {
    case filter_LPF: /* H(s) = 1 / (s^2 + s/Q + 1) */
      p->b0 =  (1 - cos(w0))/2;
      p->b1 =   1 - cos(w0);
      p->b2 =  (1 - cos(w0))/2;
      p->a0 =   1 + alpha;
      p->a1 =  -2*cos(w0);
      p->a2 =   1 - alpha;
      break;

    case filter_HPF: /* H(s) = s^2 / (s^2 + s/Q + 1) */
      p->b0 =  (1 + cos(w0))/2;
      p->b1 = -(1 + cos(w0));
      p->b2 =  (1 + cos(w0))/2;
      p->a0 =   1 + alpha;
      p->a1 =  -2*cos(w0);
      p->a2 =   1 - alpha;
      break;

    case filter_BPF_CSG: /* H(s) = s / (s^2 + s/Q + 1)  (constant skirt gain, peak gain = Q) */
      p->b0 =   sin(w0)/2;
      p->b1 =   0;
      p->b2 =  -sin(w0)/2;
      p->a0 =   1 + alpha;
      p->a1 =  -2*cos(w0);
      p->a2 =   1 - alpha;
      break;

    case filter_BPF: /* H(s) = (s/Q) / (s^2 + s/Q + 1)      (constant 0 dB peak gain) */
      p->b0 =   alpha;
      p->b1 =   0;
      p->b2 =  -alpha;
      p->a0 =   1 + alpha;
      p->a1 =  -2*cos(w0);
      p->a2 =   1 - alpha;
      break;

    case filter_notch: /* H(s) = (s^2 + 1) / (s^2 + s/Q + 1) */
      p->b0 =   1;
      p->b1 =  -2*cos(w0);
      p->b2 =   1;
      p->a0 =   1 + alpha;
      p->a1 =  -2*cos(w0);
      p->a2 =   1 - alpha;
      break;

    case filter_APF: /* H(s) = (s^2 - s/Q + 1) / (s^2 + s/Q + 1) */
      p->b0 =   1 - alpha;
      p->b1 =  -2*cos(w0);
      p->b2 =   1 + alpha;
      p->a0 =   1 + alpha;
      p->a1 =  -2*cos(w0);
      p->a2 =   1 - alpha;
      break;

    case filter_peakingEQ: /* H(s) = (s^2 + s*(A/Q) + 1) / (s^2 + s/(A*Q) + 1) */
      if (A == 1)
        return SOX_EFF_NULL;
      p->b0 =   1 + alpha*A;
      p->b1 =  -2*cos(w0);
      p->b2 =   1 - alpha*A;
      p->a0 =   1 + alpha/A;
      p->a1 =  -2*cos(w0);
      p->a2 =   1 - alpha/A;
      break;

    case filter_lowShelf: /* H(s) = A * (s^2 + (sqrt(A)/Q)*s + A)/(A*s^2 + (sqrt(A)/Q)*s + 1) */
      if (A == 1)
        return SOX_EFF_NULL;
      p->b0 =    A*( (A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha );
      p->b1 =  2*A*( (A-1) - (A+1)*cos(w0)                   );
      p->b2 =    A*( (A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha );
      p->a0 =        (A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha;
      p->a1 =   -2*( (A-1) + (A+1)*cos(w0)                   );
      p->a2 =        (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha;
      break;

    case filter_deemph: /* Falls through to high-shelf... */

    case filter_highShelf: /* H(s) = A * (A*s^2 + (sqrt(A)/Q)*s + 1)/(s^2 + (sqrt(A)/Q)*s + A) */
      if (!A)
        return SOX_EFF_NULL;
      p->b0 =    A*( (A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha );
      p->b1 = -2*A*( (A-1) + (A+1)*cos(w0)                   );
      p->b2 =    A*( (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha );
      p->a0 =        (A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha;
      p->a1 =    2*( (A-1) - (A+1)*cos(w0)                   );
      p->a2 =        (A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha;
      break;

    case filter_LPF_1: /* single-pole */
      p->a1 = -exp(-w0);
      p->b0 = 1 + p->a1;
      break;

    case filter_HPF_1: /* single-pole */
      p->a1 = -exp(-w0);
      p->b0 = (1 - p->a1)/2;
      p->b1 = -p->b0;
      break;

    case filter_BPF_SPK: case filter_BPF_SPK_N: {
      double bw_Hz;
      if (!p->width)
        p->width = p->fc / 2;
      bw_Hz = p->width_type == width_Q?  p->fc / p->width :
        p->width_type == width_bw_Hz? p->width :
        p->fc * (pow(2., p->width) - 1) * pow(2., -0.5 * p->width); /* bw_oct */
      #include "band.h" /* Has different licence */
      break;
    }

    case filter_AP1:     /* Experimental 1-pole all-pass from Tom Erbe @ UCSD */
      p->b0 = exp(-w0);
      p->b1 = -1;
      p->a1 = -exp(-w0);
      break;

    case filter_AP2:     /* Experimental 2-pole all-pass from Tom Erbe @ UCSD */
      p->b0 = 1 - sin(w0);
      p->b1 = -2 * cos(w0);
      p->b2 = 1 + sin(w0);
      p->a0 = 1 + sin(w0);
      p->a1 = -2 * cos(w0);
      p->a2 = 1 - sin(w0);
      break;

    case filter_riaa: /* http://www.dsprelated.com/showmessage/73300/3.php */
      if (effp->in_signal.rate == 44100) {
        static const double zeros[] = {-0.2014898, 0.9233820};
        static const double poles[] = {0.7083149, 0.9924091};
        make_poly_from_roots(zeros, (size_t)2, &p->b0);
        make_poly_from_roots(poles, (size_t)2, &p->a0);
      }
      else if (effp->in_signal.rate == 48000) {
        static const double zeros[] = {-0.1766069, 0.9321590};
        static const double poles[] = {0.7396325, 0.9931330};
        make_poly_from_roots(zeros, (size_t)2, &p->b0);
        make_poly_from_roots(poles, (size_t)2, &p->a0);
      }
      else if (effp->in_signal.rate == 88200) {
        static const double zeros[] = {-0.1168735, 0.9648312};
        static const double poles[] = {0.8590646, 0.9964002};
        make_poly_from_roots(zeros, (size_t)2, &p->b0);
        make_poly_from_roots(poles, (size_t)2, &p->a0);
      }
      else if (effp->in_signal.rate == 96000) {
        static const double zeros[] = {-0.1141486, 0.9676817};
        static const double poles[] = {0.8699137, 0.9966946};
        make_poly_from_roots(zeros, (size_t)2, &p->b0);
        make_poly_from_roots(poles, (size_t)2, &p->a0);
      }
      else if (effp->in_signal.rate == 192000) {
        static const double zeros[] = {-0.1040610965, 0.9837523263};
        static const double poles[] = {0.9328992971, 0.9983633125};
        make_poly_from_roots(zeros, (size_t)2, &p->b0);
        make_poly_from_roots(poles, (size_t)2, &p->a0);
      }
      else {
        lsx_fail("Sample rate must be 44.1k, 48k, 88.2k, 96k, or 192k");
        return SOX_EOF;
      }
      { /* Normalise to 0dB at 1kHz (Thanks to Glenn Davis) */
        double y = 2 * M_PI * 1000 / effp->in_signal.rate;
        double b_re = p->b0 + p->b1 * cos(-y) + p->b2 * cos(-2 * y);
        double a_re = p->a0 + p->a1 * cos(-y) + p->a2 * cos(-2 * y);
        double b_im = p->b1 * sin(-y) + p->b2 * sin(-2 * y);
        double a_im = p->a1 * sin(-y) + p->a2 * sin(-2 * y);
        double g = 1 / sqrt((sqr(b_re) + sqr(b_im)) / (sqr(a_re) + sqr(a_im)));
        p->b0 *= g; p->b1 *= g; p->b2 *= g;
      }
      mult = (p->b0 + p->b1 + p->b2) / (p->a0 + p->a1 + p->a2);
      lsx_debug("gain=%f", linear_to_dB(mult));
      break;
  }
  if (effp->in_signal.mult)
    *effp->in_signal.mult /= mult;
  return lsx_biquad_start(effp);
}


#define BIQUAD_EFFECT(name,group,usage,flags) \
sox_effect_handler_t const * lsx_##name##_effect_fn(void) { \
  static sox_effect_handler_t handler = { \
    #name, usage, flags, \
    group##_getopts, start, lsx_biquad_flow, 0, 0, 0, sizeof(biquad_t)\
  }; \
  return &handler; \
}

BIQUAD_EFFECT(highpass,  hilo2,    "[-1|-2] frequency [width[q|o|h|k](0.707q)]", 0)
BIQUAD_EFFECT(lowpass,   hilo2,    "[-1|-2] frequency [width[q|o|h|k]](0.707q)", 0)
BIQUAD_EFFECT(bandpass,  bandpass, "[-c] frequency width[h|k|q|o]", 0)
BIQUAD_EFFECT(bandreject,bandrej,  "frequency width[h|k|q|o]", 0)
BIQUAD_EFFECT(allpass,   allpass,  "frequency width[h|k|q|o]", 0)
BIQUAD_EFFECT(bass,      tone,     "gain [frequency(100) [width[s|h|k|q|o]](0.5s)]", 0)
BIQUAD_EFFECT(treble,    tone,     "gain [frequency(3000) [width[s|h|k|q|o]](0.5s)]", 0)
BIQUAD_EFFECT(equalizer, equalizer,"frequency width[q|o|h|k] gain", 0)
BIQUAD_EFFECT(band,      band,     "[-n] center [width[h|k|q|o]]", 0)
BIQUAD_EFFECT(deemph,    deemph,   NULL, 0)
BIQUAD_EFFECT(riaa,      riaa,     NULL, 0)