126 lines
2.9 KiB
C++
126 lines
2.9 KiB
C++
/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*- vi:set ts=8 sts=4 sw=4: */
|
|
|
|
/*
|
|
QM DSP Library
|
|
|
|
Centre for Digital Music, Queen Mary, University of London.
|
|
|
|
This program is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU General Public License as
|
|
published by the Free Software Foundation; either version 2 of the
|
|
License, or (at your option) any later version. See the file
|
|
COPYING included with this distribution for more information.
|
|
*/
|
|
|
|
#include "Filter.h"
|
|
|
|
#include <stdexcept>
|
|
|
|
using namespace std;
|
|
|
|
Filter::Filter(Parameters params)
|
|
{
|
|
if (params.a.empty()) {
|
|
m_fir = true;
|
|
if (params.b.empty()) {
|
|
throw logic_error("Filter must have at least one pair of coefficients");
|
|
}
|
|
} else {
|
|
m_fir = false;
|
|
if (params.a.size() != params.b.size()) {
|
|
throw logic_error("Inconsistent numbers of filter coefficients");
|
|
}
|
|
}
|
|
|
|
m_sz = int(params.b.size());
|
|
m_order = m_sz - 1;
|
|
|
|
m_a = params.a;
|
|
m_b = params.b;
|
|
|
|
// We keep some empty space at the start of the buffer, and
|
|
// encroach gradually into it as we add individual sample
|
|
// calculations at the start. Then when we run out of space, we
|
|
// move the buffer back to the end and begin again. This is
|
|
// significantly faster than moving the whole buffer along in
|
|
// 1-sample steps every time.
|
|
|
|
m_offmax = 20;
|
|
m_offa = m_offmax;
|
|
m_offb = m_offmax;
|
|
|
|
if (!m_fir) {
|
|
m_bufa.resize(m_order + m_offmax);
|
|
}
|
|
|
|
m_bufb.resize(m_sz + m_offmax);
|
|
}
|
|
|
|
Filter::~Filter()
|
|
{
|
|
}
|
|
|
|
void
|
|
Filter::reset()
|
|
{
|
|
m_offb = m_offmax;
|
|
m_offa = m_offmax;
|
|
|
|
if (!m_fir) {
|
|
m_bufa.assign(m_bufa.size(), 0.0);
|
|
}
|
|
|
|
m_bufb.assign(m_bufb.size(), 0.0);
|
|
}
|
|
|
|
void
|
|
Filter::process(const double *const __restrict in,
|
|
double *const __restrict out,
|
|
const int n)
|
|
{
|
|
for (int s = 0; s < n; ++s) {
|
|
|
|
if (m_offb > 0) --m_offb;
|
|
else {
|
|
for (int i = m_sz - 2; i >= 0; --i) {
|
|
m_bufb[i + m_offmax + 1] = m_bufb[i];
|
|
}
|
|
m_offb = m_offmax;
|
|
}
|
|
m_bufb[m_offb] = in[s];
|
|
|
|
double b_sum = 0.0;
|
|
for (int i = 0; i < m_sz; ++i) {
|
|
b_sum += m_b[i] * m_bufb[i + m_offb];
|
|
}
|
|
|
|
double outval;
|
|
|
|
if (m_fir) {
|
|
|
|
outval = b_sum;
|
|
|
|
} else {
|
|
|
|
double a_sum = 0.0;
|
|
for (int i = 0; i < m_order; ++i) {
|
|
a_sum += m_a[i + 1] * m_bufa[i + m_offa];
|
|
}
|
|
|
|
outval = b_sum - a_sum;
|
|
|
|
if (m_offa > 0) --m_offa;
|
|
else {
|
|
for (int i = m_order - 2; i >= 0; --i) {
|
|
m_bufa[i + m_offmax + 1] = m_bufa[i];
|
|
}
|
|
m_offa = m_offmax;
|
|
}
|
|
m_bufa[m_offa] = outval;
|
|
}
|
|
|
|
out[s] = outval;
|
|
}
|
|
}
|
|
|