engines/common/BiquadFilter.h

/***************************************************************************
 *                                                                         *
 *   LinuxSampler - modular, streaming capable sampler                     *
 *                                                                         *
 *   Copyright (C) 2003, 2004 by Benno Senoner and Christian Schoenebeck   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the Free Software           *
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston,                 *
 *   MA  02111-1307  USA                                                   *
 ***************************************************************************/

#ifndef __LS_BIQUADFILTER_H__
#define __LS_BIQUADFILTER_H__

#include <math.h>

/// ln(2) / 2
#define LN_2_2                  0.34657359f

#ifndef LIMIT
# define LIMIT(v,l,u)           (v < l ? l : (v > u ? u : v))
#endif

namespace LinuxSampler {

    typedef float bq_t;

    /**
     * Internal parameters of the biquad filter, which are actually the
     * final parameters of the filter's transfer function. This strucure is
     * only needed when these parameters should stored outside the
     * BiquadFilter class, e.g. to save calculation time by sharing them
     * between multiple filters.
     */
    struct biquad_param_t {
        bq_t a1;
        bq_t a2;
        bq_t b0;
        bq_t b1;
        bq_t b2;
    };

    /**
     * Bi-quadratic filter
     * (adapted from lisp code by Eli Brandt, http://www.cs.cmu.edu/~eli/)
     */
    class BiquadFilter {
        protected:
            // following five variables are only used if no external biquad_param_t reference is used
            bq_t a1;
            bq_t a2;
            bq_t b0;
            bq_t b1;
            bq_t b2;
            // following four variables are used to buffer the feedback
            bq_t x1;
            bq_t x2;
            bq_t y1;
            bq_t y2;

            const static float fbc = 0.98;

            /**
             * Prevent \a f from going into denormal mode which would slow down
             * subsequent floating point calculations, we achieve that by setting
             * \a f to zero when it falls under the denormal threshold value.
             */
            inline void KillDenormal(bq_t& f) {
                // TODO: this is a generic solution for 32bit floats, should be replaced by CPU specific asm code
                f += 1e-18f;
                f -= 1e-18f;
            }
        public:
            BiquadFilter() {
                Reset();
            }

            void Reset() {
                x1 = 0.0f;
                x2 = 0.0f;
                y1 = 0.0f;
                y2 = 0.0f;
            }

            inline bq_t Apply(const bq_t x) {
                bq_t y;

                y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
                    this->a1 * this->y1 + this->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

            inline bq_t Apply(biquad_param_t* param, const bq_t x) {
                bq_t y;

                y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
                    param->a1 * this->y1 + param->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

            // expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm6
            inline void Apply4StepsSSE(biquad_param_t* param) {
                __asm__ __volatile__ (
                    "movss (%2),%%xmm4                # b0\n\t"
                    "shufps   $0x00,%%xmm4,%%xmm4     # copy b0 to other cells\n\t"
                    "mulps  %%xmm0,%%xmm4             # xmm4 = x*b0\n\t"
                    "movups (%0),%%xmm2               # load b1,b2,a1,a2\n\t"
                    "movups (%1),%%xmm5               # load x1,x2,y1,y2\n\t"
                    /* sample 0 */
                    "movaps %%xmm5,%%xmm3\n\t"
                    "mulps  %%xmm2,%%xmm5             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "movss  %%xmm4,%%xmm6\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* sample 1 */
                    "shufps $0x39,%%xmm4,%%xmm4       # rotate xmm4 down 1 cell\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x93,%%xmm6,%%xmm6       # rotate output up 1 cell\n\t"
                    "movaps %%xmm3,%%xmm5\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell\n\t"
                    "mulps  %%xmm2,%%xmm5             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "movss  %%xmm5,%%xmm6\n\t"
                    "addss  %%xmm4,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* sample 2 */
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "shufps $0x39,%%xmm4,%%xmm4       # rotate xmm4 down 1 cell\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x93,%%xmm6,%%xmm6       # rotate output up 1 cell\n\t"
                    "movaps %%xmm3,%%xmm5\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell\n\t"
                    "mulps  %%xmm2,%%xmm5             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "movss  %%xmm5,%%xmm6\n\t"
                    "addss  %%xmm4,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* sample 3 */
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "shufps $0x39,%%xmm4,%%xmm4       # rotate xmm4 down 1 cell\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x93,%%xmm6,%%xmm6       # rotate output up 1 cell\n\t"
                    "mulps  %%xmm3,%%xmm2             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell\n\t"
                    "movss  %%xmm2,%%xmm6\n\t"
                    "shufps $0x39,%%xmm2,%%xmm2\n\t"
                    "addss  %%xmm2,%%xmm6\n\t"
                    "shufps $0x39,%%xmm2,%%xmm2\n\t"
                    "addss  %%xmm2,%%xmm6\n\t"
                    "shufps $0x39,%%xmm2,%%xmm2\n\t"
                    "addss  %%xmm2,%%xmm6\n\t"
                    "addss  %%xmm4,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* done */
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x1b,%%xmm6,%%xmm6       # swap output to correct order\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell, to restore original input\n\t"
                    "movups %%xmm3,(%1)               # store x1,x2,y1,y2\n\t"
                    : /* no output */
                    : "r" (&param->b1), /* %0 - [b1,b2,a1,a2] */
                      "r" (&x1),        /* %1 - [x1,x2,y1,y2] */
                      "r" (&param->b0)  /* %2 */
                );
            }

            inline bq_t ApplyFB(bq_t x, const bq_t fb) {
                bq_t y;

                x += this->y1 * fb * 0.98;
                y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
                    this->a1 * this->y1 + this->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

            inline bq_t ApplyFB(biquad_param_t* param, bq_t x, const bq_t fb) {
                bq_t y;

                x += this->y1 * fb * 0.98;
                y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
                    param->a1 * this->y1 + param->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

            // expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm7
            inline void ApplyFB4StepsSSE(biquad_param_t* param, const bq_t &fb) {
                float xs, ys;
                float t0, t1, t2, t3, t4, t5, t6, t7, t8; // temporary stack space
                __asm__ __volatile__ (
                    /* prepare input */
                    "movss  %15,%%xmm5\n\t"
                    "movss  %%xmm0,(%14)\n\t"
                    /* sample 0 */
                    "movss   %0, %%xmm3\n\t"
                    "movss   %1, %%xmm4\n\t"
                    "mulss   %%xmm4, %%xmm5\n\t"
                    "movss   %%xmm3, %2\n\t"
                    "movss   %%xmm5, %16\n\t"
                    "mulss   %%xmm3, %%xmm5\n\t"
                    "movss   %19, %%xmm2\n\t"
                    "movss   %3, %%xmm6\n\t"
                    "movss   %21, %%xmm3\n\t"
                    "addss   %%xmm5, %%xmm6\n\t"
                    "movss  %%xmm2, %%xmm5\n\t"
                    "movss   %20, %%xmm4\n\t"
                    "movss   %%xmm6, %4\n\t"
                    "mulss   %%xmm6, %%xmm5\n\t"
                    "movss   %5, %%xmm6\n\t"
                    "movss   %%xmm2, %6\n\t"
                    "movss   %%xmm4, %7\n\t"
                    "movss   %%xmm3, %%xmm2\n\t"
                    "mulss   %%xmm6, %%xmm4\n\t"
                    "mulss   %8, %%xmm2\n\t"
                    "movss   %%xmm3, %9\n\t"
                    "addss   %%xmm4, %%xmm5\n\t"
                    "movss   %18, %%xmm3\n\t"
                    "movss   %17, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm5\n\t"
                    "movss   %%xmm4, %10\n\t"
                    "movss   %%xmm3, %%xmm2\n\t"
                    "mulss   %11, %%xmm4\n\t"
                    "mulss   %12, %%xmm2\n\t"
                    "movss   %%xmm3, %13\n\t"
                    "addss   %%xmm4, %%xmm5\n\t"
                    "movss   %11, %%xmm3\n\t"
                    "movss   %4, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm5\n\t"
                    :: "m" (y1),  /* %0 */
                       "m" (fbc), /* %1 */
                       "m" (t0),  /* %2 */
                       "m" (xs),  /* %3 */
                       "m" (t7),  /* %4 */
                       "m" (x1),  /* %5 */
                       "m" (t1),  /* %6 */
                       "m" (t2),  /* %7 */
                       "m" (x2),  /* %8 */
                       "m" (t3),  /* %9 */
                       "m" (t4),  /* %10 */
                       "m" (t0),  /* %11 */
                       "m" (y2),  /* %12 */
                       "m" (t5),  /* %13 */
                       "r" (&xs),  /* %14 */
                       "m" (fb),   /* %15 */
                       "m" (ys),   /* %16 */
                       "m" (param->a1), /* %17 */
                       "m" (param->a2), /* %18 */
                       "m" (param->b0), /* %19 */
                       "m" (param->b1), /* %20 */
                       "m" (param->b2) /* %21 */
                );
                __asm__ __volatile__ (
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell\n\t"
                    "movss  %%xmm5,%%xmm7\n\t"
                    ::
                );
                /* sample 1 */
                __asm__ __volatile__ (
                    "movss   %0, %%xmm4\n\t"
                    "movss   %%xmm0, %%xmm3\n\t"
                    "mulss   %%xmm5, %%xmm4\n\t"
                    "mulss   %3, %%xmm6\n\t"
                    "movss   %5, %%xmm2\n\t"
                    "addss   %%xmm4, %%xmm3\n\t"
                    "mulss   %7, %%xmm2\n\t"
                    "movss   %6, %%xmm4\n\t"
                    "movss   %%xmm3, %8\n\t"
                    "mulss   %%xmm3, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm4\n\t"
                    "movss   %9, %%xmm3\n\t"
                    "mulss   %%xmm5, %%xmm3\n\t"
                    "movss   %10, %%xmm2\n\t"
                    "addss   %%xmm6, %%xmm4\n\t"
                    "mulss   %11, %%xmm2\n\t"
                    "addss   %%xmm3, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm4\n\t"
                    :: "m" (ys),  /* %0 */
                       "m" (fbc), /* %1 */
                       "m" (xs),  /* %2 */
                       "m" (t3),  /* %3 */
                       "m" (y2),  /* %4 */
                       "m" (t2),  /* %5 */
                       "m" (t1),  /* %6 */
                       "m" (t7),  /* %7 */
                       "m" (t8),  /* %8 */
                       "m" (t4),  /* %9 */
                       "m" (t5),  /* %10 */
                       "m" (t0),  /* %11 */
                       "m" (x2),  /* %12 */
                       "m" (x1),  /* %13 */
                       "m" (y1)   /* %14 */
                );
                __asm__ __volatile__ (
                    "shufps $0x93,%%xmm7,%%xmm7  # rotate up one cell\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell\n\t"
                    "movss  %%xmm4,%%xmm7\n\t"
                    ::
                );
                /* sample 2 */
                __asm__ __volatile__ (
                    "movss   %2, %%xmm6\n\t"
                    "movss   %3, %%xmm3\n\t"
                    "mulss   %%xmm4, %%xmm6\n\t"
                    "movss   %4, %%xmm2\n\t"
                    "mulss   %9, %%xmm2\n\t"
                    "addss   %%xmm0, %%xmm6\n\t"
                    "mulss   %7, %%xmm5\n\t"
                    "mulss   %%xmm6, %%xmm3\n\t"
                    "addss   %%xmm2, %%xmm3\n\t"
                    "movss   %5, %%xmm2\n\t"
                    "mulss   %8, %%xmm2\n\t"
                    "addss   %%xmm2, %%xmm3\n\t"
                    "movss   %6, %%xmm2\n\t"
                    "mulss   %%xmm4, %%xmm2\n\t"
                    "addss   %%xmm5, %%xmm2\n\t"
                    "addss   %%xmm2, %%xmm3\n\t"
                    :: "m" (xs),  /* %0 */
                       "m" (fb),  /* %1 */
                       "m" (ys), /* %2 */
                       "m" (t1),  /* %3 */
                       "m" (t2),  /* %4 */
                       "m" (t3),  /* %5 */
                       "m" (t4),  /* %6 */
                       "m" (t5),  /* %7 */
                       "m" (t7),  /* %8 */
                       "m" (t8),  /* %9 */
                       "m" (x1),  /* %10 */
                       "m" (x2),  /* %11 */
                       "m" (y1),  /* %12 */
                       "m" (y2)   /* %13 */
                );
                __asm__ __volatile__ (
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell\n\t"
                    "shufps $0x93,%%xmm7,%%xmm7  # rotate up one cell\n\t"
                    "movss  %%xmm3,%%xmm7\n\t"
                    ::
                );
                /* sample 3 */
                __asm__ __volatile__ (
                    "movss   %1, %%xmm2\n\t"
                    "mulss   %7, %%xmm4\n\t"
                    "mulss   %%xmm3, %%xmm2\n\t"
                    "movss   %3, %%xmm5\n\t"
                    "movss   %%xmm6, %11\n\t"
                    "addss   %%xmm0, %%xmm2\n\t"
                    "movss   %%xmm3, %13\n\t"
                    "mulss   %%xmm2, %%xmm5\n\t"
                    "mulss   %4, %%xmm6\n\t"
                    "movss   %%xmm2, %10\n\t"
                    "addss   %%xmm6, %%xmm5\n\t"
                    "movss   %5, %%xmm2\n\t"
                    "mulss   %9, %%xmm2\n\t"
                    "mulss   %6, %%xmm3\n\t"
                    "addss   %%xmm2, %%xmm5\n\t"
                    "addss   %%xmm3, %%xmm4\n\t"
                    "addss   %%xmm4, %%xmm5\n\t"
                    "movss   %%xmm5, %12\n\t"
                    :: "m" (xs),  /* %0 */
                       "m" (ys),  /* %1 */
                       "m" (fbc), /* %2 */
                       "m" (t1),  /* %3 */
                       "m" (t2),  /* %4 */
                       "m" (t3),  /* %5 */
                       "m" (t4),  /* %6 */
                       "m" (t5),  /* %7 */
                       "m" (t6),  /* %8 */
                       "m" (t8),  /* %9 */
                       "m" (x1),  /* %10 */
                       "m" (x2),  /* %11 */
                       "m" (y1),  /* %12 */
                       "m" (y2)   /* %13 */
                );
                __asm__ __volatile__ (
                    "shufps $0x93,%%xmm7,%%xmm7  # rotate up one cell\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell to restore original input\n\t"
                    "movss  %%xmm5,%%xmm7\n\t"
                    "shufps $0x1b,%%xmm7,%%xmm7  # swap output to correct order\n\t"
                    ::
                );
            }
    };

    class LowpassFilter : public BiquadFilter {
        public:
            inline LowpassFilter() : BiquadFilter() {}

            inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                this->b0 = a0r * (1.0 - cs) * 0.5;
                this->b1 = a0r * (1.0 - cs);
                this->b2 = a0r * (1.0 - cs) * 0.5;
                this->a1 = a0r * (2.0 * cs);
                this->a2 = a0r * (alpha - 1.0);
            }

            inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                param->b0 = a0r * (1.0 - cs) * 0.5;
                param->b1 = a0r * (1.0 - cs);
                param->b2 = a0r * (1.0 - cs) * 0.5;
                param->a1 = a0r * (2.0 * cs);
                param->a2 = a0r * (alpha - 1.0);
            }
    };

    class BandpassFilter : public BiquadFilter {
        public:
            inline BandpassFilter() : BiquadFilter() {}

            inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                this->b0 = a0r * alpha;
                this->b1 = 0.0;
                this->b2 = a0r * -alpha;
                this->a1 = a0r * (2.0 * cs);
                this->a2 = a0r * (alpha - 1.0);
            }

            inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                param->b0 = a0r * alpha;
                param->b1 = 0.0;
                param->b2 = a0r * -alpha;
                param->a1 = a0r * (2.0 * cs);
                param->a2 = a0r * (alpha - 1.0);
            }
    };

    class HighpassFilter : public BiquadFilter {
        public:
            inline HighpassFilter() : BiquadFilter() {}

            inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                this->b0 = a0r * (1.0 + cs) * 0.5;
                this->b1 = a0r * -(1.0 + cs);
                this->b2 = a0r * (1.0 + cs) * 0.5;
                this->a1 = a0r * (2.0 * cs);
                this->a2 = a0r * (alpha - 1.0);
            }

            inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                param->b0 = a0r * (1.0 + cs) * 0.5;
                param->b1 = a0r * -(1.0 + cs);
                param->b2 = a0r * (1.0 + cs) * 0.5;
                param->a1 = a0r * (2.0 * cs);
                param->a2 = a0r * (alpha - 1.0);
            }
    };

} // namespace LinuxSampler

#endif // __LS_BIQUADFILTER_H__
1	/***************************************************************************
2	* *
3	* LinuxSampler - modular, streaming capable sampler *
4	* *
5	* Copyright (C) 2003, 2004 by Benno Senoner and Christian Schoenebeck *
6	* *
7	* This program is free software; you can redistribute it and/or modify *
8	* it under the terms of the GNU General Public License as published by *
9	* the Free Software Foundation; either version 2 of the License, or *
10	* (at your option) any later version. *
11	* *
12	* This program is distributed in the hope that it will be useful, *
13	* but WITHOUT ANY WARRANTY; without even the implied warranty of *
14	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
15	* GNU General Public License for more details. *
16	* *
17	* You should have received a copy of the GNU General Public License *
18	* along with this program; if not, write to the Free Software *
19	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, *
20	* MA 02111-1307 USA *
21	***************************************************************************/
22
23	#ifndef __LS_BIQUADFILTER_H__
24	#define __LS_BIQUADFILTER_H__
25
26	#include <math.h>
27
28	/// ln(2) / 2
29	#define LN_2_2 0.34657359f
30
31	#ifndef LIMIT
32	# define LIMIT(v,l,u) (v < l ? l : (v > u ? u : v))
33	#endif
34
35	namespace LinuxSampler {
36
37	typedef float bq_t;
38
39	/**
40	* Internal parameters of the biquad filter, which are actually the
41	* final parameters of the filter's transfer function. This strucure is
42	* only needed when these parameters should stored outside the
43	* BiquadFilter class, e.g. to save calculation time by sharing them
44	* between multiple filters.
45	*/
46	struct biquad_param_t {
47	bq_t a1;
48	bq_t a2;
49	bq_t b0;
50	bq_t b1;
51	bq_t b2;
52	};
53
54	/**
55	* Bi-quadratic filter
56	* (adapted from lisp code by Eli Brandt, http://www.cs.cmu.edu/~eli/)
57	*/
58	class BiquadFilter {
59	protected:
60	// following five variables are only used if no external biquad_param_t reference is used
61	bq_t a1;
62	bq_t a2;
63	bq_t b0;
64	bq_t b1;
65	bq_t b2;
66	// following four variables are used to buffer the feedback
67	bq_t x1;
68	bq_t x2;
69	bq_t y1;
70	bq_t y2;
71
72	const static float fbc = 0.98;
73
74	/**
75	* Prevent \a f from going into denormal mode which would slow down
76	* subsequent floating point calculations, we achieve that by setting
77	* \a f to zero when it falls under the denormal threshold value.
78	*/
79	inline void KillDenormal(bq_t& f) {
80	// TODO: this is a generic solution for 32bit floats, should be replaced by CPU specific asm code
81	f += 1e-18f;
82	f -= 1e-18f;
83	}
84	public:
85	BiquadFilter() {
86	Reset();
87	}
88
89	void Reset() {
90	x1 = 0.0f;
91	x2 = 0.0f;
92	y1 = 0.0f;
93	y2 = 0.0f;
94	}
95
96	inline bq_t Apply(const bq_t x) {
97	bq_t y;
98
99	y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
100	this->a1 * this->y1 + this->a2 * this->y2;
101	KillDenormal(y);
102	this->x2 = this->x1;
103	this->x1 = x;
104	this->y2 = this->y1;
105	this->y1 = y;
106
107	return y;
108	}
109
110	inline bq_t Apply(biquad_param_t* param, const bq_t x) {
111	bq_t y;
112
113	y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
114	param->a1 * this->y1 + param->a2 * this->y2;
115	KillDenormal(y);
116	this->x2 = this->x1;
117	this->x1 = x;
118	this->y2 = this->y1;
119	this->y1 = y;
120
121	return y;
122	}
123
124	// expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm6
125	inline void Apply4StepsSSE(biquad_param_t* param) {
126	__asm__ __volatile__ (
127	"movss (%2),%%xmm4 # b0\n\t"
128	"shufps $0x00,%%xmm4,%%xmm4 # copy b0 to other cells\n\t"
129	"mulps %%xmm0,%%xmm4 # xmm4 = x*b0\n\t"
130	"movups (%0),%%xmm2 # load b1,b2,a1,a2\n\t"
131	"movups (%1),%%xmm5 # load x1,x2,y1,y2\n\t"
132	/* sample 0 */
133	"movaps %%xmm5,%%xmm3\n\t"
134	"mulps %%xmm2,%%xmm5 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
135	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
136	"movss %%xmm4,%%xmm6\n\t"
137	"addss %%xmm5,%%xmm6\n\t"
138	"shufps $0x39,%%xmm5,%%xmm5\n\t"
139	"addss %%xmm5,%%xmm6\n\t"
140	"shufps $0x39,%%xmm5,%%xmm5\n\t"
141	"addss %%xmm5,%%xmm6\n\t"
142	"shufps $0x39,%%xmm5,%%xmm5\n\t"
143	"addss %%xmm5,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
144	/* sample 1 */
145	"shufps $0x39,%%xmm4,%%xmm4 # rotate xmm4 down 1 cell\n\t"
146	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
147	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
148	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
149	"shufps $0x93,%%xmm6,%%xmm6 # rotate output up 1 cell\n\t"
150	"movaps %%xmm3,%%xmm5\n\t"
151	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell\n\t"
152	"mulps %%xmm2,%%xmm5 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
153	"movss %%xmm5,%%xmm6\n\t"
154	"addss %%xmm4,%%xmm6\n\t"
155	"shufps $0x39,%%xmm5,%%xmm5\n\t"
156	"addss %%xmm5,%%xmm6\n\t"
157	"shufps $0x39,%%xmm5,%%xmm5\n\t"
158	"addss %%xmm5,%%xmm6\n\t"
159	"shufps $0x39,%%xmm5,%%xmm5\n\t"
160	"addss %%xmm5,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
161	/* sample 2 */
162	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
163	"shufps $0x39,%%xmm4,%%xmm4 # rotate xmm4 down 1 cell\n\t"
164	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
165	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
166	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
167	"shufps $0x93,%%xmm6,%%xmm6 # rotate output up 1 cell\n\t"
168	"movaps %%xmm3,%%xmm5\n\t"
169	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell\n\t"
170	"mulps %%xmm2,%%xmm5 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
171	"movss %%xmm5,%%xmm6\n\t"
172	"addss %%xmm4,%%xmm6\n\t"
173	"shufps $0x39,%%xmm5,%%xmm5\n\t"
174	"addss %%xmm5,%%xmm6\n\t"
175	"shufps $0x39,%%xmm5,%%xmm5\n\t"
176	"addss %%xmm5,%%xmm6\n\t"
177	"shufps $0x39,%%xmm5,%%xmm5\n\t"
178	"addss %%xmm5,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
179	/* sample 3 */
180	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
181	"shufps $0x39,%%xmm4,%%xmm4 # rotate xmm4 down 1 cell\n\t"
182	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
183	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
184	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
185	"shufps $0x93,%%xmm6,%%xmm6 # rotate output up 1 cell\n\t"
186	"mulps %%xmm3,%%xmm2 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
187	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell\n\t"
188	"movss %%xmm2,%%xmm6\n\t"
189	"shufps $0x39,%%xmm2,%%xmm2\n\t"
190	"addss %%xmm2,%%xmm6\n\t"
191	"shufps $0x39,%%xmm2,%%xmm2\n\t"
192	"addss %%xmm2,%%xmm6\n\t"
193	"shufps $0x39,%%xmm2,%%xmm2\n\t"
194	"addss %%xmm2,%%xmm6\n\t"
195	"addss %%xmm4,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
196	/* done */
197	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
198	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
199	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
200	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
201	"shufps $0x1b,%%xmm6,%%xmm6 # swap output to correct order\n\t"
202	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell, to restore original input\n\t"
203	"movups %%xmm3,(%1) # store x1,x2,y1,y2\n\t"
204	: /* no output */
205	: "r" (&param->b1), /* %0 - [b1,b2,a1,a2] */
206	"r" (&x1), /* %1 - [x1,x2,y1,y2] */
207	"r" (&param->b0) /* %2 */
208	);
209	}
210
211	inline bq_t ApplyFB(bq_t x, const bq_t fb) {
212	bq_t y;
213
214	x += this->y1 * fb * 0.98;
215	y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
216	this->a1 * this->y1 + this->a2 * this->y2;
217	KillDenormal(y);
218	this->x2 = this->x1;
219	this->x1 = x;
220	this->y2 = this->y1;
221	this->y1 = y;
222
223	return y;
224	}
225
226	inline bq_t ApplyFB(biquad_param_t* param, bq_t x, const bq_t fb) {
227	bq_t y;
228
229	x += this->y1 * fb * 0.98;
230	y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
231	param->a1 * this->y1 + param->a2 * this->y2;
232	KillDenormal(y);
233	this->x2 = this->x1;
234	this->x1 = x;
235	this->y2 = this->y1;
236	this->y1 = y;
237
238	return y;
239	}
240
241	// expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm7
242	inline void ApplyFB4StepsSSE(biquad_param_t* param, const bq_t &fb) {
243	float xs, ys;
244	float t0, t1, t2, t3, t4, t5, t6, t7, t8; // temporary stack space
245	__asm__ __volatile__ (
246	/* prepare input */
247	"movss %15,%%xmm5\n\t"
248	"movss %%xmm0,(%14)\n\t"
249	/* sample 0 */
250	"movss %0, %%xmm3\n\t"
251	"movss %1, %%xmm4\n\t"
252	"mulss %%xmm4, %%xmm5\n\t"
253	"movss %%xmm3, %2\n\t"
254	"movss %%xmm5, %16\n\t"
255	"mulss %%xmm3, %%xmm5\n\t"
256	"movss %19, %%xmm2\n\t"
257	"movss %3, %%xmm6\n\t"
258	"movss %21, %%xmm3\n\t"
259	"addss %%xmm5, %%xmm6\n\t"
260	"movss %%xmm2, %%xmm5\n\t"
261	"movss %20, %%xmm4\n\t"
262	"movss %%xmm6, %4\n\t"
263	"mulss %%xmm6, %%xmm5\n\t"
264	"movss %5, %%xmm6\n\t"
265	"movss %%xmm2, %6\n\t"
266	"movss %%xmm4, %7\n\t"
267	"movss %%xmm3, %%xmm2\n\t"
268	"mulss %%xmm6, %%xmm4\n\t"
269	"mulss %8, %%xmm2\n\t"
270	"movss %%xmm3, %9\n\t"
271	"addss %%xmm4, %%xmm5\n\t"
272	"movss %18, %%xmm3\n\t"
273	"movss %17, %%xmm4\n\t"
274	"addss %%xmm2, %%xmm5\n\t"
275	"movss %%xmm4, %10\n\t"
276	"movss %%xmm3, %%xmm2\n\t"
277	"mulss %11, %%xmm4\n\t"
278	"mulss %12, %%xmm2\n\t"
279	"movss %%xmm3, %13\n\t"
280	"addss %%xmm4, %%xmm5\n\t"
281	"movss %11, %%xmm3\n\t"
282	"movss %4, %%xmm4\n\t"
283	"addss %%xmm2, %%xmm5\n\t"
284	:: "m" (y1), /* %0 */
285	"m" (fbc), /* %1 */
286	"m" (t0), /* %2 */
287	"m" (xs), /* %3 */
288	"m" (t7), /* %4 */
289	"m" (x1), /* %5 */
290	"m" (t1), /* %6 */
291	"m" (t2), /* %7 */
292	"m" (x2), /* %8 */
293	"m" (t3), /* %9 */
294	"m" (t4), /* %10 */
295	"m" (t0), /* %11 */
296	"m" (y2), /* %12 */
297	"m" (t5), /* %13 */
298	"r" (&xs), /* %14 */
299	"m" (fb), /* %15 */
300	"m" (ys), /* %16 */
301	"m" (param->a1), /* %17 */
302	"m" (param->a2), /* %18 */
303	"m" (param->b0), /* %19 */
304	"m" (param->b1), /* %20 */
305	"m" (param->b2) /* %21 */
306	);
307	__asm__ __volatile__ (
308	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell\n\t"
309	"movss %%xmm5,%%xmm7\n\t"
310	::
311	);
312	/* sample 1 */
313	__asm__ __volatile__ (
314	"movss %0, %%xmm4\n\t"
315	"movss %%xmm0, %%xmm3\n\t"
316	"mulss %%xmm5, %%xmm4\n\t"
317	"mulss %3, %%xmm6\n\t"
318	"movss %5, %%xmm2\n\t"
319	"addss %%xmm4, %%xmm3\n\t"
320	"mulss %7, %%xmm2\n\t"
321	"movss %6, %%xmm4\n\t"
322	"movss %%xmm3, %8\n\t"
323	"mulss %%xmm3, %%xmm4\n\t"
324	"addss %%xmm2, %%xmm4\n\t"
325	"movss %9, %%xmm3\n\t"
326	"mulss %%xmm5, %%xmm3\n\t"
327	"movss %10, %%xmm2\n\t"
328	"addss %%xmm6, %%xmm4\n\t"
329	"mulss %11, %%xmm2\n\t"
330	"addss %%xmm3, %%xmm4\n\t"
331	"addss %%xmm2, %%xmm4\n\t"
332	:: "m" (ys), /* %0 */
333	"m" (fbc), /* %1 */
334	"m" (xs), /* %2 */
335	"m" (t3), /* %3 */
336	"m" (y2), /* %4 */
337	"m" (t2), /* %5 */
338	"m" (t1), /* %6 */
339	"m" (t7), /* %7 */
340	"m" (t8), /* %8 */
341	"m" (t4), /* %9 */
342	"m" (t5), /* %10 */
343	"m" (t0), /* %11 */
344	"m" (x2), /* %12 */
345	"m" (x1), /* %13 */
346	"m" (y1) /* %14 */
347	);
348	__asm__ __volatile__ (
349	"shufps $0x93,%%xmm7,%%xmm7 # rotate up one cell\n\t"
350	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell\n\t"
351	"movss %%xmm4,%%xmm7\n\t"
352	::
353	);
354	/* sample 2 */
355	__asm__ __volatile__ (
356	"movss %2, %%xmm6\n\t"
357	"movss %3, %%xmm3\n\t"
358	"mulss %%xmm4, %%xmm6\n\t"
359	"movss %4, %%xmm2\n\t"
360	"mulss %9, %%xmm2\n\t"
361	"addss %%xmm0, %%xmm6\n\t"
362	"mulss %7, %%xmm5\n\t"
363	"mulss %%xmm6, %%xmm3\n\t"
364	"addss %%xmm2, %%xmm3\n\t"
365	"movss %5, %%xmm2\n\t"
366	"mulss %8, %%xmm2\n\t"
367	"addss %%xmm2, %%xmm3\n\t"
368	"movss %6, %%xmm2\n\t"
369	"mulss %%xmm4, %%xmm2\n\t"
370	"addss %%xmm5, %%xmm2\n\t"
371	"addss %%xmm2, %%xmm3\n\t"
372	:: "m" (xs), /* %0 */
373	"m" (fb), /* %1 */
374	"m" (ys), /* %2 */
375	"m" (t1), /* %3 */
376	"m" (t2), /* %4 */
377	"m" (t3), /* %5 */
378	"m" (t4), /* %6 */
379	"m" (t5), /* %7 */
380	"m" (t7), /* %8 */
381	"m" (t8), /* %9 */
382	"m" (x1), /* %10 */
383	"m" (x2), /* %11 */
384	"m" (y1), /* %12 */
385	"m" (y2) /* %13 */
386	);
387	__asm__ __volatile__ (
388	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell\n\t"
389	"shufps $0x93,%%xmm7,%%xmm7 # rotate up one cell\n\t"
390	"movss %%xmm3,%%xmm7\n\t"
391	::
392	);
393	/* sample 3 */
394	__asm__ __volatile__ (
395	"movss %1, %%xmm2\n\t"
396	"mulss %7, %%xmm4\n\t"
397	"mulss %%xmm3, %%xmm2\n\t"
398	"movss %3, %%xmm5\n\t"
399	"movss %%xmm6, %11\n\t"
400	"addss %%xmm0, %%xmm2\n\t"
401	"movss %%xmm3, %13\n\t"
402	"mulss %%xmm2, %%xmm5\n\t"
403	"mulss %4, %%xmm6\n\t"
404	"movss %%xmm2, %10\n\t"
405	"addss %%xmm6, %%xmm5\n\t"
406	"movss %5, %%xmm2\n\t"
407	"mulss %9, %%xmm2\n\t"
408	"mulss %6, %%xmm3\n\t"
409	"addss %%xmm2, %%xmm5\n\t"
410	"addss %%xmm3, %%xmm4\n\t"
411	"addss %%xmm4, %%xmm5\n\t"
412	"movss %%xmm5, %12\n\t"
413	:: "m" (xs), /* %0 */
414	"m" (ys), /* %1 */
415	"m" (fbc), /* %2 */
416	"m" (t1), /* %3 */
417	"m" (t2), /* %4 */
418	"m" (t3), /* %5 */
419	"m" (t4), /* %6 */
420	"m" (t5), /* %7 */
421	"m" (t6), /* %8 */
422	"m" (t8), /* %9 */
423	"m" (x1), /* %10 */
424	"m" (x2), /* %11 */
425	"m" (y1), /* %12 */
426	"m" (y2) /* %13 */
427	);
428	__asm__ __volatile__ (
429	"shufps $0x93,%%xmm7,%%xmm7 # rotate up one cell\n\t"
430	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell to restore original input\n\t"
431	"movss %%xmm5,%%xmm7\n\t"
432	"shufps $0x1b,%%xmm7,%%xmm7 # swap output to correct order\n\t"
433	::
434	);
435	}
436	};
437
438	class LowpassFilter : public BiquadFilter {
439	public:
440	inline LowpassFilter() : BiquadFilter() {}
441
442	inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
443	bq_t omega = 2.0 * M_PI * fc / fs;
444	bq_t sn = sin(omega);
445	bq_t cs = cos(omega);
446	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
447
448	const float a0r = 1.0 / (1.0 + alpha);
449	this->b0 = a0r * (1.0 - cs) * 0.5;
450	this->b1 = a0r * (1.0 - cs);
451	this->b2 = a0r * (1.0 - cs) * 0.5;
452	this->a1 = a0r * (2.0 * cs);
453	this->a2 = a0r * (alpha - 1.0);
454	}
455
456	inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
457	bq_t omega = 2.0 * M_PI * fc / fs;
458	bq_t sn = sin(omega);
459	bq_t cs = cos(omega);
460	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
461
462	const float a0r = 1.0 / (1.0 + alpha);
463	param->b0 = a0r * (1.0 - cs) * 0.5;
464	param->b1 = a0r * (1.0 - cs);
465	param->b2 = a0r * (1.0 - cs) * 0.5;
466	param->a1 = a0r * (2.0 * cs);
467	param->a2 = a0r * (alpha - 1.0);
468	}
469	};
470
471	class BandpassFilter : public BiquadFilter {
472	public:
473	inline BandpassFilter() : BiquadFilter() {}
474
475	inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
476	bq_t omega = 2.0 * M_PI * fc / fs;
477	bq_t sn = sin(omega);
478	bq_t cs = cos(omega);
479	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
480
481	const float a0r = 1.0 / (1.0 + alpha);
482	this->b0 = a0r * alpha;
483	this->b1 = 0.0;
484	this->b2 = a0r * -alpha;
485	this->a1 = a0r * (2.0 * cs);
486	this->a2 = a0r * (alpha - 1.0);
487	}
488
489	inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
490	bq_t omega = 2.0 * M_PI * fc / fs;
491	bq_t sn = sin(omega);
492	bq_t cs = cos(omega);
493	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
494
495	const float a0r = 1.0 / (1.0 + alpha);
496	param->b0 = a0r * alpha;
497	param->b1 = 0.0;
498	param->b2 = a0r * -alpha;
499	param->a1 = a0r * (2.0 * cs);
500	param->a2 = a0r * (alpha - 1.0);
501	}
502	};
503
504	class HighpassFilter : public BiquadFilter {
505	public:
506	inline HighpassFilter() : BiquadFilter() {}
507
508	inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
509	bq_t omega = 2.0 * M_PI * fc / fs;
510	bq_t sn = sin(omega);
511	bq_t cs = cos(omega);
512	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
513
514	const float a0r = 1.0 / (1.0 + alpha);
515	this->b0 = a0r * (1.0 + cs) * 0.5;
516	this->b1 = a0r * -(1.0 + cs);
517	this->b2 = a0r * (1.0 + cs) * 0.5;
518	this->a1 = a0r * (2.0 * cs);
519	this->a2 = a0r * (alpha - 1.0);
520	}
521
522	inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
523	bq_t omega = 2.0 * M_PI * fc / fs;
524	bq_t sn = sin(omega);
525	bq_t cs = cos(omega);
526	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
527
528	const float a0r = 1.0 / (1.0 + alpha);
529	param->b0 = a0r * (1.0 + cs) * 0.5;
530	param->b1 = a0r * -(1.0 + cs);
531	param->b2 = a0r * (1.0 + cs) * 0.5;
532	param->a1 = a0r * (2.0 * cs);
533	param->a2 = a0r * (alpha - 1.0);
534	}
535	};
536
537	} // namespace LinuxSampler
538
539	#endif // __LS_BIQUADFILTER_H__