mirror of
https://gitlab.freedesktop.org/gstreamer/gstreamer.git
synced 2024-07-11 17:06:45 +00:00
349 lines
6.9 KiB
C
349 lines
6.9 KiB
C
/********************************************************************
|
|
* *
|
|
* THIS FILE IS PART OF THE Ogg Vorbis SOFTWARE CODEC SOURCE CODE. *
|
|
* USE, DISTRIBUTION AND REPRODUCTION OF THIS SOURCE IS GOVERNED BY *
|
|
* THE GNU PUBLIC LICENSE 2, WHICH IS INCLUDED WITH THIS SOURCE. *
|
|
* PLEASE READ THESE TERMS DISTRIBUTING. *
|
|
* *
|
|
* THE OggSQUISH SOURCE CODE IS (C) COPYRIGHT 1994-2000 *
|
|
* by Monty <monty@xiph.org> and The XIPHOPHORUS Company *
|
|
* http://www.xiph.org/ *
|
|
* *
|
|
********************************************************************
|
|
|
|
function: Direct Form I, II IIR filters, plus some specializations
|
|
last mod: $Id$
|
|
|
|
********************************************************************/
|
|
|
|
/* LPC is actually a degenerate case of form I/II filters, but we need
|
|
both */
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "iir.h"
|
|
|
|
void
|
|
IIR_init (IIR_state * s, int stages, double gain, double *A, double *B)
|
|
{
|
|
memset (s, 0, sizeof (IIR_state));
|
|
s->stages = stages;
|
|
s->gain = gain;
|
|
s->coeff_A = malloc (stages * sizeof (double));
|
|
s->coeff_B = malloc ((stages + 1) * sizeof (double));
|
|
s->z_A = calloc (stages * 2, sizeof (double));
|
|
s->z_B = calloc (stages * 2, sizeof (double));
|
|
|
|
memcpy (s->coeff_A, A, stages * sizeof (double));
|
|
memcpy (s->coeff_B, B, (stages + 1) * sizeof (double));
|
|
}
|
|
|
|
void
|
|
IIR_clear (IIR_state * s)
|
|
{
|
|
if (s) {
|
|
free (s->coeff_A);
|
|
free (s->coeff_B);
|
|
free (s->z_A);
|
|
free (s->z_B);
|
|
memset (s, 0, sizeof (IIR_state));
|
|
}
|
|
}
|
|
|
|
double
|
|
IIR_filter (IIR_state * s, double in)
|
|
{
|
|
int stages = s->stages, i;
|
|
double newA;
|
|
double newB = 0;
|
|
double *zA = s->z_A + s->ring;
|
|
|
|
newA = in /= s->gain;
|
|
for (i = 0; i < stages; i++) {
|
|
newA += s->coeff_A[i] * zA[i];
|
|
newB += s->coeff_B[i] * zA[i];
|
|
}
|
|
newB += newA * s->coeff_B[stages];
|
|
|
|
zA[0] = zA[stages] = newA;
|
|
if (++s->ring >= stages)
|
|
s->ring = 0;
|
|
|
|
return (newB);
|
|
}
|
|
|
|
/* this assumes the symmetrical structure of the feed-forward stage of
|
|
a Chebyshev bandpass to save multiplies */
|
|
double
|
|
IIR_filter_ChebBand (IIR_state * s, double in)
|
|
{
|
|
int stages = s->stages, i;
|
|
double newA;
|
|
double newB = 0;
|
|
double *zA = s->z_A + s->ring;
|
|
|
|
newA = in /= s->gain;
|
|
|
|
newA += s->coeff_A[0] * zA[0];
|
|
for (i = 1; i < (stages >> 1); i++) {
|
|
newA += s->coeff_A[i] * zA[i];
|
|
newB += s->coeff_B[i] * (zA[i] - zA[stages - i]);
|
|
}
|
|
newB += s->coeff_B[i] * zA[i];
|
|
for (; i < stages; i++)
|
|
newA += s->coeff_A[i] * zA[i];
|
|
|
|
newB += newA - zA[0];
|
|
|
|
zA[0] = zA[stages] = newA;
|
|
if (++s->ring >= stages)
|
|
s->ring = 0;
|
|
|
|
return (newB);
|
|
}
|
|
|
|
#ifdef _V_SELFTEST
|
|
|
|
/* z^-stage, z^-stage+1... */
|
|
static double cheb_bandpass_B[] =
|
|
{ -1., 0., 5., 0., -10., 0., 10., 0., -5., 0., 1 };
|
|
static double cheb_bandpass_A[] = { -0.6665900311,
|
|
1.0070146601,
|
|
-3.1262875409,
|
|
3.5017171569,
|
|
-6.2779211945,
|
|
5.2966481740,
|
|
-6.7570216587,
|
|
4.0760335768,
|
|
-3.9134284363,
|
|
1.3997338886
|
|
};
|
|
|
|
static double data[128] = {
|
|
0.0426331,
|
|
0.0384521,
|
|
0.0345764,
|
|
0.0346069,
|
|
0.0314636,
|
|
0.0310059,
|
|
0.0318604,
|
|
0.0336304,
|
|
0.036438,
|
|
0.0348511,
|
|
0.0354919,
|
|
0.0343628,
|
|
0.0325623,
|
|
0.0318909,
|
|
0.0263367,
|
|
0.0225525,
|
|
0.0195618,
|
|
0.0160828,
|
|
0.0168762,
|
|
0.0145569,
|
|
0.0126343,
|
|
0.0127258,
|
|
0.00820923,
|
|
0.00787354,
|
|
0.00558472,
|
|
0.00204468,
|
|
3.05176e-05,
|
|
-0.00357056,
|
|
-0.00570679,
|
|
-0.00991821,
|
|
-0.0101013,
|
|
-0.00881958,
|
|
-0.0108948,
|
|
-0.0110168,
|
|
-0.0119324,
|
|
-0.0161438,
|
|
-0.0194702,
|
|
-0.0229187,
|
|
-0.0260315,
|
|
-0.0282288,
|
|
-0.0306091,
|
|
-0.0330505,
|
|
-0.0364685,
|
|
-0.0385742,
|
|
-0.0428772,
|
|
-0.043457,
|
|
-0.0425415,
|
|
-0.0462341,
|
|
-0.0467529,
|
|
-0.0489807,
|
|
-0.0520325,
|
|
-0.0558167,
|
|
-0.0596924,
|
|
-0.0591431,
|
|
-0.0612793,
|
|
-0.0618591,
|
|
-0.0615845,
|
|
-0.0634155,
|
|
-0.0639648,
|
|
-0.0683594,
|
|
-0.0718079,
|
|
-0.0729675,
|
|
-0.0791931,
|
|
-0.0860901,
|
|
-0.0885315,
|
|
-0.088623,
|
|
-0.089386,
|
|
-0.0899353,
|
|
-0.0886841,
|
|
-0.0910645,
|
|
-0.0948181,
|
|
-0.0919495,
|
|
-0.0891418,
|
|
-0.0916443,
|
|
-0.096344,
|
|
-0.100464,
|
|
-0.105499,
|
|
-0.108612,
|
|
-0.112213,
|
|
-0.117676,
|
|
-0.120911,
|
|
-0.124329,
|
|
-0.122162,
|
|
-0.120605,
|
|
-0.12326,
|
|
-0.12619,
|
|
-0.128998,
|
|
-0.13205,
|
|
-0.134247,
|
|
-0.137939,
|
|
-0.143555,
|
|
-0.14389,
|
|
-0.14859,
|
|
-0.153717,
|
|
-0.159851,
|
|
-0.164551,
|
|
-0.162811,
|
|
-0.164276,
|
|
-0.156952,
|
|
-0.140564,
|
|
-0.123291,
|
|
-0.10321,
|
|
-0.0827637,
|
|
-0.0652466,
|
|
-0.053772,
|
|
-0.0509949,
|
|
-0.0577698,
|
|
-0.0818176,
|
|
-0.114929,
|
|
-0.148895,
|
|
-0.181122,
|
|
-0.200714,
|
|
-0.21048,
|
|
-0.203644,
|
|
-0.179413,
|
|
-0.145325,
|
|
-0.104492,
|
|
-0.0658264,
|
|
-0.0332031,
|
|
-0.0106201,
|
|
-0.00363159,
|
|
-0.00909424,
|
|
-0.0244141,
|
|
-0.0422058,
|
|
-0.0537415,
|
|
-0.0610046,
|
|
-0.0609741,
|
|
-0.0547791
|
|
};
|
|
|
|
/* comparison test code from http://www-users.cs.york.ac.uk/~fisher/mkfilter/
|
|
(the above page kicks ass, BTW)*/
|
|
|
|
#define NZEROS 10
|
|
#define NPOLES 10
|
|
#define GAIN 4.599477515e+02
|
|
|
|
static float xv[NZEROS + 1], yv[NPOLES + 1];
|
|
|
|
static double
|
|
filterloop (double next)
|
|
{
|
|
xv[0] = xv[1];
|
|
xv[1] = xv[2];
|
|
xv[2] = xv[3];
|
|
xv[3] = xv[4];
|
|
xv[4] = xv[5];
|
|
xv[5] = xv[6];
|
|
xv[6] = xv[7];
|
|
xv[7] = xv[8];
|
|
xv[8] = xv[9];
|
|
xv[9] = xv[10];
|
|
xv[10] = next / GAIN;
|
|
yv[0] = yv[1];
|
|
yv[1] = yv[2];
|
|
yv[2] = yv[3];
|
|
yv[3] = yv[4];
|
|
yv[4] = yv[5];
|
|
yv[5] = yv[6];
|
|
yv[6] = yv[7];
|
|
yv[7] = yv[8];
|
|
yv[8] = yv[9];
|
|
yv[9] = yv[10];
|
|
yv[10] = (xv[10] - xv[0]) + 5 * (xv[2] - xv[8]) + 10 * (xv[6] - xv[4])
|
|
+ (-0.6665900311 * yv[0]) + (1.0070146601 * yv[1])
|
|
+ (-3.1262875409 * yv[2]) + (3.5017171569 * yv[3])
|
|
+ (-6.2779211945 * yv[4]) + (5.2966481740 * yv[5])
|
|
+ (-6.7570216587 * yv[6]) + (4.0760335768 * yv[7])
|
|
+ (-3.9134284363 * yv[8]) + (1.3997338886 * yv[9]);
|
|
return (yv[10]);
|
|
}
|
|
|
|
#include <stdio.h>
|
|
int
|
|
main ()
|
|
{
|
|
|
|
/* run the pregenerated Chebyshev filter, then our own distillation
|
|
through the generic and specialized code */
|
|
double *work = malloc (128 * sizeof (double));
|
|
IIR_state iir;
|
|
int i;
|
|
|
|
for (i = 0; i < 128; i++)
|
|
work[i] = filterloop (data[i]);
|
|
{
|
|
FILE *out = fopen ("IIR_ref.m", "w");
|
|
|
|
for (i = 0; i < 128; i++)
|
|
fprintf (out, "%g\n", work[i]);
|
|
fclose (out);
|
|
}
|
|
|
|
IIR_init (&iir, NPOLES, GAIN, cheb_bandpass_A, cheb_bandpass_B);
|
|
for (i = 0; i < 128; i++)
|
|
work[i] = IIR_filter (&iir, data[i]);
|
|
{
|
|
FILE *out = fopen ("IIR_gen.m", "w");
|
|
|
|
for (i = 0; i < 128; i++)
|
|
fprintf (out, "%g\n", work[i]);
|
|
fclose (out);
|
|
}
|
|
IIR_clear (&iir);
|
|
|
|
IIR_init (&iir, NPOLES, GAIN, cheb_bandpass_A, cheb_bandpass_B);
|
|
for (i = 0; i < 128; i++)
|
|
work[i] = IIR_filter_ChebBand (&iir, data[i]);
|
|
{
|
|
FILE *out = fopen ("IIR_cheb.m", "w");
|
|
|
|
for (i = 0; i < 128; i++)
|
|
fprintf (out, "%g\n", work[i]);
|
|
fclose (out);
|
|
}
|
|
IIR_clear (&iir);
|
|
|
|
return (0);
|
|
}
|
|
|
|
#endif
|