Skip to content

Instantly share code, notes, and snippets.

@Xenakios
Created October 14, 2018 18:45
Show Gist options
  • Save Xenakios/4628b5733ab06e6700ddaad819a62d7a to your computer and use it in GitHub Desktop.
Save Xenakios/4628b5733ab06e6700ddaad819a62d7a to your computer and use it in GitHub Desktop.
class smbPitchShifter
{
public:
smbPitchShifter() {}
void prepareToPlay()
{
memset(gInFIFO, 0, MAX_FRAME_LENGTH * sizeof(float));
memset(gOutFIFO, 0, MAX_FRAME_LENGTH * sizeof(float));
memset(gFFTworksp, 0, 2 * MAX_FRAME_LENGTH * sizeof(float));
memset(gLastPhase, 0, (MAX_FRAME_LENGTH / 2 + 1) * sizeof(float));
memset(gSumPhase, 0, (MAX_FRAME_LENGTH / 2 + 1) * sizeof(float));
memset(gOutputAccum, 0, 2 * MAX_FRAME_LENGTH * sizeof(float));
memset(gAnaFreq, 0, MAX_FRAME_LENGTH * sizeof(float));
memset(gAnaMagn, 0, MAX_FRAME_LENGTH * sizeof(float));
}
// Note that numSampsToProcess has to be power-of-2, it seems... :-(
void PitchShift(float pitchShift, long numSampsToProcess, long fftFrameSize, long osamp, float sampleRate, float *indata, float *outdata)
/*
Routine smbPitchShift(). See top of file for explanation
Purpose: doing pitch shifting while maintaining duration using the Short
Time Fourier Transform.
Author: (c)1999-2015 Stephan M. Bernsee <s.bernsee [AT] zynaptiq [DOT] com>
*/
{
double magn, phase, tmp, window, real, imag;
double freqPerBin, expct;
long i, k, qpd, index, inFifoLatency, stepSize, fftFrameSize2;
/* set up some handy variables */
fftFrameSize2 = fftFrameSize / 2;
stepSize = fftFrameSize / osamp;
freqPerBin = sampleRate / (double)fftFrameSize;
expct = 2.*M_PI*(double)stepSize / (double)fftFrameSize;
inFifoLatency = fftFrameSize - stepSize;
if (gRover == false) gRover = inFifoLatency;
/* main processing loop */
for (i = 0; i < numSampsToProcess; i++) {
/* As long as we have not yet collected enough data just read in */
gInFIFO[gRover] = indata[i];
outdata[i] = gOutFIFO[gRover - inFifoLatency];
gRover++;
/* now we have enough data for processing */
if (gRover >= fftFrameSize) {
gRover = inFifoLatency;
/* do windowing and re,im interleave */
for (k = 0; k < fftFrameSize; k++) {
window = -.5*cos(2.*M_PI*(double)k / (double)fftFrameSize) + .5;
gFFTworksp[2 * k] = gInFIFO[k] * window;
gFFTworksp[2 * k + 1] = 0.;
}
/* ***************** ANALYSIS ******************* */
/* do transform */
smbFft(gFFTworksp, fftFrameSize, -1);
/* this is the analysis step */
for (k = 0; k <= fftFrameSize2; k++) {
/* de-interlace FFT buffer */
real = gFFTworksp[2 * k];
imag = gFFTworksp[2 * k + 1];
/* compute magnitude and phase */
magn = 2.*sqrt(real*real + imag * imag);
phase = smbAtan2(imag, real);
/* compute phase difference */
tmp = phase - gLastPhase[k];
gLastPhase[k] = phase;
/* subtract expected phase difference */
tmp -= (double)k*expct;
/* map delta phase into +/- Pi interval */
qpd = tmp / M_PI;
if (qpd >= 0) qpd += qpd & 1;
else qpd -= qpd & 1;
tmp -= M_PI * (double)qpd;
/* get deviation from bin frequency from the +/- Pi interval */
tmp = osamp * tmp / (2.*M_PI);
/* compute the k-th partials' true frequency */
tmp = (double)k*freqPerBin + tmp * freqPerBin;
/* store magnitude and true frequency in analysis arrays */
gAnaMagn[k] = magn;
gAnaFreq[k] = tmp;
}
/* ***************** PROCESSING ******************* */
/* this does the actual pitch shifting */
memset(gSynMagn, 0, fftFrameSize * sizeof(float));
memset(gSynFreq, 0, fftFrameSize * sizeof(float));
for (k = 0; k <= fftFrameSize2; k++) {
index = k * pitchShift;
if (index <= fftFrameSize2) {
gSynMagn[index] += gAnaMagn[k];
gSynFreq[index] = gAnaFreq[k] * pitchShift;
}
}
/* ***************** SYNTHESIS ******************* */
/* this is the synthesis step */
for (k = 0; k <= fftFrameSize2; k++) {
/* get magnitude and true frequency from synthesis arrays */
magn = gSynMagn[k];
tmp = gSynFreq[k];
/* subtract bin mid frequency */
tmp -= (double)k*freqPerBin;
/* get bin deviation from freq deviation */
tmp /= freqPerBin;
/* take osamp into account */
tmp = 2.*M_PI*tmp / osamp;
/* add the overlap phase advance back in */
tmp += (double)k*expct;
/* accumulate delta phase to get bin phase */
gSumPhase[k] += tmp;
phase = gSumPhase[k];
/* get real and imag part and re-interleave */
gFFTworksp[2 * k] = magn * cos(phase);
gFFTworksp[2 * k + 1] = magn * sin(phase);
}
/* zero negative frequencies */
for (k = fftFrameSize + 2; k < 2 * fftFrameSize; k++) gFFTworksp[k] = 0.;
/* do inverse transform */
smbFft(gFFTworksp, fftFrameSize, 1);
/* do windowing and add to output accumulator */
for (k = 0; k < fftFrameSize; k++) {
window = -.5*cos(2.*M_PI*(double)k / (double)fftFrameSize) + .5;
gOutputAccum[k] += 2.*window*gFFTworksp[2 * k] / (fftFrameSize2*osamp);
}
for (k = 0; k < stepSize; k++) gOutFIFO[k] = gOutputAccum[k];
/* shift accumulator */
memmove(gOutputAccum, gOutputAccum + stepSize, fftFrameSize * sizeof(float));
/* move input FIFO */
for (k = 0; k < inFifoLatency; k++) gInFIFO[k] = gInFIFO[k + stepSize];
}
}
}
private:
float gInFIFO[MAX_FRAME_LENGTH];
float gOutFIFO[MAX_FRAME_LENGTH];
float gFFTworksp[2 * MAX_FRAME_LENGTH];
float gLastPhase[MAX_FRAME_LENGTH / 2 + 1];
float gSumPhase[MAX_FRAME_LENGTH / 2 + 1];
float gOutputAccum[2 * MAX_FRAME_LENGTH];
float gAnaFreq[MAX_FRAME_LENGTH];
float gAnaMagn[MAX_FRAME_LENGTH];
float gSynFreq[MAX_FRAME_LENGTH];
float gSynMagn[MAX_FRAME_LENGTH];
long gRover = false;
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment