shithub: pt2-clone

ref: 4f4c60b34d48ac94175de4939c2fe07e73c348c5
dir: /src/pt2_rcfilters.c/

#include "pt2_math.h"
#include "pt2_rcfilters.h"

#define SMALL_NUMBER (1E-4)

/* 1-pole RC low-pass/high-pass filter, based on:
** https://www.musicdsp.org/en/latest/Filters/116-one-pole-lp-and-hp.html
*/

void setupOnePoleFilter(double audioRate, double cutOff, onePoleFilter_t *f)
{
	if (cutOff >= audioRate/2.0)
		cutOff = (audioRate/2.0) - SMALL_NUMBER;

	const double a = 2.0 - pt2_cos((PT2_TWO_PI * cutOff) / audioRate);
	const double b = a - pt2_sqrt((a * a) - 1.0);

	f->a1 = 1.0 - b;
	f->a2 = b;
}

void clearOnePoleFilterState(onePoleFilter_t *f)
{
	f->tmpL = f->tmpR = 0.0;
}

void onePoleLPFilter(onePoleFilter_t *f, const double in, double *out)
{
	f->tmpL = (f->a1 * in) + (f->a2 * f->tmpL);
	*out = f->tmpL;
}

void onePoleLPFilterStereo(onePoleFilter_t *f, const double *in, double *out)
{
	// left channel
	f->tmpL = (f->a1 * in[0]) + (f->a2 * f->tmpL);
	out[0] = f->tmpL;

	// right channel
	f->tmpR = (f->a1 * in[1]) + (f->a2 * f->tmpR);
	out[1] = f->tmpR;
}

void onePoleHPFilter(onePoleFilter_t *f, const double in, double *out)
{
	f->tmpL = (f->a1 * in) + (f->a2 * f->tmpL);
	*out = in - f->tmpL;
}

void onePoleHPFilterStereo(onePoleFilter_t *f, const double *in, double *out)
{
	// left channel
	f->tmpL = (f->a1 * in[0]) + (f->a2 * f->tmpL);
	out[0] = in[0] - f->tmpL;

	// right channel
	f->tmpR = (f->a1 * in[1]) + (f->a2 * f->tmpR);
	out[1] = in[1] - f->tmpR;
}

/* 2-pole RC low-pass filter with Q factor, based on:
** https://www.musicdsp.org/en/latest/Filters/38-lp-and-hp-filter.html
*/

void setupTwoPoleFilter(double audioRate, double cutOff, double qFactor, twoPoleFilter_t *f)
{
	if (cutOff >= audioRate/2.0)
		cutOff = (audioRate/2.0) - SMALL_NUMBER;

	const double a = 1.0 / pt2_tan((PT2_PI * cutOff) / audioRate);
	const double b = 1.0 / qFactor;

	f->a1 = 1.0 / (1.0 + b * a + a * a);
	f->a2 = 2.0 * f->a1;
	f->b1 = 2.0 * (1.0 - a*a) * f->a1;
	f->b2 = (1.0 - b * a + a * a) * f->a1;
}

void clearTwoPoleFilterState(twoPoleFilter_t *f)
{
	f->tmpL[0] = f->tmpL[1] = f->tmpL[2] = f->tmpL[3] = 0.0;
	f->tmpR[0] = f->tmpR[1] = f->tmpR[2] = f->tmpR[3] = 0.0;
}

void twoPoleLPFilter(twoPoleFilter_t *f, const double in, double *out)
{
	const double LOut = (f->a1 * in) + (f->a2 * f->tmpL[0]) + (f->a1 * f->tmpL[1]) - (f->b1 * f->tmpL[2]) - (f->b2 * f->tmpL[3]);

	// shift states

	f->tmpL[1] = f->tmpL[0];
	f->tmpL[0] = in;
	f->tmpL[3] = f->tmpL[2];
	f->tmpL[2] = LOut;

	// set output

	*out = LOut;
}

void twoPoleLPFilterStereo(twoPoleFilter_t *f, const double *in, double *out)
{
	const double LOut = (f->a1 * in[0]) + (f->a2 * f->tmpL[0]) + (f->a1 * f->tmpL[1]) - (f->b1 * f->tmpL[2]) - (f->b2 * f->tmpL[3]);
	const double ROut = (f->a1 * in[1]) + (f->a2 * f->tmpR[0]) + (f->a1 * f->tmpR[1]) - (f->b1 * f->tmpR[2]) - (f->b2 * f->tmpR[3]);

	// shift states

	f->tmpL[1] = f->tmpL[0];
	f->tmpL[0] = in[0];
	f->tmpL[3] = f->tmpL[2];
	f->tmpL[2] = LOut;

	f->tmpR[1] = f->tmpR[0];
	f->tmpR[0] = in[1];
	f->tmpR[3] = f->tmpR[2];
	f->tmpR[2] = ROut;

	// set output

	out[0] = LOut;
	out[1] = ROut;
}