libs/zita-resampler/vresampler.cc

   1 // ----------------------------------------------------------------------------
   2 //
   3 //  Copyright (C) 2006-2013 Fons Adriaensen <fons@linuxaudio.org>
   4 //
   5 //  This program is free software; you can redistribute it and/or modify
   6 //  it under the terms of the GNU General Public License as published by
   7 //  the Free Software Foundation; either version 3 of the License, or
   8 //  (at your option) any later version.
   9 //
  10 //  This program is distributed in the hope that it will be useful,
  11 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 //  GNU General Public License for more details.
  14 //
  15 //  You should have received a copy of the GNU General Public License
  16 //  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17 //
  18 // ----------------------------------------------------------------------------
  19
  20 #include <stdlib.h>
  21 #include <stdio.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include "zita-resampler/vresampler.h"
  26
  27 using namespace ArdourZita;
  28
  29 VResampler::VResampler (void)
  30         : _table (0)
  31   , _nchan (0)
  32   , _buff  (0)
  33   , _c1 (0)
  34   , _c2 (0)
  35 {
  36         reset ();
  37 }
  38
  39 VResampler::~VResampler (void)
  40 {
  41         clear ();
  42 }
  43
  44 int
  45 VResampler::setup (double       ratio,
  46                    unsigned int nchan,
  47                    unsigned int hlen)
  48 {
  49         if ((hlen < 8) || (hlen > 96) || (16 * ratio < 1) || (ratio > 256)) return 1;
  50         return setup (ratio, nchan, hlen, 1.0 - 2.6 / hlen);
  51 }
  52
  53 int
  54 VResampler::setup (double       ratio,
  55                    unsigned int nchan,
  56                    unsigned int hlen,
  57                    double       frel)
  58 {
  59         unsigned int       h, k, n;
  60         double             s;
  61         Resampler_table    *T = 0;
  62
  63         if (! nchan) return 1;
  64         n = NPHASE;
  65         s = n / ratio;
  66         h = hlen;
  67         k = 250;
  68         if (ratio < 1) {
  69                 frel *= ratio;
  70                 h = (unsigned int)(ceil (h / ratio));
  71                 k = (unsigned int)(ceil (k / ratio));
  72         }
  73         T = Resampler_table::create (frel, h, n);
  74         clear ();
  75         if (T) {
  76                 _table = T;
  77                 _buff  = new float [nchan * (2 * h - 1 + k)];
  78                 _c1 = new float [2 * h];
  79                 _c2 = new float [2 * h];
  80                 _nchan = nchan;
  81                 _inmax = k;
  82                 _ratio = ratio;
  83                 _pstep = s;
  84                 _qstep = s;
  85                 _wstep = 1;
  86                 return reset ();
  87         }
  88         else return 1;
  89 }
  90
  91 void
  92 VResampler::clear (void)
  93 {
  94         Resampler_table::destroy (_table);
  95         delete[] _buff;
  96         delete[] _c1;
  97         delete[] _c2;
  98         _buff  = 0;
  99         _c1 = 0;
 100         _c2 = 0;
 101         _table = 0;
 102         _nchan = 0;
 103         _inmax = 0;
 104         _pstep = 0;
 105         _qstep = 0;
 106         _wstep = 1;
 107         reset ();
 108 }
 109
 110 void
 111 VResampler::set_phase (double p)
 112 {
 113         if (!_table) return;
 114         _phase = (p - floor (p)) * _table->_np;
 115 }
 116
 117 void
 118 VResampler::set_rrfilt (double t)
 119 {
 120         if (!_table) return;
 121         _wstep =  (t < 1) ? 1 : 1 - exp (-1 / t);
 122 }
 123
 124 void
 125 VResampler::set_rratio (double r)
 126 {
 127         if (!_table) return;
 128         if (r > 16.0) r = 16.0;
 129         if (r < 0.95) r = 0.95;
 130         _qstep = _table->_np / (_ratio * r);
 131 }
 132
 133 double
 134 VResampler::inpdist (void) const
 135 {
 136         if (!_table) return 0;
 137         return (int)(_table->_hl + 1 - _nread) - _phase / _table->_np;
 138 }
 139
 140 int
 141 VResampler::inpsize (void) const
 142 {
 143         if (!_table) return 0;
 144         return 2 * _table->_hl;
 145 }
 146
 147 int
 148 VResampler::reset (void)
 149 {
 150         if (!_table) return 1;
 151
 152         inp_count = 0;
 153         out_count = 0;
 154         inp_data = 0;
 155         out_data = 0;
 156         _index = 0;
 157         _phase = 0;
 158         _nread = 2 * _table->_hl;
 159         _nzero = 0;
 160         return 0;
 161 }
 162
 163 int
 164 VResampler::process (void)
 165 {
 166         unsigned int   k, np, in, nr, n, c;
 167         int            i, hl, nz;
 168         double         ph, dp, dd;
 169         float          a, b, *p1, *p2, *q1, *q2;
 170
 171         if (!_table) return 1;
 172
 173         hl = _table->_hl;
 174         np = _table->_np;
 175         in = _index;
 176         nr = _nread;
 177         nz = _nzero;
 178         ph = _phase;
 179         dp = _pstep;
 180         n = (2 * hl - nr) * _nchan;
 181         p1 = _buff + in * _nchan;
 182         p2 = p1 + n;
 183
 184         while (out_count) {
 185                 if (nr) {
 186                         if (inp_count == 0) break;
 187                         if (inp_data) {
 188                                 for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c];
 189                                 inp_data += _nchan;
 190                                 nz = 0;
 191                         } else {
 192                                 for (c = 0; c < _nchan; c++) p2 [c] = 0;
 193                                 if (nz < 2 * hl) nz++;
 194                         }
 195                         nr--;
 196                         p2 += _nchan;
 197                         inp_count--;
 198                 } else {
 199                         if (out_data) {
 200                                 if (nz < 2 * hl) {
 201                                         k = (unsigned int) ph;
 202                                         b = (float)(ph - k);
 203                                         a = 1.0f - b;
 204                                         q1 = _table->_ctab + hl * k;
 205                                         q2 = _table->_ctab + hl * (np - k);
 206                                         for (i = 0; i < hl; i++) {
 207                                                 _c1 [i] = a * q1 [i] + b * q1 [i + hl];
 208                                                 _c2 [i] = a * q2 [i] + b * q2 [i - hl];
 209                                         }
 210                                         for (c = 0; c < _nchan; c++) {
 211                                                 q1 = p1 + c;
 212                                                 q2 = p2 + c;
 213                                                 a = 1e-25f;
 214                                                 for (i = 0; i < hl; i++) {
 215                                                         q2 -= _nchan;
 216                                                         a += *q1 * _c1 [i] + *q2 * _c2 [i];
 217                                                         q1 += _nchan;
 218                                                 }
 219                                                 *out_data++ = a - 1e-25f;
 220                                         }
 221                                 } else {
 222                                         for (c = 0; c < _nchan; c++) *out_data++ = 0;
 223                                 }
 224                         }
 225                         out_count--;
 226
 227                         dd =  _qstep - dp;
 228                         if (fabs (dd) < 1e-30) dp = _qstep;
 229                         else dp += _wstep * dd;
 230                         ph += dp;
 231                         if (ph >= np) {
 232                                 nr = (unsigned int) floor( ph / np);
 233                                 ph -= nr * np;;
 234                                 in += nr;
 235                                 p1 += nr * _nchan;;
 236                                 if (in >= _inmax) {
 237                                         n = (2 * hl - nr) * _nchan;
 238                                         memcpy (_buff, p1, n * sizeof (float));
 239                                         in = 0;
 240                                         p1 = _buff;
 241                                         p2 = p1 + n;
 242                                 }
 243                         }
 244                 }
 245         }
 246         _index = in;
 247         _nread = nr;
 248         _phase = ph;
 249         _pstep = dp;
 250         _nzero = nz;
 251
 252         return 0;
 253 }