qm-dsp  1.8
DecimatorB.cpp
Go to the documentation of this file.
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 
00008     This program is free software; you can redistribute it and/or
00009     modify it under the terms of the GNU General Public License as
00010     published by the Free Software Foundation; either version 2 of the
00011     License, or (at your option) any later version.  See the file
00012     COPYING included with this distribution for more information.
00013 */
00014 
00015 #include "DecimatorB.h"
00016 
00017 #include "maths/MathUtilities.h"
00018 
00019 #include <iostream>
00020 
00021 using std::vector;
00022 
00023 DecimatorB::DecimatorB(int inLength, int decFactor)
00024 {
00025     m_inputLength = 0;
00026     m_outputLength = 0;
00027     m_decFactor = 1;
00028     m_aaBuffer = 0;
00029     m_tmpBuffer = 0;
00030 
00031     initialise(inLength, decFactor);
00032 }
00033 
00034 DecimatorB::~DecimatorB()
00035 {
00036     deInitialise();
00037 }
00038 
00039 void DecimatorB::initialise(int inLength, int decFactor)
00040 {
00041     m_inputLength = inLength;
00042     m_decFactor = decFactor;
00043     m_outputLength = m_inputLength / m_decFactor;
00044 
00045     if (m_decFactor < 2 || !MathUtilities::isPowerOfTwo(m_decFactor)) {
00046         std::cerr << "ERROR: DecimatorB::initialise: Decimation factor must be a power of 2 and at least 2 (was: " << m_decFactor << ")" << std::endl;
00047         m_decFactor = 0;
00048         return;
00049     }
00050 
00051     if (m_inputLength % m_decFactor != 0) {
00052         std::cerr << "ERROR: DecimatorB::initialise: inLength must be a multiple of decimation factor (was: " << m_inputLength << ", factor is " << m_decFactor << ")" << std::endl;
00053         m_decFactor = 0;
00054         return;
00055     }        
00056 
00057     m_aaBuffer = new double[m_inputLength];
00058     m_tmpBuffer = new double[m_inputLength];
00059 
00060     // Order 6 Butterworth lowpass filter
00061     // Calculated using e.g. MATLAB butter(6, 0.5, 'low')
00062 
00063     m_b[0] = 0.029588223638661;
00064     m_b[1] = 0.177529341831965;
00065     m_b[2] = 0.443823354579912;
00066     m_b[3] = 0.591764472773216;
00067     m_b[4] = 0.443823354579912;
00068     m_b[5] = 0.177529341831965;
00069     m_b[6] = 0.029588223638661;
00070 
00071     m_a[0] = 1.000000000000000;
00072     m_a[1] = 0.000000000000000;
00073     m_a[2] = 0.777695961855673;
00074     m_a[3] = 0.000000000000000;
00075     m_a[4] = 0.114199425062434;
00076     m_a[5] = 0.000000000000000;
00077     m_a[6] = 0.001750925956183;
00078 
00079     for (int factor = m_decFactor; factor > 1; factor /= 2) {
00080         m_o.push_back(vector<double>(6, 0.0));
00081     }
00082 }
00083 
00084 void DecimatorB::deInitialise()
00085 {
00086     delete [] m_aaBuffer;
00087     delete [] m_tmpBuffer;
00088 }
00089 
00090 void DecimatorB::doAntiAlias(const double *src, double *dst, int length,
00091                              int filteridx)
00092 {
00093     vector<double> &o = m_o[filteridx];
00094 
00095     for (int i = 0; i < length; i++) {
00096 
00097         double input = src[i];
00098         double output = input * m_b[0] + o[0];
00099 
00100         o[0] = input * m_b[1] - output * m_a[1] + o[1];
00101         o[1] = input * m_b[2] - output * m_a[2] + o[2];
00102         o[2] = input * m_b[3] - output * m_a[3] + o[3];
00103         o[3] = input * m_b[4] - output * m_a[4] + o[4];
00104         o[4] = input * m_b[5] - output * m_a[5] + o[5];
00105         o[5] = input * m_b[6] - output * m_a[6];
00106 
00107         dst[i] = output;
00108     }
00109 }
00110 
00111 void DecimatorB::doProcess()
00112 {
00113     int filteridx = 0;
00114     int factorDone = 1;
00115     int factorRemaining = m_decFactor;
00116 
00117     while (factorDone < m_decFactor) {
00118 
00119         doAntiAlias(m_tmpBuffer, m_aaBuffer,
00120                     m_inputLength / factorDone,
00121                     filteridx);
00122 
00123         filteridx ++;
00124         factorDone *= 2;
00125 
00126         for (int i = 0; i < m_inputLength / factorDone; ++i) {
00127             m_tmpBuffer[i] = m_aaBuffer[i * 2];
00128         }
00129     }
00130 }
00131 
00132 void DecimatorB::process(const double *src, double *dst)
00133 {
00134     if (m_decFactor == 0) return;
00135 
00136     for (int i = 0; i < m_inputLength; ++i) {
00137         m_tmpBuffer[i] = src[i];
00138     }
00139 
00140     doProcess();
00141     
00142     for (int i = 0; i < m_outputLength; ++i) {
00143         dst[i] = m_tmpBuffer[i];
00144     }
00145 }
00146 
00147 void DecimatorB::process(const float *src, float *dst)
00148 {
00149     if (m_decFactor == 0) return;
00150 
00151     for (int i = 0; i < m_inputLength; ++i) {
00152         m_tmpBuffer[i] = src[i];
00153     }
00154 
00155     doProcess();
00156     
00157     for (int i = 0; i < m_outputLength; ++i) {
00158         dst[i] = m_tmpBuffer[i];
00159     }
00160 }