diff options
Diffstat (limited to 'libs/resampler.cc')
| -rw-r--r-- | libs/resampler.cc | 263 |
1 files changed, 263 insertions, 0 deletions
diff --git a/libs/resampler.cc b/libs/resampler.cc new file mode 100644 index 0000000..097acfd --- /dev/null +++ b/libs/resampler.cc @@ -0,0 +1,263 @@ +// ---------------------------------------------------------------------------- +// +// Copyright (C) 2006-2012 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 3 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, see <http://www.gnu.org/licenses/>. +// +// ---------------------------------------------------------------------------- + + +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <math.h> +#include <zita-resampler/resampler.h> + + +static unsigned int gcd (unsigned int a, unsigned int b) +{ + if (a == 0) return b; + if (b == 0) return a; + while (1) + { + if (a > b) + { + a = a % b; + if (a == 0) return b; + if (a == 1) return 1; + } + else + { + b = b % a; + if (b == 0) return a; + if (b == 1) return 1; + } + } + return 1; +} + + +Resampler::Resampler (void) : + _table (0), + _nchan (0), + _buff (0) +{ + reset (); +} + + +Resampler::~Resampler (void) +{ + clear (); +} + + +int Resampler::setup (unsigned int fs_inp, + unsigned int fs_out, + unsigned int nchan, + unsigned int hlen) +{ + if ((hlen < 8) || (hlen > 96)) return 1; + return setup (fs_inp, fs_out, nchan, hlen, 1.0 - 2.6 / hlen); +} + + +int Resampler::setup (unsigned int fs_inp, + unsigned int fs_out, + unsigned int nchan, + unsigned int hlen, + double frel) +{ + unsigned int g, h, k, n, s; + double r; + float *B = 0; + Resampler_table *T = 0; + + k = s = 0; + if (fs_inp && fs_out && nchan) + { + r = (double) fs_out / (double) fs_inp; + g = gcd (fs_out, fs_inp); + n = fs_out / g; + s = fs_inp / g; + if ((16 * r >= 1) && (n <= 1000)) + { + h = hlen; + k = 250; + if (r < 1) + { + frel *= r; + h = (unsigned int)(ceil (h / r)); + k = (unsigned int)(ceil (k / r)); + } + T = Resampler_table::create (frel, h, n); + B = new float [nchan * (2 * h - 1 + k)]; + } + } + clear (); + if (T) + { + _table = T; + _buff = B; + _nchan = nchan; + _inmax = k; + _pstep = s; + return reset (); + } + else return 1; +} + + +void Resampler::clear (void) +{ + Resampler_table::destroy (_table); + delete[] _buff; + _buff = 0; + _table = 0; + _nchan = 0; + _inmax = 0; + _pstep = 0; + reset (); +} + + +double Resampler::inpdist (void) const +{ + if (!_table) return 0; + return (int)(_table->_hl + 1 - _nread) - (double)_phase / _table->_np; +} + + +int Resampler::inpsize (void) const +{ + if (!_table) return 0; + return 2 * _table->_hl; +} + + +int Resampler::reset (void) +{ + if (!_table) return 1; + + inp_count = 0; + out_count = 0; + inp_data = 0; + out_data = 0; + _index = 0; + _nread = 0; + _nzero = 0; + _phase = 0; + if (_table) + { + _nread = 2 * _table->_hl; + return 0; + } + return 1; +} + + +int Resampler::process (void) +{ + unsigned int hl, ph, np, dp, in, nr, nz, i, n, c; + float *p1, *p2; + + if (!_table) return 1; + + hl = _table->_hl; + np = _table->_np; + dp = _pstep; + in = _index; + nr = _nread; + ph = _phase; + nz = _nzero; + n = (2 * hl - nr) * _nchan; + p1 = _buff + in * _nchan; + p2 = p1 + n; + + while (out_count) + { + if (nr) + { + if (inp_count == 0) break; + if (inp_data) + { + for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c]; + inp_data += _nchan; + nz = 0; + } + else + { + for (c = 0; c < _nchan; c++) p2 [c] = 0; + if (nz < 2 * hl) nz++; + } + nr--; + p2 += _nchan; + inp_count--; + } + else + { + if (out_data) + { + if (nz < 2 * hl) + { + float *c1 = _table->_ctab + hl * ph; + float *c2 = _table->_ctab + hl * (np - ph); + for (c = 0; c < _nchan; c++) + { + float *q1 = p1 + c; + float *q2 = p2 + c; + float s = 1e-20f; + for (i = 0; i < hl; i++) + { + q2 -= _nchan; + s += *q1 * c1 [i] + *q2 * c2 [i]; + q1 += _nchan; + } + *out_data++ = s - 1e-20f; + } + } + else + { + for (c = 0; c < _nchan; c++) *out_data++ = 0; + } + } + out_count--; + + ph += dp; + if (ph >= np) + { + nr = ph / np; + ph -= nr * np; + in += nr; + p1 += nr * _nchan;; + if (in >= _inmax) + { + n = (2 * hl - nr) * _nchan; + memcpy (_buff, p1, n * sizeof (float)); + in = 0; + p1 = _buff; + p2 = p1 + n; + } + } + } + } + _index = in; + _nread = nr; + _phase = ph; + _nzero = nz; + + return 0; +} + + |