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 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 }