1 /*****************************************************************************
2 * band-limited sinc resampling
3 *
4 * written by Stefan Keller <skeller@zahlenfresser.de>
5 * placed into the public domain
6 *****************************************************************************/
7
8 /*
9 As the sampling theorem shows, a bandlimited signal can be fully reconstructed
10 using a reconstruction low-pass.
11 This reconstruction at real point t across the time domain can be viewed as a
12 superposition of an infinite number of sinc functions scaled by the sample
13 values and shifted in time to the sample time.
14 This resampling code takes 14 sample values (seven before the actual point,
15 seven after) and calculates the superposition.
16 */
17
18 #include "resample.h"
19 #include <cstring>
20 #include <cmath>
21 #include <cstdlib>
22
23 #define MOVE_THRESH 160
24 #define FILTER_DELAY_SINC_MONO 7
25 #define FILTER_DELAY_SINC_STEREO 14
26 #define FILTER_DELAY_LIN_MONO 1
27 #define FILTER_DELAY_LIN_STEREO 2
28
29 #ifndef PI /* Sometimes in math.h */
30 #define PI 3.14159265358979323846
31 #endif
32
33 bool ResampleSincMono::sinc_initialized = false;
34 /* wee need 16 (instead of 15) because of linear interpolation:
35 * if currfrc == 255, currfrc+1 will point to the element after the last */
36 float ResampleSincMono::sinc[16][256];
37
38 //--- Resample --------------------------------------------------------------//
39
Resample()40 Resample::Resample() : filter_delay(0), current(0), curfrc(0), pad_samples(0)
41 {
42 }
43
put_samples(signed short * samples,unsigned int num_samples)44 int Resample::put_samples(signed short* samples, unsigned int num_samples)
45 {
46 if(current >= MOVE_THRESH) {
47 this->samples.erase(this->samples.begin(), this->samples.begin()+current-filter_delay);
48 current = filter_delay;
49 }
50
51 for(unsigned i=0; i<num_samples; i++) {
52 this->samples.push_back((float)samples[i]);
53 }
54
55 return num_samples;
56 }
57
58 //--- ResampleSincMono ------------------------------------------------------//
59
ResampleSincMono()60 ResampleSincMono::ResampleSincMono()
61 {
62 filter_delay = FILTER_DELAY_SINC_MONO;
63 if(!sinc_initialized) {
64 init_sinc();
65 sinc_initialized = true;
66 }
67 current = filter_delay;
68 samples.assign(filter_delay, 0.0);
69 }
70
ResampleSincMono(bool do_pad,float max_rate)71 ResampleSincMono::ResampleSincMono(bool do_pad, float max_rate)
72 {
73 filter_delay = FILTER_DELAY_SINC_MONO;
74 if(!sinc_initialized) {
75 init_sinc();
76 sinc_initialized = true;
77 }
78 if(do_pad) {
79 pad_samples = (int)((float)filter_delay * max_rate + 0.5);
80 }
81 samples.assign(filter_delay, 0.0);
82 current = filter_delay;
83 }
84
85
init_sinc()86 void ResampleSincMono::init_sinc()
87 {
88 int i, k;
89
90 // calculate sinc table
91 for (i=0; i<15; i++) {
92 for(k=0; k<256; k++) {
93 if(i == 7 && k == 0) sinc[i][k] = 1.0;
94 else sinc[i][k] = sin((i + k/256.0 - 7) * PI) / ((i + k/256.0 - 7) * PI);
95 }
96 }
97 // apply hamming window
98 for (i=0; i<15; i++) {
99 for(k=0; k<256; k++) {
100 sinc[i][k] *= (0.54 + 0.46 * cos(2.0*PI*(i + k/256.0 - 7) / 16));
101 }
102 }
103 memset(&sinc[15], 0, sizeof(*sinc[15]));
104 }
105
upsample(float * src,unsigned int current,unsigned int curfrc)106 inline float ResampleSincMono::upsample(float *src, unsigned int current, unsigned int curfrc)
107 {
108 float sinc_val, curfloat;
109 float samp;
110 curfloat = (curfrc & 0xffffff) / 16777216.0;
111
112 // linear interpolation between sinc values
113 sinc_val = (sinc[14][(curfrc>>24)+1] - sinc[14][curfrc>>24]) * curfloat + sinc[14][curfrc>>24];
114 samp = src[current-7]*sinc_val;
115
116 sinc_val = (sinc[13][(curfrc>>24)+1] - sinc[13][curfrc>>24]) * curfloat + sinc[13][curfrc>>24];
117 samp += src[current-6]*sinc_val;
118
119 sinc_val = (sinc[12][(curfrc>>24)+1] - sinc[12][curfrc>>24]) * curfloat + sinc[12][curfrc>>24];
120 samp += src[current-5]*sinc_val;
121
122 sinc_val = (sinc[11][(curfrc>>24)+1] - sinc[11][curfrc>>24]) * curfloat + sinc[11][curfrc>>24];
123 samp += src[current-4]*sinc_val;
124
125 sinc_val = (sinc[10][(curfrc>>24)+1] - sinc[10][curfrc>>24]) * curfloat + sinc[10][curfrc>>24];
126 samp += src[current-3]*sinc_val;
127
128 sinc_val = (sinc[9][(curfrc>>24)+1] - sinc[9][curfrc>>24]) * curfloat + sinc[9][curfrc>>24];
129 samp += src[current-2]*sinc_val;
130
131 sinc_val = (sinc[8][(curfrc>>24)+1] - sinc[8][curfrc>>24]) * curfloat + sinc[8][curfrc>>24];
132 samp += src[current-1]*sinc_val;
133
134 sinc_val = (sinc[7][(curfrc>>24)+1] - sinc[7][curfrc>>24]) * curfloat + sinc[7][curfrc>>24];
135 samp += src[current]*sinc_val;
136
137 sinc_val = (sinc[6][(curfrc>>24)+1] - sinc[6][curfrc>>24]) * curfloat + sinc[6][curfrc>>24];
138 samp += src[current+1]*sinc_val;
139
140 sinc_val = (sinc[5][(curfrc>>24)+1] - sinc[5][curfrc>>24]) * curfloat + sinc[5][curfrc>>24];
141 samp += src[current+2]*sinc_val;
142
143 sinc_val = (sinc[4][(curfrc>>24)+1] - sinc[4][curfrc>>24]) * curfloat + sinc[4][curfrc>>24];
144 samp += src[current+3]*sinc_val;
145
146 sinc_val = (sinc[3][(curfrc>>24)+1] - sinc[3][curfrc>>24]) * curfloat + sinc[3][curfrc>>24];
147 samp += src[current+4]*sinc_val;
148
149 sinc_val = (sinc[2][(curfrc>>24)+1] - sinc[2][curfrc>>24]) * curfloat + sinc[2][curfrc>>24];
150 samp += src[current+5]*sinc_val;
151
152 sinc_val = (sinc[1][(curfrc>>24)+1] - sinc[1][curfrc>>24]) * curfloat + sinc[1][curfrc>>24];
153 samp += src[current+6]*sinc_val;
154
155 sinc_val = (sinc[0][(curfrc>>24)+1] - sinc[0][curfrc>>24]) * curfloat + sinc[0][curfrc>>24];
156 samp += src[current+7]*sinc_val;
157
158 return samp;
159 }
160
downsample(float * src,unsigned int current,unsigned int curfrc,unsigned long long sinc_increment,float scale)161 inline float ResampleSincMono::downsample(float *src, unsigned int current, unsigned int curfrc, unsigned long long sinc_increment, float scale)
162 {
163 long long sinc_current;
164 float samp;
165
166 sinc_current = ((sinc_increment * curfrc)>>32) + ((long long)7<<32);
167 sinc_current -= sinc_increment * 7;
168
169 // todo: linear interpolation between sinc values
170 samp = src[current+7]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
171
172 sinc_current += sinc_increment;
173 samp += src[current+6]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
174
175 sinc_current += sinc_increment;
176 samp += src[current+5]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
177
178 sinc_current += sinc_increment;
179 samp += src[current+4]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
180
181 sinc_current += sinc_increment;
182 samp += src[current+3]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
183
184 sinc_current += sinc_increment;
185 samp += src[current+2]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
186
187 sinc_current += sinc_increment;
188 samp += src[current+1]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
189
190 sinc_current += sinc_increment;
191 samp += src[current]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
192
193 sinc_current += sinc_increment;
194 samp += src[current-1]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
195
196 sinc_current += sinc_increment;
197 samp += src[current-2]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
198
199 sinc_current += sinc_increment;
200 samp += src[current-3]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
201
202 sinc_current += sinc_increment;
203 samp += src[current-4]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
204
205 sinc_current += sinc_increment;
206 samp += src[current-5]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
207
208 sinc_current += sinc_increment;
209 samp += src[current-6]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
210
211 sinc_current += sinc_increment;
212 samp += src[current-7]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
213
214 // scale sinc factor:
215 samp *= scale;
216
217 return samp;
218 }
219
resample(signed short * dst,float rate,unsigned num_samples)220 int ResampleSincMono::resample(signed short *dst, float rate, unsigned num_samples)
221 {
222 float samp;
223
224 unsigned long long current_fixed;
225 long long increment_fixed;
226 int done = 0;
227
228 increment_fixed = (1.0/rate) * 4294967296.0;
229 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL) | (curfrc & 0xffffffff);
230
231 if(pad_samples) {
232 if(num_samples < pad_samples) {
233 pad_samples = 0;
234 return 0;
235 }
236 for(unsigned i=0; i<pad_samples; i++) {
237 *dst = 0; dst++;
238 }
239 done += pad_samples;
240 num_samples -= pad_samples;
241 pad_samples = 0;
242 }
243
244 if(rate < 1.0) { // downsampling, need to scale sinc apropriatly
245 // calculate sinc increment
246 // todo: with all tis crap here, a floating point increment
247 // (for the resampling as well) may be better (and faster)
248 unsigned long long sinc_increment = rate * 4294967296.0;
249
250 // this code stretches the sinc to fit the new cut-off frequency
251 // (at 1/2 of our new fs, which is lower than 1/2 of the sample fs)
252 // this stretching is done by calculating the sinc increment above
253 // finally the new sinc has to be scaled to fit this new filter
254 // (the def. for the lowpass is 2.0 * fc * sinc(...))
255 // this is done at the end
256
257 while(num_samples > 0) {
258 unsigned int current = (current_fixed>>32) & 0xffffffff;
259 unsigned int curfrc = current_fixed & 0xffffffff;
260
261 if(current >= samples.size() - filter_delay) break;
262
263 samp = ResampleSincMono::downsample(&samples[0], current, curfrc, sinc_increment, rate);
264
265 // should never happen, but maybe because of rounding etc. it does:
266 if(samp > 32767.0) samp = 32767.0;
267 else if (samp < -32768.0) samp = -32768.0;
268
269 *dst = (short)samp; dst++;
270
271 current_fixed += increment_fixed;
272
273 num_samples--; done++;
274 }
275 } else if (rate > 1.0 || this->curfrc) {
276 while(num_samples > 0) {
277 unsigned int current = (current_fixed>>32) & 0xffffffff;
278 unsigned int curfrc = current_fixed & 0xffffffff;
279
280 if(current >= samples.size() - filter_delay) break;
281
282 samp = ResampleSincMono::upsample(&samples[0], current, curfrc);
283
284 // should never happen, but maybe because of rounding etc. it does:
285 if(samp > 32767.0) samp = 32767.0;
286 else if (samp < -32768.0) samp = -32768.0;
287
288 *dst = (short)samp; dst++;
289
290 current_fixed += increment_fixed;
291
292 num_samples--; done++;
293 }
294 } else {
295 /* rate == 1.0 && curfrc == 0 */
296 /* just copy */
297 unsigned int current = this->current;
298 while(num_samples > 0) {
299 if(current >= samples.size() - filter_delay) break;
300
301 *dst = (short)samples[current]; dst++;
302
303 current++;
304
305 num_samples--; done++;
306 }
307 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL);
308 }
309
310 this->current = (current_fixed>>32) & 0xffffffff;
311 this->curfrc = current_fixed & 0xffffffff;
312
313 return done;
314 }
315
316
317 //--- ResampleSincStereo ----------------------------------------------------//
318
ResampleSincStereo()319 ResampleSincStereo::ResampleSincStereo()
320 {
321 filter_delay = FILTER_DELAY_SINC_STEREO;
322 current = filter_delay;
323 samples.assign(filter_delay, 0.0);
324 }
325
ResampleSincStereo(bool do_pad,float max_rate)326 ResampleSincStereo::ResampleSincStereo(bool do_pad, float max_rate)
327 {
328 filter_delay = FILTER_DELAY_SINC_STEREO;
329 if(do_pad) {
330 pad_samples = (int)((float)(filter_delay/2) * max_rate + 0.5);
331 }
332 samples.assign(filter_delay, 0.0);
333 current = filter_delay;
334 }
335
upsample(float * src,unsigned int current,unsigned int curfrc)336 inline float ResampleSincStereo::upsample(float *src, unsigned int current, unsigned int curfrc)
337 {
338 float sinc_val, curfloat;
339 float samp;
340 curfloat = (curfrc & 0xffffff) / 16777216.0;
341
342 // linear interpolation between sinc values
343 sinc_val = (sinc[14][(curfrc>>24)+1] - sinc[14][curfrc>>24]) * curfloat + sinc[14][curfrc>>24];
344 samp = src[current*2-14]*sinc_val;
345
346 sinc_val = (sinc[13][(curfrc>>24)+1] - sinc[13][curfrc>>24]) * curfloat + sinc[13][curfrc>>24];
347 samp += src[current*2-12]*sinc_val;
348
349 sinc_val = (sinc[12][(curfrc>>24)+1] - sinc[12][curfrc>>24]) * curfloat + sinc[12][curfrc>>24];
350 samp += src[current*2-10]*sinc_val;
351
352 sinc_val = (sinc[11][(curfrc>>24)+1] - sinc[11][curfrc>>24]) * curfloat + sinc[11][curfrc>>24];
353 samp += src[current*2-8]*sinc_val;
354
355 sinc_val = (sinc[10][(curfrc>>24)+1] - sinc[10][curfrc>>24]) * curfloat + sinc[10][curfrc>>24];
356 samp += src[current*2-6]*sinc_val;
357
358 sinc_val = (sinc[9][(curfrc>>24)+1] - sinc[9][curfrc>>24]) * curfloat + sinc[9][curfrc>>24];
359 samp += src[current*2-4]*sinc_val;
360
361 sinc_val = (sinc[8][(curfrc>>24)+1] - sinc[8][curfrc>>24]) * curfloat + sinc[8][curfrc>>24];
362 samp += src[current*2-2]*sinc_val;
363
364 sinc_val = (sinc[7][(curfrc>>24)+1] - sinc[7][curfrc>>24]) * curfloat + sinc[7][curfrc>>24];
365 samp += src[current*2]*sinc_val;
366
367 sinc_val = (sinc[6][(curfrc>>24)+1] - sinc[6][curfrc>>24]) * curfloat + sinc[6][curfrc>>24];
368 samp += src[current*2+2]*sinc_val;
369
370 sinc_val = (sinc[5][(curfrc>>24)+1] - sinc[5][curfrc>>24]) * curfloat + sinc[5][curfrc>>24];
371 samp += src[current*2+4]*sinc_val;
372
373 sinc_val = (sinc[4][(curfrc>>24)+1] - sinc[4][curfrc>>24]) * curfloat + sinc[4][curfrc>>24];
374 samp += src[current*2+6]*sinc_val;
375
376 sinc_val = (sinc[3][(curfrc>>24)+1] - sinc[3][curfrc>>24]) * curfloat + sinc[3][curfrc>>24];
377 samp += src[current*2+8]*sinc_val;
378
379 sinc_val = (sinc[2][(curfrc>>24)+1] - sinc[2][curfrc>>24]) * curfloat + sinc[2][curfrc>>24];
380 samp += src[current*2+10]*sinc_val;
381
382 sinc_val = (sinc[1][(curfrc>>24)+1] - sinc[1][curfrc>>24]) * curfloat + sinc[1][curfrc>>24];
383 samp += src[current*2+12]*sinc_val;
384
385 sinc_val = (sinc[0][(curfrc>>24)+1] - sinc[0][curfrc>>24]) * curfloat + sinc[0][curfrc>>24];
386 samp += src[current*2+14]*sinc_val;
387
388 return samp;
389 }
390
downsample(float * src,unsigned int current,unsigned int curfrc,unsigned long long sinc_increment,float scale)391 inline float ResampleSincStereo::downsample(float *src, unsigned int current, unsigned int curfrc, unsigned long long sinc_increment, float scale)
392 {
393 long long sinc_current;
394 float samp;
395
396 sinc_current = ((sinc_increment * curfrc)>>32) + ((long long)7<<32);
397 sinc_current -= sinc_increment * 7;
398
399 // todo: linear interpolation between sinc values
400 samp = src[current*2+14]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
401
402 sinc_current += sinc_increment;
403 samp += src[current*2+12]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
404
405 sinc_current += sinc_increment;
406 samp += src[current*2+10]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
407
408 sinc_current += sinc_increment;
409 samp += src[current*2+8]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
410
411 sinc_current += sinc_increment;
412 samp += src[current*2+6]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
413
414 sinc_current += sinc_increment;
415 samp += src[current*2+4]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
416
417 sinc_current += sinc_increment;
418 samp += src[current*2+2]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
419
420 sinc_current += sinc_increment;
421 samp += src[current*2]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
422
423 sinc_current += sinc_increment;
424 samp += src[current*2-2]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
425
426 sinc_current += sinc_increment;
427 samp += src[current*2-4]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
428
429 sinc_current += sinc_increment;
430 samp += src[current*2-6]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
431
432 sinc_current += sinc_increment;
433 samp += src[current*2-8]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
434
435 sinc_current += sinc_increment;
436 samp += src[current*2-10]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
437
438 sinc_current += sinc_increment;
439 samp += src[current*2-12]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
440
441 sinc_current += sinc_increment;
442 samp += src[current*2-14]*sinc[(sinc_current>>32) & 0xffffffff][(sinc_current & 0xffffffff)>>24];
443
444 // scale sinc factor:
445 samp *= scale;
446
447 return samp;
448 }
449
resample(signed short * dst,float rate,unsigned num_samples)450 int ResampleSincStereo::resample(signed short *dst, float rate, unsigned num_samples)
451 {
452 float samp_l;
453 float samp_r;
454
455 unsigned long long current_fixed;
456 long long increment_fixed;
457 int done = 0;
458
459 increment_fixed = (1.0/rate) * 4294967296.0;
460 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL) | (curfrc & 0xffffffff);
461
462 if(pad_samples) {
463 if(num_samples < pad_samples) {
464 pad_samples = 0;
465 return 0;
466 }
467 for(unsigned i=0; i<pad_samples; i++) {
468 *dst = 0; dst++;
469 *dst = 0; dst++;
470 }
471 done += pad_samples;
472 num_samples -= pad_samples;
473 pad_samples = 0;
474 }
475
476 if(rate < 1.0) { // downsampling, need to scale sinc apropriatly
477 // calculate sinc increment
478 // todo: with all tis crap here, a floating point increment
479 // (for the resampling as well) may be better (and faster)
480 unsigned long long sinc_increment = rate * 4294967296.0;
481
482 // this code stretches the sinc to fit the new cut-off frequency
483 // (at 1/2 of our new fs, which is lower than 1/2 of the sample fs)
484 // this stretching is done by calculating the sinc increment above
485 // finally the new sinc has to be scaled to fit this new filter
486 // (the def. for the lowpass is 2.0 * fc * sinc(...))
487 // this is done at the end
488
489 while(num_samples > 0) {
490 unsigned int current = (current_fixed>>32) & 0xffffffff;
491 unsigned int curfrc = current_fixed & 0xffffffff;
492
493 if((current*2) >= samples.size() - filter_delay - 1) break;
494
495 samp_l = ResampleSincStereo::downsample(&samples[0], current, curfrc, sinc_increment, rate);
496 samp_r = ResampleSincStereo::downsample(&samples[1], current, curfrc, sinc_increment, rate);
497
498 // should never happen, but maybe because of rounding etc. it does:
499 if(samp_l > 32767.0) samp_l = 32767.0;
500 else if (samp_l < -32768.0) samp_l = -32768.0;
501 if(samp_r > 32767.0) samp_r = 32767.0;
502 else if (samp_r < -32768.0) samp_r = -32768.0;
503
504 *dst = (short)samp_l; dst++;
505 *dst = (short)samp_r; dst++;
506
507 current_fixed += increment_fixed;
508
509 num_samples--; done++;
510 }
511 } else if (rate > 1.0 || this->curfrc) {
512 while(num_samples > 0) {
513 unsigned int current = (current_fixed>>32) & 0xffffffff;
514 unsigned int curfrc = current_fixed & 0xffffffff;
515
516 if((current*2) >= samples.size() - filter_delay - 1) break;
517
518 samp_l = ResampleSincStereo::upsample(&samples[0], current, curfrc);
519 samp_r = ResampleSincStereo::upsample(&samples[1], current, curfrc);
520
521 // should never happen, but maybe because of rounding etc. it does:
522 if(samp_l > 32767.0) samp_l = 32767.0;
523 else if (samp_l < -32768.0) samp_l = -32768.0;
524 if(samp_r > 32767.0) samp_r = 32767.0;
525 else if (samp_r < -32768.0) samp_r = -32768.0;
526
527 *dst = (short)samp_l; dst++;
528 *dst = (short)samp_r; dst++;
529
530 current_fixed += increment_fixed;
531
532 num_samples--; done++;
533 }
534 } else {
535 /* rate == 1.0 && curfrc == 0 */
536 /* just copy */
537 unsigned int current = this->current;
538 while(num_samples > 0) {
539 if((current*2) >= samples.size() - filter_delay - 1) break;
540
541 *dst = (short)samples[current*2]; dst++;
542 *dst = (short)samples[current*2+1]; dst++;
543
544 current++;
545
546 num_samples--; done++;
547 }
548 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL);
549 }
550
551 this->current = (current_fixed>>32) & 0xffffffff;
552 this->curfrc = current_fixed & 0xffffffff;
553
554 return done;
555 }
556
557 //--- ResampleLinMono -------------------------------------------------------//
558
ResampleLinMono()559 ResampleLinMono::ResampleLinMono()
560 {
561 filter_delay = FILTER_DELAY_LIN_MONO;
562 current = 0;
563 }
564
ResampleLinMono(bool do_pad,float max_rate)565 ResampleLinMono::ResampleLinMono(bool do_pad, float max_rate)
566 {
567 filter_delay = FILTER_DELAY_LIN_MONO;
568 if(do_pad) {
569 pad_samples = (int)((float)filter_delay * max_rate + 0.5);
570 }
571 current = 0;
572 }
573
updownsample(float * src,unsigned int current,unsigned int curfrc)574 inline float ResampleLinMono::updownsample(float *src, unsigned int current, unsigned int curfrc)
575 {
576 float samp;
577
578 samp = src[current] + (src[current+1] - src[current]) * (curfrc / 4294967296.0);
579
580 return samp;
581 }
582
resample(signed short * dst,float rate,unsigned num_samples)583 int ResampleLinMono::resample(signed short *dst, float rate, unsigned num_samples)
584 {
585 float samp;
586
587 unsigned long long current_fixed;
588 long long increment_fixed;
589 int done = 0;
590
591 increment_fixed = (1.0/rate) * 4294967296.0;
592 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL) | (curfrc & 0xffffffff);
593
594 if(pad_samples) {
595 if(num_samples < pad_samples) {
596 pad_samples = 0;
597 return 0;
598 }
599 for(unsigned i=0; i<pad_samples; i++) {
600 *dst = 0; dst++;
601 }
602 done += pad_samples;
603 num_samples -= pad_samples;
604 pad_samples = 0;
605 }
606
607 if(rate < 1.0) { // downsampling, need to scale sinc apropriatly
608 while(num_samples > 0) {
609 unsigned int current = (current_fixed>>32) & 0xffffffff;
610 unsigned int curfrc = current_fixed & 0xffffffff;
611
612 if(current >= samples.size() - filter_delay) break;
613
614 samp = ResampleLinMono::updownsample(&samples[0], current, curfrc);
615
616 // should never happen, but maybe because of rounding etc. it does:
617 if(samp > 32767.0) samp = 32767.0;
618 else if (samp < -32768.0) samp = -32768.0;
619
620 *dst = (short)samp; dst++;
621
622 current_fixed += increment_fixed;
623
624 num_samples--; done++;
625 }
626 } else if (rate > 1.0 || this->curfrc) {
627 while(num_samples > 0) {
628 unsigned int current = (current_fixed>>32) & 0xffffffff;
629 unsigned int curfrc = current_fixed & 0xffffffff;
630
631 if(current >= samples.size() - filter_delay) break;
632
633 samp = ResampleLinMono::updownsample(&samples[0], current, curfrc);
634
635 // should never happen, but maybe because of rounding etc. it does:
636 if(samp > 32767.0) samp = 32767.0;
637 else if (samp < -32768.0) samp = -32768.0;
638
639 *dst = (short)samp; dst++;
640
641 current_fixed += increment_fixed;
642
643 num_samples--; done++;
644 }
645 } else {
646 /* rate == 1.0 && curfrc == 0 */
647 /* just copy */
648 unsigned int current = this->current;
649 while(num_samples > 0) {
650 if(current >= samples.size() - filter_delay) break;
651
652 *dst = (short)samples[current]; dst++;
653
654 current++;
655
656 num_samples--; done++;
657 }
658 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL);
659 }
660
661 this->current = (current_fixed>>32) & 0xffffffff;
662 this->curfrc = current_fixed & 0xffffffff;
663
664 return done;
665 }
666
667 //--- ResampleLinStereo -----------------------------------------------------//
668
ResampleLinStereo()669 ResampleLinStereo::ResampleLinStereo()
670 {
671 filter_delay = FILTER_DELAY_LIN_STEREO;
672 current = 0;
673 }
674
ResampleLinStereo(bool do_pad,float max_rate)675 ResampleLinStereo::ResampleLinStereo(bool do_pad, float max_rate)
676 {
677 filter_delay = FILTER_DELAY_LIN_STEREO;
678 if(do_pad) {
679 pad_samples = (int)((float)(filter_delay/2) * max_rate + 0.5);
680 }
681 current = 0;
682 }
683
updownsample(float * src,unsigned int current,unsigned int curfrc)684 inline float ResampleLinStereo::updownsample(float *src, unsigned int current, unsigned int curfrc)
685 {
686 float samp;
687
688 samp = src[current*2] + (src[current*2+2] - src[current*2]) * (curfrc / 4294967296.0);
689
690 return samp;
691 }
692
resample(signed short * dst,float rate,unsigned num_samples)693 int ResampleLinStereo::resample(signed short *dst, float rate, unsigned num_samples)
694 {
695 float samp_l;
696 float samp_r;
697
698 unsigned long long current_fixed;
699 long long increment_fixed;
700 int done = 0;
701
702 increment_fixed = (1.0/rate) * 4294967296.0;
703 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL) | (curfrc & 0xffffffff);
704
705 if(pad_samples) {
706 if(num_samples < pad_samples) {
707 pad_samples = 0;
708 return 0;
709 }
710 for(unsigned i=0; i<pad_samples; i++) {
711 *dst = 0; dst++;
712 *dst = 0; dst++;
713 }
714 done += pad_samples;
715 num_samples -= pad_samples;
716 pad_samples = 0;
717 }
718
719 if(rate < 1.0) { // downsampling, need to scale sinc apropriatly
720 while(num_samples > 0) {
721 unsigned int current = (current_fixed>>32) & 0xffffffff;
722 unsigned int curfrc = current_fixed & 0xffffffff;
723
724 if((current*2) >= samples.size() - filter_delay - 1) break;
725
726 samp_l = ResampleLinStereo::updownsample(&samples[0], current, curfrc);
727 samp_r = ResampleLinStereo::updownsample(&samples[1], current, curfrc);
728
729 // should never happen, but maybe because of rounding etc. it does:
730 if(samp_l > 32767.0) samp_l = 32767.0;
731 else if (samp_l < -32768.0) samp_l = -32768.0;
732 if(samp_r > 32767.0) samp_r = 32767.0;
733 else if (samp_r < -32768.0) samp_r = -32768.0;
734
735 *dst = (short)samp_l; dst++;
736 *dst = (short)samp_r; dst++;
737
738 current_fixed += increment_fixed;
739
740 num_samples--; done++;
741 }
742 } else if (rate > 1.0 || this->curfrc) {
743 while(num_samples > 0) {
744 unsigned int current = (current_fixed>>32) & 0xffffffff;
745 unsigned int curfrc = current_fixed & 0xffffffff;
746
747 if((current*2) >= samples.size() - filter_delay - 1) break;
748
749 samp_l = ResampleLinStereo::updownsample(&samples[0], current, curfrc);
750 samp_r = ResampleLinStereo::updownsample(&samples[1], current, curfrc);
751
752 // should never happen, but maybe because of rounding etc. it does:
753 if(samp_l > 32767.0) samp_l = 32767.0;
754 else if (samp_l < -32768.0) samp_l = -32768.0;
755 if(samp_r > 32767.0) samp_r = 32767.0;
756 else if (samp_r < -32768.0) samp_r = -32768.0;
757
758 *dst = (short)samp_l; dst++;
759 *dst = (short)samp_r; dst++;
760
761 current_fixed += increment_fixed;
762
763 num_samples--; done++;
764 }
765 } else {
766 /* rate == 1.0 && curfrc == 0 */
767 /* just copy */
768 unsigned int current = this->current;
769 while(num_samples > 0) {
770 if((current*2) >= samples.size() - filter_delay - 1) break;
771
772 *dst = (short)samples[current*2]; dst++;
773 *dst = (short)samples[current*2+1]; dst++;
774
775 current++;
776
777 num_samples--; done++;
778 }
779 current_fixed = (((unsigned long long)current<<32)&0xFFFFFFFF00000000LL);
780 }
781
782 this->current = (current_fixed>>32) & 0xffffffff;
783 this->curfrc = current_fixed & 0xffffffff;
784
785 return done;
786 }
787
createResampleObj(bool doPad,float maxRatio,interpolType interpol_type,sampleType sample_type)788 Resample* ResampleFactory::createResampleObj(bool doPad, float maxRatio, interpolType interpol_type, sampleType sample_type)
789 {
790 if(interpol_type == INTERPOL_LINEAR) {
791 if(sample_type == SAMPLE_MONO) {
792 return new ResampleLinMono(doPad, maxRatio);
793 } else {
794 return new ResampleLinStereo(doPad, maxRatio);
795 }
796 } else {
797 if(sample_type == SAMPLE_MONO) {
798 return new ResampleSincMono(doPad, maxRatio);
799 } else {
800 return new ResampleSincStereo(doPad, maxRatio);
801 }
802 }
803 }
804
805