comparison dsp/rateconversion/Resampler.cpp @ 505:930b5b0f707d

Merge branch 'codestyle-and-tidy'
author Chris Cannam <cannam@all-day-breakfast.com>
date Wed, 05 Jun 2019 12:55:15 +0100
parents 8b92623e81c9
children
comparison
equal deleted inserted replaced
471:e3335cb213da 505:930b5b0f707d
15 #include "Resampler.h" 15 #include "Resampler.h"
16 16
17 #include "maths/MathUtilities.h" 17 #include "maths/MathUtilities.h"
18 #include "base/KaiserWindow.h" 18 #include "base/KaiserWindow.h"
19 #include "base/SincWindow.h" 19 #include "base/SincWindow.h"
20 #include "base/Restrict.h"
20 21
21 #include <iostream> 22 #include <iostream>
22 #include <vector> 23 #include <vector>
23 #include <map> 24 #include <map>
24 #include <cassert> 25 #include <cassert>
68 // antialiasing filter, should be slightly below nyquist 69 // antialiasing filter, should be slightly below nyquist
69 m_peakToPole = m_peakToPole / (1.0 - bandwidth/2.0); 70 m_peakToPole = m_peakToPole / (1.0 - bandwidth/2.0);
70 } 71 }
71 72
72 KaiserWindow::Parameters params = 73 KaiserWindow::Parameters params =
73 KaiserWindow::parametersForBandwidth(snr, bandwidth, higher / m_gcd); 74 KaiserWindow::parametersForBandwidth(snr, bandwidth, higher / m_gcd);
74 75
75 params.length = 76 params.length =
76 (params.length % 2 == 0 ? params.length + 1 : params.length); 77 (params.length % 2 == 0 ? params.length + 1 : params.length);
77 78
78 params.length = 79 params.length =
79 (params.length > 200001 ? 200001 : params.length); 80 (params.length > 200001 ? 200001 : params.length);
80 81
81 m_filterLength = params.length; 82 m_filterLength = params.length;
172 173
173 m_phaseData = new Phase[inputSpacing]; 174 m_phaseData = new Phase[inputSpacing];
174 175
175 for (int phase = 0; phase < inputSpacing; ++phase) { 176 for (int phase = 0; phase < inputSpacing; ++phase) {
176 177
177 Phase p; 178 Phase p;
178 179
179 p.nextPhase = phase - outputSpacing; 180 p.nextPhase = phase - outputSpacing;
180 while (p.nextPhase < 0) p.nextPhase += inputSpacing; 181 while (p.nextPhase < 0) p.nextPhase += inputSpacing;
181 p.nextPhase %= inputSpacing; 182 p.nextPhase %= inputSpacing;
182 183
183 p.drop = int(ceil(std::max(0.0, double(outputSpacing - phase)) 184 p.drop = int(ceil(std::max(0.0, double(outputSpacing - phase))
184 / inputSpacing)); 185 / inputSpacing));
185 186
186 int filtZipLength = int(ceil(double(m_filterLength - phase) 187 int filtZipLength = int(ceil(double(m_filterLength - phase)
187 / inputSpacing)); 188 / inputSpacing));
188 189
189 for (int i = 0; i < filtZipLength; ++i) { 190 for (int i = 0; i < filtZipLength; ++i) {
190 p.filter.push_back(filter[i * inputSpacing + phase]); 191 p.filter.push_back(filter[i * inputSpacing + phase]);
191 } 192 }
192 193
193 m_phaseData[phase] = p; 194 m_phaseData[phase] = p;
194 } 195 }
195 196
196 #ifdef DEBUG_RESAMPLER 197 #ifdef DEBUG_RESAMPLER
197 int cp = 0; 198 int cp = 0;
198 int totDrop = 0; 199 int totDrop = 0;
266 m_buffer = vector<double>(fill, 0); 267 m_buffer = vector<double>(fill, 0);
267 m_bufferOrigin = 0; 268 m_bufferOrigin = 0;
268 269
269 #ifdef DEBUG_RESAMPLER 270 #ifdef DEBUG_RESAMPLER
270 cerr << "initial phase " << m_phase << " (as " << (m_filterLength/2) << " % " << inputSpacing << ")" 271 cerr << "initial phase " << m_phase << " (as " << (m_filterLength/2) << " % " << inputSpacing << ")"
271 << ", latency " << m_latency << endl; 272 << ", latency " << m_latency << endl;
272 #endif 273 #endif
273 } 274 }
274 275
275 double 276 double
276 Resampler::reconstructOne() 277 Resampler::reconstructOne()
283 cerr << "ERROR: n + m_bufferOrigin > m_buffer.size() [" << n << " + " 284 cerr << "ERROR: n + m_bufferOrigin > m_buffer.size() [" << n << " + "
284 << m_bufferOrigin << " > " << m_buffer.size() << "]" << endl; 285 << m_bufferOrigin << " > " << m_buffer.size() << "]" << endl;
285 throw std::logic_error("n + m_bufferOrigin > m_buffer.size()"); 286 throw std::logic_error("n + m_bufferOrigin > m_buffer.size()");
286 } 287 }
287 288
288 #if defined(__MSVC__) 289 const double *const QM_R__ buf(m_buffer.data() + m_bufferOrigin);
289 #define R__ __restrict 290 const double *const QM_R__ filt(pd.filter.data());
290 #elif defined(__GNUC__)
291 #define R__ __restrict__
292 #else
293 #define R__
294 #endif
295
296 const double *const R__ buf(m_buffer.data() + m_bufferOrigin);
297 const double *const R__ filt(pd.filter.data());
298 291
299 for (int i = 0; i < n; ++i) { 292 for (int i = 0; i < n; ++i) {
300 // NB gcc can only vectorize this with -ffast-math 293 // NB gcc can only vectorize this with -ffast-math
301 v += buf[i] * filt[i]; 294 v += buf[i] * filt[i];
302 } 295 }
303 296
304 m_bufferOrigin += pd.drop; 297 m_bufferOrigin += pd.drop;
305 m_phase = pd.nextPhase; 298 m_phase = pd.nextPhase;
306 return v; 299 return v;
319 #endif 312 #endif
320 313
321 double scaleFactor = (double(m_targetRate) / m_gcd) / m_peakToPole; 314 double scaleFactor = (double(m_targetRate) / m_gcd) / m_peakToPole;
322 315
323 while (outidx < maxout && 316 while (outidx < maxout &&
324 m_buffer.size() >= m_phaseData[m_phase].filter.size() + m_bufferOrigin) { 317 m_buffer.size() >= m_phaseData[m_phase].filter.size() + m_bufferOrigin) {
325 dst[outidx] = scaleFactor * reconstructOne(); 318 dst[outidx] = scaleFactor * reconstructOne();
326 outidx++; 319 outidx++;
327 } 320 }
328 321
329 if (m_bufferOrigin > (int)m_buffer.size()) { 322 if (m_bufferOrigin > (int)m_buffer.size()) {
330 cerr << "ERROR: m_bufferOrigin > m_buffer.size() [" 323 cerr << "ERROR: m_bufferOrigin > m_buffer.size() ["
331 << m_bufferOrigin << " > " << m_buffer.size() << "]" << endl; 324 << m_bufferOrigin << " > " << m_buffer.size() << "]" << endl;
390 #endif 383 #endif
391 #ifdef DEBUG_RESAMPLER_VERBOSE 384 #ifdef DEBUG_RESAMPLER_VERBOSE
392 int printN = 50; 385 int printN = 50;
393 cerr << "first " << printN << " in:" << endl; 386 cerr << "first " << printN << " in:" << endl;
394 for (int i = 0; i < printN && i < n; ++i) { 387 for (int i = 0; i < printN && i < n; ++i) {
395 if (i % 5 == 0) cerr << endl << i << "... "; 388 if (i % 5 == 0) cerr << endl << i << "... ";
396 cerr << data[i] << " "; 389 cerr << data[i] << " ";
397 } 390 }
398 cerr << endl; 391 cerr << endl;
399 #endif 392 #endif
400 393
401 int toReturn = got - latency; 394 int toReturn = got - latency;
402 if (toReturn > m) toReturn = m; 395 if (toReturn > m) toReturn = m;
403 396
404 vector<double> sliced(out.begin() + latency, 397 vector<double> sliced(out.begin() + latency,
405 out.begin() + latency + toReturn); 398 out.begin() + latency + toReturn);
406 399
407 #ifdef DEBUG_RESAMPLER_VERBOSE 400 #ifdef DEBUG_RESAMPLER_VERBOSE
408 cerr << "first " << printN << " out (after latency compensation), length " << sliced.size() << ":"; 401 cerr << "first " << printN << " out (after latency compensation), length " << sliced.size() << ":";
409 for (int i = 0; i < printN && i < sliced.size(); ++i) { 402 for (int i = 0; i < printN && i < sliced.size(); ++i) {
410 if (i % 5 == 0) cerr << endl << i << "... "; 403 if (i % 5 == 0) cerr << endl << i << "... ";
411 cerr << sliced[i] << " "; 404 cerr << sliced[i] << " ";
412 } 405 }
413 cerr << endl; 406 cerr << endl;
414 #endif 407 #endif
415 408
416 return sliced; 409 return sliced;