crap/include/util_def.h

142 lines
3.2 KiB
C
Raw Normal View History

2013-05-22 15:56:59 -07:00
#include <stdlib.h>
#include <math.h>
#include <stdint.h>
2015-04-04 06:48:27 -07:00
INNER void
2014-02-05 23:53:23 -08:00
disable_denormals()
{
#if __SSE2__
_mm_setcsr(_mm_getcsr() | 0x8040);
#endif
}
2013-11-11 07:59:19 -08:00
/* via http://www.rgba.org/articles/sfrand/sfrand.htm */
static unsigned int mirand = 1;
2015-04-04 06:48:27 -07:00
INNER float
2014-02-05 23:47:16 -08:00
whitenoise()
{
2013-11-16 10:19:05 -08:00
union either {
float f;
unsigned int i;
} white;
mirand *= 16807;
white.i = (mirand & 0x007FFFFF) | 0x40000000;
return white.f - 3;
2013-11-11 07:59:19 -08:00
}
2013-05-22 15:56:59 -07:00
/* used to resemble https://github.com/swh/ladspa/blob/master/util/biquad.h */
2015-04-04 06:48:27 -07:00
INNER void
2014-02-05 23:47:16 -08:00
biquad_init(biquad *bq)
{
bq->x1 = bq->x2 = bq->y1 = bq->y2 = 0;
2013-05-22 15:56:59 -07:00
}
2015-04-06 11:26:47 -07:00
static biquad_interim
design(double cw, double sw,
double num0, double num1, double num2,
2014-02-05 23:47:16 -08:00
double den0, double den1, double den2)
{
2013-05-22 15:56:59 -07:00
return (biquad_interim) {
.b0 = num0* (1 + cw) + num1*sw + num2* (1 - cw),
.b1 = num0*-2*(1 + cw) + num2*2*(1 - cw),
.b2 = num0* (1 + cw) - num1*sw + num2* (1 - cw),
.a0 = den0* (1 + cw) + den1*sw + den2* (1 - cw),
.a1 = den0*-2*(1 + cw) + den2*2*(1 - cw),
.a2 = den0* (1 + cw) - den1*sw + den2* (1 - cw),
2013-05-22 15:56:59 -07:00
};
}
static biquad
2014-02-05 23:47:16 -08:00
biquad_gen(filter_t type, double fc, double gain, double bw, double fs)
{
double w0, cw, sw, A, As, Q;
2013-05-22 15:56:59 -07:00
w0 = ANGULAR_LIM(fc, fs);
cw = cos(w0);
sw = sin(w0);
A = DB2LIN(gain/2);
As = sqrt(A);
Q = M_SQRT1_2*(1 - (w0/M_PI)*(w0/M_PI))/bw;
/* skip = (fabs(A - 1) <= TINY); */
2013-05-22 15:56:59 -07:00
biquad_interim bqi;
#define d(n0,n1,n2,d0,d1,d2) bqi = design(cw,sw,n0,n1,n2,d0,d1,d2)
switch (type) {
case FILT_PEAKING: d(1, A/Q, 1, 1, 1/A/Q, 1); break;
case FILT_LOWSHELF: d(1, As/Q, A, 1, 1/As/Q, 1/A); break;
case FILT_HIGHSHELF: d(A, As/Q, 1, 1/A, 1/As/Q, 1); break;
case FILT_LOWPASS: d(0, 0, 1, 1, 1/Q, 1); break;
case FILT_HIGHPASS: d(1, 0, 0, 1, 1/Q, 1); break;
case FILT_ALLPASS: d(1, -1/Q, 1, 1, 1/Q, 1); break;
case FILT_BANDPASS: d(0, 1, 0, 1, 1/Q, 1); break;
case FILT_BANDPASS_2: d(0, 1/Q, 0, 1, 1/Q, 1); break;
case FILT_NOTCH: d(1, 0, 1, 1, 1/Q, 1); break;
case FILT_GAIN: d(A, A, A, 1/A, 1/A, 1/A); break;
}
#undef d
2013-05-22 15:56:59 -07:00
double a0r = 1/bqi.a0;
2013-11-15 19:15:50 -08:00
biquad out;
out.b0 = a0r*bqi.b0;
out.b1 = a0r*bqi.b1;
out.b2 = a0r*bqi.b2;
out.a1 = -a0r*bqi.a1;
out.a2 = -a0r*bqi.a2;
return out;
2013-05-22 15:56:59 -07:00
}
2015-04-04 06:48:27 -07:00
INNER double
2014-02-05 23:47:16 -08:00
biquad_run(biquad *bq, double x)
{
2014-02-05 23:32:24 -08:00
double y;
2013-05-22 15:56:59 -07:00
2013-11-16 10:19:05 -08:00
y = bq->b0*x + bq->b1*bq->x1 + bq->b2*bq->x2
+ bq->a1*bq->y1 + bq->a2*bq->y2;
2013-05-22 15:56:59 -07:00
bq->x2 = bq->x1;
bq->x1 = x;
bq->y2 = bq->y1;
bq->y1 = y;
return y;
}
INNER void
biquad_run_block_stereo(biquad *bq_L, biquad *bq_R,
2015-04-06 11:26:47 -07:00
v2df *buf, ulong count)
{
2015-04-06 11:26:47 -07:00
v2df b0, b1, b2, a1, a2, x1, x2, y1, y2;
b0 = (v2df){bq_L->b0, bq_L->b0};
b1 = (v2df){bq_L->b1, bq_L->b1};
b2 = (v2df){bq_L->b2, bq_L->b2};
a1 = (v2df){bq_L->a1, bq_L->a1};
a2 = (v2df){bq_L->a2, bq_L->a2};
x1 = (v2df){bq_L->x1, bq_R->x1};
x2 = (v2df){bq_L->x2, bq_R->x2};
y1 = (v2df){bq_L->y1, bq_R->y1};
y2 = (v2df){bq_L->y2, bq_R->y2};
for (ulong i = 0; i < count; i++) {
v2df x = buf[i];
v2df y = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2;
x2 = x1;
y2 = y1;
x1 = x;
y1 = y;
2015-04-06 11:26:47 -07:00
buf[i] = y;
}
2015-04-06 11:26:47 -07:00
bq_L->x1 = x1[0];
bq_R->x1 = x1[1];
bq_L->x2 = x2[0];
bq_R->x2 = x2[1];
bq_L->y1 = y1[0];
bq_R->y1 = y1[1];
bq_L->y2 = y2[0];
bq_R->y2 = y2[1];
}