diff options
Diffstat (limited to 'src/pareq.cc')
| -rw-r--r-- | src/pareq.cc | 199 |
1 files changed, 199 insertions, 0 deletions
diff --git a/src/pareq.cc b/src/pareq.cc new file mode 100644 index 0000000..4feb991 --- /dev/null +++ b/src/pareq.cc @@ -0,0 +1,199 @@ +// ---------------------------------------------------------------------- +// +// Copyright (C) 2010-2013 Fons Adriaensen <fons@linuxaudio.org> +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// ---------------------------------------------------------------------- + + +#include <math.h> +#include <string.h> +#include "pareq.h" + + +Pareq::Pareq (void) : + _touch0 (0), + _touch1 (0), + _state (BYPASS), + _g0 (1), + _g1 (1), + _f0 (1e3f), + _f1 (1e3f) +{ + setfsamp (0.0f); +} + + +Pareq::~Pareq (void) +{ +} + + +void Pareq::setfsamp (float fsamp) +{ + _fsamp = fsamp; + reset (); +} + + +void Pareq::reset (void) +{ + memset (_z1, 0, sizeof (float) * MAXCH); + memset (_z2, 0, sizeof (float) * MAXCH); +} + + +void Pareq::prepare (int nsamp) +{ + bool upd = false; + float g, f; + + if (_touch1 != _touch0) + { + if (_g0 < 0.1f) _g0 = 0.1f; + if (_g0 > 10.0f) _g0 = 10.0f; + if (_f0 < 20.0f) _f0 = 20.0f; + if (_f0 > 20e3f) _f0 = 20e3f; + g = _g0; + f = _f0; + if (g != _g1) + { + upd = true; + if (g > 2 * _g1) _g1 *= 2; + else if (_g1 > 2 * g) _g1 /= 2; + else _g1 = g; + } + if (f != _f1) + { + upd = true; + if (f > 2 * _f1) _f1 *= 2; + else if (_f1 > 2 * f) _f1 /= 2; + else _f1 = f; + } + if (upd) + { + if ((_state == BYPASS) && (_g1 == 1)) + { + calcpar1 (0, _g1, _f1); + } + else + { + _state = SMOOTH; + calcpar1 (nsamp, _g1, _f1); + } + } + else + { + _touch1 = _touch0; + if (fabs (_g1 - 1) < 0.001f) + { + _state = BYPASS; + reset (); + } + else + { + _state = STATIC; + } + } + } +} + + +void Pareq::calcpar1 (int nsamp, float g, float f) +{ + float b, c1, c2, gg; + + f *= float (M_PI) / _fsamp; + b = 2 * f / sqrtf (g); + gg = 0.5f * (g - 1); + c1 = -cosf (2 * f); + c2 = (1 - b) / (1 + b); + if (nsamp) + { + _dc1 = (c1 - _c1) / nsamp + 1e-30f; + _dc2 = (c2 - _c2) / nsamp + 1e-30f; + _dgg = (gg - _gg) / nsamp + 1e-30f; + } + else + { + _c1 = c1; + _c2 = c2; + _gg = gg; + } +} + + +void Pareq::process1 (int nsamp, int nchan, float *data[]) +{ + int i, j; + float c1, c2, gg; + float x, y, z1, z2; + float *p; + + c1 = _c1; + c2 = _c2; + gg = _gg; + if (_state == SMOOTH) + { + for (i = 0; i < nchan; i++) + { + p = data [i]; + z1 = _z1 [i]; + z2 = _z2 [i]; + c1 = _c1; + c2 = _c2; + gg = _gg; + for (j = 0; j < nsamp; j++) + { + c1 += _dc1; + c2 += _dc2; + gg += _dgg; + x = *p; + y = x - c2 * z2; + *p++ = x - gg * (z2 + c2 * y - x); + y -= c1 * z1; + z2 = z1 + c1 * y; + z1 = y + 1e-20f; + } + _z1 [i] = z1; + _z2 [i] = z2; + } + _c1 = c1; + _c2 = c2; + _gg = gg; + } + else + { + for (i = 0; i < nchan; i++) + { + p = data [i]; + z1 = _z1 [i]; + z2 = _z2 [i]; + for (j = 0; j < nsamp; j++) + { + x = *p; + y = x - c2 * z2; + *p++ = x - gg * (z2 + c2 * y - x); + y -= c1 * z1; + z2 = z1 + c1 * y; + z1 = y + 1e-20f; + } + _z1 [i] = z1; + _z2 [i] = z2; + } + } +} + |