Add experimental code for biquad and low-pass filters.

cdigit · Jan 9, 2020 · 2b526f6 · 2b526f6
1 parent 45499c8
commit 2b526f6
Show file tree

Hide file tree

Showing 5 changed files with 411 additions and 48 deletions.
diff --git a/research/ma_lpf.h b/research/ma_lpf.h
@@ -0,0 +1,237 @@
+#ifndef ma_lpf_h
+#define ma_lpf_h
+
+/*
+TODO:
+  - Document passthrough behaviour of the biquad filter and how it doesn't update previous inputs and outputs.
+  - Document how changing biquad constants requires reinitialization of the filter (due to issue above).
+*/
+
+typedef struct
+{
+    ma_format format;
+    ma_uint32 channels;
+    float a0;
+    float a1;
+    float a2;
+    float b0;
+    float b1;
+    float b2;
+} ma_biquad_config;
+
+ma_biquad_config ma_biquad_config_init(ma_format format, ma_uint32 channels, float a0, float a1, float a2, float b0, float b1, float b2);
+
+typedef struct
+{
+    ma_biquad_config config;
+    ma_bool32 isPassthrough;
+    ma_uint32 prevFrameCount;
+    float x1[MA_MAX_CHANNELS];   /* x[n-1] */
+    float x2[MA_MAX_CHANNELS];   /* x[n-2] */
+    float y1[MA_MAX_CHANNELS];   /* y[n-1] */
+    float y2[MA_MAX_CHANNELS];   /* y[n-2] */
+} ma_biquad;
+
+ma_result ma_biquad_init(const ma_biquad_config* pConfig, ma_biquad* pBQ);
+ma_result ma_biquad_process(ma_biquad* pBQ, void* pFramesOut, const void* pFramesIn, ma_uint64 frameCount);
+
+
+typedef struct
+{
+    ma_format format;
+    ma_uint32 channels;
+    ma_uint32 sampleRate;
+    ma_uint32 cutoffFrequency;
+} ma_lpf_config;
+
+ma_lpf_config ma_lpf_config_init(ma_format format, ma_uint32 channels, ma_uint32 sampleRate, ma_uint32 cutoffFrequency);
+
+typedef struct
+{
+    ma_biquad bq;   /* The low-pass filter is implemented as a biquad filter. */
+    ma_lpf_config config;
+} ma_lpf;
+
+ma_result ma_lpf_init(const ma_lpf_config* pConfig, ma_lpf* pLPF);
+ma_result ma_lpf_process(ma_lpf* pLPF, void* pFramesOut, const void* pFramesIn, ma_uint64 frameCount);
+
+#endif  /* ma_lpf_h */
+
+
+
+
+#if defined(MINIAUDIO_IMPLEMENTATION)
+
+ma_biquad_config ma_biquad_config_init(ma_format format, ma_uint32 channels, float a0, float a1, float a2, float b0, float b1, float b2)
+{
+    ma_biquad_config config;
+
+    MA_ZERO_OBJECT(&config);
+    config.format = format;
+    config.channels = channels;
+    config.a0 = a0;
+    config.a1 = a1;
+    config.a2 = a2;
+    config.b0 = b0;
+    config.b1 = b1;
+    config.b2 = b2;
+
+    return config;
+}
+
+ma_result ma_biquad_init(const ma_biquad_config* pConfig, ma_biquad* pBQ)
+{
+    if (pBQ == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    MA_ZERO_OBJECT(pBQ);
+
+    if (pConfig == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    if (pConfig->a0 == 0) {
+        return MA_INVALID_ARGS; /* Division by zero. */
+    }
+
+    /* Currently only supporting f32, but support for other formats will be added later. */
+    if (pConfig->format != ma_format_f32) {
+        return MA_INVALID_ARGS;
+    }
+
+    pBQ->config = *pConfig;
+
+    if (pConfig->a0 == 1 && pConfig->a1 == 0 && pConfig->a2 == 0 &&
+        pConfig->b0 == 1 && pConfig->b1 == 0 && pConfig->b2 == 0) {
+        pBQ->isPassthrough = MA_TRUE;
+    }
+
+    /* Normalize. */
+    pBQ->config.a1 /= pBQ->config.a0;
+    pBQ->config.a2 /= pBQ->config.a0;
+    pBQ->config.b0 /= pBQ->config.a0;
+    pBQ->config.b1 /= pBQ->config.a0;
+    pBQ->config.b2 /= pBQ->config.a0;
+
+    return MA_SUCCESS;
+}
+
+ma_result ma_biquad_process(ma_biquad* pBQ, void* pFramesOut, const void* pFramesIn, ma_uint64 frameCount)
+{
+    ma_uint32 n;
+    ma_uint32 c;
+    float a1 = pBQ->config.a1;
+    float a2 = pBQ->config.a2;
+    float b0 = pBQ->config.b0;
+    float b1 = pBQ->config.b1;
+    float b2 = pBQ->config.b2;
+
+    if (pBQ == NULL || pFramesOut == NULL || pFramesIn == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    /* Fast path for passthrough. */
+    if (pBQ->isPassthrough) {
+        ma_copy_memory_64(pFramesOut, pFramesIn, frameCount * ma_get_bytes_per_frame(pBQ->config.format, pBQ->config.channels));
+        return MA_SUCCESS;
+    }
+
+    /* Currently only supporting f32. */
+    if (pBQ->config.format == ma_format_f32) {
+              float* pY = (      float*)pFramesOut;
+        const float* pX = (const float*)pFramesIn;
+
+        for (n = 0; n < frameCount; n += 1) {
+            for (c = 0; c < pBQ->config.channels; c += 1) {
+                float x2 = pBQ->x2[c];
+                float x1 = pBQ->x1[c];
+                float x0 = pX[n*pBQ->config.channels + c];
+                float y2 = pBQ->y2[c];
+                float y1 = pBQ->y1[c];
+                float y0 = b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2;
+
+                pY[n*pBQ->config.channels + c] = y0;
+                pBQ->x2[c] = x1;
+                pBQ->x1[c] = x0;
+                pBQ->y2[c] = y1;
+                pBQ->y1[c] = y0;
+            }
+        }
+    } else {
+        return MA_INVALID_ARGS; /* Format not supported. Should never hit this because it's checked in ma_biquad_init(). */
+    }
+
+    return MA_SUCCESS;
+}
+
+
+ma_lpf_config ma_lpf_config_init(ma_format format, ma_uint32 channels, ma_uint32 sampleRate, ma_uint32 cutoffFrequency)
+{
+    ma_lpf_config config;
+
+    MA_ZERO_OBJECT(&config);
+    config.format = format;
+    config.channels = channels;
+    config.sampleRate = sampleRate;
+    config.cutoffFrequency = cutoffFrequency;
+
+    return config;
+}
+
+ma_result ma_lpf_init(const ma_lpf_config* pConfig, ma_lpf* pLPF)
+{
+    ma_result result;
+    ma_biquad_config bqConfig;
+    double q;
+    double w;
+    double s;
+    double c;
+    double a;
+
+    if (pLPF == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    MA_ZERO_OBJECT(pLPF);
+
+    if (pConfig == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    pLPF->config = *pConfig;
+
+    q = 1 / sqrt(2);
+    w = 2 * MA_PI_D * pConfig->cutoffFrequency / pConfig->sampleRate;
+    s = sin(w);
+    c = cos(w);
+    a = s / (2*q);
+
+    bqConfig.a0 = (float)( 1 + a);
+    bqConfig.a1 = (float)(-2 * c);
+    bqConfig.a2 = (float)( 1 - a);
+    bqConfig.b0 = (float)((1 - c) / 2);
+    bqConfig.b1 = (float)( 1 - c);
+    bqConfig.b2 = (float)((1 - c) / 2);
+
+    bqConfig.format = pConfig->format;
+    bqConfig.channels = pConfig->channels;
+
+    result = ma_biquad_init(&bqConfig, &pLPF->bq);
+    if (result != MA_SUCCESS) {
+        return result;
+    }
+
+    return MA_SUCCESS;
+}
+
+ma_result ma_lpf_process(ma_lpf* pLPF, void* pFramesOut, const void* pFramesIn, ma_uint64 frameCount)
+{
+    if (pLPF == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    return ma_biquad_process(&pLPF->bq, pFramesOut, pFramesIn, frameCount);
+}
+
+#endif