PhaseVocoder.cpp

Go to the documentation of this file.

namespace krotos
{
    PhaseVocoder::PhaseVocoder(int fftSize, int hopSize, int inputBufferSize, WindowType windowFunction)
        : m_inputBufferSize(inputBufferSize), m_outputBufferSize(inputBufferSize), m_hopSizeAnalysis(hopSize),
          m_fft(static_cast<int>(log2f(static_cast<float>(fftSize))))
    {
        m_fftSize = fftSize;
        // set synthesis hopSize from timeStrechFactor
        m_hopSizeSynthesis = static_cast<int>((float)m_hopSizeAnalysis * m_timeStretchRatio);
 
        // set num channels to stereo.
        setNumChannels(NUM_CHANNELS_MAX);
 
        // intialise the windowing functions
        m_analysisWindow.resize(m_fftSize);
        m_synthesisWindow.resize(m_fftSize);
 
        // generate windows
        m_analysisWindow = WindowFunctions::generateWindow(m_fftSize, windowFunction);
        m_synthesisWindow = m_analysisWindow;
    }
 
    void PhaseVocoder::setWindowFunction(WindowType windowType)
    {
        m_windowType = windowType;
        m_analysisWindow = WindowFunctions::generateWindow(m_fftSize, windowType);
        m_synthesisWindow = m_analysisWindow;
    }
 
    void PhaseVocoder::setNumChannels(int numChannels)
    {
        // Set the size for the two circular buffers
        m_inputBuffer.resize(numChannels, std::vector<float>(m_inputBufferSize));
        m_outputBuffer.resize(numChannels, std::vector<float>(m_outputBufferSize));
 
        for (int i = 0; i < numChannels; i++)
        {
            // Start the write pointer ahead of the read pointer by at least window +
            // hop, with some margin
            m_outputWritePointer.push_back(m_fftSize + 4 * m_hopSizeAnalysis);
        }
 
        // For Pitch and Time Streching
        m_previousPhasesInput.resize(numChannels, std::vector<float>(m_fftSize));
        m_previousPhasesOutput.resize(numChannels, std::vector<float>(m_fftSize));
        m_magnitudesAnalysis.resize(numChannels, std::vector<float>(m_fftSize / 2 + 1));
        m_magnitudesSynthesis.resize(numChannels, std::vector<float>(m_fftSize / 2 + 1));
        m_binFrequenciesAnalysis.resize(numChannels, std::vector<float>(m_fftSize / 2 + 1));
        m_binFrequenciesSynthesis.resize(numChannels, std::vector<float>(m_fftSize / 2 + 1));
    }
 
    void PhaseVocoder::setGeneralParam(float paramValue)
    {
        // Robotizer: param is the hopSize
        if (m_mode == PhaseVocoderMode::Robotizer)
        {
            // map from 0..1 to 64...fftSize
            m_hopSizeAnalysis =
                    static_cast<int>(jmap<float>(paramValue, 0.f, 1.f, 64.f, static_cast<float>(m_fftSize)));
            m_hopSizeSynthesis = m_hopSizeAnalysis;
 
            int newWinLength = m_hopSizeAnalysis * m_overlapFactor;
            // clip to fftSize
            newWinLength = (newWinLength > m_fftSize) ? m_fftSize : newWinLength;
 
            // recaclulate window size to maintain COLA
            m_analysisWindow = WindowFunctions::generateWindow(m_fftSize, m_windowType);
            m_synthesisWindow = m_analysisWindow;
        }
        // Pitch Shifter: param is the pitch ratio
        else if (m_mode == PhaseVocoder::PitchShifter)
        {
            // set hopSize to an appropriate value, TODO: Investigate more
            m_hopSizeAnalysis = 128;
            m_hopSizeSynthesis = m_hopSizeAnalysis;
 
            // map from 0...1 to -2 ... 2
            m_pitchShiftRatio = (jmap<float>(paramValue, 0.f, 1.f, -12.0f, 12.0f));
 
            // convert to ratio
            m_pitchShiftRatio = pow(2.0f, m_pitchShiftRatio / 12.0f);
        }
        else if (m_mode == PhaseVocoder::TimeStrecher)
        {
            // map from 0...1 to -2 ... 2
            m_timeStretchRatio = jmap<float>(paramValue, 0.f, 1.f, 0.5f, 2.0f);
            // change synthesis hopSize according to ratio
            m_hopSizeSynthesis = static_cast<int>(float(m_hopSizeAnalysis) * m_timeStretchRatio);
        }
    }
 
    float PhaseVocoder::process(float sampleIn, int numChannel)
    {
        m_numChannel = numChannel;
 
        // store in inputBuffer and increment pointer, wrap around if necessary
        m_inputBuffer[m_numChannel][m_inputWritePointer[m_numChannel]++] = sampleIn;
        if (m_inputWritePointer[m_numChannel] >= m_inputBufferSize)
        {
            m_inputWritePointer[m_numChannel] = 0;
        }
 
        // read output sample from outputBuffer and clear it
        float sampleOut = m_outputBuffer[m_numChannel][m_outputReadPointer[m_numChannel]];
        m_outputBuffer[m_numChannel][m_outputReadPointer[m_numChannel]] = 0.0f;
 
        // scale output by overlap factor - TODO:: Investigate depening on window and
        // ovp sampleOut *= m_windowGainCorrection;
 
        // increment read pointer in output circular buffer and wrap around if
        // necessary
        m_outputReadPointer[m_numChannel]++;
        if (m_outputReadPointer[m_numChannel] >= m_outputBufferSize)
        {
            m_outputReadPointer[m_numChannel] = 0;
        }
 
        // increment the hop counter and start new FFT if we have reached the hopSize
        if (++m_hopCounter[m_numChannel] >= m_hopSizeAnalysis)
        {
            m_hopCounter[m_numChannel] = 0;
            processFrame(m_inputBuffer[m_numChannel], m_inputWritePointer[m_numChannel], m_outputBuffer[m_numChannel],
                         m_outputWritePointer[m_numChannel]);
 
            // Update the output buffer write pointer
            m_outputWritePointer[m_numChannel] =
                    (m_outputWritePointer[m_numChannel] + m_hopSizeSynthesis) % m_outputBufferSize;
        }
 
        return sampleOut;
    }
 
    void PhaseVocoder::processFrame(std::vector<float>& inBuffer, int inPointer, std::vector<float>& outBuffer,
                                    int outPointer)
    {
        // linear buffer to hold unwrapped values for fft output
        std::vector<float> unwrappedBuffer(2 * m_fftSize);
 
        // copy input circular buffer into the linear buffer
        for (int i = 0; i < m_fftSize; i++)
        {
            // unwrap using modulo operation
            int circBufferIndex = (inPointer + i - m_fftSize + m_inputBufferSize) % m_inputBufferSize;
            // copy and apply window
            unwrappedBuffer[i] = inBuffer[circBufferIndex];
        }
 
        // caclulate input frame energy
        float inEnergy = caclulateFrameRMS(unwrappedBuffer);
 
        // apply analysis window
        for (int i = 0; i < m_fftSize; i++)
        {
            // apply window
            unwrappedBuffer[i] *= m_analysisWindow[i];
        }
 
        // perform fft
        m_fft.performRealOnlyForwardTransform(unwrappedBuffer.data());
 
        // do processing
        switch (m_mode)
        {
        case krotos::PhaseVocoder::Robotizer:
            robotize(unwrappedBuffer);
            break;
        case krotos::PhaseVocoder::PitchShifter:
            pitchShift(unwrappedBuffer);
            break;
        case krotos::PhaseVocoder::TimeStrecher:
            timeStrech(unwrappedBuffer);
            break;
        }
 
        // perform ifft
        m_fft.performRealOnlyInverseTransform(unwrappedBuffer.data());
 
        // apply synthesis window
        for (int i = 0; i < m_fftSize; i++)
        {
            // apply synthesis window
            unwrappedBuffer[i] *= m_synthesisWindow[i];
        }
 
        // caclulate output frame energy
        float outEnergy = caclulateFrameRMS(unwrappedBuffer);
        // calculate gain
        float gain = inEnergy / (outEnergy + 1e-12f);
 
        // add the re trasnformed time domain signal to the output buffer
        for (int i = 0; i < m_fftSize; i++)
        {
            // start from the write pointer
            int circBufferIndex = (outPointer + i) % m_outputBufferSize;
            // Overlap add and apply gain correction
            outBuffer[circBufferIndex] += unwrappedBuffer[i] * gain;
        }
    }
 
    void PhaseVocoder::detectFrequency(std::vector<float>& fftData)
    {
        // collect separately real and imaginary parts of the FFT
        std::vector<float> realPart(m_fftSize / 2 + 1);
        std::vector<float> imagPart(m_fftSize / 2 + 1);
        int k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            realPart[i] = fftData[k];
            imagPart[i] = fftData[k + 1];
            k += 2;
        }
 
        // for each bin
        for (int i = 0; i < m_fftSize / 2; i++)
        {
            // calculate magnitude and phase from Re and Imag
            float mag = std::sqrtf(powf(realPart[i], 2.0f) + powf(imagPart[i], 2.0f));
            float phase = std::atan2f(imagPart[i], realPart[i]);
 
            // calculate phase difference between last hop and this one,
            // gives us the direct frequency
            float phaseDiff = phase - m_previousPhasesInput[m_numChannel][i];
 
            // calculate bin frequency
            float binFreq = 2.0f * float(M_PI) * float(i) / float(m_fftSize);
 
            // TODO : WRAP TO PI!!!!!!
            phaseDiff = wrapToPi(phaseDiff - binFreq * m_hopSizeAnalysis);
 
            // calculate phase deviation in number of bins from the center freq
            float binDeviation = phaseDiff * (float)m_fftSize / (float(m_hopSizeAnalysis) * 2.0f * float(M_PI));
 
            // add original bin number to get the bin this partial belongs to
            m_binFrequenciesAnalysis[m_numChannel][i] = (float)i + binDeviation;
 
            // save magnitude for later
            m_magnitudesAnalysis[m_numChannel][i] = mag;
 
            // update phase for next hop
            m_previousPhasesInput[m_numChannel][i] = phase;
 
            // Find the bin with max magnitude to display in GUI
            if (mag > m_maxBinValue)
            {
                m_maxBinValue = mag;
                m_maxBinIndex = i;
            }
        }
 
        m_frequencyDetected = m_binFrequenciesAnalysis[m_numChannel][m_maxBinIndex] * m_sampleRate / m_fftSize;
 
        // reconstruct fftData
        k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            fftData[k] = realPart[i];
            fftData[k + 1] = imagPart[i];
            k += 2;
        }
 
        // DBG(m_frequencyDetected);
    }
 
    void PhaseVocoder::robotize(std::vector<float>& fftData)
    {
        // collect separately real and imaginary parts of the FFT
        std::vector<float> realPart(m_fftSize / 2 + 1);
        std::vector<float> imagPart(m_fftSize / 2 + 1);
        int k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            realPart[i] = fftData[k];
            imagPart[i] = fftData[k + 1];
            k += 2;
        }
 
        // for each bin
        for (int i = 0; i < m_fftSize / 2; i++)
        {
            // calculate magnitude and phase from Re and Imag (Rect to Polar)
            float mag = std::sqrtf(powf(realPart[i], 2.0f) + powf(imagPart[i], 2.0f));
            // set phase of all bins to 0
            float phase = 0.0f;
 
            // go back to real and imag (Polar to Rect)
            realPart[i] = mag * std::cosf(phase);
            imagPart[i] = mag * std::sinf(phase);
        }
 
        // reconstruct fftData
        k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            fftData[k] = realPart[i];
            fftData[k + 1] = imagPart[i];
            k += 2;
        }
    }
 
    void PhaseVocoder::pitchShift(std::vector<float>& fftData)
    {
        // collect separately real and imaginary parts of the FFT
        std::vector<float> realPart(m_fftSize / 2 + 1);
        std::vector<float> imagPart(m_fftSize / 2 + 1);
        int k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            realPart[i] = fftData[k];
            imagPart[i] = fftData[k + 1];
            k += 2;
        }
 
        // for each bin
        for (int i = 0; i < m_fftSize / 2; i++)
        {
            // calculate magnitude and phase from Re and Imag
            float mag = std::sqrt(pow(realPart[i], 2.0f) + pow(imagPart[i], 2.0f));
            float phase = std::atan2(imagPart[i], realPart[i]);
 
            // calculate phase difference between last hop and this one,
            // gives us the direct frequency
            float phaseDiff = phase - m_previousPhasesInput[m_numChannel][i];
 
            // calculate bin frequency
            float binFreq = 2.0f * float(M_PI) * float(i) / float(m_fftSize);
 
            // wrap to pi
            phaseDiff = wrapToPi(phaseDiff - binFreq * m_hopSizeAnalysis);
 
            // calculate phase deviation in number of bins from the center freq
            float binDeviation = phaseDiff * (float)m_fftSize / (float(m_hopSizeAnalysis) * 2.0f * float(M_PI));
 
            // add original bin number to get the bin this partial belongs to
            m_binFrequenciesAnalysis[m_numChannel][i] = (float)i + binDeviation;
 
            // save magnitude for later
            m_magnitudesAnalysis[m_numChannel][i] = mag;
 
            // update phase for next hop
            m_previousPhasesInput[m_numChannel][i] = phase;
        }
 
        // zero out synthesis bins, ready for new data
        // fill m_magnitudesSynthesis with zeros
        std::fill(m_magnitudesSynthesis[m_numChannel].begin(), m_magnitudesSynthesis[m_numChannel].end(), 0.0f);
        // fill m_binFrequenciesSynthesis with zeros
        std::fill(m_binFrequenciesSynthesis[m_numChannel].begin(), m_binFrequenciesSynthesis[m_numChannel].end(), 0.0f);
 
        // handle the pitch shift, storing frequencies to new bins
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            // find nearest bin to shifted frequency
            int newBin = static_cast<int>(floor(i * m_pitchShiftRatio + 0.5f));
 
            // ignore shifted bins above Nyquist
            if (newBin <= m_fftSize / 2)
            {
                m_magnitudesSynthesis[m_numChannel][newBin] += m_magnitudesAnalysis[m_numChannel][i];
                m_binFrequenciesSynthesis[m_numChannel][newBin] =
                        m_binFrequenciesAnalysis[m_numChannel][i] * m_pitchShiftRatio;
            }
        }
 
        // synthesise frequencies into new magnitude and phase values for FFT bins
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            float mag = m_magnitudesSynthesis[m_numChannel][i];
 
            // get the fractional offset from the bin center frequency
            float binDeviation = m_binFrequenciesSynthesis[m_numChannel][i] - i;
 
            // multiply to get back to a phase value
            float phaseDiff = binDeviation * 2.0f * float(M_PI) * float(m_hopSizeAnalysis) / float(m_fftSize);
 
            // add the expected phase increment based on the bin center frequency
            float binCenterFreq = 2.0f * float(M_PI) * float(i) / float(m_fftSize);
            phaseDiff += binCenterFreq * float(m_hopSizeAnalysis);
 
            // advance the phase from the previous hop
            float outPhase = wrapToPi(m_previousPhasesOutput[m_numChannel][i] + phaseDiff);
 
            // go back to real and imag (Polar to Rect)
            realPart[i] = mag * std::cos(outPhase);
            imagPart[i] = mag * std::sin(outPhase);
 
            // save phases for the next hop
            m_previousPhasesOutput[m_numChannel][i] = outPhase;
        }
 
        // reconstruct fftData
        k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            fftData[k] = realPart[i];
            fftData[k + 1] = imagPart[i];
            k += 2;
        }
    }
 
    void PhaseVocoder::timeStrech(std::vector<float>& fftData)
    {
        // collect separately real and imaginary parts of the FFT
        std::vector<float> realPart(m_fftSize / 2 + 1);
        std::vector<float> imagPart(m_fftSize / 2 + 1);
        int k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            realPart[i] = fftData[k];
            imagPart[i] = fftData[k + 1];
            k += 2;
        }
 
        // for each bin
        for (int i = 0; i < m_fftSize / 2; i++)
        {
            // calculate magnitude and phase from Re and Imag (Rect to Polar)
            float mag = std::sqrt(pow(realPart[i], 2.0f) + pow(imagPart[i], 2.0f));
            // set phase of all bins to 0
            float phase = std::atan2(imagPart[i], realPart[i]);
 
            // go back to real and imag (Polar to Rect)
            realPart[i] = mag * std::cos(phase);
            imagPart[i] = mag * std::sin(phase);
        }
 
        // reconstruct fftData
        k = 0;
        for (int i = 0; i <= m_fftSize / 2; i++)
        {
            fftData[k] = realPart[i];
            fftData[k + 1] = imagPart[i];
            k += 2;
        }
    }
 
    float PhaseVocoder::wrapToPi(float phaseIn)
    {
        if (phaseIn < -float(M_PI) || phaseIn > float(M_PI))
        {
            if (phaseIn >= 0.0f)
                return fmod(phaseIn + float(M_PI), 2.0f * float(M_PI)) - float(M_PI);
            else
                return fmod(phaseIn - float(M_PI), -2.0f * float(M_PI)) + float(M_PI);
        }
        else
            return phaseIn;
    }
 
    float PhaseVocoder::caclulateFrameRMS(std::vector<float> inputFrame)
    {
        float sumOfSquares = 0;
 
        for (int i = 0; i < inputFrame.size(); i++)
        {
            sumOfSquares += inputFrame[i] * inputFrame[i];
        }
 
        float meanValue = sumOfSquares / (float)inputFrame.size();
 
        float rmsValue = sqrtf(meanValue);
 
        return rmsValue;
    }
 
} // namespace krotos