cannam@0: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
cannam@0: 
cannam@0: /*
cannam@0:     QM DSP Library
cannam@0: 
cannam@0:     Centre for Digital Music, Queen Mary, University of London.
Chris@115:     This file 2005-2006 Christian Landone, copyright 2013 QMUL.
Chris@84: 
Chris@84:     This program is free software; you can redistribute it and/or
Chris@84:     modify it under the terms of the GNU General Public License as
Chris@84:     published by the Free Software Foundation; either version 2 of the
Chris@84:     License, or (at your option) any later version.  See the file
Chris@84:     COPYING included with this distribution for more information.
cannam@0: */
cannam@0: 
cannam@0: #include "PhaseVocoder.h"
cannam@0: #include "dsp/transforms/FFT.h"
Chris@115: #include "maths/MathUtilities.h"
cannam@0: #include <math.h>
cannam@0: 
Chris@119: #include <cassert>
Chris@119: 
Chris@115: #include <iostream>
Chris@115: using std::cerr;
Chris@115: using std::endl;
cannam@0: 
Chris@115: PhaseVocoder::PhaseVocoder(int n, int hop) :
Chris@115:     m_n(n),
Chris@115:     m_hop(hop)
cannam@0: {
cannam@64:     m_fft = new FFTReal(m_n);
Chris@119:     m_time = new double[m_n];
Chris@115:     m_real = new double[m_n];
Chris@115:     m_imag = new double[m_n];
Chris@115:     m_phase = new double[m_n/2 + 1];
Chris@115:     m_unwrapped = new double[m_n/2 + 1];
Chris@115: 
Chris@115:     for (int i = 0; i < m_n/2 + 1; ++i) {
Chris@115:         m_phase[i] = 0.0;
Chris@115:         m_unwrapped[i] = 0.0;
Chris@115:     }
Chris@115: 
Chris@115:     reset();
cannam@0: }
cannam@0: 
cannam@0: PhaseVocoder::~PhaseVocoder()
cannam@0: {
Chris@119:     delete[] m_unwrapped;
Chris@119:     delete[] m_phase;
Chris@119:     delete[] m_real;
Chris@119:     delete[] m_imag;
Chris@119:     delete[] m_time;
cannam@64:     delete m_fft;
cannam@0: }
cannam@0: 
Chris@115: void PhaseVocoder::FFTShift(double *src)
cannam@0: {
Chris@115:     const int hs = m_n/2;
cannam@55:     for (int i = 0; i < hs; ++i) {
cannam@55:         double tmp = src[i];
cannam@55:         src[i] = src[i + hs];
cannam@55:         src[i + hs] = tmp;
cannam@0:     }
cannam@0: }
cannam@0: 
Chris@119: void PhaseVocoder::processTimeDomain(const double *src,
Chris@119:                                      double *mag, double *theta,
Chris@119:                                      double *unwrapped)
cannam@0: {
Chris@119:     for (int i = 0; i < m_n; ++i) {
Chris@119:         m_time[i] = src[i];
Chris@119:     }
Chris@119:     FFTShift(m_time);
Chris@119:     m_fft->forward(m_time, m_real, m_imag);
Chris@119:     getMagnitudes(mag);
Chris@119:     getPhases(theta);
Chris@119:     unwrapPhases(theta, unwrapped);
Chris@119: }
cannam@0: 
Chris@119: void PhaseVocoder::processFrequencyDomain(const double *reals, 
Chris@119:                                           const double *imags,
Chris@119:                                           double *mag, double *theta,
Chris@119:                                           double *unwrapped)
Chris@119: {
Chris@119:     for (int i = 0; i < m_n/2 + 1; ++i) {
Chris@119:         m_real[i] = reals[i];
Chris@119:         m_imag[i] = imags[i];
Chris@119:     }
Chris@115:     getMagnitudes(mag);
Chris@115:     getPhases(theta);
Chris@115:     unwrapPhases(theta, unwrapped);
cannam@0: }
cannam@0: 
Chris@115: void PhaseVocoder::reset()
Chris@115: {
Chris@115:     for (int i = 0; i < m_n/2 + 1; ++i) {
Chris@115:         // m_phase stores the "previous" phase, so set to one step
Chris@115:         // behind so that a signal with initial phase at zero matches
Chris@115:         // the expected values. This is completely unnecessary for any
Chris@115:         // analytical purpose, it's just tidier.
Chris@115:         double omega = (2 * M_PI * m_hop * i) / m_n;
Chris@115:         m_phase[i] = -omega;
Chris@115:         m_unwrapped[i] = -omega;
cannam@0:     }
cannam@0: }
cannam@0: 
Chris@115: void PhaseVocoder::getMagnitudes(double *mag)
Chris@115: {	
Chris@115:     for (int i = 0; i < m_n/2 + 1; i++) {
Chris@115: 	mag[i] = sqrt(m_real[i] * m_real[i] + m_imag[i] * m_imag[i]);
Chris@115:     }
Chris@115: }
Chris@115: 
Chris@115: void PhaseVocoder::getPhases(double *theta)
cannam@0: {
Chris@115:     for (int i = 0; i < m_n/2 + 1; i++) {
Chris@115: 	theta[i] = atan2(m_imag[i], m_real[i]);
cannam@0:     }	
cannam@0: }
Chris@115: 
Chris@115: void PhaseVocoder::unwrapPhases(double *theta, double *unwrapped)
Chris@115: {
Chris@115:     for (int i = 0; i < m_n/2 + 1; ++i) {
Chris@115: 
Chris@115:         double omega = (2 * M_PI * m_hop * i) / m_n;
Chris@115:         double expected = m_phase[i] + omega;
Chris@115:         double error = MathUtilities::princarg(theta[i] - expected);
Chris@115: 
Chris@115:         unwrapped[i] = m_unwrapped[i] + omega + error;
Chris@115: 
Chris@115:         m_phase[i] = theta[i];
Chris@115:         m_unwrapped[i] = unwrapped[i];
Chris@115:     }
Chris@115: }
Chris@115: