blob: e9523e229d1160480a9cc8a7e5508f17e150974d (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
|
/* -*- 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;
}
}
|
