ref: b906c98cd92b4661135e293fa270921fb389c246
dir: /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 http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
*
* 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) {
priv_t * p = (priv_t *)effp->priv;
p->fc = 5283;
p->width = 0.4845;
p->gain = -9.477;
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 = 2 * M_PI * p->fc / effp->in_signal.rate;
double A = exp(p->gain / 40 * log(10.));
double 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: /* See deemph.plt for documentation */
if (effp->in_signal.rate != 44100) {
lsx_fail("Sample rate must be 44100 (audio-CD)");
return SOX_EOF;
}
/* Falls through... */
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 {
lsx_fail("Sample rate must be 44.1k, 48k, 88.2k, or 96k");
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)