dplimin
/
F203
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
F203/Core/Src/iir.c

674 lines
21 KiB
C
Raw Normal View History

first commit
2023-12-13 16:59:07 +03:00
/*
* iir.c
*
* Created on: 1 <EFBFBD><EFBFBD><EFBFBD>. 2021 <EFBFBD>.
* Author: Toporov
*/
#include "my.h"
#include <math.h>
#include "arm_math.h"
#include "FFTCode.h"
extern volatile uint32_t Fs;
#define NUM_PLOT_PTS 128
TIIRCoeff IIR; // This gets returned.
//---------------------------------------------------------------------------
// H(s) = ( Ds^2 + Es + F ) / ( As^2 + Bs + C )
// H(z) = ( b2z^2 + b1z + b0 ) / ( a2z^2 + a1z + a0 )
TIIRCoeff CalcIIRFilterCoeff(TIIRFilterTypes ProtoType, double Beta, TIIRPassTypes PassType, int NumPoles, double Fc, double BW)
{
int j, k;
double Scalar;
double Coeff[5], RealRoot[4], ImagRoot[4];
double A, B, C, D, E, F, T, Q, Arg;
double a2[ARRAY_DIM], a1[ARRAY_DIM], a0[ARRAY_DIM], b2[ARRAY_DIM], b1[ARRAY_DIM], b0[ARRAY_DIM];
double OmegaC;
TSPlaneCoeff SPlaneCoeff;
OmegaC = ((Fc * 2) / ((float64_t) Fs));
// Init the IIR structure. a3, a4, b3,and b4 are only used to calculate the 2nd order
// sections for the bandpass and notch filters. They get discarded.
for(j=0; j<ARRAY_DIM; j++)
{
IIR.a0[j] = 0.0; IIR.b0[j] = 0.0;
IIR.a1[j] = 0.0; IIR.b1[j] = 0.0;
IIR.a2[j] = 0.0; IIR.b2[j] = 0.0;
IIR.a3[j] = 0.0; IIR.b3[j] = 0.0;
IIR.a4[j] = 0.0; IIR.b4[j] = 0.0;
}
IIR.NumSections = 0;
// This gets the 2nd order s plane filter coefficients from the tables in FilterCoefficients.hpp.
// If you have code that computes these coefficients, then use that instead.
SPlaneCoeff = GetSPlaneCoefficients(ProtoType, NumPoles, Beta);
IIR.NumSections = SPlaneCoeff.NumSections; // NumSections = (NumPoles + 1)/2
// T sets the filter's corner frequency, or center freqency.
// The Bilinear transform is defined as: s = 2/T * tan(Omega/2) = 2/T * (1 - z)/(1 + z)
T = 2.0 * tan(OmegaC * M_PI_2);
Q = 1.0 + OmegaC;
if(Q > 1.95)Q = 1.95;
Q = 0.8 * tan(Q * M_PI_4); // This is a correction factor for Q. Q is used with band pass and notch flters.
Q = OmegaC / BW / Q; // This is the corrected Q.
k = 0;
for(j=0; j<SPlaneCoeff.NumSections; j++)
{
A = SPlaneCoeff.A[j]; // We use A - F to make the code easier to read.
B = SPlaneCoeff.B[j];
C = SPlaneCoeff.C[j];
D = SPlaneCoeff.D[j];
E = SPlaneCoeff.E[j];
F = SPlaneCoeff.F[j];
// b's are the numerator a's are the denominator
if(PassType == iirLPF)
{
if(A == 0.0 && D == 0.0) // 1 pole case
{
Arg = (2.0*B + C*T);
IIR.a2[j] = 0.0;
IIR.a1[j] = (-2.0*B + C*T) / Arg;
IIR.a0[j] = 1.0; // The filter implementation depends on a0 = 1.
IIR.b2[j] = 0.0;
IIR.b1[j] = (-2.0*E + F*T) / Arg * C/F;
IIR.b0[j] = ( 2.0*E + F*T) / Arg * C/F;
}
else // 2 poles
{
Arg = (4.0*A + 2.0*B*T + C*T*T);
IIR.a2[j] = (4.0*A - 2.0*B*T + C*T*T) / Arg;
IIR.a1[j] = (2.0*C*T*T - 8.0*A) / Arg;
IIR.a0[j] = 1.0; // The filter implementation depends on a0 = 1.
// With all pole filters, our LPF numerator is (z+1)^2, so all our Z Plane zeros are at -1
IIR.b2[j] = (4.0*D - 2.0*E*T + F*T*T) / Arg * C/F;
IIR.b1[j] = (2.0*F*T*T - 8.0*D) / Arg * C/F;
IIR.b0[j] = (4*D + F*T*T + 2.0*E*T) / Arg * C/F;
}
}
if(PassType == iirHPF)
{
if(A == 0.0 && D == 0.0) // 1 pole
{
Arg = 2.0*C + B*T;
IIR.a2[j] = 0.0;
IIR.a1[j] = (B*T - 2.0*C) / Arg;
IIR.a0[j] = 1.0; // The filter implementation depends on a0 = 1.
IIR.b2[j] = 0.0;
IIR.b1[j] = (E*T - 2.0*F) / Arg * C/F;
IIR.b0[j] = (E*T + 2.0*F) / Arg * C/F;
}
else // 2 poles
{
Arg = A*T*T + 4.0*C + 2.0*B*T;
IIR.a2[j] = (A*T*T + 4.0*C - 2.0*B*T) / Arg;
IIR.a1[j] = (2.0*A*T*T - 8.0*C) / Arg;
IIR.a0[j] = 1.0;
// With all pole filters, our HPF numerator is (z-1)^2, so all our Z Plane zeros are at 1
IIR.b2[j] = (D*T*T - 2.0*E*T + 4.0*F) / Arg * C/F;
IIR.b1[j] = (2.0*D*T*T - 8.0*F) / Arg * C/F;
IIR.b0[j] = (D*T*T + 4.0*F + 2.0*E*T) / Arg * C/F;
}
}
if(PassType == iirBPF)
{
if(A == 0.0 && D == 0.0) // 1 pole
{
Arg = 4.0*B*Q + 2.0*C*T + B*Q*T*T;
a2[k] = (B*Q*T*T + 4.0*B*Q - 2.0*C*T) / Arg;
a1[k] = (2.0*B*Q*T*T - 8.0*B*Q) / Arg ;
a0[k] = 1.0;
b2[k] = (E*Q*T*T + 4.0*E*Q - 2.0*F*T) / Arg * C/F;
b1[k] = (2.0*E*Q*T*T - 8.0*E*Q) / Arg * C/F;
b0[k] = (4.0*E*Q + 2.0*F*T + E*Q*T*T) / Arg * C/F;
k++;
}
else //2 Poles
{
IIR.a4[j] = (16.0*A*Q*Q + A*Q*Q*T*T*T*T + 8.0*A*Q*Q*T*T - 2.0*B*Q*T*T*T - 8.0*B*Q*T + 4.0*C*T*T) * F;
IIR.a3[j] = (4.0*T*T*T*T*A*Q*Q - 4.0*Q*T*T*T*B + 16.0*Q*B*T - 64.0*A*Q*Q) * F;
IIR.a2[j] = (96.0*A*Q*Q - 16.0*A*Q*Q*T*T + 6.0*A*Q*Q*T*T*T*T - 8.0*C*T*T) * F;
IIR.a1[j] = (4.0*T*T*T*T*A*Q*Q + 4.0*Q*T*T*T*B - 16.0*Q*B*T - 64.0*A*Q*Q) * F;
IIR.a0[j] = (16.0*A*Q*Q + A*Q*Q*T*T*T*T + 8.0*A*Q*Q*T*T + 2.0*B*Q*T*T*T + 8.0*B*Q*T + 4.0*C*T*T) * F;
// With all pole filters, our BPF numerator is (z-1)^2 * (z+1)^2 so the zeros come back as +/- 1 pairs
IIR.b4[j] = (8.0*D*Q*Q*T*T - 8.0*E*Q*T + 16.0*D*Q*Q - 2.0*E*Q*T*T*T + D*Q*Q*T*T*T*T + 4.0*F*T*T) * C;
IIR.b3[j] = (16.0*E*Q*T - 4.0*E*Q*T*T*T - 64.0*D*Q*Q + 4.0*D*Q*Q*T*T*T*T) * C;
IIR.b2[j] = (96.0*D*Q*Q - 8.0*F*T*T + 6.0*D*Q*Q*T*T*T*T - 16.0*D*Q*Q*T*T) * C;
IIR.b1[j] = (4.0*D*Q*Q*T*T*T*T - 64.0*D*Q*Q + 4.0*E*Q*T*T*T - 16.0*E*Q*T) * C;
IIR.b0[j] = (16.0*D*Q*Q + 8.0*E*Q*T + 8.0*D*Q*Q*T*T + 2.0*E*Q*T*T*T + 4.0*F*T*T + D*Q*Q*T*T*T*T) * C;
// T = 2 makes these values approach 0.0 (~ 1.0E-12) The root solver needs 0.0 for numerical reasons.
if(fabs(T-2.0) < 0.0005)
{
IIR.a3[j] = 0.0;
IIR.a1[j] = 0.0;
IIR.b3[j] = 0.0;
IIR.b1[j] = 0.0;
}
// We now have a 4th order poly in the form a4*s^4 + a3*s^3 + a2*s^2 + a2*s + a0
// We find the roots of this so we can form two 2nd order polys.
Coeff[0] = IIR.a4[j];
Coeff[1] = IIR.a3[j];
Coeff[2] = IIR.a2[j];
Coeff[3] = IIR.a1[j];
Coeff[4] = IIR.a0[j];
QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);
// In effect, the root finder scales the poly by 1/a4 so we have to apply
// this factor back into the two 2nd order equations we are forming.
if(IIR.a4[j] < 0.0)Scalar = -sqrt(-IIR.a4[j]);
else Scalar = sqrt(IIR.a4[j]);
// Form the 2nd order polys from the roots.
a2[k] = Scalar;
a1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
a0[k] = (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
k++;
a2[k] = Scalar;
a1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
a0[k] = (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
k--;
// Now do the same with the numerator.
Coeff[0] = IIR.b4[j];
Coeff[1] = IIR.b3[j];
Coeff[2] = IIR.b2[j];
Coeff[3] = IIR.b1[j];
Coeff[4] = IIR.b0[j];
QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);
if(IIR.b4[j] < 0.0)Scalar = -sqrt(-IIR.b4[j]);
else Scalar = sqrt(IIR.b4[j]);
b2[k] = Scalar;
if(ProtoType == ftINVERSE_CHEBY || ProtoType >= ftELLIPTIC_00)
b1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
else // else the prototype is an all pole filter
b1[k] = 0.0; // b1 = 0 for all pole filters, but the addition above won't always equal zero exactly.
b0[k] = (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
k++;
b2[k] = Scalar;
if(ProtoType == ftINVERSE_CHEBY || ProtoType >= ftELLIPTIC_00)
b1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
else
b1[k] = 0.0;
b0[k] = (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
k++;
// Go below to see where we store these 2nd order polys back into IIR
}
}
if(PassType == iirNOTCH)
{
if(A == 0.0 && D == 0.0) // 1 pole
{
Arg = 2.0*B*T + C*Q*T*T + 4.0*C*Q;
a2[k] = (4.0*C*Q - 2.0*B*T + C*Q*T*T) / Arg;
a1[k] = (2.0*C*Q*T*T - 8.0*C*Q) / Arg;
a0[k] = 1.0;
b2[k] = (4.0*F*Q - 2.0*E*T + F*Q*T*T) / Arg * C/F;
b1[k] = (2.0*F*Q*T*T - 8.0*F*Q) / Arg *C/F;
b0[k] = (2.0*E*T + F*Q*T*T +4.0*F*Q) / Arg *C/F;
k++;
}
else
{
IIR.a4[j] = (4.0*A*T*T - 2.0*B*T*T*T*Q + 8.0*C*Q*Q*T*T - 8.0*B*T*Q + C*Q*Q*T*T*T*T + 16.0*C*Q*Q) * -F;
IIR.a3[j] = (16.0*B*T*Q + 4.0*C*Q*Q*T*T*T*T - 64.0*C*Q*Q - 4.0*B*T*T*T*Q) * -F;
IIR.a2[j] = (96.0*C*Q*Q - 8.0*A*T*T - 16.0*C*Q*Q*T*T + 6.0*C*Q*Q*T*T*T*T) * -F;
IIR.a1[j] = (4.0*B*T*T*T*Q - 16.0*B*T*Q - 64.0*C*Q*Q + 4.0*C*Q*Q*T*T*T*T) * -F;
IIR.a0[j] = (4.0*A*T*T + 2.0*B*T*T*T*Q + 8.0*C*Q*Q*T*T + 8.0*B*T*Q + C*Q*Q*T*T*T*T + 16.0*C*Q*Q) * -F;
// Our Notch Numerator isn't simple. [ (4+T^2)*z^2 - 2*(4-T^2)*z + (4+T^2) ]^2
IIR.b4[j] = (2.0*E*T*T*T*Q - 4.0*D*T*T - 8.0*F*Q*Q*T*T + 8.0*E*T*Q - 16.0*F*Q*Q - F*Q*Q*T*T*T*T) * C;
IIR.b3[j] = (64.0*F*Q*Q + 4.0*E*T*T*T*Q - 16.0*E*T*Q - 4.0*F*Q*Q*T*T*T*T) * C;
IIR.b2[j] = (8.0*D*T*T - 96.0*F*Q*Q + 16.0*F*Q*Q*T*T - 6.0*F*Q*Q*T*T*T*T) * C;
IIR.b1[j] = (16.0*E*T*Q - 4.0*E*T*T*T*Q + 64.0*F*Q*Q - 4.0*F*Q*Q*T*T*T*T) * C;
IIR.b0[j] = (-4.0*D*T*T - 2.0*E*T*T*T*Q - 8.0*E*T*Q - 8.0*F*Q*Q*T*T - F*Q*Q*T*T*T*T - 16.0*F*Q*Q) * C;
// T = 2 makes these values approach 0.0 (~ 1.0E-12) The root solver needs 0.0 for numerical reasons.
if(fabs(T-2.0) < 0.0005)
{
IIR.a3[j] = 0.0;
IIR.a1[j] = 0.0;
IIR.b3[j] = 0.0;
IIR.b1[j] = 0.0;
}
// We now have a 4th order poly in the form a4*s^4 + a3*s^3 + a2*s^2 + a2*s + a0
// We find the roots of this so we can form two 2nd order polys.
Coeff[0] = IIR.a4[j];
Coeff[1] = IIR.a3[j];
Coeff[2] = IIR.a2[j];
Coeff[3] = IIR.a1[j];
Coeff[4] = IIR.a0[j];
QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);
// In effect, the root finder scales the poly by 1/a4 so we have to apply
// this factor back into the two 2nd order equations we are forming.
if(IIR.a4[j] < 0.0)Scalar = -sqrt(-IIR.a4[j]);
else Scalar = sqrt(IIR.a4[j]);
a2[k] = Scalar;
a1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
a0[k] = (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
k++;
a2[k] = Scalar;
a1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
a0[k] = (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
k--;
// Now do the same with the numerator.
Coeff[0] = IIR.b4[j];
Coeff[1] = IIR.b3[j];
Coeff[2] = IIR.b2[j];
Coeff[3] = IIR.b1[j];
Coeff[4] = IIR.b0[j];
QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);
if(IIR.b4[j] < 0.0)Scalar = -sqrt(-IIR.b4[j]);
else Scalar = sqrt(IIR.b4[j]);
b2[k] = Scalar;
b1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
b0[k] = (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
k++;
b2[k] = Scalar;
b1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
b0[k] = (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
k++;
}
}
}
if(PassType == iirBPF || PassType == iirNOTCH)
{
// In the calcs above for the BPF and Notch, we didn't set a0=1, so we do it here.
// k will equal the number of poles.
for(j=0; j<k; j++)
{
b2[j] /= a0[j];
b1[j] /= a0[j];
b0[j] /= a0[j];
a2[j] /= a0[j];
a1[j] /= a0[j];
a0[j] = 1.0;
}
for(j=0; j<k; j++)
{
IIR.a0[j] = a0[j];
IIR.a1[j] = a1[j];
IIR.a2[j] = a2[j];
IIR.b0[j] = b0[j];
IIR.b1[j] = b1[j];
IIR.b2[j] = b2[j];
}
}
return(IIR); // IIR is the structure containing the coefficients.
}
TSPlaneCoeff GetSPlaneCoefficients(uint8_t FilterType, int NumPoles, double Beta)
{
#include "FilterCoefficients.h" // This file contains all the data arrays used below.
int j, n, p, z, MinNumPoles, MaxNumPoles;
int ArrayNumber, OuterArrayDim, NumSections;
double LoopBeta, BetaMin, BetaStep, BetaMax;
TSPlaneCoeff SPlaneCoeff;
BetaMin = BetaMinArray[FilterType];
BetaStep = BetaStepArray[FilterType];
BetaMax = BetaMaxArray[FilterType];
MinNumPoles = MinNumPolesArray[FilterType];
MaxNumPoles = MaxNumPolesArray[FilterType];
// Range check
if(NumPoles < MinNumPoles)NumPoles = MinNumPoles;
if(NumPoles > MaxNumPoles)NumPoles = MaxNumPoles;
if(Beta < BetaMin)Beta = BetaMin;
if(Beta > BetaMax)Beta = BetaMax;
// Need the array number that corresponds to Beta.
// The max array size was originally determined with this piece of code,
// so we use it here so we can range check ArrayNumber.
OuterArrayDim = 0;
for(LoopBeta=BetaMin; LoopBeta<=BetaMax; LoopBeta+=BetaStep)OuterArrayDim++;
ArrayNumber = 0;
for(LoopBeta=BetaMin; LoopBeta<=BetaMax; LoopBeta+=BetaStep)
{
if( LoopBeta >= Beta - BetaStep/1.9999 && LoopBeta <= Beta + BetaStep/1.9999)break;
ArrayNumber++;
}
if(ArrayNumber > OuterArrayDim-1)ArrayNumber = OuterArrayDim-1; // i.e. If the array is dimensioned to N, the highest we can access is N-1
// n is the array location for the given pole count.
// The arrays start at 0. MinNumPoles is either 2 or 4.
n = NumPoles - MinNumPoles;
if(n < 0)n = 0;
// NumSections is the number of biquad sections for a given pole count.
NumSections = (NumPoles + 1) / 2;
p = z = 0;
switch(FilterType)
{
case ftBUTTERWORTH:
for(j=0; j<NumSections; j++)
{
SPlaneCoeff.A[j] = ButterworthDenominator[n][p++];
SPlaneCoeff.B[j] = ButterworthDenominator[n][p++];
SPlaneCoeff.C[j] = ButterworthDenominator[n][p++];
SPlaneCoeff.D[j] = 0.0;
SPlaneCoeff.E[j] = 0.0;
SPlaneCoeff.F[j] = 1.0;
}
break;
case ftCHEBYSHEV:
for(j=0; j<NumSections; j++)
{
SPlaneCoeff.A[j] = ChebyshevDenominator[ArrayNumber][n][p++];
SPlaneCoeff.B[j] = ChebyshevDenominator[ArrayNumber][n][p++];
SPlaneCoeff.C[j] = ChebyshevDenominator[ArrayNumber][n][p++];
SPlaneCoeff.D[j] = 0.0;
SPlaneCoeff.E[j] = 0.0;
SPlaneCoeff.F[j] = 1.0;
}
break;
case ftINVERSE_CHEBY:
for(j=0; j<NumSections; j++)
{
SPlaneCoeff.A[j] = InvChebyDenominator[ArrayNumber][n][p++];
SPlaneCoeff.B[j] = InvChebyDenominator[ArrayNumber][n][p++];
SPlaneCoeff.C[j] = InvChebyDenominator[ArrayNumber][n][p++];
SPlaneCoeff.D[j] = InvChebyNumerator[ArrayNumber][n][z++];
SPlaneCoeff.E[j] = InvChebyNumerator[ArrayNumber][n][z++];
SPlaneCoeff.F[j] = InvChebyNumerator[ArrayNumber][n][z++];
}
break;
}
SPlaneCoeff.NumSections = NumSections;
return(SPlaneCoeff);
}
int QuadCubicRoots(int N, double *Coeff, double *RootsReal, double *RootsImag)
{
if(N <= 1 || N > 4)
{
return(0);
}
int j, k;
long double P[5], RealRoot[4], ImagRoot[4];
// Must init to zero, in case N is reduced.
for(j=0; j<4; j++)RealRoot[j] = ImagRoot[j] = 0.0;
for(j=0; j<5; j++)P[j] = 0.0;
// Reduce the order if there are trailing zeros.
for(k=N; k>=0; k--)
{
if(fabs(Coeff[k]) > ZERO_PLUS)break; // break on the 1st nonzero coeff
Coeff[k] = 0.0;
N--;
}
// Mandatory to remove leading zeros.
while( fabs(Coeff[0]) < ZERO_PLUS && N>0)
{
for(k=0; k<N; k++)
{
Coeff[k] = Coeff[k+1];
}
Coeff[N] = 0.0;
N--;
}
// The functions below modify the coeff array, so we pass P instead of Coeff.
for(j=0; j<=N; j++)P[j] = Coeff[j];
// Mandatory to normalize the coefficients
if(P[0] != 1.0)
{
for(k=1; k<=N; k++)
{
P[k] /= P[0];
}
P[0] = 1.0;
}
if(N==4)BiQuadRoots(P, RealRoot, ImagRoot);
if(N==3)CubicRoots(P, RealRoot, ImagRoot);
if(N==2)QuadRoots(P, RealRoot, ImagRoot);
if(N==1)
{
RealRoot[0] = -P[1]/P[0];
ImagRoot[0] = 0.0;
}
// We used separate long double arrays in the function calls for interface reasons.
// if N==0 all zeros are returned (we init RealRoot and ImagRoot to 0).
// for(j=0; j<4; j++)Roots[j] = ComplexD(RealRoot[j], ImagRoot[j]);
for(j=0; j<4; j++)RootsReal[j] = RealRoot[j];
for(j=0; j<4; j++)RootsImag[j] = ImagRoot[j];
return(N);
}
//---------------------------------------------------------------------------
// This function is the quadratic formula with P[0] = 1
// y = P[0]*x^2 + P[1]*x + P[2]
void QuadRoots(long double *P, long double *RealRoot, long double *ImagRoot)
{
long double g;
g = P[1]*P[1] - 4.0*P[2];
if(fabsl(g) < ZERO_PLUS)g = 0.0;
if(g >= 0.0) // 2 real roots
{
RealRoot[0] = (-P[1] + sqrtl(g)) / 2.0;
RealRoot[1] = (-P[1] - sqrtl(g)) / 2.0;
ImagRoot[0] = 0.0;
ImagRoot[1] = 0.0;
}
else // 2 complex roots
{
RealRoot[0] = -P[1] / 2.0;
RealRoot[1] = -P[1] / 2.0;
ImagRoot[0] = sqrtl(-g) / 2.0;
ImagRoot[1] = -sqrtl(-g) / 2.0;
}
}
//---------------------------------------------------------------------------
// This finds the roots of y = P0x^3 + P1x^2 + P2x+ P3 P[0] = 1
// The return value indicates the location of the largest positive root which is used by BiQuadRoots.
int CubicRoots(long double *P, long double *RealRoot, long double *ImagRoot)
{
long double s, t, b, c, d;
s = P[1] / 3.0;
b = (6.0*P[1]*P[1]*P[1] - 27.0*P[1]*P[2] + 81.0*P[3]) / 162.0;
t = (P[1]*P[1] - 3.0*P[2]) / 9.0;
c = t * t * t;
d = 2.0*P[1]*P[1]*P[1] - 9.0*P[1]*P[2] + 27.0*P[3];
d = d * d / 2916.0 - c;
if(d > ZERO_PLUS) // 1 complex and 1 real root
{
d = powl( (sqrtl(d) + fabsl(b)), 1.0/3.0);
if(d != 0.0)
{
if(b>0) b = -d;
else b = d;
c = t / b;
}
d = M_SQRT3_2 * (b-c);
b = b + c;
c = -b/2.0 - s;
RealRoot[0] = c;
ImagRoot[0] = -d;
RealRoot[1] = c;
ImagRoot[1] = d;
RealRoot[2] = b-s;
if( fabsl(RealRoot[2]) < ZERO_PLUS)RealRoot[2] = 0.0;
ImagRoot[2] = 0.0;
return(2); // Return 2 because it contains the real root.
}
else // d < 0.0 3 real roots
{
if(b == 0.0)d = M_PI_2 / 3.0; // b can be as small as 1.0E-25
else d = atanl(sqrtl(fabsl(d))/fabsl(b)) / 3.0;
if(b < 0.0) b = 2.0 * sqrtl(fabsl(t));
else b = -2.0 * sqrtl(fabsl(t));
c = cosl(d) * b;
t = -M_SQRT3_2 * sinl(d) * b - 0.5 * c;
RealRoot[0] = t - s;
RealRoot[1] = c - s;
RealRoot[2] = -(t + c + s);
ImagRoot[0] = 0.0;
ImagRoot[1] = 0.0;
ImagRoot[2] = 0.0;
if( fabsl(RealRoot[0]) < ZERO_PLUS)RealRoot[0] = 0.0;
if( fabsl(RealRoot[1]) < ZERO_PLUS)RealRoot[1] = 0.0;
if( fabsl(RealRoot[2]) < ZERO_PLUS)RealRoot[2] = 0.0;
int MaxK = 0;
if(RealRoot[1] > RealRoot[MaxK])MaxK = 1;
if(RealRoot[2] > RealRoot[MaxK])MaxK = 2;
return(MaxK); // Return the index with the largest real root.
}
}
//---------------------------------------------------------------------------
// This finds the roots of y = P0x^4 + P1x^3 + P2x^2 + P3x + P4 P[0] = 1
// This function calls CubicRoots
void BiQuadRoots(long double *P, long double *RealRoot, long double *ImagRoot)
{
int k, MaxK;
long double a, b, c, d, e, g, P1, P3Limit;
P1 = P[1];
e = P[1]*0.25;
d = P[1]*P[1]*0.1875; // 0.1875 = 3/16
b = P[3] + P[1]*P[1]*P[1]*0.125 - P[1]*P[2]*0.5;
c = 256.0*P[4] + 16.0*P[1]*P[1]*P[2];
c += -3.0*P[1]*P[1]*P[1]*P[1] - 64.0*P[1]*P[3];
c *= 0.00390625; // 0.00390625 = 1/256
a = P[2] - P[1]*P[1]*0.375; // 0.375 = 3/8
P[1] = P[2]*0.5 - P[1]*P[1]*0.1875; // 0.1875 = 3/16
P[2] = 16.0*P[2]*P[2] - 16.0*P1*P1*P[2] + 3.0*P1*P1*P1*P1;
P[2] += -64.0*P[4] + 16.0*P1*P[3];
P[2] *= 3.90625E-3; // 3.90625E-3 = 1/256
P[3] = -b*b*0.015625; // 0.015625 = 1/64
if(P[3] > 0.0)P[3] = 0.0; // Only numerical errors make P[3] > 0
/* Any inaccuracies in this algorithm are directly attributable to the magnitude of P[3]
which takes on values of: -1E30 < P[3] <= 0.0
If P[3] = 0 we use the quadratic formula to get the roots (i.e. the 3rd root is at 0).
Inaccuracies are caused in CubicRoots when P[3] is either huge or tiny because of the loss of
significance that ocurrs there in the calculation of b and d. P[3] can also cause problems when it
should have calculated to zero (just above) but it is instead ~ -1.0E-17 because of round off errors.
Consequently, we need to determine whether a tiny P[3] is due to roundoff, or if it is legitimately small.
It can legitimately have values of ~-1E-28 . When this happens, we assume P2 should also be small.
We use the following criteria to test for a legitimate tiny P[3], which we must to send to CubicRoots */
// if P[3] is tiny && legitimately tiny we use 0 as the threshold.
// else P[3] must be more negative than roundoff errors cause.
if(P[3] > ZERO_MINUS && fabs(P[2]) < 1.0E-6) P3Limit = 0.0;
else P3Limit = ZERO_MINUS;
if(P[3] < P3Limit)
{
MaxK = CubicRoots(P, RealRoot, ImagRoot);
if(RealRoot[MaxK] > 0.0) // MaxK is the index of the largest real root.
{
d = 4.0*RealRoot[MaxK];
a += d;
if(a*b < 0.0)P[1] = -sqrtl(d);
else P[1] = sqrtl(d);
b = 0.5 * (a + b/P[1]);
goto QUAD;
}
}
if(P[2] < -1.0E-8) // 2 sets of equal imag roots
{
b = sqrtl(fabsl(c));
d = b + b - a;
if(d > 0.0)P[1] = sqrtl(fabsl(d));
else P[1] = 0.0;
}
else
{
if(P[1] > 0.0)b = 2.0*sqrtl(fabsl(P[2])) + P[1];
else b = -2.0*sqrtl(fabsl(P[2])) + P[1];
if(fabsl(b) < 10.0*ZERO_PLUS) // 4 equal real roots. Was originally if(b == 0.0)
{
for(k=0; k<4; k++)
{
RealRoot[k] = -e;
ImagRoot[k] = 0.0;
}
return;
}
else P[1] = 0.0;
}
// Calc the roots from two 2nd order polys and subtract e from the real part.
QUAD:
P[2] = c/b;
QuadRoots(P, RealRoot, ImagRoot);
P[1] = -P[1];
P[2] = b;
QuadRoots(P, RealRoot+2, ImagRoot+2);
for(k=0; k<4; k++)RealRoot[k] -= e;
}