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

using Godot;

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

  public class Biquad
  {

    public float b0 = 0;
    public float b1 = 0;
    public float b2 = 0;
    public float a1 = 0;
    public float a2 = 0;

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

    float process( float inputSample )
    {
      var result =  this.b0 * inputSample + 
                    this.b1 * this.x1 + 
                    this.b2 * this.x2 -
                    this.a1 * this.y1 - 
                    this.a2 * 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 A = 0f; 
      var omega = 0f;
      var sin = 0f;
      var cos = 0f;
      var alpha = 0f;
      var beta = 0f;

      var a0 = 0f;
      var a1 = 0f;
      var a2 = 0f;
      var b0 = 0f;
      var b1 = 0f;
      var b2 = 0f;

      var ln2 = 0.69314718055994530942f;

      A = Mathf.Pow( 10f, gainDB / 40f );
      omega = 2f * Mathf.Pi * frequency / sampleRate;
      sin = Mathf.Sin( omega );
      cos = Mathf.Cos( omega );
      alpha = ( sin * Mathf.Sinh( ln2 / 2f * bandwidth * omega / sin ) );
      beta = Mathf.Sqrt(A + A);

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

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

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

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

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

        case BiquadType.HighShelf:
        {
          b0 = A * ((A + 1) + (A - 1) * cos + beta * sin);
          b1 = -2 * A * ((A - 1) + (A + 1) * cos);
          b2 = A * ((A + 1) + (A - 1) * cos - beta * sin);
          a0 = (A + 1) - (A - 1) * cos + beta * sin;
          a1 = 2 * ((A - 1) - (A + 1) * cos);
          a2 = (A + 1) - (A - 1) * cos - beta * sin;
        }
        break;
        
        case BiquadType.AllPass:
        {
          var allPassAlpha = cos / ( 2.0f * bandwidth );
          b0 = 1.0f - allPassAlpha;
          b1 = -2.0f * cos;
          b2 = 1.0f + allPassAlpha;
          a0 = 1.0f + allPassAlpha;
          a1 = -2.0f * cos;
          a2 = 1.0f - allPassAlpha;

        }
        break;

      }

      var filter = new Biquad();

      filter.b0 = b0 / a0;
      filter.b1 = b1 / a0;
      filter.b2 = b2 / a0;
      filter.a1 = a1 / a0;
      filter.a2 = 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;
    }
  }
}