13
0
livetrax/libs/zita-resampler/resampler.cc

241 lines
4.6 KiB
C++

// ----------------------------------------------------------------------------
//
// 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"
using namespace ArdourZita;
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 || !B) {
Resampler_table::destroy (T);
delete [] B;
return 1;
}
_table = T;
_buff = B;
_nchan = nchan;
_inmax = k;
_pstep = s;
return reset ();
}
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;
}