lbajardsilogic@0: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
lbajardsilogic@0: 
lbajardsilogic@0: /*
lbajardsilogic@0:     Sonic Visualiser
lbajardsilogic@0:     An audio file viewer and annotation editor.
lbajardsilogic@0:     Centre for Digital Music, Queen Mary, University of London.
lbajardsilogic@0:     This file copyright 2006 Chris Cannam and QMUL.
lbajardsilogic@0:     FFT code from Don Cross's public domain FFT implementation.
lbajardsilogic@0:     
lbajardsilogic@0:     This program is free software; you can redistribute it and/or
lbajardsilogic@0:     modify it under the terms of the GNU General Public License as
lbajardsilogic@0:     published by the Free Software Foundation; either version 2 of the
lbajardsilogic@0:     License, or (at your option) any later version.  See the file
lbajardsilogic@0:     COPYING included with this distribution for more information.
lbajardsilogic@0: */
lbajardsilogic@0: 
lbajardsilogic@0: #include "FFTapi.h"
lbajardsilogic@0: 
lbajardsilogic@0: #ifndef HAVE_FFTW3F
lbajardsilogic@0: 
lbajardsilogic@0: #include <cmath>
lbajardsilogic@0: #include <iostream>
lbajardsilogic@0: 
lbajardsilogic@0: void
lbajardsilogic@0: fft(unsigned int n, bool inverse, double *ri, double *ii, double *ro, double *io)
lbajardsilogic@0: {
lbajardsilogic@0:     if (!ri || !ro || !io) return;
lbajardsilogic@0: 
lbajardsilogic@0:     unsigned int bits;
lbajardsilogic@0:     unsigned int i, j, k, m;
lbajardsilogic@0:     unsigned int blockSize, blockEnd;
lbajardsilogic@0: 
lbajardsilogic@0:     double tr, ti;
lbajardsilogic@0: 
lbajardsilogic@0:     if (n < 2) return;
lbajardsilogic@0:     if (n & (n-1)) return;
lbajardsilogic@0: 
lbajardsilogic@0:     double angle = 2.0 * M_PI;
lbajardsilogic@0:     if (inverse) angle = -angle;
lbajardsilogic@0: 
lbajardsilogic@0:     for (i = 0; ; ++i) {
lbajardsilogic@0: 	if (n & (1 << i)) {
lbajardsilogic@0: 	    bits = i;
lbajardsilogic@0: 	    break;
lbajardsilogic@0: 	}
lbajardsilogic@0:     }
lbajardsilogic@0: 
lbajardsilogic@0:     int *table = new int[n];
lbajardsilogic@0: 
lbajardsilogic@0:     for (i = 0; i < n; ++i) {
lbajardsilogic@0: 	
lbajardsilogic@0:         m = i;
lbajardsilogic@0:         
lbajardsilogic@0:         for (j = k = 0; j < bits; ++j) {
lbajardsilogic@0:             k = (k << 1) | (m & 1);
lbajardsilogic@0:             m >>= 1;
lbajardsilogic@0:         }
lbajardsilogic@0:         
lbajardsilogic@0:         table[i] = k;
lbajardsilogic@0:     }
lbajardsilogic@0: 
lbajardsilogic@0:     if (ii) {
lbajardsilogic@0: 	for (i = 0; i < n; ++i) {
lbajardsilogic@0: 	    ro[table[i]] = ri[i];
lbajardsilogic@0: 	    io[table[i]] = ii[i];
lbajardsilogic@0: 	}
lbajardsilogic@0:     } else {
lbajardsilogic@0: 	for (i = 0; i < n; ++i) {
lbajardsilogic@0: 	    ro[table[i]] = ri[i];
lbajardsilogic@0: 	    io[table[i]] = 0.0;
lbajardsilogic@0: 	}
lbajardsilogic@0:     }
lbajardsilogic@0: 
lbajardsilogic@0:     blockEnd = 1;
lbajardsilogic@0: 
lbajardsilogic@0:     for (blockSize = 2; blockSize <= n; blockSize <<= 1) {
lbajardsilogic@0: 
lbajardsilogic@0: 	double delta = angle / (double)blockSize;
lbajardsilogic@0: 	double sm2 = -sin(-2 * delta);
lbajardsilogic@0: 	double sm1 = -sin(-delta);
lbajardsilogic@0: 	double cm2 = cos(-2 * delta);
lbajardsilogic@0: 	double cm1 = cos(-delta);
lbajardsilogic@0: 	double w = 2 * cm1;
lbajardsilogic@0: 	double ar[3], ai[3];
lbajardsilogic@0: 
lbajardsilogic@0: 	for (i = 0; i < n; i += blockSize) {
lbajardsilogic@0: 
lbajardsilogic@0: 	    ar[2] = cm2;
lbajardsilogic@0: 	    ar[1] = cm1;
lbajardsilogic@0: 
lbajardsilogic@0: 	    ai[2] = sm2;
lbajardsilogic@0: 	    ai[1] = sm1;
lbajardsilogic@0: 
lbajardsilogic@0: 	    for (j = i, m = 0; m < blockEnd; j++, m++) {
lbajardsilogic@0: 
lbajardsilogic@0: 		ar[0] = w * ar[1] - ar[2];
lbajardsilogic@0: 		ar[2] = ar[1];
lbajardsilogic@0: 		ar[1] = ar[0];
lbajardsilogic@0: 
lbajardsilogic@0: 		ai[0] = w * ai[1] - ai[2];
lbajardsilogic@0: 		ai[2] = ai[1];
lbajardsilogic@0: 		ai[1] = ai[0];
lbajardsilogic@0: 
lbajardsilogic@0: 		k = j + blockEnd;
lbajardsilogic@0: 		tr = ar[0] * ro[k] - ai[0] * io[k];
lbajardsilogic@0: 		ti = ar[0] * io[k] + ai[0] * ro[k];
lbajardsilogic@0: 
lbajardsilogic@0: 		ro[k] = ro[j] - tr;
lbajardsilogic@0: 		io[k] = io[j] - ti;
lbajardsilogic@0: 
lbajardsilogic@0: 		ro[j] += tr;
lbajardsilogic@0: 		io[j] += ti;
lbajardsilogic@0: 	    }
lbajardsilogic@0: 	}
lbajardsilogic@0: 
lbajardsilogic@0: 	blockEnd = blockSize;
lbajardsilogic@0:     }
lbajardsilogic@0: 
lbajardsilogic@0: /* fftw doesn't normalise, so nor will we
lbajardsilogic@0: 
lbajardsilogic@0:     if (inverse) {
lbajardsilogic@0: 
lbajardsilogic@0: 	double denom = (double)n;
lbajardsilogic@0: 
lbajardsilogic@0: 	for (i = 0; i < n; i++) {
lbajardsilogic@0: 	    ro[i] /= denom;
lbajardsilogic@0: 	    io[i] /= denom;
lbajardsilogic@0: 	}
lbajardsilogic@0:     }
lbajardsilogic@0: */
lbajardsilogic@0:     delete[] table;
lbajardsilogic@0: }
lbajardsilogic@0: 
lbajardsilogic@0: struct fftf_plan_ {
lbajardsilogic@0:     int size;
lbajardsilogic@0:     int inverse;
lbajardsilogic@0:     float *real;
lbajardsilogic@0:     fftf_complex *cplx;
lbajardsilogic@0: };
lbajardsilogic@0: 
lbajardsilogic@0: fftf_plan
lbajardsilogic@0: fftf_plan_dft_r2c_1d(int n, float *in, fftf_complex *out, unsigned)
lbajardsilogic@0: {
lbajardsilogic@0:     if (n < 2) return 0;
lbajardsilogic@0:     if (n & (n-1)) return 0;
lbajardsilogic@0:     
lbajardsilogic@0:     fftf_plan_ *plan = new fftf_plan_;
lbajardsilogic@0:     plan->size = n;
lbajardsilogic@0:     plan->inverse = 0;
lbajardsilogic@0:     plan->real = in;
lbajardsilogic@0:     plan->cplx = out;
lbajardsilogic@0:     return plan;
lbajardsilogic@0: }
lbajardsilogic@0: 
lbajardsilogic@0: fftf_plan
lbajardsilogic@0: fftf_plan_dft_c2r_1d(int n, fftf_complex *in, float *out, unsigned)
lbajardsilogic@0: {
lbajardsilogic@0:     if (n < 2) return 0;
lbajardsilogic@0:     if (n & (n-1)) return 0;
lbajardsilogic@0:     
lbajardsilogic@0:     fftf_plan_ *plan = new fftf_plan_;
lbajardsilogic@0:     plan->size = n;
lbajardsilogic@0:     plan->inverse = 1;
lbajardsilogic@0:     plan->real = out;
lbajardsilogic@0:     plan->cplx = in;
lbajardsilogic@0:     return plan;
lbajardsilogic@0: }
lbajardsilogic@0: 
lbajardsilogic@0: void
lbajardsilogic@0: fftf_destroy_plan(fftf_plan p)
lbajardsilogic@0: {
lbajardsilogic@0:     delete p;
lbajardsilogic@0: }
lbajardsilogic@0: 
lbajardsilogic@0: void
lbajardsilogic@0: fftf_execute(const fftf_plan p)
lbajardsilogic@0: {
lbajardsilogic@0:     float *real = p->real;
lbajardsilogic@0:     fftf_complex *cplx = p->cplx;
lbajardsilogic@0:     int n = p->size;
lbajardsilogic@0:     int forward = !p->inverse;
lbajardsilogic@0: 
lbajardsilogic@0:     double *ri = new double[n];
lbajardsilogic@0:     double *ro = new double[n];
lbajardsilogic@0:     double *io = new double[n];
lbajardsilogic@0: 
lbajardsilogic@0:     double *ii = 0;
lbajardsilogic@0:     if (!forward) ii = new double[n];
lbajardsilogic@0: 
lbajardsilogic@0:     if (forward) {
lbajardsilogic@0:         for (int i = 0; i < n; ++i) {
lbajardsilogic@0:             ri[i] = real[i];
lbajardsilogic@0:         }
lbajardsilogic@0:     } else {
lbajardsilogic@0:         for (int i = 0; i < n/2+1; ++i) {
lbajardsilogic@0:             ri[i] = cplx[i][0];
lbajardsilogic@0:             ii[i] = cplx[i][1];
lbajardsilogic@0:             if (i > 0) {
lbajardsilogic@0:                 ri[n-i] = ri[i];
lbajardsilogic@0:                 ii[n-i] = -ii[i];
lbajardsilogic@0:             }
lbajardsilogic@0:         }
lbajardsilogic@0:     }
lbajardsilogic@0: 
lbajardsilogic@0:     fft(n, !forward, ri, ii, ro, io);
lbajardsilogic@0: 
lbajardsilogic@0:     if (forward) {
lbajardsilogic@0:         for (int i = 0; i < n/2+1; ++i) {
lbajardsilogic@0:             cplx[i][0] = ro[i];
lbajardsilogic@0:             cplx[i][1] = io[i];
lbajardsilogic@0:         }
lbajardsilogic@0:     } else {
lbajardsilogic@0:         for (int i = 0; i < n; ++i) {
lbajardsilogic@0:             real[i] = ro[i];
lbajardsilogic@0:         }
lbajardsilogic@0:     }
lbajardsilogic@0: 
lbajardsilogic@0:     delete[] ri;
lbajardsilogic@0:     delete[] ro;
lbajardsilogic@0:     delete[] io;
lbajardsilogic@0:     if (ii) delete[] ii;
lbajardsilogic@0: }
lbajardsilogic@0: 
lbajardsilogic@0: #endif