private static void RLP2InitCoeffHelper(
ref IIR2DirectIIRec Coeff,
float A0,
float A1,
float A2,
float B1,
float B2)
{
Coeff.A0 = A0;
Coeff.A1 = A1;
Coeff.A2 = A2;
Coeff.B1 = B1;
Coeff.B2 = B2;
}
/* compute filter coefficients */
private static void ComputeResonantLowpass2Coefficients(
ref IIR2DirectIIRec Coeff0,
ref IIR2DirectIIRec Coeff1,
ref IIR2DirectIIRec Coeff2,
int Order,
double Cutoff,
double Q,
double SamplingRate)
{
int Order2 = Order / 2;
#if DEBUG
if ((Order2 < 1) || (Order2 > RLP2_MAXDEGREE) || (Order % 2 != 0))
{
Debug.Assert(false);
throw new ArgumentException();
}
if (RLP2_MAXDEGREE != IndexedB1Coef.Length)
{
// internal inconsistency
Debug.Assert(false);
throw new ArgumentException();
}
#endif
/* initialize some constants */
double TwoSamplingRate = 2 * SamplingRate;
/* invariants */
double WPInv = 1 / (TwoSamplingRate * Math.Tan(Math.PI * Cutoff / SamplingRate));
double QInv = 1 / Q;
/* prepare section coefficients */
double[] B1Coef = IndexedB1Coef[Order2 - 1];
#if DEBUG
Debug.Assert(B1Coef.Length == Order2);
#endif
for (int i = 0; i < Order2; i++)
{
double B1s = B1Coef[i] * WPInv * QInv;
double FourSamplingRateSquaredB2s = TwoSamplingRate * TwoSamplingRate * WPInv * WPInv;
double BetaI = 1 / (FourSamplingRateSquaredB2s + B1s * TwoSamplingRate + 1);
float A0 = (float)BetaI;
float A1 = 2;
float A2 = 1;
float B1 = (float)((2 - 2 * FourSamplingRateSquaredB2s) * BetaI);
float B2 = (float)((FourSamplingRateSquaredB2s - B1s * TwoSamplingRate + 1) * BetaI);
if (i == 0)
{
RLP2InitCoeffHelper(ref Coeff0, A0, A1, A2, B1, B2);
}
else if (i == 1)
{
RLP2InitCoeffHelper(ref Coeff1, A0, A1, A2, B1, B2);
}
else
{
#if DEBUG
Debug.Assert(i == 2);
#endif
RLP2InitCoeffHelper(ref Coeff2, A0, A1, A2, B1, B2);
}
}
}
