Mercurial > hg > qm-dsp
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; |