32 lines
1003 B
C++
32 lines
1003 B
C++
#include "filter.h"
|
|
#include "common.h"
|
|
#include <complex>
|
|
#include <cstdint>
|
|
|
|
using namespace Mengu;
|
|
using namespace dsp;
|
|
|
|
Complex Mengu::dsp::quad_filter_trans(Complex z, float a1, float a2, float b0, float b1, float b2) {
|
|
Complex z1 = std::conj(z); //z^-1
|
|
Complex z2 = z1 * z1;
|
|
return (b0 + b1 * z1 + b2 * z2) / (1.0f + a1 * z1 + a2 * z2);
|
|
}
|
|
|
|
BiquadFilter::BiquadFilter(float pa1, float pa2, float pb0, float pb1, float pb2) :
|
|
a1(pa1), a2(pa2), b0(pb0), b1(pb1), b2(pb2) {}
|
|
|
|
void BiquadFilter::transform(const float *input, float *output, uint32_t size) {
|
|
for (uint32_t i = 0; i < size; i++) {
|
|
float m = input[i] - a1 * _last_ms[_last_offset] - a2 * _last_ms[(_last_offset + 1) % 2];
|
|
output[i] = b0 * m + b1 * _last_ms[_last_offset] + b2 * _last_ms[(_last_offset + 1) % 2];
|
|
|
|
// push the last m
|
|
_last_offset = (_last_offset + 1) % 2;
|
|
_last_ms[_last_offset] = m;
|
|
}
|
|
}
|
|
|
|
void BiquadFilter::reset() {
|
|
_last_ms[0] = _last_ms[1] = 0.0f;
|
|
}
|