IIR resonant low pass filter

iir_filter.c

#include <stdlib.h>
#include <string.h>

#define _USE_MATH_DEFINES
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include "iir_filter.h"

typedef struct iir_filter
{
    float b02,  b1,  a1,  a2;
    
    float h0, h1;
    
    float flpf, last_flpf;
    float q_lin, filter_gain;
} iir_filter;

void * iir_filter_create()
{
    iir_filter * f = (iir_filter *) malloc(sizeof(iir_filter));
    if (f)
    {
        f->b02 = 0.0f; f->b1 = 0.0f; f->a1 = 0.0f; f->a2 = 0.0f;
        iir_filter_reset(f);
    }
    return f;
}

void * iir_filter_dup(const void *_f)
{
    void * nf = malloc(sizeof(iir_filter));
    if (nf)
        iir_filter_dup_inplace(nf, _f);
    return nf;
}

void iir_filter_dup_inplace(void * t, const void * s)
{
    memcpy(t, s, sizeof(iir_filter));
}

void iir_filter_destroy(void * _f)
{
    free(_f);
}

void iir_filter_reset(void * _f)
{
    if (_f)
    {
        iir_filter * f = (iir_filter *)_f;
        f->h0 = 0.0f; f->h1 = 0.0f;
        f->last_flpf = -1.0f;
    }
}

void iir_filter_set_q_db(void * _f, float q_db)
{
    if (_f)
    {
        iir_filter * f = (iir_filter *)_f;
        f->q_lin = (float) (pow(10.0f, q_db / 20.0f));
        f->filter_gain = (float) (1.0 / sqrt(f->q_lin));
        f->last_flpf = -1.0f;
    }
}

void iir_filter_set_lpf(void * _f, float lpf_freq)
{
    if (_f)
    {
        iir_filter * f = (iir_filter *)_f;
        f->flpf = lpf_freq;
        f->last_flpf = -1.0f;
    }
}

void iir_filter_calculate(void * _f, float output_freq)
{
    if (_f)
    {
        iir_filter * f = (iir_filter *)_f;
        float flpf = f->flpf;
        
        if (flpf > 0.45f * output_freq)
            flpf = 0.45f * output_freq;
        else if (flpf < 0.0001f * output_freq)
            flpf = 0.0001f * output_freq;
        
        if (fabs(flpf - f->last_flpf) > 0.01)
        {
            float omega, sin_coeff, cos_coeff, alpha_coeff, a0_inv;
            float a1, a2, b1, b02;
            
            f->last_flpf = flpf;
            
            omega = (float) (2.0f * M_PI * (flpf / output_freq));
            sin_coeff = (float) sin(omega);
            cos_coeff = (float) cos(omega);
            alpha_coeff = sin_coeff / (2.0f * f->q_lin);
            a0_inv = 1.0f / (1.0f + alpha_coeff);
            
            a1 = -2.0f * cos_coeff * a0_inv;
            a2 = (1.0f - alpha_coeff) * a0_inv;
            b1 = (1.0f - cos_coeff) * a0_inv * f->filter_gain;
            b02 = b1 * 0.5f;
            
            f->a1 = a1;
            f->a2 = a2;
            f->b1 = b1;
            f->b02 = b02;
        }
    }
}

void iir_filter_apply(void * _f, float * buf, unsigned int count)
{
    if (_f)
    {
        iir_filter * f = (iir_filter *)_f;
        float h0 = f->h0, h1 = f->h1;
        float a1 = f->a1, a2 = f->a2, b02 = f->b02, b1 = f->b1;
        
        float centernode;
        
        unsigned int i;
        
        if (fabs(h0) < 1e-20) h0 = 0.0f;
        
        for (i = 0; i < count; ++i)
        {
            centernode = buf[i] - a1 * h0 - a2 * h1;
            buf[i] = b02 * (centernode + h1) + b1 * h0;
            h1 = h0;
            h0 = centernode;
        }
        
        f->h0 = h0; f->h1 = h1;
    }
}

iir_filter.h

#ifndef _IIR_FILTER_H_
#define _IIR_FILTER_H_

#ifdef IIRFILTER_DECORATE
#define PASTE(a,b) a ## b
#define EVALUATE(a,b) PASTE(a,b)
#define iir_filter_create EVALUATE(IIRFILTER_DECORATE,_iir_filter_create)
#define iir_filter_dup EVALUATE(IIRFILTER_DECORATE,_iir_filter_dup)
#define iir_filter_dup_inplace EVALUATE(IIRFILTER_DECORATE,_iir_filter_dup_inplace)
#define iir_filter_destroy EVALUATE(IIRFILTER_DECORATE,_iir_filter_destroy)
#define iir_filter_reset EVALUATE(IIRFILTER_DECORATE,_iir_filter_reset)
#define iir_filter_set_q_db EVALUATE(IIRFILTER_DECORATE,_iir_filter_set_q_db)
#define iir_filter_set_lpf EVALUATE(IIRFILTER_DECORATE,_iir_filter_set_lpf)
#define iir_filter_calculate EVALUATE(IIRFILTER_DECORATE,_iir_filter_calculate)
#define iir_filter_apply EVALUATE(IIRFILTER_DECORATE,_iir_filter_apply)
#endif

void * iir_filter_create();
void * iir_filter_dup(const void *);
void iir_filter_dup_inplace(void *, const void *);
void iir_filter_destroy(void *);

void iir_filter_reset(void *);

void iir_filter_set_q_db(void *, float q_db);
void iir_filter_set_lpf(void *, float lpf_freq);

void iir_filter_calculate(void *, float output_freq);

void iir_filter_apply(void *, float * buf, unsigned int count);

#endif