public static bqd_filter bqd_hp_init(double Fs, double Fc)
{
bqd_filter fp = new bqd_filter();
double n, w;
if (Fs < Fc * 2.0)
{
throw new InvalidOperationException($"fo_init: cutoff frequency ({Fc:N1}) should be less than half of the sampling rate ({Fs:N2})");
}
w = Math.Tan(Math.PI * Fc / Fs);
n = 1.0 / (1.0 + w);
fp.a1 = n * (w - 1);
fp.b0 = n;
fp.b1 = -fp.b0;
return(fp);
}
private static void FillAudio(IntPtr udata, IntPtr stream, int len)
{
sudata sud = udata.ToStruct <sudata>();
if (len > sud.bsize)
{
Log.Write("fill_audio: OUCH, len > bsize!");
len = (int)sud.bsize;
}
StdLib.MemSet(stream, sud.obtained.silence, len);
for (int i = 0; i < len / sizeof(short); i++)
{
bqd_filter hp_fltr = sud.hp_fltr.ToStruct <bqd_filter>();
bqd_filter lp_fltr = sud.lp_fltr.ToStruct <bqd_filter>();
double sample = GetSample();
sample = Calc.bqd_apply(ref hp_fltr, (sample - 127.0) * 128.0);
sample = Calc.bqd_apply(ref lp_fltr, sample);
Marshal.WriteInt16(sud.buf, i * sizeof(short), (short)Math.Round(sample));
}
SDL.SDL_MixAudioFormat(stream, sud.buf, sud.obtained.format, (uint)len, SDL.SDL_MIX_MAXVOLUME);
}
public static double bqd_apply(ref bqd_filter fp, double x)
{
fp.z1 = (x * fp.b0) + (fp.z0 * fp.b1) - (fp.z1 * fp.a1);
fp.z0 = x;
return(fp.z1);
}