qm-dsp
1.8
|
00001 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*- vi:set ts=8 sts=4 sw=4: */ 00002 00003 /* 00004 QM DSP Library 00005 00006 Centre for Digital Music, Queen Mary, University of London. 00007 This file 2005-2006 Christian Landone, copyright 2013 QMUL. 00008 00009 This program is free software; you can redistribute it and/or 00010 modify it under the terms of the GNU General Public License as 00011 published by the Free Software Foundation; either version 2 of the 00012 License, or (at your option) any later version. See the file 00013 COPYING included with this distribution for more information. 00014 */ 00015 00016 #include "PhaseVocoder.h" 00017 #include "dsp/transforms/FFT.h" 00018 #include "maths/MathUtilities.h" 00019 #include <math.h> 00020 00021 #include <cassert> 00022 00023 #include <iostream> 00024 using std::cerr; 00025 using std::endl; 00026 00027 PhaseVocoder::PhaseVocoder(int n, int hop) : 00028 m_n(n), 00029 m_hop(hop) 00030 { 00031 m_fft = new FFTReal(m_n); 00032 m_time = new double[m_n]; 00033 m_real = new double[m_n]; 00034 m_imag = new double[m_n]; 00035 m_phase = new double[m_n/2 + 1]; 00036 m_unwrapped = new double[m_n/2 + 1]; 00037 00038 for (int i = 0; i < m_n/2 + 1; ++i) { 00039 m_phase[i] = 0.0; 00040 m_unwrapped[i] = 0.0; 00041 } 00042 00043 reset(); 00044 } 00045 00046 PhaseVocoder::~PhaseVocoder() 00047 { 00048 delete[] m_unwrapped; 00049 delete[] m_phase; 00050 delete[] m_real; 00051 delete[] m_imag; 00052 delete[] m_time; 00053 delete m_fft; 00054 } 00055 00056 void PhaseVocoder::FFTShift(double *src) 00057 { 00058 const int hs = m_n/2; 00059 for (int i = 0; i < hs; ++i) { 00060 double tmp = src[i]; 00061 src[i] = src[i + hs]; 00062 src[i + hs] = tmp; 00063 } 00064 } 00065 00066 void PhaseVocoder::processTimeDomain(const double *src, 00067 double *mag, double *theta, 00068 double *unwrapped) 00069 { 00070 for (int i = 0; i < m_n; ++i) { 00071 m_time[i] = src[i]; 00072 } 00073 FFTShift(m_time); 00074 m_fft->forward(m_time, m_real, m_imag); 00075 getMagnitudes(mag); 00076 getPhases(theta); 00077 unwrapPhases(theta, unwrapped); 00078 } 00079 00080 void PhaseVocoder::processFrequencyDomain(const double *reals, 00081 const double *imags, 00082 double *mag, double *theta, 00083 double *unwrapped) 00084 { 00085 for (int i = 0; i < m_n/2 + 1; ++i) { 00086 m_real[i] = reals[i]; 00087 m_imag[i] = imags[i]; 00088 } 00089 getMagnitudes(mag); 00090 getPhases(theta); 00091 unwrapPhases(theta, unwrapped); 00092 } 00093 00094 void PhaseVocoder::reset() 00095 { 00096 for (int i = 0; i < m_n/2 + 1; ++i) { 00097 // m_phase stores the "previous" phase, so set to one step 00098 // behind so that a signal with initial phase at zero matches 00099 // the expected values. This is completely unnecessary for any 00100 // analytical purpose, it's just tidier. 00101 double omega = (2 * M_PI * m_hop * i) / m_n; 00102 m_phase[i] = -omega; 00103 m_unwrapped[i] = -omega; 00104 } 00105 } 00106 00107 void PhaseVocoder::getMagnitudes(double *mag) 00108 { 00109 for (int i = 0; i < m_n/2 + 1; i++) { 00110 mag[i] = sqrt(m_real[i] * m_real[i] + m_imag[i] * m_imag[i]); 00111 } 00112 } 00113 00114 void PhaseVocoder::getPhases(double *theta) 00115 { 00116 for (int i = 0; i < m_n/2 + 1; i++) { 00117 theta[i] = atan2(m_imag[i], m_real[i]); 00118 } 00119 } 00120 00121 void PhaseVocoder::unwrapPhases(double *theta, double *unwrapped) 00122 { 00123 for (int i = 0; i < m_n/2 + 1; ++i) { 00124 00125 double omega = (2 * M_PI * m_hop * i) / m_n; 00126 double expected = m_phase[i] + omega; 00127 double error = MathUtilities::princarg(theta[i] - expected); 00128 00129 unwrapped[i] = m_unwrapped[i] + omega + error; 00130 00131 m_phase[i] = theta[i]; 00132 m_unwrapped[i] = unwrapped[i]; 00133 } 00134 } 00135