c@225: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
c@225: /*
c@225:     QM DSP Library
c@225: 
c@225:     Centre for Digital Music, Queen Mary, University of London.
c@309:     This file 2005-2006 Christian Landone.
c@309: 
c@309:     This program is free software; you can redistribute it and/or
c@309:     modify it under the terms of the GNU General Public License as
c@309:     published by the Free Software Foundation; either version 2 of the
c@309:     License, or (at your option) any later version.  See the file
c@309:     COPYING included with this distribution for more information.
c@225: */
c@225: 
c@225: #include "ConstantQ.h"
c@225: #include "dsp/transforms/FFT.h"
cannam@495: #include "base/Window.h"
c@225: 
c@245: #include <iostream>
c@245: 
c@225: //----------------------------------------------------------------------------
c@225: 
cannam@495: ConstantQ::ConstantQ( CQConfig config ) :
c@276:     m_sparseKernel(0)
c@225: {
cannam@495:     initialise(config);
c@225: }
c@225: 
c@225: ConstantQ::~ConstantQ()
c@225: {
c@225:     deInitialise();
c@225: }
c@225: 
cannam@495: static double squaredModule(const double & xx, const double & yy) {
cannam@495:     return xx*xx + yy*yy;
cannam@495: }
cannam@495: 
c@225: void ConstantQ::sparsekernel()
c@225: {
c@276:     SparseKernel *sk = new SparseKernel();
c@276: 
cannam@495:     double* windowRe = new double [ m_FFTLength ];
cannam@495:     double* windowIm = new double [ m_FFTLength ];
cannam@495:     double* transfWindowRe = new double [ m_FFTLength ];
cannam@495:     double* transfWindowIm = new double [ m_FFTLength ];
cannam@483:         
cannam@495:     // for each bin value K, calculate temporal kernel, take its fft
cannam@495:     // to calculate the spectral kernel then threshold it to make it
cannam@495:     // sparse and add it to the sparse kernels matrix
cannam@495:     
c@225:     double squareThreshold = m_CQThresh * m_CQThresh;
c@225: 
cannam@495:     FFT fft(m_FFTLength);
cannam@483:         
cannam@497:     for (int j = m_uK - 1; j >= 0; --j) {
c@228:         
cannam@495:         for (int i = 0; i < m_FFTLength; ++i) {
cannam@495:             windowRe[i] = 0;
cannam@495:             windowIm[i] = 0;
cannam@483:         }
c@225: 
cannam@495:         // Compute a complex sinusoid windowed with a hamming window
cannam@495:         // of the right length
cannam@495:         
cannam@495:         int windowLength = (int)ceil
cannam@495:             (m_dQ * m_FS / (m_FMin * pow(2, (double)j / (double)m_BPO)));
cannam@495: 
cannam@495:         int origin = m_FFTLength/2 - windowLength/2;
cannam@495: 
cannam@495:         for (int i = 0; i < windowLength; ++i) {
cannam@495:             double angle = (2.0 * M_PI * m_dQ * i) / windowLength;
cannam@495:             windowRe[origin + i] = cos(angle);
cannam@495:             windowIm[origin + i] = sin(angle);
cannam@495:         }
cannam@495: 
cannam@495:         // Shape with hamming window
cannam@495:         Window<double> hamming(HammingWindow, windowLength);
cannam@495:         hamming.cut(windowRe + origin);
cannam@495:         hamming.cut(windowIm + origin);
cannam@495: 
cannam@495:         // Scale
cannam@495:         for (int i = 0; i < windowLength; ++i) {
cannam@495:             windowRe[origin + i] /= windowLength;
cannam@495:         }
cannam@495:         for (int i = 0; i < windowLength; ++i) {
cannam@495:             windowIm[origin + i] /= windowLength;
cannam@495:         }
cannam@495: 
cannam@495:         // Input is expected to have been fftshifted, so do the
cannam@495:         // same to the input to the fft that contains the kernel
cannam@495:         for (int i = 0; i < m_FFTLength/2; ++i) {
cannam@495:             double temp = windowRe[i];
cannam@495:             windowRe[i] = windowRe[i + m_FFTLength/2];
cannam@495:             windowRe[i + m_FFTLength/2] = temp;
cannam@495:         }
cannam@495:         for (int i = 0; i < m_FFTLength/2; ++i) {
cannam@495:             double temp = windowIm[i];
cannam@495:             windowIm[i] = windowIm[i + m_FFTLength/2];
cannam@495:             windowIm[i + m_FFTLength/2] = temp;
c@228:         }
c@228:     
cannam@495:         fft.process(false, windowRe, windowIm, transfWindowRe, transfWindowIm);
cannam@495: 
cannam@495:         // convert to sparse form
cannam@495:         for (int i = 0; i < m_FFTLength; i++) {
cannam@495: 
cannam@483:             // perform thresholding
cannam@495:             double mag = squaredModule(transfWindowRe[i], transfWindowIm[i]);
cannam@495:             if (mag <= squareThreshold) continue;
cannam@483:                 
cannam@483:             // Insert non-zero position indexes
cannam@495:             sk->is.push_back(i);
cannam@495:             sk->js.push_back(j);
c@225: 
cannam@495:             // take conjugate, normalise and add to array for sparse kernel
cannam@495:             sk->real.push_back( transfWindowRe[i] / m_FFTLength);
cannam@495:             sk->imag.push_back(-transfWindowIm[i] / m_FFTLength);
cannam@483:         }
c@225:     }
c@225: 
cannam@495:     delete [] windowRe;
cannam@495:     delete [] windowIm;
cannam@495:     delete [] transfWindowRe;
cannam@495:     delete [] transfWindowIm;
c@225: 
c@276:     m_sparseKernel = sk;
cannam@495: }
cannam@495: 
cannam@495: void ConstantQ::initialise( CQConfig Config )
cannam@495: {
cannam@495:     m_FS = Config.FS;             // Sample rate
cannam@495:     m_FMin = Config.min;          // Minimum frequency
cannam@495:     m_FMax = Config.max;          // Maximum frequency
cannam@495:     m_BPO = Config.BPO;           // Bins per octave
cannam@495:     m_CQThresh = Config.CQThresh; // Threshold for sparse kernel generation
cannam@495: 
cannam@495:     // Q value for filter bank
cannam@495:     m_dQ = 1/(pow(2,(1/(double)m_BPO))-1);
cannam@495: 
cannam@495:     // No. of constant Q bins
cannam@495:     m_uK = (int)ceil(m_BPO * log(m_FMax/m_FMin)/log(2.0));
cannam@495: 
cannam@495:     // Length of fft required for this Constant Q filter bank
cannam@495:     m_FFTLength = MathUtilities::nextPowerOfTwo(int(ceil(m_dQ * m_FS/m_FMin)));
cannam@495: 
cannam@495:     // Hop from one frame to next
cannam@495:     m_hop = m_FFTLength / 8;
cannam@495: 
cannam@495:     // allocate memory for cqdata
cannam@495:     m_CQdata = new double [2*m_uK];
cannam@495: }
cannam@495: 
cannam@495: void ConstantQ::deInitialise()
cannam@495: {
cannam@495:     delete [] m_CQdata;
cannam@495:     delete m_sparseKernel;
c@225: }
c@225: 
c@225: //-----------------------------------------------------------------------------
c@257: double* ConstantQ::process( const double* fftdata )
c@225: {
c@276:     if (!m_sparseKernel) {
c@276:         std::cerr << "ERROR: ConstantQ::process: Sparse kernel has not been initialised" << std::endl;
c@276:         return m_CQdata;
c@276:     }
c@276: 
c@276:     SparseKernel *sk = m_sparseKernel;
c@276: 
cannam@495:     for (int row=0; row < 2 * m_uK; row++) {
cannam@483:         m_CQdata[ row ] = 0;
cannam@483:         m_CQdata[ row+1 ] = 0;
c@225:     }
cannam@495:     const int *fftbin = &(sk->is[0]);
cannam@495:     const int *cqbin = &(sk->js[0]);
cannam@495:     const double *real = &(sk->real[0]);
cannam@495:     const double *imag = &(sk->imag[0]);
cannam@495:     const int sparseCells = int(sk->real.size());
cannam@483:         
cannam@495:     for (int i = 0; i < sparseCells; i++) {
cannam@495:         const int row = cqbin[i];
cannam@495:         const int col = fftbin[i];
cannam@469:         if (col == 0) continue;
cannam@495:         const double & r1 = real[i];
cannam@495:         const double & i1 = imag[i];
cannam@495:         const double & r2 = fftdata[ (2*m_FFTLength) - 2*col - 2 ];
cannam@495:         const double & i2 = fftdata[ (2*m_FFTLength) - 2*col - 2 + 1 ];
cannam@483:         // add the multiplication
cannam@483:         m_CQdata[ 2*row  ] += (r1*r2 - i1*i2);
cannam@483:         m_CQdata[ 2*row+1] += (r1*i2 + i1*r2);
c@225:     }
c@225: 
c@225:     return m_CQdata;
c@225: }
c@225: 
c@257: void ConstantQ::process(const double *FFTRe, const double* FFTIm,
c@257:                         double *CQRe, double *CQIm)
c@225: {
c@276:     if (!m_sparseKernel) {
c@276:         std::cerr << "ERROR: ConstantQ::process: Sparse kernel has not been initialised" << std::endl;
c@276:         return;
c@276:     }
c@276: 
c@276:     SparseKernel *sk = m_sparseKernel;
c@276: 
cannam@495:     for (int row = 0; row < m_uK; row++) {
cannam@483:         CQRe[ row ] = 0;
cannam@483:         CQIm[ row ] = 0;
c@225:     }
c@225: 
cannam@495:     const int *fftbin = &(sk->is[0]);
cannam@495:     const int *cqbin = &(sk->js[0]);
cannam@495:     const double *real = &(sk->real[0]);
cannam@495:     const double *imag = &(sk->imag[0]);
cannam@495:     const int sparseCells = int(sk->real.size());
cannam@483:         
cannam@495:     for (int i = 0; i<sparseCells; i++) {
cannam@495:         const int row = cqbin[i];
cannam@495:         const int col = fftbin[i];
cannam@469:         if (col == 0) continue;
cannam@495:         const double & r1 = real[i];
cannam@495:         const double & i1 = imag[i];
cannam@495:         const double & r2 = FFTRe[ m_FFTLength - col ];
cannam@495:         const double & i2 = FFTIm[ m_FFTLength - col ];
cannam@483:         // add the multiplication
cannam@483:         CQRe[ row ] += (r1*r2 - i1*i2);
cannam@483:         CQIm[ row ] += (r1*i2 + i1*r2);
c@225:     }
c@225: }