|
| 1 | +#include "filter.hpp" |
| 2 | + |
| 3 | +#include <algorithm> |
| 4 | +#include <cassert> |
| 5 | + |
| 6 | +using namespace std; |
| 7 | + |
| 8 | +#ifndef M_PI |
| 9 | +#define M_PI 3.14159265358979323846 |
| 10 | +#endif |
| 11 | + |
| 12 | +namespace filter |
| 13 | +{ |
| 14 | +// n��2���Ƃ���ΐ�(��̂�) |
| 15 | +int log2(int n) |
| 16 | +{ |
| 17 | + assert(n >= 1); |
| 18 | + int x = 0; |
| 19 | + while (n) |
| 20 | + { |
| 21 | + ++x; |
| 22 | + n >>= 1; |
| 23 | + } |
| 24 | + return x - 1; |
| 25 | +} |
| 26 | +// n��2���Ƃ���ΐ�(��グ) |
| 27 | +int log2_ceil(int n) |
| 28 | +{ |
| 29 | + assert(n >= 1); |
| 30 | + int x = 0; |
| 31 | + while (n > 1) |
| 32 | + { |
| 33 | + ++x; |
| 34 | + n >>= 1; |
| 35 | + } |
| 36 | + return x; |
| 37 | +} |
| 38 | + |
| 39 | +// �����t�[���G�ϊ� |
| 40 | +void fft(complex<double> dst[], const complex<double> src[], int n) |
| 41 | +{ |
| 42 | + assert(n >= 1); |
| 43 | + int size = pow2(n); |
| 44 | + vector<int> reversal(size); |
| 45 | + for (int i = 0; i < size; ++i) |
| 46 | + { |
| 47 | + int a = i, b = 0; |
| 48 | + for (int j = 0; j < n; ++j) |
| 49 | + { |
| 50 | + b <<= 1; |
| 51 | + b |= a & 1; |
| 52 | + a >>= 1; |
| 53 | + } |
| 54 | + reversal[i] = b; |
| 55 | + } |
| 56 | + vector<complex<double>> x0(&src[0], &src[size]); |
| 57 | + vector<complex<double>> x1(size); |
| 58 | + complex<double> A = exp(complex<double>(0, -2 * M_PI / size)); |
| 59 | + for (int r = 1; r <= n; ++r) |
| 60 | + { |
| 61 | + int n_r = n - r; |
| 62 | + int bit = pow2(n_r); |
| 63 | + for (int i = 0; i < size; ++i) |
| 64 | + { |
| 65 | + int s = i & ~((bit << 1) - 1); // s = i / (bit * 2) * (bit * 2); |
| 66 | + s = reversal[s]; |
| 67 | + s <<= n_r; // s *= bit; |
| 68 | + const complex<double> &src1 = x0[i & ~bit]; |
| 69 | + const complex<double> &src2 = x0[i | bit]; |
| 70 | + if (i & bit) |
| 71 | + { |
| 72 | + x1[i] = src1 - src2 * pow(A, s); |
| 73 | + } |
| 74 | + else |
| 75 | + { |
| 76 | + x1[i] = src1 + src2 * pow(A, s); |
| 77 | + } |
| 78 | + } |
| 79 | + if (r < n) |
| 80 | + { |
| 81 | + x0.swap(x1); |
| 82 | + } |
| 83 | + } |
| 84 | + for (int i = 0; i < size; ++i) |
| 85 | + { |
| 86 | + dst[i] = x1[reversal[i]]; |
| 87 | + } |
| 88 | +} |
| 89 | + |
| 90 | +// �t�����t�[���G�ϊ� |
| 91 | +void ifft(complex<double> dst[], const complex<double> src[], int n) |
| 92 | +{ |
| 93 | + assert(n >= 1); |
| 94 | + int size = pow2(n); |
| 95 | + vector<int> reversal(size); |
| 96 | + for (int i = 0; i < size; ++i) |
| 97 | + { |
| 98 | + int a = i, b = 0; |
| 99 | + for (int j = 0; j < n; ++j) |
| 100 | + { |
| 101 | + b <<= 1; |
| 102 | + b |= a & 1; |
| 103 | + a >>= 1; |
| 104 | + } |
| 105 | + reversal[i] = b; |
| 106 | + } |
| 107 | + vector<complex<double>> x0(&src[0], &src[size]); |
| 108 | + vector<complex<double>> x1(size); |
| 109 | + complex<double> A = exp(complex<double>(0, 2 * M_PI / size)); |
| 110 | + for (int r = 1; r <= n; ++r) |
| 111 | + { |
| 112 | + int n_r = n - r; |
| 113 | + int bit = pow2(n_r); |
| 114 | + for (int i = 0; i < size; i++) |
| 115 | + { |
| 116 | + int s = i & ~((bit << 1) - 1); // s = i / (bit * 2) * (bit * 2); |
| 117 | + s = reversal[s]; |
| 118 | + s <<= n_r; // s *= bit; |
| 119 | + const complex<double> &src1 = x0[i & ~bit]; |
| 120 | + const complex<double> &src2 = x0[i | bit]; |
| 121 | + if (i & bit) |
| 122 | + { |
| 123 | + x1[i] = src1 - src2 * pow(A, s); |
| 124 | + } |
| 125 | + else |
| 126 | + { |
| 127 | + x1[i] = src1 + src2 * pow(A, s); |
| 128 | + } |
| 129 | + } |
| 130 | + if (r < n) |
| 131 | + { |
| 132 | + x0.swap(x1); |
| 133 | + } |
| 134 | + } |
| 135 | + for (int i = 0; i < size; ++i) |
| 136 | + { |
| 137 | + dst[i] = x1[reversal[i]] / static_cast<double>(size); |
| 138 | + } |
| 139 | +} |
| 140 | + |
| 141 | +// �n�j���O�� |
| 142 | +void hanning_window(double dst[], const double src[], size_t n) |
| 143 | +{ |
| 144 | + double t = 2 * M_PI / n; |
| 145 | + for (size_t i = 0; i < n; ++i) |
| 146 | + { |
| 147 | + dst[i] = src[i] * (0.5 - 0.5 * cos(t * i)); |
| 148 | + } |
| 149 | +} |
| 150 | + |
| 151 | +// FIR�R���X�g���N�^ |
| 152 | +finite_impulse_response::finite_impulse_response() |
| 153 | +{ |
| 154 | + buffer.resize(1); |
| 155 | + h.assign(1, 1); |
| 156 | + pos = 0; |
| 157 | + hlen = 1; |
| 158 | +} |
| 159 | +// FIR�W���ݒ� |
| 160 | +void finite_impulse_response::set_impulse_response(const double *h_, size_t length) |
| 161 | +{ |
| 162 | + hlen = length; |
| 163 | + h.resize(pow2(log2_ceil(length))); |
| 164 | + for (size_t i = 0; i < length; ++i) |
| 165 | + { |
| 166 | + h[i] = static_cast<long>(h_[i] * (1 << 12)); |
| 167 | + } |
| 168 | + for (size_t i = length; i < h.size(); ++i) |
| 169 | + { |
| 170 | + h[i] = 0; |
| 171 | + } |
| 172 | + while (hlen > 1 && h[hlen - 1] == 0) |
| 173 | + { |
| 174 | + --hlen; |
| 175 | + } |
| 176 | + length = h.size(); |
| 177 | + if (buffer.size() < length) |
| 178 | + { |
| 179 | + size_t size = buffer.size(); |
| 180 | + size_t d = length - size; |
| 181 | + buffer.resize(length); |
| 182 | + memmove(&buffer[pos + d], &buffer[pos], sizeof(buffer[0]) * (size - pos)); |
| 183 | + memset(&buffer[pos], 0, sizeof(buffer[0]) * d); |
| 184 | + } |
| 185 | +} |
| 186 | +// FIR�t�B���^�K�p |
| 187 | +void finite_impulse_response::apply(int_least32_t *out, const int_least32_t *in, size_t length, std::size_t stride) |
| 188 | +{ |
| 189 | + std::size_t buflenmask = buffer.size() - 1; |
| 190 | + while (length > 0) |
| 191 | + { |
| 192 | + buffer[pos] = *in; |
| 193 | + pos = (pos + 1) & buflenmask; |
| 194 | + size_t offset = pos + buffer.size() - hlen; |
| 195 | + int_least32_t result = 0; |
| 196 | + for (size_t i = 0; i < hlen; ++i) |
| 197 | + { |
| 198 | + result += h[i] * buffer[(offset + i) & buflenmask] >> 12; |
| 199 | + } |
| 200 | + *out = result; |
| 201 | + in = reinterpret_cast<const int_least32_t *>(reinterpret_cast<const char *>(in) + stride); |
| 202 | + out = reinterpret_cast<int_least32_t *>(reinterpret_cast<char *>(out) + stride); |
| 203 | + --length; |
| 204 | + } |
| 205 | +} |
| 206 | + |
| 207 | +// �C�R���C�UFIR�t�B���^�쐬 |
| 208 | +void compute_equalizer_fir(double *h, std::size_t length, double rate, const std::map<double, double> &gains) |
| 209 | +{ |
| 210 | + for (std::size_t i = 0; i < length; ++i) |
| 211 | + { |
| 212 | + h[i] = 0; |
| 213 | + } |
| 214 | + if (gains.empty()) |
| 215 | + { |
| 216 | + h[0] = 1; |
| 217 | + } |
| 218 | + else |
| 219 | + { |
| 220 | + int h_bits = log2(length); |
| 221 | + size_t length = pow2(h_bits); |
| 222 | + size_t half_length = length / 2; |
| 223 | + std::map<double, double> gain_bounds; |
| 224 | + std::map<double, double>::const_iterator i = gains.begin(); |
| 225 | + gain_bounds[0] = i->second; |
| 226 | + for (;;) |
| 227 | + { |
| 228 | + double fL = i->first; |
| 229 | + double gL = i->second; |
| 230 | + ++i; |
| 231 | + if (i == gains.end()) |
| 232 | + { |
| 233 | + break; |
| 234 | + } |
| 235 | + double fR = i->first; |
| 236 | + double gR = i->second; |
| 237 | + double log_fL = log(fL); |
| 238 | + double log_fR = log(fR); |
| 239 | + const int n = 16; |
| 240 | + for (int i = 0; i < n; ++i) |
| 241 | + { |
| 242 | + double ft = (i + 0.5) / n; |
| 243 | + double f = exp(log_fL * (1 - ft) + log_fR * ft); |
| 244 | + double gt = static_cast<double>(i) / n; |
| 245 | + double g = gL * (1 - gt) + gR * gt; |
| 246 | + gain_bounds[f] = g; |
| 247 | + } |
| 248 | + } |
| 249 | + double T = 1 / rate; |
| 250 | + for (size_t k = 0; k < half_length; ++k) |
| 251 | + { |
| 252 | + double kT = k * T; |
| 253 | + double hk = 0; |
| 254 | + i = gain_bounds.begin(); |
| 255 | + while (i != gain_bounds.end()) |
| 256 | + { |
| 257 | + double gain = i->second; |
| 258 | + double f0 = i->first; |
| 259 | + ++i; |
| 260 | + double f1 = i == gain_bounds.end() ? rate / 2 : i->first; |
| 261 | + double w0 = f0 * 2 * M_PI; |
| 262 | + double w1 = f1 * 2 * M_PI; |
| 263 | + if (k == 0) |
| 264 | + { |
| 265 | + hk += gain * (w1 - w0 + (-w0) - (-w1)); |
| 266 | + } |
| 267 | + else |
| 268 | + { |
| 269 | + double w0kT = w0 * kT; |
| 270 | + double w1kT = w1 * kT; |
| 271 | + /* |
| 272 | + hk += + gain * exp(complex<double>(0, w1kT)) / complex<double>(0, kT) |
| 273 | + - gain * exp(complex<double>(0, w0kT)) / complex<double>(0, kT) |
| 274 | + + gain * exp(complex<double>(0, -w0kT)) / complex<double>(0, kT) |
| 275 | + - gain * exp(complex<double>(0, -w1kT)) / complex<double>(0, kT); |
| 276 | + */ |
| 277 | + hk += gain * (sin(w1kT) - sin(w0kT)) * 2 / kT; |
| 278 | + } |
| 279 | + } |
| 280 | + hk *= T / (2 * M_PI); |
| 281 | + h[half_length - 1 - k] = hk; |
| 282 | + h[half_length - 1 + k] = hk; |
| 283 | + } |
| 284 | + } |
| 285 | +} |
| 286 | +} // namespace filter |
0 commit comments