1 /***************************************************************************
2 * Copyright 1991, 1992, 1993, 1994, 1995, 1996, 2001, 2002 *
3 * David R. Hill, Leonard Manzara, Craig Schock *
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 // 2014-09
19 // This file was copied from Gnuspeech and modified by Marcelo Y. Matuda.
20
21 #include "SampleRateConverter.h"
22
23 #include <cmath>
24
25 #define BETA 5.658 /* kaiser window parameters */
26 #define IzeroEPSILON 1E-21
27
28 /* SAMPLE RATE CONVERSION CONSTANTS */
29 #define ZERO_CROSSINGS 13 /* SRC CUTOFF FRQ */
30 #define LP_CUTOFF (11.0/13.0) /* (0.846 OF NYQUIST) */
31 #define FILTER_LENGTH (ZERO_CROSSINGS * L_RANGE)
32
33 //#define N_BITS 16
34 #define L_BITS 8
35 #define L_RANGE 256 /* must be 2^L_BITS */
36 #define M_BITS 8
37 #define M_RANGE 256 /* must be 2^M_BITS */
38 #define FRACTION_BITS (L_BITS + M_BITS)
39 #define FRACTION_RANGE 65536 /* must be 2^FRACTION_BITS */
40 #define FILTER_LIMIT (FILTER_LENGTH - 1)
41
42 #define N_MASK 0xFFFF0000
43 #define L_MASK 0x0000FF00
44 #define M_MASK 0x000000FF
45 #define FRACTION_MASK 0x0000FFFF
46
47 #define nValue(x) (((x) & N_MASK) >> FRACTION_BITS)
48 #define lValue(x) (((x) & L_MASK) >> M_BITS)
49 #define mValue(x) ((x) & M_MASK)
50 #define fractionValue(x) ((x) & FRACTION_MASK)
51
52 #define BUFFER_SIZE 1024 /* ring buffer size */
53
54
55
56 namespace GS {
57 namespace TRM {
58
SampleRateConverter(int sampleRate,float outputRate,std::vector<float> & outputData)59 SampleRateConverter::SampleRateConverter(int sampleRate, float outputRate, std::vector<float>& outputData)
60 : sampleRateRatio_(0.0)
61 , fillPtr_(0)
62 , emptyPtr_(0)
63 , padSize_(0)
64 , fillSize_(0)
65 , timeRegisterIncrement_(0)
66 , filterIncrement_(0)
67 , phaseIncrement_(0)
68 , timeRegister_(0)
69 , fillCounter_(0)
70 , maximumSampleValue_(0.0)
71 , numberSamples_(0)
72 , h_(FILTER_LENGTH)
73 , deltaH_(FILTER_LENGTH)
74 , buffer_(BUFFER_SIZE)
75 , outputData_(outputData)
76 {
77 initializeConversion(sampleRate, outputRate);
78 }
79
~SampleRateConverter()80 SampleRateConverter::~SampleRateConverter()
81 {
82 }
83
84 void
reset()85 SampleRateConverter::reset()
86 {
87 emptyPtr_ = 0;
88 timeRegister_ = 0;
89 fillCounter_ = 0;
90 maximumSampleValue_ = 0.0;
91 numberSamples_ = 0;
92 initializeBuffer();
93 }
94
95 /******************************************************************************
96 *
97 * function: initializeConversion
98 *
99 * purpose: Initializes all the sample rate conversion functions.
100 *
101 ******************************************************************************/
102 void
initializeConversion(int sampleRate,float outputRate)103 SampleRateConverter::initializeConversion(int sampleRate, float outputRate)
104 {
105 /* INITIALIZE FILTER IMPULSE RESPONSE */
106 initializeFilter();
107
108 /* CALCULATE SAMPLE RATE RATIO */
109 sampleRateRatio_ = (double) outputRate / (double) sampleRate;
110
111 /* CALCULATE TIME REGISTER INCREMENT */
112 timeRegisterIncrement_ =
113 (int) rint(pow(2.0, FRACTION_BITS) / sampleRateRatio_);
114
115 /* CALCULATE ROUNDED SAMPLE RATE RATIO */
116 double roundedSampleRateRatio =
117 pow(2.0, FRACTION_BITS) / (double) timeRegisterIncrement_;
118
119 /* CALCULATE PHASE OR FILTER INCREMENT */
120 if (sampleRateRatio_ >= 1.0) {
121 filterIncrement_ = L_RANGE;
122 } else {
123 phaseIncrement_ = (unsigned int) rint(sampleRateRatio_ * (double) FRACTION_RANGE);
124 }
125
126 /* CALCULATE PAD SIZE */
127 padSize_ = (sampleRateRatio_ >= 1.0) ?
128 ZERO_CROSSINGS :
129 (int) ((float) ZERO_CROSSINGS / roundedSampleRateRatio) + 1;
130
131 /* INITIALIZE THE RING BUFFER */
132 initializeBuffer();
133 }
134
135 /******************************************************************************
136 *
137 * function: Izero
138 *
139 * purpose: Returns the value for the modified Bessel function of
140 * the first kind, order 0, as a double.
141 *
142 ******************************************************************************/
143 double
Izero(double x)144 SampleRateConverter::Izero(double x)
145 {
146 double sum, u, halfx, temp;
147 int n;
148
149 sum = u = n = 1;
150 halfx = x / 2.0;
151
152 do {
153 temp = halfx / (double) n;
154 n += 1;
155 temp *= temp;
156 u *= temp;
157 sum += u;
158 } while (u >= (IzeroEPSILON * sum));
159
160 return sum;
161 }
162
163 /******************************************************************************
164 *
165 * function: initializeBuffer
166 *
167 * purpose: Initializes the ring buffer used for sample rate
168 * conversion.
169 *
170 ******************************************************************************/
171 void
initializeBuffer()172 SampleRateConverter::initializeBuffer()
173 {
174 /* FILL THE RING BUFFER WITH ALL ZEROS */
175 for (int i = 0; i < BUFFER_SIZE; i++) {
176 buffer_[i] = 0.0;
177 }
178
179 /* INITIALIZE FILL POINTER */
180 fillPtr_ = padSize_;
181
182 /* CALCULATE FILL SIZE */
183 fillSize_ = BUFFER_SIZE - (2 * padSize_);
184 }
185
186 /******************************************************************************
187 *
188 * function: initializeFilter
189 *
190 * purpose: Initializes filter impulse response and impulse delta
191 * values.
192 *
193 ******************************************************************************/
194 void
initializeFilter()195 SampleRateConverter::initializeFilter()
196 {
197 /* INITIALIZE THE FILTER IMPULSE RESPONSE */
198 h_[0] = LP_CUTOFF;
199 double x = M_PI / (double) L_RANGE;
200 for (int i = 1; i < FILTER_LENGTH; i++) {
201 double y = (double) i * x;
202 h_[i] = sin(y * LP_CUTOFF) / y;
203 }
204
205 /* APPLY A KAISER WINDOW TO THE IMPULSE RESPONSE */
206 double IBeta = 1.0 / Izero(BETA);
207 for (int i = 0; i < FILTER_LENGTH; i++) {
208 double temp = (double) i / FILTER_LENGTH;
209 h_[i] *= Izero(BETA * sqrt(1.0 - (temp * temp))) * IBeta;
210 }
211
212 /* INITIALIZE THE FILTER IMPULSE RESPONSE DELTA VALUES */
213 for (int i = 0; i < FILTER_LIMIT; i++) {
214 deltaH_[i] = h_[i + 1] - h_[i];
215 }
216 deltaH_[FILTER_LIMIT] = 0.0 - h_[FILTER_LIMIT];
217 }
218
219 /******************************************************************************
220 *
221 * function: dataFill
222 *
223 * purpose: Fills the ring buffer with a single sample, increments
224 * the counters and pointers, and empties the buffer when
225 * full.
226 *
227 ******************************************************************************/
228 void
dataFill(double data)229 SampleRateConverter::dataFill(double data)
230 {
231 /* PUT THE DATA INTO THE RING BUFFER */
232 buffer_[fillPtr_] = data;
233
234 /* INCREMENT THE FILL POINTER, MODULO THE BUFFER SIZE */
235 srIncrement(&fillPtr_, BUFFER_SIZE);
236
237 /* INCREMENT THE COUNTER, AND EMPTY THE BUFFER IF FULL */
238 if (++fillCounter_ >= fillSize_) {
239 dataEmpty();
240 /* RESET THE FILL COUNTER */
241 fillCounter_ = 0;
242 }
243 }
244
245 /******************************************************************************
246 *
247 * function: dataEmpty
248 *
249 * purpose: Converts available portion of the input signal to the
250 * new sampling rate, and outputs the samples to the
251 * sound struct.
252 *
253 ******************************************************************************/
254 void
dataEmpty()255 SampleRateConverter::dataEmpty()
256 {
257 /* CALCULATE END POINTER */
258 int endPtr = fillPtr_ - padSize_;
259
260 /* ADJUST THE END POINTER, IF LESS THAN ZERO */
261 if (endPtr < 0) {
262 endPtr += BUFFER_SIZE;
263 }
264
265 /* ADJUST THE ENDPOINT, IF LESS THEN THE EMPTY POINTER */
266 if (endPtr < emptyPtr_) {
267 endPtr += BUFFER_SIZE;
268 }
269
270 /* UPSAMPLE LOOP (SLIGHTLY MORE EFFICIENT THAN DOWNSAMPLING) */
271 if (sampleRateRatio_ >= 1.0) {
272 while (emptyPtr_ < endPtr) {
273 /* RESET ACCUMULATOR TO ZERO */
274 double output = 0.0;
275
276 /* CALCULATE INTERPOLATION VALUE (STATIC WHEN UPSAMPLING) */
277 double interpolation = (double) mValue(timeRegister_) / (double) M_RANGE;
278
279 /* COMPUTE THE LEFT SIDE OF THE FILTER CONVOLUTION */
280 int index = emptyPtr_;
281 for (int filterIndex = lValue(timeRegister_);
282 filterIndex < FILTER_LENGTH;
283 srDecrement(&index,BUFFER_SIZE), filterIndex += filterIncrement_) {
284 output += (buffer_[index] *
285 (h_[filterIndex] + (deltaH_[filterIndex] * interpolation)));
286 }
287
288 /* ADJUST VALUES FOR RIGHT SIDE CALCULATION */
289 timeRegister_ = ~timeRegister_;
290 interpolation = (double) mValue(timeRegister_) / (double) M_RANGE;
291
292 /* COMPUTE THE RIGHT SIDE OF THE FILTER CONVOLUTION */
293 index = emptyPtr_;
294 srIncrement(&index,BUFFER_SIZE);
295 for (int filterIndex = lValue(timeRegister_);
296 filterIndex < FILTER_LENGTH;
297 srIncrement(&index,BUFFER_SIZE), filterIndex += filterIncrement_) {
298 output += (buffer_[index] *
299 (h_[filterIndex] + (deltaH_[filterIndex] * interpolation)));
300 }
301
302 /* RECORD MAXIMUM SAMPLE VALUE */
303 double absoluteSampleValue = fabs(output);
304 if (absoluteSampleValue > maximumSampleValue_) {
305 maximumSampleValue_ = absoluteSampleValue;
306 }
307
308 /* INCREMENT SAMPLE NUMBER */
309 numberSamples_++;
310
311 /* SAVE THE SAMPLE */
312 outputData_.push_back(static_cast<float>(output));
313
314 /* CHANGE TIME REGISTER BACK TO ORIGINAL FORM */
315 timeRegister_ = ~timeRegister_;
316
317 /* INCREMENT THE TIME REGISTER */
318 timeRegister_ += timeRegisterIncrement_;
319
320 /* INCREMENT THE EMPTY POINTER, ADJUSTING IT AND END POINTER */
321 emptyPtr_ += nValue(timeRegister_);
322
323 if (emptyPtr_ >= BUFFER_SIZE) {
324 emptyPtr_ -= BUFFER_SIZE;
325 endPtr -= BUFFER_SIZE;
326 }
327
328 /* CLEAR N PART OF TIME REGISTER */
329 timeRegister_ &= (~N_MASK);
330 }
331 } else {
332 /* DOWNSAMPLING CONVERSION LOOP */
333
334 while (emptyPtr_ < endPtr) {
335
336 /* RESET ACCUMULATOR TO ZERO */
337 double output = 0.0;
338
339 /* COMPUTE P PRIME */
340 unsigned int phaseIndex = (unsigned int) rint(
341 ((double) fractionValue(timeRegister_)) * sampleRateRatio_);
342
343 /* COMPUTE THE LEFT SIDE OF THE FILTER CONVOLUTION */
344 int index = emptyPtr_;
345 unsigned int impulseIndex;
346 while ((impulseIndex = (phaseIndex >> M_BITS)) < FILTER_LENGTH) {
347 double impulse = h_[impulseIndex] + (deltaH_[impulseIndex] *
348 (((double) mValue(phaseIndex)) / (double) M_RANGE));
349 output += (buffer_[index] * impulse);
350 srDecrement(&index, BUFFER_SIZE);
351 phaseIndex += phaseIncrement_;
352 }
353
354 /* COMPUTE P PRIME, ADJUSTED FOR RIGHT SIDE */
355 phaseIndex = (unsigned int) rint(
356 ((double) fractionValue(~timeRegister_)) * sampleRateRatio_);
357
358 /* COMPUTE THE RIGHT SIDE OF THE FILTER CONVOLUTION */
359 index = emptyPtr_;
360 srIncrement(&index, BUFFER_SIZE);
361 while ((impulseIndex = (phaseIndex >> M_BITS)) < FILTER_LENGTH) {
362 double impulse = h_[impulseIndex] + (deltaH_[impulseIndex] *
363 (((double) mValue(phaseIndex)) / (double) M_RANGE));
364 output += (buffer_[index] * impulse);
365 srIncrement(&index, BUFFER_SIZE);
366 phaseIndex += phaseIncrement_;
367 }
368
369 /* RECORD MAXIMUM SAMPLE VALUE */
370 double absoluteSampleValue = fabs(output);
371 if (absoluteSampleValue > maximumSampleValue_) {
372 maximumSampleValue_ = absoluteSampleValue;
373 }
374
375 /* INCREMENT SAMPLE NUMBER */
376 numberSamples_++;
377
378 /* SAVE THE SAMPLE */
379 outputData_.push_back(static_cast<float>(output));
380
381 /* INCREMENT THE TIME REGISTER */
382 timeRegister_ += timeRegisterIncrement_;
383
384 /* INCREMENT THE EMPTY POINTER, ADJUSTING IT AND END POINTER */
385 emptyPtr_ += nValue(timeRegister_);
386 if (emptyPtr_ >= BUFFER_SIZE) {
387 emptyPtr_ -= BUFFER_SIZE;
388 endPtr -= BUFFER_SIZE;
389 }
390
391 /* CLEAR N PART OF TIME REGISTER */
392 timeRegister_ &= (~N_MASK);
393 }
394 }
395 }
396
397 /******************************************************************************
398 *
399 * function: srIncrement
400 *
401 * purpose: Increments the pointer, keeping it within the range
402 * 0 to (modulus-1).
403 *
404 ******************************************************************************/
405 void
srIncrement(int * pointer,int modulus)406 SampleRateConverter::srIncrement(int *pointer, int modulus)
407 {
408 if (++(*pointer) >= modulus) {
409 (*pointer) -= modulus;
410 }
411 }
412
413 /******************************************************************************
414 *
415 * function: srDecrement
416 *
417 * purpose: Decrements the pointer, keeping it within the range
418 * 0 to (modulus-1).
419 *
420 ******************************************************************************/
421 void
srDecrement(int * pointer,int modulus)422 SampleRateConverter::srDecrement(int *pointer, int modulus)
423 {
424 if (--(*pointer) < 0) {
425 (*pointer) += modulus;
426 }
427 }
428
429 /******************************************************************************
430 *
431 * function: flushBuffer
432 *
433 * purpose: Pads the buffer with zero samples, and flushes it by
434 * converting the remaining samples.
435 *
436 ******************************************************************************/
437 void
flushBuffer()438 SampleRateConverter::flushBuffer()
439 {
440 /* PAD END OF RING BUFFER WITH ZEROS */
441 for (int i = 0; i < padSize_ * 2; i++) {
442 dataFill(0.0);
443 }
444
445 /* FLUSH UP TO FILL POINTER - PADSIZE */
446 dataEmpty();
447 }
448
449 } /* namespace TRM */
450 } /* namespace GS */
451