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
21 #include <stdlib.h>
22 #include <stdio.h>
23 #include <string.h>
24 #include <math.h>
25 #include "vresampler.h"
26
27
VResampler(void)28 VResampler::VResampler (void) :
29 _table (0),
30 _nchan (0),
31 _buff (0),
32 _c1 (0),
33 _c2 (0)
34 {
35 reset ();
36 }
37
38
~VResampler(void)39 VResampler::~VResampler (void)
40 {
41 clear ();
42 }
43
44
setup(double ratio,unsigned int nchan,unsigned int hlen)45 int 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
setup(double ratio,unsigned int nchan,unsigned int hlen,double frel)54 int 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 {
70 frel *= ratio;
71 h = (unsigned int)(ceil (h / ratio));
72 k = (unsigned int)(ceil (k / ratio));
73 }
74 T = Resampler_table::create (frel, h, n);
75 clear ();
76 if (T)
77 {
78 _table = T;
79 _buff = new float [nchan * (2 * h - 1 + k)];
80 _c1 = new float [2 * h];
81 _c2 = new float [2 * h];
82 _nchan = nchan;
83 _inmax = k;
84 _ratio = ratio;
85 _pstep = s;
86 _qstep = s;
87 _wstep = 1;
88 return reset ();
89 }
90 else return 1;
91 }
92
93
clear(void)94 void VResampler::clear (void)
95 {
96 Resampler_table::destroy (_table);
97 delete[] _buff;
98 delete[] _c1;
99 delete[] _c2;
100 _buff = 0;
101 _c1 = 0;
102 _c2 = 0;
103 _table = 0;
104 _nchan = 0;
105 _inmax = 0;
106 _pstep = 0;
107 _qstep = 0;
108 _wstep = 1;
109 reset ();
110 }
111
112
set_phase(double p)113 void VResampler::set_phase (double p)
114 {
115 if (!_table) return;
116 _phase = (p - floor (p)) * _table->_np;
117 }
118
119
set_rrfilt(double t)120 void VResampler::set_rrfilt (double t)
121 {
122 if (!_table) return;
123 _wstep = (t < 1) ? 1 : 1 - exp (-1 / t);
124 }
125
126
set_rratio(double r)127 void VResampler::set_rratio (double r)
128 {
129 if (!_table) return;
130 if (r > 16.0) r = 16.0;
131 if (r < 0.95) r = 0.95;
132 _qstep = _table->_np / (_ratio * r);
133 }
134
135
inpdist(void) const136 double VResampler::inpdist (void) const
137 {
138 if (!_table) return 0;
139 return (int)(_table->_hl + 1 - _nread) - _phase / _table->_np;
140 }
141
142
inpsize(void) const143 int VResampler::inpsize (void) const
144 {
145 if (!_table) return 0;
146 return 2 * _table->_hl;
147 }
148
149
reset(void)150 int VResampler::reset (void)
151 {
152 if (!_table) return 1;
153
154 inp_count = 0;
155 out_count = 0;
156 inp_data = 0;
157 out_data = 0;
158 _index = 0;
159 _phase = 0;
160 _nread = 2 * _table->_hl;
161 _nzero = 0;
162 return 0;
163 }
164
165
process(void)166 int VResampler::process (void)
167 {
168 unsigned int k, np, in, nr, n, c;
169 int i, hl, nz;
170 double ph, dp, dd;
171 float a, b, *p1, *p2, *q1, *q2;
172
173 if (!_table) return 1;
174
175 hl = _table->_hl;
176 np = _table->_np;
177 in = _index;
178 nr = _nread;
179 nz = _nzero;
180 ph = _phase;
181 dp = _pstep;
182 n = (2 * hl - nr) * _nchan;
183 p1 = _buff + in * _nchan;
184 p2 = p1 + n;
185
186 while (out_count)
187 {
188 if (nr)
189 {
190 if (inp_count == 0) break;
191 if (inp_data)
192 {
193 for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c];
194 inp_data += _nchan;
195 nz = 0;
196 }
197 else
198 {
199 for (c = 0; c < _nchan; c++) p2 [c] = 0;
200 if (nz < 2 * hl) nz++;
201 }
202 nr--;
203 p2 += _nchan;
204 inp_count--;
205 }
206 else
207 {
208 if (out_data)
209 {
210 if (nz < 2 * hl)
211 {
212 k = (unsigned int) ph;
213 b = (float)(ph - k);
214 a = 1.0f - b;
215 q1 = _table->_ctab + hl * k;
216 q2 = _table->_ctab + hl * (np - k);
217 for (i = 0; i < hl; i++)
218 {
219 _c1 [i] = a * q1 [i] + b * q1 [i + hl];
220 _c2 [i] = a * q2 [i] + b * q2 [i - hl];
221 }
222 for (c = 0; c < _nchan; c++)
223 {
224 q1 = p1 + c;
225 q2 = p2 + c;
226 a = 1e-25f;
227 for (i = 0; i < hl; i++)
228 {
229 q2 -= _nchan;
230 a += *q1 * _c1 [i] + *q2 * _c2 [i];
231 q1 += _nchan;
232 }
233 *out_data++ = a - 1e-25f;
234 }
235 }
236 else
237 {
238 for (c = 0; c < _nchan; c++) *out_data++ = 0;
239 }
240 }
241 out_count--;
242
243 dd = _qstep - dp;
244 if (fabs (dd) < 1e-30) dp = _qstep;
245 else dp += _wstep * dd;
246 ph += dp;
247 if (ph >= np)
248 {
249 nr = (unsigned int) floor( ph / np);
250 ph -= nr * np;;
251 in += nr;
252 p1 += nr * _nchan;;
253 if (in >= _inmax)
254 {
255 n = (2 * hl - nr) * _nchan;
256 memcpy (_buff, p1, n * sizeof (float));
257 in = 0;
258 p1 = _buff;
259 p2 = p1 + n;
260 }
261 }
262 }
263 }
264 _index = in;
265 _nread = nr;
266 _phase = ph;
267 _pstep = dp;
268 _nzero = nz;
269
270 return 0;
271 }
272
273