c@225
|
1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*- vi:set ts=8 sts=4 sw=4: */
|
c@225
|
2
|
c@225
|
3 /*
|
c@225
|
4 QM DSP Library
|
c@225
|
5
|
c@225
|
6 Centre for Digital Music, Queen Mary, University of London.
|
c@340
|
7 This file 2005-2006 Christian Landone, copyright 2013 QMUL.
|
c@309
|
8
|
c@309
|
9 This program is free software; you can redistribute it and/or
|
c@309
|
10 modify it under the terms of the GNU General Public License as
|
c@309
|
11 published by the Free Software Foundation; either version 2 of the
|
c@309
|
12 License, or (at your option) any later version. See the file
|
c@309
|
13 COPYING included with this distribution for more information.
|
c@225
|
14 */
|
c@225
|
15
|
c@225
|
16 #include "PhaseVocoder.h"
|
c@225
|
17 #include "dsp/transforms/FFT.h"
|
c@340
|
18 #include "maths/MathUtilities.h"
|
c@225
|
19 #include <math.h>
|
c@225
|
20
|
c@344
|
21 #include <cassert>
|
c@344
|
22
|
c@340
|
23 #include <iostream>
|
c@340
|
24 using std::cerr;
|
c@340
|
25 using std::endl;
|
c@225
|
26
|
c@340
|
27 PhaseVocoder::PhaseVocoder(int n, int hop) :
|
c@340
|
28 m_n(n),
|
c@340
|
29 m_hop(hop)
|
c@225
|
30 {
|
c@289
|
31 m_fft = new FFTReal(m_n);
|
c@344
|
32 m_time = new double[m_n];
|
c@340
|
33 m_real = new double[m_n];
|
c@340
|
34 m_imag = new double[m_n];
|
c@340
|
35 m_phase = new double[m_n/2 + 1];
|
c@340
|
36 m_unwrapped = new double[m_n/2 + 1];
|
c@340
|
37
|
c@340
|
38 for (int i = 0; i < m_n/2 + 1; ++i) {
|
c@340
|
39 m_phase[i] = 0.0;
|
c@340
|
40 m_unwrapped[i] = 0.0;
|
c@340
|
41 }
|
c@340
|
42
|
c@340
|
43 reset();
|
c@225
|
44 }
|
c@225
|
45
|
c@225
|
46 PhaseVocoder::~PhaseVocoder()
|
c@225
|
47 {
|
c@344
|
48 delete[] m_unwrapped;
|
c@344
|
49 delete[] m_phase;
|
c@344
|
50 delete[] m_real;
|
c@344
|
51 delete[] m_imag;
|
c@344
|
52 delete[] m_time;
|
c@289
|
53 delete m_fft;
|
c@225
|
54 }
|
c@225
|
55
|
c@340
|
56 void PhaseVocoder::FFTShift(double *src)
|
c@225
|
57 {
|
c@340
|
58 const int hs = m_n/2;
|
c@280
|
59 for (int i = 0; i < hs; ++i) {
|
c@280
|
60 double tmp = src[i];
|
c@280
|
61 src[i] = src[i + hs];
|
c@280
|
62 src[i + hs] = tmp;
|
c@225
|
63 }
|
c@225
|
64 }
|
c@225
|
65
|
c@344
|
66 void PhaseVocoder::processTimeDomain(const double *src,
|
c@344
|
67 double *mag, double *theta,
|
c@344
|
68 double *unwrapped)
|
c@225
|
69 {
|
c@344
|
70 for (int i = 0; i < m_n; ++i) {
|
c@344
|
71 m_time[i] = src[i];
|
c@344
|
72 }
|
c@344
|
73 FFTShift(m_time);
|
c@344
|
74 m_fft->forward(m_time, m_real, m_imag);
|
c@344
|
75 getMagnitudes(mag);
|
c@344
|
76 getPhases(theta);
|
c@344
|
77 unwrapPhases(theta, unwrapped);
|
c@344
|
78 }
|
c@225
|
79
|
c@344
|
80 void PhaseVocoder::processFrequencyDomain(const double *reals,
|
c@344
|
81 const double *imags,
|
c@344
|
82 double *mag, double *theta,
|
c@344
|
83 double *unwrapped)
|
c@344
|
84 {
|
c@344
|
85 for (int i = 0; i < m_n/2 + 1; ++i) {
|
c@344
|
86 m_real[i] = reals[i];
|
c@344
|
87 m_imag[i] = imags[i];
|
c@344
|
88 }
|
c@340
|
89 getMagnitudes(mag);
|
c@340
|
90 getPhases(theta);
|
c@340
|
91 unwrapPhases(theta, unwrapped);
|
c@225
|
92 }
|
c@225
|
93
|
c@340
|
94 void PhaseVocoder::reset()
|
c@340
|
95 {
|
c@340
|
96 for (int i = 0; i < m_n/2 + 1; ++i) {
|
c@340
|
97 // m_phase stores the "previous" phase, so set to one step
|
c@340
|
98 // behind so that a signal with initial phase at zero matches
|
c@340
|
99 // the expected values. This is completely unnecessary for any
|
c@340
|
100 // analytical purpose, it's just tidier.
|
c@340
|
101 double omega = (2 * M_PI * m_hop * i) / m_n;
|
c@340
|
102 m_phase[i] = -omega;
|
c@340
|
103 m_unwrapped[i] = -omega;
|
c@225
|
104 }
|
c@225
|
105 }
|
c@225
|
106
|
c@340
|
107 void PhaseVocoder::getMagnitudes(double *mag)
|
cannam@483
|
108 {
|
c@340
|
109 for (int i = 0; i < m_n/2 + 1; i++) {
|
cannam@483
|
110 mag[i] = sqrt(m_real[i] * m_real[i] + m_imag[i] * m_imag[i]);
|
c@340
|
111 }
|
c@340
|
112 }
|
c@340
|
113
|
c@340
|
114 void PhaseVocoder::getPhases(double *theta)
|
c@225
|
115 {
|
c@340
|
116 for (int i = 0; i < m_n/2 + 1; i++) {
|
cannam@483
|
117 theta[i] = atan2(m_imag[i], m_real[i]);
|
cannam@483
|
118 }
|
c@225
|
119 }
|
c@340
|
120
|
c@340
|
121 void PhaseVocoder::unwrapPhases(double *theta, double *unwrapped)
|
c@340
|
122 {
|
c@340
|
123 for (int i = 0; i < m_n/2 + 1; ++i) {
|
c@340
|
124
|
c@340
|
125 double omega = (2 * M_PI * m_hop * i) / m_n;
|
c@340
|
126 double expected = m_phase[i] + omega;
|
c@340
|
127 double error = MathUtilities::princarg(theta[i] - expected);
|
c@340
|
128
|
c@340
|
129 unwrapped[i] = m_unwrapped[i] + omega + error;
|
c@340
|
130
|
c@340
|
131 m_phase[i] = theta[i];
|
c@340
|
132 m_unwrapped[i] = unwrapped[i];
|
c@340
|
133 }
|
c@340
|
134 }
|
c@340
|
135
|