Add second order low shelf filter.

2026-04-22 00:06:59 +02:00 · 2020-02-23 21:03:22 +10:00
parent b6d0591042
commit 5b385670f3
3 changed files with 346 additions and 7 deletions
@@ -1898,6 +1898,34 @@ ma_result ma_peak2_process_pcm_frames(ma_peak2* pFilter, void* pFramesOut, const
 ma_uint32 ma_peak2_get_latency(ma_peak2* pFilter);


+/**************************************************************************************************************************************************************
+
+Low Shelf Filter
+
+**************************************************************************************************************************************************************/
+typedef struct
+{
+    ma_format format;
+    ma_uint32 channels;
+    ma_uint32 sampleRate;
+    double gainDB;
+    double shelfSlope;
+    double frequency;
+} ma_loshelf2_config;
+
+ma_loshelf2_config ma_loshelf2_config_init(ma_format format, ma_uint32 channels, ma_uint32 sampleRate, double gainDB, double shelfSlope, double frequency);
+
+typedef struct
+{
+    ma_biquad bq;
+} ma_loshelf2;
+
+ma_result ma_loshelf2_init(const ma_loshelf2_config* pConfig, ma_loshelf2* pFilter);
+ma_result ma_loshelf2_reinit(const ma_loshelf2_config* pConfig, ma_loshelf2* pFilter);
+ma_result ma_loshelf2_process_pcm_frames(ma_loshelf2* pFilter, void* pFramesOut, const void* pFramesIn, ma_uint64 frameCount);
+ma_uint32 ma_loshelf2_get_latency(ma_loshelf2* pFilter);
+
+

 /************************************************************************************************************************************************************
 *************************************************************************************************************************************************************
@@ -5725,11 +5753,6 @@ static MA_INLINE double ma_sin(double x)
    return sin(x);
 }

-static MA_INLINE double ma_cos(double x)
-{
-    return ma_sin((MA_PI*0.5) - x);
-}
-
 static MA_INLINE double ma_exp(double x)
 {
    /* TODO: Implement custom exp(x). */
@@ -5748,6 +5771,18 @@ static MA_INLINE double ma_pow(double x, double y)
    return pow(x, y);
 }

+static MA_INLINE double ma_sqrt(double x)
+{
+    /* TODO: Implement custom sqrt(x). */
+    return sqrt(x);
+}
+
+
+static MA_INLINE double ma_cos(double x)
+{
+    return ma_sin((MA_PI*0.5) - x);
+}
+
 static MA_INLINE double ma_log10(double x)
 {
    return ma_log(x) * 0.43429448190325182765;
@@ -31148,6 +31183,135 @@ ma_uint32 ma_peak2_get_latency(ma_peak2* pFilter)
 }


+/**************************************************************************************************************************************************************
+
+Low Shelf Filter
+
+**************************************************************************************************************************************************************/
+ma_loshelf2_config ma_loshelf2_config_init(ma_format format, ma_uint32 channels, ma_uint32 sampleRate, double gainDB, double shelfSlope, double frequency)
+{
+    ma_loshelf2_config config;
+
+    MA_ZERO_OBJECT(&config);
+    config.format     = format;
+    config.channels   = channels;
+    config.sampleRate = sampleRate;
+    config.gainDB     = gainDB;
+    config.shelfSlope = shelfSlope;
+    config.frequency  = frequency;
+
+    if (config.gainDB == 0) {
+        config.gainDB = 6;
+    }
+
+    return config;
+}
+
+
+static MA_INLINE ma_biquad_config ma_loshelf2__get_biquad_config(const ma_loshelf2_config* pConfig)
+{
+    ma_biquad_config bqConfig;
+    double w;
+    double s;
+    double c;
+    double A;
+    double S;
+    double a;
+    double sqrtA;
+
+    MA_ASSERT(pConfig != NULL);
+
+    w = 2 * MA_PI_D * pConfig->frequency / pConfig->sampleRate;
+    s = ma_sin(w);
+    c = ma_cos(w);
+    A = ma_pow(10, (pConfig->gainDB / 40));
+    S = pConfig->shelfSlope;
+    a = s/2 * ma_sqrt((A + 1/A) * (1/S - 1) + 2);
+    sqrtA = 2*ma_sqrt(A)*a;
+
+    bqConfig.b0 =  A * ((A + 1) - (A - 1)*c + sqrtA);
+    bqConfig.b1 =  2 * A * ((A - 1) - (A + 1)*c);
+    bqConfig.b2 =  A * ((A + 1) - (A - 1)*c - sqrtA);
+    bqConfig.a0 =  (A + 1) + (A - 1)*c + sqrtA;
+    bqConfig.a1 = -2 * ((A - 1) + (A + 1)*c);
+    bqConfig.a2 =  (A + 1) + (A - 1)*c - sqrtA;
+
+    bqConfig.format   = pConfig->format;
+    bqConfig.channels = pConfig->channels;
+
+    return bqConfig;
+}
+
+ma_result ma_loshelf2_init(const ma_loshelf2_config* pConfig, ma_loshelf2* pFilter)
+{
+    ma_result result;
+    ma_biquad_config bqConfig;
+
+    if (pFilter == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    MA_ZERO_OBJECT(pFilter);
+
+    if (pConfig == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    bqConfig = ma_loshelf2__get_biquad_config(pConfig);
+    result = ma_biquad_init(&bqConfig, &pFilter->bq);
+    if (result != MA_SUCCESS) {
+        return result;
+    }
+
+    return MA_SUCCESS;
+}
+
+ma_result ma_loshelf2_reinit(const ma_loshelf2_config* pConfig, ma_loshelf2* pFilter)
+{
+    ma_result result;
+    ma_biquad_config bqConfig;
+
+    if (pFilter == NULL || pConfig == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    bqConfig = ma_loshelf2__get_biquad_config(pConfig);
+    result = ma_biquad_reinit(&bqConfig, &pFilter->bq);
+    if (result != MA_SUCCESS) {
+        return result;
+    }
+
+    return MA_SUCCESS;
+}
+
+static MA_INLINE void ma_loshelf2_process_pcm_frame_s16(ma_loshelf2* pFilter, ma_int16* pFrameOut, const ma_int16* pFrameIn)
+{
+    ma_biquad_process_pcm_frame_s16(&pFilter->bq, pFrameOut, pFrameIn);
+}
+
+static MA_INLINE void ma_loshelf2_process_pcm_frame_f32(ma_loshelf2* pFilter, float* pFrameOut, const float* pFrameIn)
+{
+    ma_biquad_process_pcm_frame_f32(&pFilter->bq, pFrameOut, pFrameIn);
+}
+
+ma_result ma_loshelf2_process_pcm_frames(ma_loshelf2* pFilter, void* pFramesOut, const void* pFramesIn, ma_uint64 frameCount)
+{
+    if (pFilter == NULL) {
+        return MA_INVALID_ARGS;
+    }
+
+    return ma_biquad_process_pcm_frames(&pFilter->bq, pFramesOut, pFramesIn, frameCount);
+}
+
+ma_uint32 ma_loshelf2_get_latency(ma_loshelf2* pFilter)
+{
+    if (pFilter == NULL) {
+        return 0;
+    }
+
+    return ma_biquad_get_latency(&pFilter->bq);
+}
+


 /**************************************************************************************************************************************************************
@@ -26,8 +26,9 @@ ma_result filtering_init_decoder_and_encoder(const char* pInputFilePath, const c
 #include "ma_test_filtering_lpf.c"
 #include "ma_test_filtering_hpf.c"
 #include "ma_test_filtering_bpf.c"
-#include "ma_test_filtering_peak.c"
 #include "ma_test_filtering_notch.c"
+#include "ma_test_filtering_peak.c"
+#include "ma_test_filtering_loshelf.c"

 int main(int argc, char** argv)
 {
@@ -59,17 +60,23 @@ int main(int argc, char** argv)
        hasError = MA_TRUE;
    }

+    result = ma_register_test("Notching Filtering", test_entry__notch);
+    if (result != MA_SUCCESS) {
+        hasError = MA_TRUE;
+    }
+
    result = ma_register_test("Peaking EQ Filtering", test_entry__peak);
    if (result != MA_SUCCESS) {
        hasError = MA_TRUE;
    }
 #endif

-    result = ma_register_test("Notching Filtering", test_entry__notch);
+    result = ma_register_test("Low Shelf Filtering", test_entry__loshelf);
    if (result != MA_SUCCESS) {
        hasError = MA_TRUE;
    }
    
+
    for (iTest = 0; iTest < g_Tests.count; iTest += 1) {
        printf("=== BEGIN %s ===\n", g_Tests.pTests[iTest].pName);
        result = g_Tests.pTests[iTest].onEntry(argc, argv);
@@ -0,0 +1,168 @@
+ma_result loshelf_init_decoder_and_encoder(const char* pInputFilePath, const char* pOutputFilePath, ma_format format, ma_decoder* pDecoder, ma_encoder* pEncoder)
+{
+    return filtering_init_decoder_and_encoder(pInputFilePath, pOutputFilePath, format, 0, 0, pDecoder, pEncoder);
+}
+
+
+ma_result test_loshelf2__by_format(const char* pInputFilePath, const char* pOutputFilePath, ma_format format)
+{
+    ma_result result;
+    ma_decoder decoder;
+    ma_encoder encoder;
+    ma_loshelf2_config loshelfConfig;
+    ma_loshelf2 loshelf;
+
+    printf("    %s\n", pOutputFilePath);
+
+    result = loshelf_init_decoder_and_encoder(pInputFilePath, pOutputFilePath, format, &decoder, &encoder);
+    if (result != MA_SUCCESS) {
+        return result;
+    }
+
+    loshelfConfig = ma_loshelf2_config_init(decoder.outputFormat, decoder.outputChannels, decoder.outputSampleRate, 6, 1, 1000);
+    result = ma_loshelf2_init(&loshelfConfig, &loshelf);
+    if (result != MA_SUCCESS) {
+        ma_decoder_uninit(&decoder);
+        ma_encoder_uninit(&encoder);
+        return result;
+    }
+
+    for (;;) {
+        ma_uint8 tempIn[4096];
+        ma_uint8 tempOut[4096];
+        ma_uint64 tempCapIn  = sizeof(tempIn)  / ma_get_bytes_per_frame(decoder.outputFormat, decoder.outputChannels);
+        ma_uint64 tempCapOut = sizeof(tempOut) / ma_get_bytes_per_frame(decoder.outputFormat, decoder.outputChannels);
+        ma_uint64 framesToRead;
+        ma_uint64 framesJustRead;
+
+        framesToRead = ma_min(tempCapIn, tempCapOut);
+        framesJustRead = ma_decoder_read_pcm_frames(&decoder, tempIn, framesToRead);
+
+        /* Filter */
+        ma_loshelf2_process_pcm_frames(&loshelf, tempOut, tempIn, framesJustRead);
+
+        /* Write to the WAV file. */
+        ma_encoder_write_pcm_frames(&encoder, tempOut, framesJustRead);
+
+        if (framesJustRead < framesToRead) {
+            break;
+        }
+    }
+
+    ma_encoder_uninit(&encoder);
+    return MA_SUCCESS;
+}
+
+ma_result test_loshelf2__f32(const char* pInputFilePath)
+{
+    return test_loshelf2__by_format(pInputFilePath, "output/loshelf2_f32.wav", ma_format_f32);
+}
+
+ma_result test_loshelf2__s16(const char* pInputFilePath)
+{
+    return test_loshelf2__by_format(pInputFilePath, "output/loshelf2_s16.wav", ma_format_s16);
+}
+
+#if 0
+ma_result test_loshelf4__by_format(const char* pInputFilePath, const char* pOutputFilePath, ma_format format)
+{
+    ma_result result;
+    ma_decoder decoder;
+    ma_encoder encoder;
+    ma_loshelf_config loshelfConfig;
+    ma_loshelf loshelf;
+
+    printf("    %s\n", pOutputFilePath);
+
+    result = loshelf_init_decoder_and_encoder(pInputFilePath, pOutputFilePath, format, &decoder, &encoder);
+    if (result != MA_SUCCESS) {
+        return result;
+    }
+
+    loshelfConfig = ma_loshelf_config_init(decoder.outputFormat, decoder.outputChannels, decoder.outputSampleRate, 2000, 4);
+    result = ma_loshelf_init(&loshelfConfig, &loshelf);
+    if (result != MA_SUCCESS) {
+        ma_decoder_uninit(&decoder);
+        ma_encoder_uninit(&encoder);
+        return result;
+    }
+
+    for (;;) {
+        ma_uint8 tempIn[4096];
+        ma_uint8 tempOut[4096];
+        ma_uint64 tempCapIn  = sizeof(tempIn)  / ma_get_bytes_per_frame(decoder.outputFormat, decoder.outputChannels);
+        ma_uint64 tempCapOut = sizeof(tempOut) / ma_get_bytes_per_frame(decoder.outputFormat, decoder.outputChannels);
+        ma_uint64 framesToRead;
+        ma_uint64 framesJustRead;
+
+        framesToRead = ma_min(tempCapIn, tempCapOut);
+        framesJustRead = ma_decoder_read_pcm_frames(&decoder, tempIn, framesToRead);
+
+        /* Filter */
+        ma_loshelf_process_pcm_frames(&loshelf, tempOut, tempIn, framesJustRead);
+
+        /* Write to the WAV file. */
+        ma_encoder_write_pcm_frames(&encoder, tempOut, framesJustRead);
+
+        if (framesJustRead < framesToRead) {
+            break;
+        }
+    }
+
+    ma_encoder_uninit(&encoder);
+    return MA_SUCCESS;
+}
+
+ma_result test_loshelf4__f32(const char* pInputFilePath)
+{
+    return test_loshelf4__by_format(pInputFilePath, "output/loshelf4_f32.wav", ma_format_f32);
+}
+
+ma_result test_loshelf4__s16(const char* pInputFilePath)
+{
+    return test_loshelf4__by_format(pInputFilePath, "output/loshelf4_s16.wav", ma_format_s16);
+}
+#endif
+
+int test_entry__loshelf(int argc, char** argv)
+{
+    ma_result result;
+    ma_bool32 hasError = MA_FALSE;
+    const char* pInputFilePath;
+
+    if (argc < 2) {
+        printf("No input file.\n");
+        return -1;
+    }
+
+    pInputFilePath = argv[1];
+    
+
+    result = test_loshelf2__f32(pInputFilePath);
+    if (result != MA_SUCCESS) {
+        hasError = MA_TRUE;
+    }
+
+    result = test_loshelf2__s16(pInputFilePath);
+    if (result != MA_SUCCESS) {
+        hasError = MA_TRUE;
+    }
+
+#if 0
+    result = test_loshelf4__f32(pInputFilePath);
+    if (result != MA_SUCCESS) {
+        hasError = MA_TRUE;
+    }
+
+    result = test_loshelf4__s16(pInputFilePath);
+    if (result != MA_SUCCESS) {
+        hasError = MA_TRUE;
+    }
+#endif
+
+    if (hasError) {
+        return -1;
+    } else {
+        return 0;
+    }
+}