cannam@0: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
cannam@0: /*
cannam@0:     QM DSP Library
cannam@0: 
cannam@0:     Centre for Digital Music, Queen Mary, University of London.
cannam@0:     This file copyright 2005-2006 Christian Landone.
cannam@0:     All rights reserved.
cannam@0: */
cannam@0: 
cannam@0: #include "ConstantQ.h"
cannam@0: #include "dsp/transforms/FFT.h"
cannam@0: 
cannam@20: #include <iostream>
cannam@20: 
cannam@0: //---------------------------------------------------------------------------
cannam@0: // nextpow2 returns the smallest integer n such that 2^n >= x.
cannam@0: static double nextpow2(double x) {
cannam@0:     double y = ceil(log(x)/log(2.0));
cannam@0:     return(y);
cannam@0: }
cannam@0: 
cannam@0: static double squaredModule(const double & xx, const double & yy) {
cannam@0:     return xx*xx + yy*yy;
cannam@0: }
cannam@0: 
cannam@0: //----------------------------------------------------------------------------
cannam@0: 
cannam@0: ConstantQ::ConstantQ( CQConfig Config ) 
cannam@0: {
cannam@0:     initialise( Config );
cannam@0: }
cannam@0: 
cannam@0: ConstantQ::~ConstantQ()
cannam@0: {
cannam@0:     deInitialise();
cannam@0: }
cannam@0: 
cannam@0: //----------------------------------------------------------------------------
cannam@0: void ConstantQ::sparsekernel()
cannam@0: {
cannam@0:     //generates spectral kernel matrix (upside down?)
cannam@0:     // initialise temporal kernel with zeros, twice length to deal w. complex numbers
cannam@0: 
cannam@0:     double* hammingWindowRe = new double [ m_FFTLength ];
cannam@0:     double* hammingWindowIm = new double [ m_FFTLength ];
cannam@0:     double* transfHammingWindowRe = new double [ m_FFTLength ];
cannam@0:     double* transfHammingWindowIm = new double [ m_FFTLength ];
cannam@0: 
cannam@0:     for (unsigned u=0; u < m_FFTLength; u++) 
cannam@0:     {
cannam@0: 	hammingWindowRe[u] = 0;
cannam@0: 	hammingWindowIm[u] = 0;
cannam@0:     }
cannam@0: 
cannam@0: 
cannam@0:     // Here, fftleng*2 is a guess of the number of sparse cells in the matrix
cannam@0:     // The matrix K x fftlength but the non-zero cells are an antialiased
cannam@0:     // square root function. So mostly is a line, with some grey point.
cannam@0:     m_sparseKernelIs.reserve( m_FFTLength*2 );
cannam@0:     m_sparseKernelJs.reserve( m_FFTLength*2 );
cannam@0:     m_sparseKernelRealValues.reserve( m_FFTLength*2 );
cannam@0:     m_sparseKernelImagValues.reserve( m_FFTLength*2 );
cannam@0: 	
cannam@0:     // for each bin value K, calculate temporal kernel, take its fft to
cannam@0:     //calculate the spectral kernel then threshold it to make it sparse and 
cannam@0:     //add it to the sparse kernels matrix
cannam@0:     double squareThreshold = m_CQThresh * m_CQThresh;
cannam@0: 
cannam@0:     FFT m_FFT;
cannam@0: 	
cannam@0:     for (unsigned k = m_uK; k--; ) 
cannam@0:     {
cannam@3:         for (unsigned u=0; u < m_FFTLength; u++) 
cannam@3:         {
cannam@3:             hammingWindowRe[u] = 0;
cannam@3:             hammingWindowIm[u] = 0;
cannam@3:         }
cannam@3:         
cannam@0: 	// Computing a hamming window
cannam@0: 	const unsigned hammingLength = (int) ceil( m_dQ * m_FS / ( m_FMin * pow(2,((double)(k))/(double)m_BPO)));
cannam@3: 
cannam@3:         unsigned origin = m_FFTLength/2 - hammingLength/2;
cannam@3: 
cannam@0: 	for (unsigned i=0; i<hammingLength; i++) 
cannam@0: 	{
cannam@0: 	    const double angle = 2*PI*m_dQ*i/hammingLength;
cannam@0: 	    const double real = cos(angle);
cannam@0: 	    const double imag = sin(angle);
cannam@0: 	    const double absol = hamming(hammingLength, i)/hammingLength;
cannam@3: 	    hammingWindowRe[ origin + i ] = absol*real;
cannam@3: 	    hammingWindowIm[ origin + i ] = absol*imag;
cannam@0: 	}
cannam@0: 
cannam@3:         for (unsigned i = 0; i < m_FFTLength/2; ++i) {
cannam@3:             double temp = hammingWindowRe[i];
cannam@3:             hammingWindowRe[i] = hammingWindowRe[i + m_FFTLength/2];
cannam@3:             hammingWindowRe[i + m_FFTLength/2] = temp;
cannam@3:             temp = hammingWindowIm[i];
cannam@3:             hammingWindowIm[i] = hammingWindowIm[i + m_FFTLength/2];
cannam@3:             hammingWindowIm[i + m_FFTLength/2] = temp;
cannam@3:         }
cannam@3:     
cannam@0: 	//do fft of hammingWindow
cannam@0: 	m_FFT.process( m_FFTLength, 0, hammingWindowRe, hammingWindowIm, transfHammingWindowRe, transfHammingWindowIm );
cannam@0: 
cannam@0: 		
cannam@0: 	for (unsigned j=0; j<( m_FFTLength ); j++) 
cannam@0: 	{
cannam@0: 	    // perform thresholding
cannam@0: 	    const double squaredBin = squaredModule( transfHammingWindowRe[ j ], transfHammingWindowIm[ j ]);
cannam@0: 	    if (squaredBin <= squareThreshold) continue;
cannam@0: 		
cannam@0: 	    // Insert non-zero position indexes, doubled because they are floats
cannam@0: 	    m_sparseKernelIs.push_back(j);
cannam@0: 	    m_sparseKernelJs.push_back(k);
cannam@0: 
cannam@0: 	    // take conjugate, normalise and add to array sparkernel
cannam@0: 	    m_sparseKernelRealValues.push_back( transfHammingWindowRe[ j ]/m_FFTLength);
cannam@0: 	    m_sparseKernelImagValues.push_back(-transfHammingWindowIm[ j ]/m_FFTLength);
cannam@0: 	}
cannam@0: 
cannam@0:     }
cannam@0: 
cannam@0:     delete [] hammingWindowRe;
cannam@0:     delete [] hammingWindowIm;
cannam@0:     delete [] transfHammingWindowRe;
cannam@0:     delete [] transfHammingWindowIm;
cannam@0: 
cannam@0: }
cannam@0: 
cannam@0: //-----------------------------------------------------------------------------
cannam@32: double* ConstantQ::process( const double* fftdata )
cannam@0: {
cannam@0:     for (unsigned row=0; row<2*m_uK; row++) 
cannam@0:     {
cannam@0: 	m_CQdata[ row ] = 0;
cannam@0: 	m_CQdata[ row+1 ] = 0;
cannam@0:     }
cannam@0:     const unsigned *fftbin = &(m_sparseKernelIs[0]);
cannam@0:     const unsigned *cqbin  = &(m_sparseKernelJs[0]);
cannam@0:     const double   *real   = &(m_sparseKernelRealValues[0]);
cannam@0:     const double   *imag   = &(m_sparseKernelImagValues[0]);
cannam@0:     const unsigned int sparseCells = m_sparseKernelRealValues.size();
cannam@0: 	
cannam@0:     for (unsigned i = 0; i<sparseCells; i++)
cannam@0:     {
cannam@0: 	const unsigned row = cqbin[i];
cannam@0: 	const unsigned col = fftbin[i];
cannam@0: 	const double & r1  = real[i];
cannam@0: 	const double & i1  = imag[i];
cannam@0: 	const double & r2  = fftdata[ (2*m_FFTLength) - 2*col];
cannam@0: 	const double & i2  = fftdata[ (2*m_FFTLength) - 2*col+1];
cannam@0: 	// add the multiplication
cannam@0: 	m_CQdata[ 2*row  ] += (r1*r2 - i1*i2);
cannam@0: 	m_CQdata[ 2*row+1] += (r1*i2 + i1*r2);
cannam@0:     }
cannam@0: 
cannam@0:     return m_CQdata;
cannam@0: }
cannam@0: 
cannam@0: 
cannam@0: void ConstantQ::initialise( CQConfig Config )
cannam@0: {
cannam@0:     m_FS = Config.FS;
cannam@0:     m_FMin = Config.min;		// min freq
cannam@0:     m_FMax = Config.max;		// max freq
cannam@0:     m_BPO = Config.BPO;		// bins per octave
cannam@0:     m_CQThresh = Config.CQThresh;// ConstantQ threshold for kernel generation
cannam@0: 
cannam@0:     m_dQ = 1/(pow(2,(1/(double)m_BPO))-1);	// Work out Q value for Filter bank
cannam@0:     m_uK = (unsigned int) ceil(m_BPO * log(m_FMax/m_FMin)/log(2.0));	// No. of constant Q bins
cannam@0: 
cannam@24: //    std::cerr << "ConstantQ::initialise: rate = " << m_FS << ", fmin = " << m_FMin << ", fmax = " << m_FMax << ", bpo = " << m_BPO << ", K = " << m_uK << ", Q = " << m_dQ << std::endl;
cannam@20: 
cannam@0:     // work out length of fft required for this constant Q Filter bank
cannam@0:     m_FFTLength = (int) pow(2, nextpow2(ceil( m_dQ*m_FS/m_FMin )));
cannam@0: 
cannam@0:     m_hop = m_FFTLength/8; // <------ hop size is window length divided by 32
cannam@0: 
cannam@24: //    std::cerr << "ConstantQ::initialise: -> fft length = " << m_FFTLength << ", hop = " << m_hop << std::endl;
cannam@20: 
cannam@0:     // allocate memory for cqdata
cannam@0:     m_CQdata = new double [2*m_uK];
cannam@0: }
cannam@0: 
cannam@0: void ConstantQ::deInitialise()
cannam@0: {
cannam@0:     delete [] m_CQdata;
cannam@0: }
cannam@0: 
cannam@32: void ConstantQ::process(const double *FFTRe, const double* FFTIm,
cannam@32:                         double *CQRe, double *CQIm)
cannam@0: {
cannam@0:     for (unsigned row=0; row<m_uK; row++) 
cannam@0:     {
cannam@0: 	CQRe[ row ] = 0;
cannam@0: 	CQIm[ row ] = 0;
cannam@0:     }
cannam@0: 
cannam@0:     const unsigned *fftbin = &(m_sparseKernelIs[0]);
cannam@0:     const unsigned *cqbin  = &(m_sparseKernelJs[0]);
cannam@0:     const double   *real   = &(m_sparseKernelRealValues[0]);
cannam@0:     const double   *imag   = &(m_sparseKernelImagValues[0]);
cannam@0:     const unsigned int sparseCells = m_sparseKernelRealValues.size();
cannam@0: 	
cannam@0:     for (unsigned i = 0; i<sparseCells; i++)
cannam@0:     {
cannam@0: 	const unsigned row = cqbin[i];
cannam@0: 	const unsigned col = fftbin[i];
cannam@0: 	const double & r1  = real[i];
cannam@0: 	const double & i1  = imag[i];
cannam@24: 	const double & r2  = FFTRe[ m_FFTLength - col];
cannam@0: 	const double & i2  = FFTIm[ m_FFTLength - col];
cannam@0: 	// add the multiplication
cannam@0: 	CQRe[ row ] += (r1*r2 - i1*i2);
cannam@0: 	CQIm[ row ] += (r1*i2 + i1*r2);
cannam@0:     }
cannam@0: }