SV platform dependency builds

#include <math.h>

#include <string.h>

#include "biquad_filter.h"

/**

 *        Unit_BiquadFilter implements a second order IIR filter. 

        Here is the equation that we use for this filter:

     y(n) = a0*x(n) + a1*x(n-1)  + a2*x(n-2) - b1*y(n-1)  - b2*y(n-2)

 *

 * @author (C) 2002 Phil Burk, SoftSynth.com, All Rights Reserved

 */

#define FILTER_PI  (3.141592653589793238462643)

/***********************************************************

** Calculate coefficients common to many parametric biquad filters.

*/

static void BiquadFilter_CalculateCommon( BiquadFilter *filter, double ratio, double Q )

{

        double omega;

        memset( filter, 0, sizeof(BiquadFilter) );

/* Don't let frequency get too close to Nyquist or filter will blow up. */

        if( ratio >= 0.499 ) ratio = 0.499; 

        omega = 2.0 * (double)FILTER_PI * ratio;

        filter->cos_omega = (double) cos( omega );

        filter->sin_omega = (double) sin( omega );

        filter->alpha = filter->sin_omega / (2.0 * Q);

}

/*********************************************************************************

 ** Calculate coefficients for Highpass filter.

 */

void BiquadFilter_SetupHighPass( BiquadFilter *filter, double ratio, double Q )

{

        double    scalar, opc;

        if( ratio  < BIQUAD_MIN_RATIO )  ratio  = BIQUAD_MIN_RATIO;

        if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;

        BiquadFilter_CalculateCommon( filter, ratio, Q );

        scalar = 1.0 / (1.0 + filter->alpha);

        opc = (1.0 + filter->cos_omega);

        filter->a0 = opc * 0.5 * scalar;

        filter->a1 =  - opc * scalar;

    filter->a2 = filter->a0;

        filter->b1 = -2.0 * filter->cos_omega * scalar;

        filter->b2 = (1.0 - filter->alpha) * scalar;

}

/*********************************************************************************

 ** Calculate coefficients for Notch filter.

 */

void BiquadFilter_SetupNotch( BiquadFilter *filter, double ratio, double Q )

{

        double    scalar, opc;

        if( ratio  < BIQUAD_MIN_RATIO )  ratio  = BIQUAD_MIN_RATIO;

        if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;

        BiquadFilter_CalculateCommon( filter, ratio, Q );

        scalar = 1.0 / (1.0 + filter->alpha);

        opc = (1.0 + filter->cos_omega);

        filter->a0 = scalar;

        filter->a1 =  -2.0 * filter->cos_omega * scalar;

    filter->a2 = filter->a0;

        filter->b1 = filter->a1;

        filter->b2 = (1.0 - filter->alpha) * scalar;

}

/*****************************************************************

** Perform core IIR filter calculation without permutation.

*/

void BiquadFilter_Filter( BiquadFilter *filter, float *inputs, float *outputs, int numSamples )

{

        int i;

    double xn, yn;

        // Pull values from structure to speed up the calculation.

        double a0 = filter->a0;

        double a1 = filter->a1;

        double a2 = filter->a2;

        double b1 = filter->b1;

        double b2 = filter->b2;

        double xn1 = filter->xn1;

        double xn2 = filter->xn2;

        double yn1 = filter->yn1;

        double yn2 = filter->yn2;

        for( i=0; i<numSamples; i++)

        {

                // Generate outputs by filtering inputs.

                xn = inputs[i];

                yn = (a0 * xn) + (a1 * xn1) + (a2 * xn2) - (b1 * yn1) - (b2 * yn2);

                outputs[i] = yn;

                // Delay input and output values.

        xn2 = xn1;

        xn1 = xn;

        yn2 = yn1;

        yn1 = yn;

                if( (i & 7) == 0 )

                {

                        // Apply a small bipolar impulse to filter to prevent arithmetic underflow.

                        // Underflows can cause the FPU to interrupt the CPU.

                        yn1 += (double) 1.0E-26;

                        yn2 -= (double) 1.0E-26;

                }

        }

        filter->xn1 = xn1;

        filter->xn2 = xn2;

        filter->yn1 = yn1;

        filter->yn2 = yn2;                

}