diff --git a/Makefile b/Makefile index 2f192b3..f793473 100755 --- a/Makefile +++ b/Makefile @@ -79,7 +79,7 @@ COMPINCLUDE = $(edirs) $(eddirs) $(pindirs) $(bsdirs) -WARNING_FLAGS = -Wall -Wwrite-strings -Wunsupported -Wall -Wextra -Wwrite-strings -Wno-unused-parameter -Wno-unused-function -Wno-missing-braces -Wno-incompatible-function-pointer-types -Wno-gnu-statement-expression -Wno-complex-component-init -pedantic +WARNING_FLAGS = #-Wall -Wwrite-strings -Wunsupported -Wall -Wextra -Wwrite-strings -Wno-unused-parameter -Wno-unused-function -Wno-missing-braces -Wno-incompatible-function-pointer-types -Wno-gnu-statement-expression -Wno-complex-component-init -pedantic diff --git a/source/engine/datastream.c b/source/engine/datastream.c index d4501da..c4b94cd 100755 --- a/source/engine/datastream.c +++ b/source/engine/datastream.c @@ -73,9 +73,24 @@ void ds_openvideo(struct datastream *ds, const char *video, const char *adriver) struct dsp_filter astream_filter; astream_filter.data = &ds->astream; astream_filter.filter = soundstream_fillbuf; - first_free_bus(astream_filter); - printf("Circbuf size is %u.\n", ds->astream.buf.len); + //struct dsp_filter lpf = lpf_make(8, 10000); + struct dsp_filter lpf = lpf_make(1, 200); + struct dsp_iir *iir = lpf.data; + iir->in = astream_filter; + + + struct dsp_filter hpf = hpf_make(1, 2000); + struct dsp_iir *hiir = hpf.data; + hiir->in = astream_filter; + +/* + struct dsp_filter llpf = lp_fir_make(20); + struct dsp_fir *fir = llpf.data; + fir->in = astream_filter; +*/ + + first_free_bus(hpf); plm_set_video_decode_callback(ds->plm, render_frame, ds); plm_set_audio_decode_callback(ds->plm, render_audio, ds); diff --git a/source/engine/dsp.c b/source/engine/dsp.c index 5441d61..bfeeb25 100644 --- a/source/engine/dsp.c +++ b/source/engine/dsp.c @@ -4,6 +4,7 @@ #include "limits.h" #include "math.h" #include "stdlib.h" +#include "iir.h" #define PI 3.14159265 @@ -193,19 +194,225 @@ void osc_fillbuf(struct osc *osc, short *buf, int n) { for (int i = 0; i < n; i++) { short val = SHRT_MAX * osc->f(phasor_step(&osc->p)); - buf[i*2] = buf[i*2+1] = val; + buf[i*CHANNELS] = buf[i*CHANNELS+1] = val; } } -void gen_whitenoise(void *data, short *buf, int n) +void gen_whitenoise(void *data, short *out, int n) { for (int i = 0; i < n; i++) { - for (int j = 0; j < 2; j++) { - buf[i*2+j] = (rand()>>15) - USHRT_MAX; + for (int j = 0; j < CHANNELS; j++) { + out[i*CHANNELS+j] = (rand()>>15) - USHRT_MAX; } } } +void gen_pinknoise(void *data, short *out, int n) +{ + gen_whitenoise(NULL, out, n); + + double b[2][7] = {0}; + double ccof[6] = {0.99886, 0.99332, 0.96900, 0.8550, 0.55000, -0.76160}; + double dcof[6] = {0.0555179, 0.0750759, 0.1538520, 0.3104856, 0.5329522, 0.0168960}; + + for (int i = 0; i < n; i++) { + for (int j = 0; j < CHANNELS; j++) { + double pink; + double white = (double)out[i*CHANNELS+j]/SHRT_MAX; + + for (int k = 0; k < 5; k++) { + b[j][k] = ccof[k]*b[j][k] + white * dcof[k]; + pink += b[j][k]; + } + + pink += b[j][5] + white*0.5362; + b[j][5] = white*0.115926; + + out[i*CHANNELS+j] = pink * SHRT_MAX; + } + } + /* + * The above is a loopified version of this + * https://www.firstpr.com.au/dsp/pink-noise/ + b0 = 0.99886 * b0 + white * 0.0555179; + b1 = 0.99332 * b1 + white * 0.0750759; + b2 = 0.96900 * b2 + white * 0.1538520; + b3 = 0.86650 * b3 + white * 0.3104856; + b4 = 0.55000 * b4 + white * 0.5329522; + b5 = -0.7616 * b5 - white * 0.0168980; + pink = b0 + b1 + b2 + b3 + b4 + b5 + b6 + white * 0.5362; + b6 = white * 0.115926; + */ +} + +short iir_filter(struct dsp_iir *iir, short val) +{ + float a = 0.f; + + iir->dx[0] = (float)val/SHRT_MAX; + for (int i = 0; i < iir->n; i++) + a += iir->ccof[i] * iir->dx[i]; + + for (int i = iir->n-1; i > 0; i--) + iir->dx[i] = iir->dx[i-1]; + + + for (int i =0; i < iir->n; i++) + a -= iir->dcof[i] * iir->dy[i]; + + iir->dy[0] = a; + + for (int i = iir->n-1; i > 0; i--) + iir->dy[i] = iir->dy[i-1]; + + + + return a * SHRT_MAX; +} + +void dsp_iir_fillbuf(struct dsp_iir *iir, short *out, int n) +{ + dsp_run(iir->in, out, n); + + for (int i = 0; i < n; i++) { + short v = iir_filter(iir, out[i*CHANNELS]); + + for (int j = 0; j < CHANNELS; j++) { + out[i*CHANNELS+j] = v; + } + } +} + +struct dsp_iir new_iir(int poles, float freq) +{ + struct dsp_iir new; + new.poles = poles; + new.freq = freq; + new.n = new.poles+1; + new.ccof = malloc(new.n*sizeof(float)); + new.dcof = malloc(new.n*sizeof(float)); + new.dy = malloc(new.n*sizeof(float)); + new.dx = malloc(new.n*sizeof(float)); + return new; +} + +struct dsp_filter lpf_make(int poles, float freq) +{ + struct dsp_iir *new = malloc(sizeof(*new)); + *new = new_iir(poles, freq); + + double fcf = new->freq*2/SAMPLERATE; + double sf = sf_bwlp(poles, fcf); + + printf("Making LPF filter, fcf: %f, coeffs: %i, scale %1.15lf\n", fcf, new->n, sf); + + int *ccof = ccof_bwlp(new->poles); + double *dcof = dcof_bwlp(new->poles, fcf); + + for (int i = 0; i < new->n; i++) { + new->ccof[i] = ccof[i] * sf; + new->dcof[i] = dcof[i]; + } + + new->dcof[0] = 0.f; + + free(ccof); + free(dcof); + + printf("LPF coefficients are:\n"); + + for (int i = 0; i < new->n; i++) { + printf("%f, %f\n", new->ccof[i], new->dcof[i]); + } + + struct dsp_filter lpf; + lpf.data = new; + lpf.filter = dsp_iir_fillbuf; + + return lpf; +} + +struct dsp_filter hpf_make(int poles, float freq) +{ + struct dsp_iir *new = malloc(sizeof(*new)); + *new = new_iir(poles, freq); + + double fcf = new->freq*2/SAMPLERATE; + double sf = sf_bwhp(new->poles, fcf); + + int *ccof = ccof_bwhp(new->poles); + double *dcof = dcof_bwhp(new->poles, fcf); + + for (int i = 0; i < new->n; i++) { + new->ccof[i] = ccof[i] * sf; + new->dcof[i] = dcof[i]; + } + + for (int i = 0; i < new->n; i++) { + printf("%f, %f\n", new->ccof[i], new->dcof[i]); + } + + free(ccof); + free(dcof); + + struct dsp_filter hpf; + hpf.data = new; + hpf.filter = dsp_iir_fillbuf; + + return hpf; +} + +short fir_filter(struct dsp_fir *fir, short val) +{ + float ret = 0.f; + fir->dx[fir->head] = (float)val/SHRT_MAX; + + for (int i = 0; i < fir->n; i++) { + ret += fir->cof[i] * fir->dx[fir->head--]; + + if (fir->head < 0) fir->head = fir->n-1; + } + + return ret * SHRT_MAX; +} + +void dsp_fir_fillbuf(struct dsp_fir *fir, short *out, int n) +{ + dsp_run(fir->in, out, n); + + for (int i = 0; i < n; i++) { + short val = fir_filter(fir, out[i*CHANNELS]); + // printf("%hd\n", val); + + for (int j = 0; j < CHANNELS; j++) + out[i*CHANNELS + j] = val*5; + } +} + +struct dsp_filter lp_fir_make(float freq) +{ + struct dsp_fir fir; + fir.freq = freq; + fir.n = 9; + fir.head = 0; + double fcf = freq * 2 / SAMPLERATE; + fir.dx = calloc(sizeof(float), fir.n); + fir.cof = fir_lp(fir.n, fcf); + + struct dsp_filter new; + new.data = malloc(sizeof(fir)); + *(struct dsp_fir*)(new.data) = fir; + new.filter = dsp_fir_fillbuf; + + for (int i = 0; i < fir.n; i++) { + printf("%f\n", fir.cof[i]); + } + + return new; +} + + + struct dsp_delay dsp_delay_make(unsigned int ms_delay) { struct dsp_delay new; @@ -226,7 +433,7 @@ void dsp_delay_filbuf(struct dsp_delay *delay, short *buf, int n) static short cache[BUF_FRAMES*2]; dsp_run(delay->in, cache, n); - for (int i = 0; i < n*2; i++) { + for (int i = 0; i < n*CHANNELS; i++) { cbuf_push(&delay->buf, cache[i] / 2); buf[i] = cache[i] + cbuf_shift(&delay->buf); } diff --git a/source/engine/dsp.h b/source/engine/dsp.h index dd65bdb..7c83fbe 100644 --- a/source/engine/dsp.h +++ b/source/engine/dsp.h @@ -16,6 +16,37 @@ struct dsp_filter { void *data; }; +struct dsp_iir { + float freq; + int poles; + int n; // Amount of constants + int order; // How many times it's applied + float *ccof; + float *dcof; + float *dx; + float *dy; + + struct dsp_filter in; +}; + +struct dsp_fir { + float freq; + int n; + int head; + float *cof; + float *dx; + + struct dsp_filter in; +}; + +struct dsp_filter lp_fir_make(float freq); + +void dsp_iir_fillbuf(struct dsp_iir *iir, short *out, int n); +struct dsp_filter hpf_make(int poles, float freq); +struct dsp_filter lpf_make(int poles, float freq); +struct dsp_filter bpf_make(int poles, float freq1, float freq2); +struct dsp_filter npf_make(int poles, float freq1, float freq2); + struct dsp_delay { unsigned int ms_delay; struct circbuf buf; @@ -40,13 +71,7 @@ struct dsp_limiter { }; -struct dsp_hpf { -}; - -struct dsp_lpf { - -}; struct phasor { unsigned int sr; @@ -74,6 +99,9 @@ struct wav gen_square(float amp, float freq, int sr, int ch); struct wav gen_triangle(float amp, float freq, int sr, int ch); struct wav gen_saw(float amp, float freq, int sr, int ch); +void gen_whitenoise(void *data, short *out, int n); +void gen_pinknoise(void *data, short *out, int n); + float sin_phasor(float p); float square_phasor(float p); diff --git a/source/engine/iir.c b/source/engine/iir.c new file mode 100644 index 0000000..834e0b5 --- /dev/null +++ b/source/engine/iir.c @@ -0,0 +1,1089 @@ +/* + * COPYRIGHT + * + * liir - Recursive digital filter functions + * Copyright (C) 2007 Exstrom Laboratories LLC + * + * 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. + * + * A copy of the GNU General Public License is available on the internet at: + * + * http://www.gnu.org/copyleft/gpl.html + * + * or you can write to: + * + * The Free Software Foundation, Inc. + * 675 Mass Ave + * Cambridge, MA 02139, USA + * + * You can contact Exstrom Laboratories LLC via Email at: + * + * stefan(AT)exstrom.com + * + * or you can write to: + * + * Exstrom Laboratories LLC + * P.O. Box 7651 + * Longmont, CO 80501, USA + * + */ + +#include +#include +#include +#include +#include "limits.h" +#include "iir.h" +#include "dsp.h" + +/********************************************************************** + binomial_mult - multiplies a series of binomials together and returns + the coefficients of the resulting polynomial. + + The multiplication has the following form: + + (x+p[0])*(x+p[1])*...*(x+p[n-1]) + + The p[i] coefficients are assumed to be complex and are passed to the + function as a pointer to an array of doubles of length 2n. + + The resulting polynomial has the following form: + + x^n + a[0]*x^n-1 + a[1]*x^n-2 + ... +a[n-2]*x + a[n-1] + + The a[i] coefficients can in general be complex but should in most + cases turn out to be real. The a[i] coefficients are returned by the + function as a pointer to an array of doubles of length 2n. Storage + for the array is allocated by the function and should be freed by the + calling program when no longer needed. + + Function arguments: + + n - The number of binomials to multiply + p - Pointer to an array of doubles where p[2i] (i=0...n-1) is + assumed to be the real part of the coefficient of the ith binomial + and p[2i+1] is assumed to be the imaginary part. The overall size + of the array is then 2n. +*/ + +double *binomial_mult( int n, double *p ) +{ + int i, j; + double *a; + + a = (double *)calloc( 2 * n, sizeof(double) ); + if( a == NULL ) return( NULL ); + + for( i = 0; i < n; ++i ) + { + for( j = i; j > 0; --j ) + { + a[2*j] += p[2*i] * a[2*(j-1)] - p[2*i+1] * a[2*(j-1)+1]; + a[2*j+1] += p[2*i] * a[2*(j-1)+1] + p[2*i+1] * a[2*(j-1)]; + } + a[0] += p[2*i]; + a[1] += p[2*i+1]; + } + return( a ); +} + + +/********************************************************************** + trinomial_mult - multiplies a series of trinomials together and returns + the coefficients of the resulting polynomial. + + The multiplication has the following form: + + (x^2 + b[0]x + c[0])*(x^2 + b[1]x + c[1])*...*(x^2 + b[n-1]x + c[n-1]) + + The b[i] and c[i] coefficients are assumed to be complex and are passed + to the function as a pointers to arrays of doubles of length 2n. The real + part of the coefficients are stored in the even numbered elements of the + array and the imaginary parts are stored in the odd numbered elements. + + The resulting polynomial has the following form: + + x^2n + a[0]*x^2n-1 + a[1]*x^2n-2 + ... +a[2n-2]*x + a[2n-1] + + The a[i] coefficients can in general be complex but should in most cases + turn out to be real. The a[i] coefficients are returned by the function as + a pointer to an array of doubles of length 4n. The real and imaginary + parts are stored, respectively, in the even and odd elements of the array. + Storage for the array is allocated by the function and should be freed by + the calling program when no longer needed. + + Function arguments: + + n - The number of trinomials to multiply + b - Pointer to an array of doubles of length 2n. + c - Pointer to an array of doubles of length 2n. +*/ + +double *trinomial_mult( int n, double *b, double *c ) +{ + int i, j; + double *a; + + a = (double *)calloc( 4 * n, sizeof(double) ); + if( a == NULL ) return( NULL ); + + a[2] = c[0]; + a[3] = c[1]; + a[0] = b[0]; + a[1] = b[1]; + + for( i = 1; i < n; ++i ) + { + a[2*(2*i+1)] += c[2*i]*a[2*(2*i-1)] - c[2*i+1]*a[2*(2*i-1)+1]; + a[2*(2*i+1)+1] += c[2*i]*a[2*(2*i-1)+1] + c[2*i+1]*a[2*(2*i-1)]; + + for( j = 2*i; j > 1; --j ) + { + a[2*j] += b[2*i] * a[2*(j-1)] - b[2*i+1] * a[2*(j-1)+1] + + c[2*i] * a[2*(j-2)] - c[2*i+1] * a[2*(j-2)+1]; + a[2*j+1] += b[2*i] * a[2*(j-1)+1] + b[2*i+1] * a[2*(j-1)] + + c[2*i] * a[2*(j-2)+1] + c[2*i+1] * a[2*(j-2)]; + } + + a[2] += b[2*i] * a[0] - b[2*i+1] * a[1] + c[2*i]; + a[3] += b[2*i] * a[1] + b[2*i+1] * a[0] + c[2*i+1]; + a[0] += b[2*i]; + a[1] += b[2*i+1]; + } + + return( a ); +} + + +/********************************************************************** + dcof_bwlp - calculates the d coefficients for a butterworth lowpass + filter. The coefficients are returned as an array of doubles. + +*/ + +double *dcof_bwlp( int n, double fcf ) +{ + int k; // loop variables + double theta; // M_PI * fcf / 2.0 + double st; // sine of theta + double ct; // cosine of theta + double parg; // pole angle + double sparg; // sine of the pole angle + double cparg; // cosine of the pole angle + double a; // workspace variable + double *rcof; // binomial coefficients + double *dcof; // dk coefficients + + rcof = (double *)calloc( 2 * n, sizeof(double) ); + if( rcof == NULL ) return( NULL ); + + theta = M_PI * fcf; + st = sin(theta); + ct = cos(theta); + + for( k = 0; k < n; ++k ) + { + parg = M_PI * (double)(2*k+1)/(double)(2*n); + sparg = sin(parg); + cparg = cos(parg); + a = 1.0 + st*sparg; + rcof[2*k] = -ct/a; + rcof[2*k+1] = -st*cparg/a; + } + + dcof = binomial_mult( n, rcof ); + free( rcof ); + + dcof[1] = dcof[0]; + dcof[0] = 1.0; + for( k = 3; k <= n; ++k ) + dcof[k] = dcof[2*k-2]; + return( dcof ); +} + +/********************************************************************** + dcof_bwhp - calculates the d coefficients for a butterworth highpass + filter. The coefficients are returned as an array of doubles. + +*/ + +double *dcof_bwhp( int n, double fcf ) +{ + return( dcof_bwlp( n, fcf ) ); +} + + +/********************************************************************** + dcof_bwbp - calculates the d coefficients for a butterworth bandpass + filter. The coefficients are returned as an array of doubles. + +*/ + +double *dcof_bwbp( int n, double f1f, double f2f ) +{ + int k; // loop variables + double theta; // M_PI * (f2f - f1f) / 2.0 + double cp; // cosine of phi + double st; // sine of theta + double ct; // cosine of theta + double s2t; // sine of 2*theta + double c2t; // cosine 0f 2*theta + double *rcof; // z^-2 coefficients + double *tcof; // z^-1 coefficients + double *dcof; // dk coefficients + double parg; // pole angle + double sparg; // sine of pole angle + double cparg; // cosine of pole angle + double a; // workspace variables + + cp = cos(M_PI * (f2f + f1f) / 2.0); + theta = M_PI * (f2f - f1f) / 2.0; + st = sin(theta); + ct = cos(theta); + s2t = 2.0*st*ct; // sine of 2*theta + c2t = 2.0*ct*ct - 1.0; // cosine of 2*theta + + rcof = (double *)calloc( 2 * n, sizeof(double) ); + tcof = (double *)calloc( 2 * n, sizeof(double) ); + + for( k = 0; k < n; ++k ) + { + parg = M_PI * (double)(2*k+1)/(double)(2*n); + sparg = sin(parg); + cparg = cos(parg); + a = 1.0 + s2t*sparg; + rcof[2*k] = c2t/a; + rcof[2*k+1] = s2t*cparg/a; + tcof[2*k] = -2.0*cp*(ct+st*sparg)/a; + tcof[2*k+1] = -2.0*cp*st*cparg/a; + } + + dcof = trinomial_mult( n, tcof, rcof ); + free( tcof ); + free( rcof ); + + dcof[1] = dcof[0]; + dcof[0] = 1.0; + for( k = 3; k <= 2*n; ++k ) + dcof[k] = dcof[2*k-2]; + return( dcof ); +} + +/********************************************************************** + dcof_bwbs - calculates the d coefficients for a butterworth bandstop + filter. The coefficients are returned as an array of doubles. + +*/ + +double *dcof_bwbs( int n, double f1f, double f2f ) +{ + int k; // loop variables + double theta; // M_PI * (f2f - f1f) / 2.0 + double cp; // cosine of phi + double st; // sine of theta + double ct; // cosine of theta + double s2t; // sine of 2*theta + double c2t; // cosine 0f 2*theta + double *rcof; // z^-2 coefficients + double *tcof; // z^-1 coefficients + double *dcof; // dk coefficients + double parg; // pole angle + double sparg; // sine of pole angle + double cparg; // cosine of pole angle + double a; // workspace variables + + cp = cos(M_PI * (f2f + f1f) / 2.0); + theta = M_PI * (f2f - f1f) / 2.0; + st = sin(theta); + ct = cos(theta); + s2t = 2.0*st*ct; // sine of 2*theta + c2t = 2.0*ct*ct - 1.0; // cosine 0f 2*theta + + rcof = (double *)calloc( 2 * n, sizeof(double) ); + tcof = (double *)calloc( 2 * n, sizeof(double) ); + + for( k = 0; k < n; ++k ) + { + parg = M_PI * (double)(2*k+1)/(double)(2*n); + sparg = sin(parg); + cparg = cos(parg); + a = 1.0 + s2t*sparg; + rcof[2*k] = c2t/a; + rcof[2*k+1] = -s2t*cparg/a; + tcof[2*k] = -2.0*cp*(ct+st*sparg)/a; + tcof[2*k+1] = 2.0*cp*st*cparg/a; + } + + dcof = trinomial_mult( n, tcof, rcof ); + free( tcof ); + free( rcof ); + + dcof[1] = dcof[0]; + dcof[0] = 1.0; + for( k = 3; k <= 2*n; ++k ) + dcof[k] = dcof[2*k-2]; + return( dcof ); +} + +/********************************************************************** + ccof_bwlp - calculates the c coefficients for a butterworth lowpass + filter. The coefficients are returned as an array of integers. + +*/ + +int *ccof_bwlp( int n ) +{ + int *ccof; + int m; + int i; + + ccof = (int *)calloc( n+1, sizeof(int) ); + if( ccof == NULL ) return( NULL ); + + ccof[0] = 1; + ccof[1] = n; + m = n/2; + for( i=2; i <= m; ++i) + { + ccof[i] = (n-i+1)*ccof[i-1]/i; + ccof[n-i]= ccof[i]; + } + ccof[n-1] = n; + ccof[n] = 1; + + return( ccof ); +} + +/********************************************************************** + ccof_bwhp - calculates the c coefficients for a butterworth highpass + filter. The coefficients are returned as an array of integers. + +*/ + +int *ccof_bwhp( int n ) +{ + int *ccof; + int i; + + ccof = ccof_bwlp( n ); + if( ccof == NULL ) return( NULL ); + + for( i = 0; i <= n; ++i) + if( i % 2 ) ccof[i] = -ccof[i]; + + return( ccof ); +} + +/********************************************************************** + ccof_bwbp - calculates the c coefficients for a butterworth bandpass + filter. The coefficients are returned as an array of integers. + +*/ + +int *ccof_bwbp( int n ) +{ + int *tcof; + int *ccof; + int i; + + ccof = (int *)calloc( 2*n+1, sizeof(int) ); + if( ccof == NULL ) return( NULL ); + + tcof = ccof_bwhp(n); + if( tcof == NULL ) return( NULL ); + + for( i = 0; i < n; ++i) + { + ccof[2*i] = tcof[i]; + ccof[2*i+1] = 0.0; + } + ccof[2*n] = tcof[n]; + + free( tcof ); + return( ccof ); +} + +/********************************************************************** + ccof_bwbs - calculates the c coefficients for a butterworth bandstop + filter. The coefficients are returned as an array of integers. + +*/ + +double *ccof_bwbs( int n, double f1f, double f2f ) +{ + double alpha; + double *ccof; + int i, j; + + alpha = -2.0 * cos(M_PI * (f2f + f1f) / 2.0) / cos(M_PI * (f2f - f1f) / 2.0); + + ccof = (double *)calloc( 2*n+1, sizeof(double) ); + + ccof[0] = 1.0; + + ccof[2] = 1.0; + ccof[1] = alpha; + + for( i = 1; i < n; ++i ) + { + ccof[2*i+2] += ccof[2*i]; + for( j = 2*i; j > 1; --j ) + ccof[j+1] += alpha * ccof[j] + ccof[j-1]; + + ccof[2] += alpha * ccof[1] + 1.0; + ccof[1] += alpha; + } + + return( ccof ); +} + +/********************************************************************** + sf_bwlp - calculates the scaling factor for a butterworth lowpass filter. + The scaling factor is what the c coefficients must be multiplied by so + that the filter response has a maximum value of 1. + +*/ + +double sf_bwlp( int n, double fcf ) +{ + int m, k; // loop variables + double omega; // M_PI * fcf + double fomega; // function of omega + double parg0; // zeroth pole angle + double sf; // scaling factor + + omega = M_PI * fcf; + fomega = sin(omega); + parg0 = M_PI / (double)(2*n); + + m = n / 2; + sf = 1.0; + for( k = 0; k < n/2; ++k ) + sf *= 1.0 + fomega * sin((double)(2*k+1)*parg0); + + fomega = sin(omega / 2.0); + + if( n % 2 ) sf *= fomega + cos(omega / 2.0); + sf = pow( fomega, n ) / sf; + + return sf; +} + +/********************************************************************** + sf_bwhp - calculates the scaling factor for a butterworth highpass filter. + The scaling factor is what the c coefficients must be multiplied by so + that the filter response has a maximum value of 1. + +*/ + +double sf_bwhp( int n, double fcf ) +{ + int m, k; // loop variables + double omega; // M_PI * fcf + double fomega; // function of omega + double parg0; // zeroth pole angle + double sf; // scaling factor + + omega = M_PI * fcf; + fomega = sin(omega); + parg0 = M_PI / (double)(2*n); + + m = n / 2; + sf = 1.0; + for( k = 0; k < n/2; ++k ) + sf *= 1.0 + fomega * sin((double)(2*k+1)*parg0); + + fomega = cos(omega / 2.0); + + if( n % 2 ) sf *= fomega + sin(omega / 2.0); + sf = pow( fomega, n ) / sf; + + return(sf); +} + +/********************************************************************** + sf_bwbp - calculates the scaling factor for a butterworth bandpass filter. + The scaling factor is what the c coefficients must be multiplied by so + that the filter response has a maximum value of 1. + +*/ + +double sf_bwbp( int n, double f1f, double f2f ) +{ + int k; // loop variables + double ctt; // cotangent of theta + double sfr, sfi; // real and imaginary parts of the scaling factor + double parg; // pole angle + double sparg; // sine of pole angle + double cparg; // cosine of pole angle + double a, b, c; // workspace variables + + ctt = 1.0 / tan(M_PI * (f2f - f1f) / 2.0); + sfr = 1.0; + sfi = 0.0; + + for( k = 0; k < n; ++k ) + { + parg = M_PI * (double)(2*k+1)/(double)(2*n); + sparg = ctt + sin(parg); + cparg = cos(parg); + a = (sfr + sfi)*(sparg - cparg); + b = sfr * sparg; + c = -sfi * cparg; + sfr = b - c; + sfi = a - b - c; + } + + return( 1.0 / sfr ); +} + +/********************************************************************** + sf_bwbs - calculates the scaling factor for a butterworth bandstop filter. + The scaling factor is what the c coefficients must be multiplied by so + that the filter response has a maximum value of 1. + +*/ + +double sf_bwbs( int n, double f1f, double f2f ) +{ + int k; // loop variables + double tt; // tangent of theta + double sfr, sfi; // real and imaginary parts of the scaling factor + double parg; // pole angle + double sparg; // sine of pole angle + double cparg; // cosine of pole angle + double a, b, c; // workspace variables + + tt = tan(M_PI * (f2f - f1f) / 2.0); + sfr = 1.0; + sfi = 0.0; + + for( k = 0; k < n; ++k ) + { + parg = M_PI * (double)(2*k+1)/(double)(2*n); + sparg = tt + sin(parg); + cparg = cos(parg); + a = (sfr + sfi)*(sparg - cparg); + b = sfr * sparg; + c = -sfi * cparg; + sfr = b - c; + sfi = a - b - c; + } + + return( 1.0 / sfr ); +} + + + + +float *fir_lp(int n, double fcf) +{ + float *ret = malloc(n * sizeof(*ret)); + + double d1 = ((double)n - 1.f) / 2.f; + double d2, fc, h; + + fc = fcf * M_PI; + + for (int i = 0; i < n; i++) { + d2 = (double)i - d1; + h = d2 == 0.f ? fc / M_PI : sin(fc * d2) / (M_PI * d2); + ret[i] = h; + } + + return ret; +} + +float *fir_hp(int n, double fcf) +{ + float *ret = malloc(n * sizeof(*ret)); + + double d1 = ((double)n - 1.f) / 2.f; + double d2, fc, h; + + fc = fcf * M_PI; + + for (int i = 0; i < n; i++) { + d2 = (double)i - d1; + h = d2 == 0.f ? 1.f - fc / M_PI : (sin(M_PI * d2) - sin(fc * d2)) / (M_PI * d2); + ret[i] = h; + } + + return ret; +} + +float *fir_bpf(int n, double fcf1, double fcf2) +{ + float *ret = malloc(n * sizeof(*ret)); + + double d1 = ((double)n - 1.f) / 2.f; + double d2, fc1, fc2, h; + + fc1 = fcf1 * M_PI; + fc2 = fcf2 * M_PI; + + for (int i = 0; i < n; i++) { + d2 = (double)i - d1; + h = d2 == 0.f ? (fc2 - fc1) / M_PI : (sin(fc2 * d2) - sin(fc1 * d2)) / (M_PI * d2); + ret[i] = h; + } + + return ret; +} + + + + + + + + + +/* Biquad filters */ + +struct dsp_iir biquad_iir() +{ + struct dsp_iir new; + new.dcof = malloc(sizeof(float) * 3); + new.ccof = malloc(sizeof(float) * 3); + new.dx = malloc(sizeof(float) * 3); + new.dy = malloc(sizeof(float) * 3); + new.n = 3; + new.poles = 2; + return new; +} + +void biquad_iir_fill(struct dsp_iir bq, double *a, double *b) +{ + bq.ccof[0] = (b[0] / a[0]); + bq.ccof[1] = (b[1] / a[0]); + bq.ccof[2] = (b[2] / a[0]); + bq.dcof[0] = 0.f; + bq.dcof[1] = (a[1] / a[0]); + bq.dcof[2] = (a[2] / a[0]); +} + +struct dsp_iir bqlp_dcof(double fcf, float Q) +{ + double w0 = M_PI * fcf; + + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + + b[0] = (1 - cos(w0)) / 2; + b[1] = 1 - cos(w0); + b[2] = b[0]; + + a[0] = 1 + az; + a[1] = -2 * cos(w0); + a[2] = 1 - az; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new; +} + +struct dsp_iir bqhp_dcof(double fcf, float Q) +{ + double w0 = M_PI * fcf; + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + + b[0] = (1 + cos(w0)) / 2; + b[1] = -(1 + cos(w0)); + b[2] = b[0]; + + a[0] = 1 + az; + a[1] = -2 * cos(w0); + a[2] = 1 - az; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new; +} + +struct dsp_iir bqbp_dcof(double fcf, float Q) +{ + double w0 = M_PI * fcf; + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + + b[0] = az; + b[1] = 0; + b[2] = -b[0]; + + a[0] = 1 + az; + a[1] = -2 * cos(w0); + a[2] = 1 - az; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new;} + +struct dsp_iir bqnotch_dcof(double fcf, float Q) +{ + double w0 = M_PI * fcf; + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + + b[0] = 1; + b[1] = -2 * cos(w0); + b[2] = 1; + + a[0] = 1 + az; + a[1] = -2 * cos(w0); + a[2] = 1 - az; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new; +} + +struct dsp_iir bqapf_dcof(double fcf, float Q) +{ + double w0 = M_PI * fcf; + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + + b[0] = 1 - az; + b[1] = -2 * cos(w0); + b[2] = 1 + az; + + a[0] = 1 + az; + a[1] = -2 * cos(w0); + a[2] = 1 - az; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new; +} + +struct dsp_iir bqpeq_dcof(double fcf, float Q, float dbgain) +{ + double w0 = M_PI * fcf; + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + double A = dbgain * 10 / 40; + + b[0] = 1+ az * A; + b[1] = -2 * cos(w0); + b[2] = 1 - az * A; + + a[0] = 1 + az /A; + a[1] = -2 * cos(w0); + a[2] = 1 - az / A; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new;} + +struct dsp_iir bqls_dcof(double fcf, float Q, float dbgain) +{ + double w0 = M_PI * fcf; + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + double A = dbgain * 10 / 40; + + b[0] = A * ((A + 1) - (A - 1) * cos(w0) + 2 * sqrt(A) * az); + b[1] = 2 * A * ((A - 1) - (A + 1) * cos(w0)); + b[2] = A * ((A + 1) - (A - 1) * cos(w0) - 2 * sqrt(A) * az); + + a[0] = (A + 1) + (A - 1) * cos(w0) + 2 * sqrt(A) * az; + a[1] = -2 * ((A - 1) + (A + 1) * cos(w0)); + a[2] = (A + 1) + (A - 1) * cos(w0) - 2 * sqrt(A) * az; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new; +} + +struct dsp_iir bqhs_dcof(double fcf, float Q, float dbgain) +{ + double w0 = M_PI * fcf; + double a[3]; + double b[3]; + double az = sin(w0) / (2 * Q); + double A = dbgain * 10 / 40; + + b[0] = A * ((A + 1) - (A - 1) * cos(w0) + 2 * sqrt(A) * az); + b[1] = -2 * A * ((A - 1) - (A + 1) * cos(w0)); + b[2] = A * ((A + 1) - (A - 1) * cos(w0) - 2 * sqrt(A) * az); + + a[0] = (A + 1) - (A - 1) * cos(w0) + 2 * sqrt(A) * az; + a[1] = -2 * ((A - 1) + (A + 1) * cos(w0)); + a[2] = (A + 1) - (A - 1) * cos(w0) - 2 * sqrt(A) * az; + + struct dsp_iir new = biquad_iir(); + biquad_iir_fill(new, a, b); + return new; +} + + + +/* Bipole Butterworth, Critically damped, and Bessel */ +/* https://unicorn.us.com/trading/allpolefilters.html */ + +void p2_ccalc(double fcf, double p, double g, double *a, double *b) +{ + double w0 = tan(M_PI * fcf); + double k[2]; + k[0] = p * w0; + k[1] = g * pow2(w0); + + a[0] = k[1] / (1 + k[0] + k[1]); + a[1] = 2 * a[0]; + a[2] = a[0]; + b[0] = 0.f; + b[1] = 2 * a[0] * (1/k[1] - 1); + b[2] = 1 - (a[0] + a[1] + a[2] + b[1]); +} + +struct dsp_iir p2_bwlp(double fcf) +{ + double p = sqrt(2); + double g = 1; + + struct dsp_iir new = biquad_iir(); + p2_ccalc(fcf, p, g, new.ccof, new.dcof); + + return new; +} + +struct dsp_iir p2_bwhp(double fcf) +{ + struct dsp_iir new = p2_bwlp(fcf); + new.ccof[1] *= -1; + new.dcof[1] *= -1; + + return new; +} + +struct dsp_iir p2_cdlp(double fcf) +{ + double g = 1; + double p = 2; + + struct dsp_iir new = biquad_iir(); + p2_ccalc(fcf, p, g, new.ccof, new.dcof); + + return new; +} + +struct dsp_iir p2_cdhp(double fcf) +{ + struct dsp_iir new = p2_cdlp(fcf); + new.ccof[1] *= -1; + new.dcof[1] *= -1; + + return new; +} + +struct dsp_iir p2_beslp(double fcf) +{ + double g = 3; + double p = 3; + + struct dsp_iir new = biquad_iir(); + p2_ccalc(fcf, p, g, new.ccof, new.dcof); + + return new; +} + +struct dsp_iir p2_beshp(double fcf) +{ + struct dsp_iir new = p2_beslp(fcf); + new.ccof[1] *= -1; + new.dcof[1] *= -1; + + return new; +} + +struct dsp_iir p2_iir_order(int order) +{ + struct dsp_iir new; + new.n = 3; + new.order = order; + + new.ccof = calloc(sizeof(float), 3 * order); + new.dcof = calloc(sizeof(float), 3 * order); + new.dx = calloc(sizeof(float), 3 * order); + new.dy = calloc(sizeof(float), 3 * order); + + return new; +} + +short p2_filter(struct dsp_iir iir, short val) +{ + float a = (float)val/SHRT_MAX; + + for (int i = 0; i < iir.order; i++) { + int indx = i * iir.n; + + iir.dx[indx] = a; + + a = 0.f; + + for (int j = 0; j < iir.n; j++) + a += iir.ccof[indx + j] * iir.dx[indx]; + + for (int j = iir.n-1; j > 0; j--) + iir.dx[indx] = iir.dx[indx-1]; + + for (int j = 0; j < iir.n; j++) + a -= iir.dcof[indx+j] * iir.dy[indx]; + + iir.dy[indx] = a; + + for (int j = iir.n-1; j > 0; j--) + iir.dy[indx] = iir.dy[indx-1]; + } + + return a * SHRT_MAX; +} + + +struct dsp_iir che_lp(int order, double fcf, double e) +{ + struct dsp_iir new = p2_iir_order(order); + + + double a = tan(M_PI * fcf); + double a2 = pow2(a); + double u = log((1.f + sqrt(1.f + pow2(e)))/e); + double su = sinh(u/new.order); + double cu = cosh(u/new.order); + double b, c, s; + double ep = 2.f/e; + + for (int i = 0; i < new.order; ++i) + { + b = sin(M_PI * (2.f*i + 1.f)/(2.f*new.order)) * su; + c = cos(M_PI * (2.f*i + 1.f)/(2.f*new.order)) * cu; + c = pow2(b) + pow2(c); + s = a2*c + 2.f*a*b + 1.f; + double A = a2/(4.f); + + new.ccof[0*i] = ep * 1.f/A; + new.ccof[1*i] = ep * -2.f/A; + new.ccof[2*i] = ep * 1.f/A; + + new.dcof[0*i] = ep * 0.f; + new.dcof[1*i] = ep * 2.f*(1-a2*c); + new.dcof[2*i] = ep * -(a2*c - 2.f*a*b + 1.f); + } + + return new; +} + +struct dsp_iir che_hp(int order, double fcf, double e) +{ + struct dsp_iir new = che_lp(order, fcf, e); + + double a = tan(M_PI * fcf); + double a2 = pow2(a); + double u = log((1.f + sqrt(1.f + pow2(e)))/e); + double su = sinh(u/new.order); + double cu = cosh(u/new.order); + double b, c, s; + double ep = 2.f/e; + + for (int i = 0; i < new.order; ++i) + { + b = sin(M_PI * (2.f*i + 1.f)/(2.f*new.order)) * su; + c = cos(M_PI * (2.f*i + 1.f)/(2.f*new.order)) * cu; + c = pow2(b) + pow2(c); + s = a2*c + 2.f*a*b + 1.f; + double A = 1.f/(4.f); + + new.ccof[0*i] = ep * 1.f/A; + new.ccof[1*i] = ep * -2.f/A; + new.ccof[2*i] = ep * 1.f/A; + + + } + + return new; +} + +struct dsp_iir che_bp(int order, double fcf1, double fcf2, double e) +{ + struct dsp_iir new = p2_iir_order(order); + + double a = cos(M_PI*(fcf1+fcf2)/2) / cos(M_PI*(fcf2-fcf1)); + double a2 = pow2(a); + double b = tan(M_PI*(fcf2-fcf1)); + double b2 = pow2(b); + double u = log((1.f+sqrt(1.f+pow2(e)))/e); + double su = sinh(2.f*u/new.order); + double cu = cosh(2.f*u/new.order); + double A = b2/(4.f); + double r, c, s; + + for (int i = 0; i < new.order; ++i) { + r = sin(M_PI*(2.f*i+1.f)/new.order)*su; + c = cos(M_PI*(2.f*i+1.f)/new.order)*su; + c = pow2(r) + pow2(c); + s = b2*c + 2.f*b*r + 1.f; + + new.ccof[0*i] = ep * 1.f/A; + new.ccof[1*i] = ep * -2.f/A; + new.ccof[2*i] = ep * 1.f/A; + + new.ddof[0*i] = 0.f; + new.dcof[1*i] = ep * 4.f*a*(1.f+b*r)/s; + new.dcof[2*i] = ep * 2.f*(b2*c-2.f*a2-1.f)/s; + new.dcof[3*i] = ep * 4.f*a*(1.f-b*r)/s; + new.dcof[4*i] = ep * -(b2*c - 2.f*b*r + 1.f) / s; + } + + return new; +} + +struct dsp_iir che_notch(int order, double fcf1, double fcf2, double e) +{ + struct dsp_iir new = p2_iir_order(order); + + double a = cos(M_PI*(fcf1+fcf2)/2) / cos(M_PI*(fcf2-fcf1)); + double a2 = pow2(a); + double b = tan(M_PI*(fcf2-fcf1)); + double b2 = pow2(b); + double u = log((1.f+sqrt(1.f+pow2(e)))/e); + double su = sinh(2.f*u/n); + double cu = cosh(2.f*u/n); + double A = b2/(4.f*s); + double r, c, s; + + for (int i = 0; i < new.order; ++i) { + r = sin(M_PI*(2.f*i+1.f)/new.order)*su; + c = cos(M_PI*(2.f*i+1.f)/ew.order)*su; + c = pow2(r) + pow2(c); + s = b2*c + 2.f*b*r + 1.f; + + new.ccof[0*i] = ep * 1.f/A; + new.ccof[1*i] = ep * -2.f/A; + new.ccof[2*i] = ep * 1.f/A; + + new.ddof[0*i] = 0.f; + new.dcof[1*i] = ep * 4.f*a*(c+b*r)/s; + new.dcof[2*i] = ep * 2.f*(b2-2.f*a2*c-c)/s; + new.dcof[3*i] = ep * 4.f*a*(c-b*r)/s; + new.dcof[4*i] = ep * -(b2 - 2.f*b*r + c) / s; + } + + return new; +} diff --git a/source/engine/iir.h b/source/engine/iir.h new file mode 100644 index 0000000..95a3aff --- /dev/null +++ b/source/engine/iir.h @@ -0,0 +1,48 @@ +#ifndef IIR_H +#define IIR_H + +double *binomial_mult( int n, double *p ); +double *trinomial_mult( int n, double *b, double *c ); + +double *dcof_bwlp( int n, double fcf ); +double *dcof_bwhp( int n, double fcf ); +double *dcof_bwbp( int n, double f1f, double f2f ); +double *dcof_bwbs( int n, double f1f, double f2f ); + +int *ccof_bwlp( int n ); +int *ccof_bwhp( int n ); +int *ccof_bwbp( int n ); +double *ccof_bwbs( int n, double f1f, double f2f ); + +double sf_bwlp( int n, double fcf ); +double sf_bwhp( int n, double fcf ); +double sf_bwbp( int n, double f1f, double f2f ); +double sf_bwbs( int n, double f1f, double f2f ); + +float *fir_lp(int n, double fcf); +float *fir_hp(int n, double fcf); +float *fir_bpf(int n, double fcf1, double fcf2); + +struct dsp_iir bqlp_dcof(double fcf, float Q); +struct dsp_iir bqhp_dcof(double fcf, float Q); +struct dsp_iir bqbpq_dcof(double fcf, float Q); +struct dsp_iir bqbp_dcof(double fcf, float Q); +struct dsp_iir bqnotch_dcof(double fcf, float Q); +struct dsp_iir bqapf_dcof(double fcf, float Q); +struct dsp_iir bqpeq_dcof(double fcf, float Q, float dbgain); +struct dsp_iir bqls_dcof(double fcf, float Q, float dbgain); +struct dsp_iir bqhs_dcof(double fcf, float Q, float dbgain); + +struct dsp_iir p2_bwlp(double fcf); +struct dsp_iir p2_bwhp(double fcf); +struct dsp_iir p2_cdlp(double fcf); +struct dsp_iir p2_cdhp(double fcf); +struct dsp_iir p2_beslp(double fcf); +struct dsp_iir p2_beshp(double fcf); + +struct dsp_iir che_lp(int order, double fcf, double e); +struct dsp_iir che_hp(int order, double fcf, double e); +struct dsp_iir che_bp(int order, double fcf1, double fcf2, double e); +struct dsp_iir che_notch(int order, double fcf1, double fcf2, double e); + +#endif diff --git a/source/engine/sound.c b/source/engine/sound.c index 394de94..8a3a486 100755 --- a/source/engine/sound.c +++ b/source/engine/sound.c @@ -177,7 +177,10 @@ void sound_init() del_filter.data = &dspdel; //first_free_bus(s600); - first_free_bus(del_filter); + + struct dsp_filter wn; + wn.filter = gen_pinknoise; + //first_free_bus(wn); /* if (!drmp3_init_file(&mp3, "sounds/circus.mp3", NULL)) { @@ -366,8 +369,9 @@ struct soundstream soundstream_make() void soundstream_fillbuf(struct soundstream *s, short *buf, int n) { int max = s->buf.write - s->buf.read; - for (int i = 0; i < n*CHANNELS; i++) { - buf[i] = (i < max) ? cbuf_shift(&s->buf) : 0; + int lim = (max < n*CHANNELS) ? max : n*CHANNELS; + for (int i = 0; i < lim; i++) { + buf[i] = cbuf_shift(&s->buf); } }