libs/zita-resampler/cresampler.cc

   1 // ----------------------------------------------------------------------------
   2 //
   3 //  Copyright (C) 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/cresampler.h"
  26
  27 using namespace ArdourZita;
  28
  29 CResampler::CResampler (void)
  30         : _nchan (0)
  31         , _buff  (0)
  32 {
  33         reset ();
  34 }
  35
  36 CResampler::~CResampler (void)
  37 {
  38         clear ();
  39 }
  40
  41 int
  42 CResampler::setup (double       ratio,
  43                    unsigned int nchan)
  44 {
  45         if (! nchan) return 1;
  46         clear ();
  47         _inmax = 50;
  48         _buff = new float [nchan * (3 + _inmax)];
  49         _nchan = nchan;
  50         _pstep = 1 / ratio;
  51         return reset ();
  52 }
  53
  54 void
  55 CResampler::clear (void)
  56 {
  57         delete[] _buff;
  58         _buff  = 0;
  59         _nchan = 0;
  60         _inmax = 0;
  61         _pstep = 0;
  62         reset ();
  63 }
  64
  65 void
  66 CResampler::set_phase (double p)
  67 {
  68         _phase = p - floor (p);
  69 }
  70
  71 void
  72 CResampler::set_ratio (double r)
  73 {
  74         _pstep = 1.0 / r;
  75 }
  76
  77 double
  78 CResampler::inpdist (void) const
  79 {
  80         return (int)(3 - _nread) - _phase;
  81 }
  82
  83 int
  84 CResampler::inpsize (void) const
  85 {
  86         return 4;
  87 }
  88
  89 int
  90 CResampler::reset (void)
  91 {
  92         inp_count = 0;
  93         out_count = 0;
  94         inp_data = 0;
  95         out_data = 0;
  96         _index = 0;
  97         _phase = 0;
  98         _nread = 4;
  99         _nzero = 0;
 100         return 0;
 101 }
 102
 103 int
 104 CResampler::process (void)
 105 {
 106         unsigned int   in, nr, n, c;
 107         int            nz;
 108         double         ph;
 109         float          *pb, a, b, d, m0, m1, m2, m3;
 110
 111         in = _index;
 112         nr = _nread;
 113         nz = _nzero;
 114         ph = _phase;
 115         pb = _buff + in * _nchan;
 116
 117         while (out_count) {
 118                 if (nr) {
 119                         if (inp_count == 0) break;
 120                         n = (4 - nr) * _nchan;
 121                         if (inp_data) {
 122                                 for (c = 0; c < _nchan; c++) pb [n + c] = inp_data [c];
 123                                 inp_data += _nchan;
 124                                 nz = 0;
 125                         } else {
 126                                 for (c = 0; c < _nchan; c++) pb [n + c] = 0;
 127                                 if (nz < 4) nz++;
 128                         }
 129                         nr--;
 130                         inp_count--;
 131                 } else {
 132                         n = _nchan;
 133                         if (out_data) {
 134                                 if (nz < 4) {
 135                                         a = ph;
 136                                         b = 1 - a;
 137                                         d = a * b / 2;
 138                                         m0 = -d * b;
 139                                         m1 = b + (3 * b - 1) * d;
 140                                         m2 = a + (3 * a - 1) * d;
 141                                         m3 = -d * a;
 142                                         for (c = 0; c < n; c++) {
 143                                                 *out_data++ = m0 * pb [0]
 144                                                         + m1 * pb [n]
 145                                                         + m2 * pb [2 * n]
 146                                                         + m3 * pb [3 * n];
 147                                                 pb++;
 148                                         }
 149                                         pb -= n;
 150                                 } else {
 151                                         for (c = 0; c < n; c++) *out_data++ = 0;
 152                                 }
 153                         }
 154                         out_count--;
 155
 156                         ph += _pstep;
 157                         if (ph >= 1.0) {
 158                                 nr = (unsigned int) floor (ph);
 159                                 ph -= nr;
 160                                 in += nr;
 161                                 pb += nr * _nchan;
 162                                 if (in >= _inmax) {
 163                                         memcpy (_buff, pb, (4 - nr) * _nchan * sizeof (float));
 164                                         in = 0;
 165                                         pb = _buff;
 166                                 }
 167                         }
 168                 }
 169         }
 170
 171         _index = in;
 172         _nread = nr;
 173         _nzero = nz;
 174         _phase = ph;
 175
 176         return 0;
 177 }