rj-action-library/Runtime/Math/Biquad.cs

using Godot;

namespace Rokojori
{
  public enum BiquadType
  {
    LowPass,
    BandPass,
    HighPass,
    Peak,
    LowShelf,
    HighShelf,
    Notch
  }

  public class Biquad
  {

    public float a0 = 0;
    public float a1 = 0;
    public float a2 = 0;
    public float a3 = 0;
    public float a4 = 0;

    public float x1 = 0;
    public float x2 = 0;
    public float y1 = 0;
    public float y2 = 0;

    float process( float inputSample )
    {
      var result =  this.a0 * inputSample + 
                    this.a1 * this.x1 + 
                    this.a2 * this.x2 -
                    this.a3 * this.y1 - 
                    this.a4 * this.y2;

      this.x2 = this.x1;
      this.x1 = inputSample;

      this.y2 = this.y1;
      this.y1 = result;

      return result;
    }

    public static Biquad Create( BiquadType filterType, float gainDB, float frequency, float bandwidth, float sampleRate )
    {
      var filter = new Biquad();
      float A= 0, omega= 0, sn= 0, cs= 0, alpha= 0, beta = 0;
      float a0= 0, a1= 0, a2= 0, b0= 0, b1= 0, b2 = 0;

      var M_LN2 = 0.69314718055994530942f;

      A = Mathf.Pow( 10, gainDB / 40 );
      omega = 2 * Mathf.Pi * frequency / sampleRate;
      sn = Mathf.Sin( omega );
      cs = Mathf.Cos( omega );
      alpha = ( sn * Mathf.Sinh( M_LN2 / 2 * bandwidth * omega / sn ) );
      beta = Mathf.Sqrt(A + A);

      switch ( filterType ) 
      {        
        case BiquadType.LowPass:
        {
          b0 = (1 - cs) /2;
          b1 = 1 - cs;
          b2 = (1 - cs) /2;
          a0 = 1 + alpha;
          a1 = -2 * cs;
          a2 = 1 - alpha;
        }
        break;

        case BiquadType.HighPass:
        {
          b0 = (1 + cs) /2;
          b1 = -(1 + cs);
          b2 = (1 + cs) /2;
          a0 = 1 + alpha;
          a1 = -2 * cs;
          a2 = 1 - alpha;
        }
        break;

        case BiquadType.BandPass:
        {
          b0 = alpha;
          b1 = 0;
          b2 = -alpha;
          a0 = 1 + alpha;
          a1 = -2 * cs;
          a2 = 1 - alpha;
        }
        break;

        case BiquadType.Notch:
        {
          b0 = 1;
          b1 = -2 * cs;
          b2 = 1;
          a0 = 1 + alpha;
          a1 = -2 * cs;
          a2 = 1 - alpha;
        }
        break;
            
        case BiquadType.Peak:
        {
          b0 = 1 + (alpha * A);
          b1 = -2 * cs;
          b2 = 1 - (alpha * A);
          a0 = 1 + (alpha /A);
          a1 = -2 * cs;
          a2 = 1 - (alpha /A);
        }
        break;

        case BiquadType.LowShelf:
        {
          b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
          b1 = 2 * A * ((A - 1) - (A + 1) * cs);
          b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
          a0 = (A + 1) + (A - 1) * cs + beta * sn;
          a1 = -2 * ((A - 1) + (A + 1) * cs);
          a2 = (A + 1) + (A - 1) * cs - beta * sn;
        }
        break;

        case BiquadType.HighShelf:
        {
          b0 = A * ((A + 1) + (A - 1) * cs + beta * sn);
          b1 = -2 * A * ((A - 1) + (A + 1) * cs);
          b2 = A * ((A + 1) + (A - 1) * cs - beta * sn);
          a0 = (A + 1) - (A - 1) * cs + beta * sn;
          a1 = 2 * ((A - 1) - (A + 1) * cs);
          a2 = (A + 1) - (A - 1) * cs - beta * sn;
        }
        break;

      }

      filter.a0 = b0 / a0;
      filter.a1 = b1 / a0;
      filter.a2 = b2 / a0;
      filter.a3 = a1 / a0;
      filter.a4 = a2 / a0;

      filter.x1 = filter.x2 = 0;
      filter.y1 = filter.y2 = 0;

      return filter;
    }

    public static float filter( float inputSample, float[] coefficients, float[] buffer )
    {      
      var result =  coefficients[ 0 ] * inputSample + 
                    coefficients[ 1 ] * buffer[ 0 ] + 
                    coefficients[ 2 ] * buffer[ 1 ] -
                    coefficients[ 3 ] * buffer[ 2 ] - 
                    coefficients[ 4 ] * buffer[ 3 ];

      buffer[ 1 ] = buffer[ 0 ];
      buffer[ 0 ] = inputSample;

      buffer[ 3 ] = buffer[ 2 ];
      buffer[ 2 ] = result;      

      return result;
    }
  }
}