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 is based on Don Cross's public domain FFT implementation. 00008 */ 00009 00010 #include "FFT.h" 00011 00012 #include "maths/MathUtilities.h" 00013 00014 #include "kiss_fft.h" 00015 #include "kiss_fftr.h" 00016 00017 #include <cmath> 00018 00019 #include <iostream> 00020 00021 #include <stdexcept> 00022 00023 class FFT::D 00024 { 00025 public: 00026 D(int n) : m_n(n) { 00027 m_planf = kiss_fft_alloc(m_n, 0, NULL, NULL); 00028 m_plani = kiss_fft_alloc(m_n, 1, NULL, NULL); 00029 m_kin = new kiss_fft_cpx[m_n]; 00030 m_kout = new kiss_fft_cpx[m_n]; 00031 } 00032 00033 ~D() { 00034 kiss_fft_free(m_planf); 00035 kiss_fft_free(m_plani); 00036 delete[] m_kin; 00037 delete[] m_kout; 00038 } 00039 00040 void process(bool inverse, 00041 const double *ri, 00042 const double *ii, 00043 double *ro, 00044 double *io) { 00045 00046 for (int i = 0; i < m_n; ++i) { 00047 m_kin[i].r = ri[i]; 00048 m_kin[i].i = (ii ? ii[i] : 0.0); 00049 } 00050 00051 if (!inverse) { 00052 00053 kiss_fft(m_planf, m_kin, m_kout); 00054 00055 for (int i = 0; i < m_n; ++i) { 00056 ro[i] = m_kout[i].r; 00057 io[i] = m_kout[i].i; 00058 } 00059 00060 } else { 00061 00062 kiss_fft(m_plani, m_kin, m_kout); 00063 00064 double scale = 1.0 / m_n; 00065 00066 for (int i = 0; i < m_n; ++i) { 00067 ro[i] = m_kout[i].r * scale; 00068 io[i] = m_kout[i].i * scale; 00069 } 00070 } 00071 } 00072 00073 private: 00074 int m_n; 00075 kiss_fft_cfg m_planf; 00076 kiss_fft_cfg m_plani; 00077 kiss_fft_cpx *m_kin; 00078 kiss_fft_cpx *m_kout; 00079 }; 00080 00081 FFT::FFT(int n) : 00082 m_d(new D(n)) 00083 { 00084 } 00085 00086 FFT::~FFT() 00087 { 00088 delete m_d; 00089 } 00090 00091 void 00092 FFT::process(bool inverse, 00093 const double *p_lpRealIn, const double *p_lpImagIn, 00094 double *p_lpRealOut, double *p_lpImagOut) 00095 { 00096 m_d->process(inverse, 00097 p_lpRealIn, p_lpImagIn, 00098 p_lpRealOut, p_lpImagOut); 00099 } 00100 00101 class FFTReal::D 00102 { 00103 public: 00104 D(int n) : m_n(n) { 00105 if (n % 2) { 00106 throw std::invalid_argument 00107 ("nsamples must be even in FFTReal constructor"); 00108 } 00109 m_planf = kiss_fftr_alloc(m_n, 0, NULL, NULL); 00110 m_plani = kiss_fftr_alloc(m_n, 1, NULL, NULL); 00111 m_c = new kiss_fft_cpx[m_n]; 00112 } 00113 00114 ~D() { 00115 kiss_fftr_free(m_planf); 00116 kiss_fftr_free(m_plani); 00117 delete[] m_c; 00118 } 00119 00120 void forward(const double *ri, double *ro, double *io) { 00121 00122 kiss_fftr(m_planf, ri, m_c); 00123 00124 for (int i = 0; i <= m_n/2; ++i) { 00125 ro[i] = m_c[i].r; 00126 io[i] = m_c[i].i; 00127 } 00128 00129 for (int i = 0; i + 1 < m_n/2; ++i) { 00130 ro[m_n - i - 1] = ro[i + 1]; 00131 io[m_n - i - 1] = -io[i + 1]; 00132 } 00133 } 00134 00135 void forwardMagnitude(const double *ri, double *mo) { 00136 00137 double *io = new double[m_n]; 00138 00139 forward(ri, mo, io); 00140 00141 for (int i = 0; i < m_n; ++i) { 00142 mo[i] = sqrt(mo[i] * mo[i] + io[i] * io[i]); 00143 } 00144 00145 delete[] io; 00146 } 00147 00148 void inverse(const double *ri, const double *ii, double *ro) { 00149 00150 // kiss_fftr.h says 00151 // "input freqdata has nfft/2+1 complex points" 00152 00153 for (int i = 0; i < m_n/2 + 1; ++i) { 00154 m_c[i].r = ri[i]; 00155 m_c[i].i = ii[i]; 00156 } 00157 00158 kiss_fftri(m_plani, m_c, ro); 00159 00160 double scale = 1.0 / m_n; 00161 00162 for (int i = 0; i < m_n; ++i) { 00163 ro[i] *= scale; 00164 } 00165 } 00166 00167 private: 00168 int m_n; 00169 kiss_fftr_cfg m_planf; 00170 kiss_fftr_cfg m_plani; 00171 kiss_fft_cpx *m_c; 00172 }; 00173 00174 FFTReal::FFTReal(int n) : 00175 m_d(new D(n)) 00176 { 00177 } 00178 00179 FFTReal::~FFTReal() 00180 { 00181 delete m_d; 00182 } 00183 00184 void 00185 FFTReal::forward(const double *ri, double *ro, double *io) 00186 { 00187 m_d->forward(ri, ro, io); 00188 } 00189 00190 void 00191 FFTReal::forwardMagnitude(const double *ri, double *mo) 00192 { 00193 m_d->forwardMagnitude(ri, mo); 00194 } 00195 00196 void 00197 FFTReal::inverse(const double *ri, const double *ii, double *ro) 00198 { 00199 m_d->inverse(ri, ii, ro); 00200 } 00201 00202 00203