reverb.c

MIDI解码程序(用VC编写)

	tb0 = t3;
	*stream = tb4;
	*high = t3 - tb4;
	*b0 = tb0, *b1 = tb1, *b2 = tb2, *b3 = tb3, *b4 = tb4;
}

static void init_filter_moog_dist(filter_moog_dist *svf)
{
	svf->b0 = svf->b1 = svf->b2 = svf->b3 = svf->b4 = 0.0f;
}

/*! calculate Moog VCF coefficients */
void calc_filter_moog_dist(filter_moog_dist *svf)
{
	double res, fr, p, q, f;

	if (svf->freq > play_mode->rate / 2) {svf->freq = play_mode->rate / 2;}
	else if (svf->freq < 20) {svf->freq = 20;}

	if (svf->freq != svf->last_freq || svf->res_dB != svf->last_res_dB
		 || svf->dist != svf->last_dist) {
		if (svf->last_freq == 0) {init_filter_moog_dist(svf);}
		svf->last_freq = svf->freq, svf->last_res_dB = svf->res_dB,
			svf->last_dist = svf->dist;

		res = pow(10.0f, (svf->res_dB - 96.0f) / 20.0f);
		fr = 2.0f * (double)svf->freq / (double)play_mode->rate;
		q = 1.0f - fr;
		p = fr + 0.8f * fr * q;
		f = p + p - 1.0f;
		q = res * (1.0f + 0.5f * q * (1.0f - q + 5.6f * q * q));
		svf->f = f;
		svf->p = p;
		svf->q = q;
		svf->d = 1.0f + svf->dist;
	}
}

static inline void do_filter_moog_dist(double *stream, double *high, double *band, double f, double p, double q, double d,
								   double *b0, double *b1, double *b2, double *b3, double *b4)
{
	double t1, t2, in, tb0 = *b0, tb1 = *b1, tb2 = *b2, tb3 = *b3, tb4 = *b4;
	in = *stream;
	in -= q * tb4;
	t1 = tb1;  tb1 = (in + tb0) * p - tb1 * f;
	t2 = tb2;  tb2 = (tb1 + t1) * p - tb2 * f;
	t1 = tb3;  tb3 = (tb2 + t2) * p - tb3 * f;
	tb4 = (tb3 + t1) * p - tb4 * f;
	tb4 *= d;
	tb4 = tb4 - tb4 * tb4 * tb4 * 0.166667f;
	tb0 = in;
	*stream = tb4;
	*high = in - tb4;
	*band = 3.0f * (tb3 - tb4);
	*b0 = tb0, *b1 = tb1, *b2 = tb2, *b3 = tb3, *b4 = tb4;
}

static inline void do_filter_moog_dist_band(double *stream, double f, double p, double q, double d,
								   double *b0, double *b1, double *b2, double *b3, double *b4)
{
	double t1, t2, in, tb0 = *b0, tb1 = *b1, tb2 = *b2, tb3 = *b3, tb4 = *b4;
	in = *stream;
	in -= q * tb4;
	t1 = tb1;  tb1 = (in + tb0) * p - tb1 * f;
	t2 = tb2;  tb2 = (tb1 + t1) * p - tb2 * f;
	t1 = tb3;  tb3 = (tb2 + t2) * p - tb3 * f;
	tb4 = (tb3 + t1) * p - tb4 * f;
	tb4 *= d;
	tb4 = tb4 - tb4 * tb4 * tb4 * 0.166667f;
	tb0 = in;
	*stream = 3.0f * (tb3 - tb4);
	*b0 = tb0, *b1 = tb1, *b2 = tb2, *b3 = tb3, *b4 = tb4;
}

static void init_filter_lpf18(filter_lpf18 *p)
{
	p->ay1 = p->ay2 = p->aout = p->lastin = 0;
}

/*! calculate LPF18 coefficients */
void calc_filter_lpf18(filter_lpf18 *p)
{
	double kfcn, kp, kp1, kp1h, kres, value;

	if(p->freq != p->last_freq || p->dist != p->last_dist
		|| p->res != p->last_res) {
		if(p->last_freq == 0) {init_filter_lpf18(p);}
		p->last_freq = p->freq, p->last_dist = p->dist, p->last_res = p->res;

		kfcn = 2.0 * (double)p->freq / (double)play_mode->rate;
		kp = ((-2.7528 * kfcn + 3.0429) * kfcn + 1.718) * kfcn - 0.9984;
		kp1 = kp + 1.0;
		kp1h = 0.5 * kp1;
		kres = p->res * (((-2.7079 * kp1 + 10.963) * kp1 - 14.934) * kp1 + 8.4974);
		value = 1.0 + (p->dist * (1.5 + 2.0 * kres * (1.0 - kfcn)));

		p->kp = kp, p->kp1h = kp1h, p->kres = kres, p->value = value;
	}
}

static inline void do_filter_lpf18(double *stream, double *ay1, double *ay2, double *aout, double *lastin,
								   double kres, double value, double kp, double kp1h)
{
	double ax1, ay11, ay31;
	ax1 = *lastin;
	ay11 = *ay1;
	ay31 = *ay2;
	*lastin = *stream - tanh(kres * *aout);
	*ay1 = kp1h * (*lastin + ax1) - kp * *ay1;
	*ay2 = kp1h * (*ay1 + ay11) - kp * *ay2;
	*aout = kp1h * (*ay2 + ay31) - kp * *aout;
	*stream = tanh(*aout * value);
}

#define WS_AMP_MAX	((int32) 0x0fffffff)
#define WS_AMP_MIN	((int32)-0x0fffffff)

static void do_dummy_clipping(int32 *stream, int32 d) {}

static void do_hard_clipping(int32 *stream, int32 d)
{
	int32 x;
	x = imuldiv24(*stream, d);
	x = (x > WS_AMP_MAX) ? WS_AMP_MAX
			: (x < WS_AMP_MIN) ? WS_AMP_MIN : x;
	*stream = x;
}

static void do_soft_clipping1(int32 *stream, int32 d)
{
	int32 x, ai = TIM_FSCALE(1.5f, 24), bi = TIM_FSCALE(0.5f, 24);
	x = imuldiv24(*stream, d);
	x = (x > WS_AMP_MAX) ? WS_AMP_MAX
			: (x < WS_AMP_MIN) ? WS_AMP_MIN : x;
	x = imuldiv24(x, ai) - imuldiv24(imuldiv28(imuldiv28(x, x), x), bi);
	*stream = x;
}

static void do_soft_clipping2(int32 *stream, int32 d)
{
	int32 x;
	x = imuldiv24(*stream, d);
	x = (x > WS_AMP_MAX) ? WS_AMP_MAX
			: (x < WS_AMP_MIN) ? WS_AMP_MIN : x;
	x = signlong(x) * ((abs(x) << 1) - imuldiv28(x, x));
	*stream = x;
}

/*! 1st order lowpass filter */
void init_filter_lowpass1(filter_lowpass1 *p)
{
	if (p->a > 1.0f) {p->a = 1.0f;}
	p->x1l = p->x1r = 0;
	p->ai = TIM_FSCALE(p->a, 24);
	p->iai = TIM_FSCALE(1.0 - p->a, 24);
}

static inline void do_filter_lowpass1(int32 *stream, int32 *x1, int32 a, int32 ia)
{
	*stream = *x1 = imuldiv24(*x1, ia) + imuldiv24(*stream, a);
}

void do_filter_lowpass1_stereo(int32 *buf, int32 count, filter_lowpass1 *p)
{
	int32 i, a = p->ai, ia = p->iai, x1l = p->x1l, x1r = p->x1r;

	for(i = 0; i < count; i++) {
		do_filter_lowpass1(&buf[i], &x1l, a, ia);
		++i;
		do_filter_lowpass1(&buf[i], &x1r, a, ia);
	}
	p->x1l = x1l, p->x1r = x1r;
}

void init_filter_biquad(filter_biquad *p)
{
	p->x1l = 0, p->x2l = 0, p->y1l = 0, p->y2l = 0, p->x1r = 0,
		p->x2r = 0, p->y1r = 0, p->y2r = 0;
}

/*! biquad lowpass filter */
void calc_filter_biquad_low(filter_biquad *p)
{
	double a0, a1, a2, b1, b02, omega, sn, cs, alpha;

	if(p->freq != p->last_freq || p->q != p->last_q) {
		if (p->last_freq == 0) {init_filter_biquad(p);}
		p->last_freq = p->freq, p->last_q = p->q;
		omega = 2.0 * M_PI * (double)p->freq / (double)play_mode->rate;
		sn = sin(omega);
		cs = cos(omega);
		if (p->q == 0 || p->freq < 0 || p->freq > play_mode->rate / 2) {
			p->b02 = TIM_FSCALE(1.0, 24);
			p->a1 = p->a2 = p->b1 = 0;
			return;
		} else {alpha = sn / (2.0 * p->q);}

		a0 = 1.0f / (1.0f + alpha);
		b02 = ((1.0f - cs) / 2.0f) * a0;
		b1 = (1.0f - cs) * a0;
		a1 = (-2.0f * cs) * a0;
		a2 = (1.0f - alpha) * a0;

		p->b1 = TIM_FSCALE(b1, 24);
		p->a2 = TIM_FSCALE(a2, 24);
		p->a1 = TIM_FSCALE(a1, 24);
		p->b02 = TIM_FSCALE(b02, 24);
	}
}

/*! biquad highpass filter */
void calc_filter_biquad_high(filter_biquad *p)
{
	double a0, a1, a2, b1, b02, omega, sn, cs, alpha;

	if(p->freq != p->last_freq || p->q != p->last_q) {
		if (p->last_freq == 0) {init_filter_biquad(p);}
		p->last_freq = p->freq, p->last_q = p->q;
		omega = 2.0 * M_PI * (double)p->freq / (double)play_mode->rate;
		sn = sin(omega);
		cs = cos(omega);
		if (p->q == 0 || p->freq < 0 || p->freq > play_mode->rate / 2) {
			p->b02 = TIM_FSCALE(1.0, 24);
			p->a1 = p->a2 = p->b1 = 0;
			return;
		} else {alpha = sn / (2.0 * p->q);}

		a0 = 1.0f / (1.0f + alpha);
		b02 = ((1.0f + cs) / 2.0f) * a0;
		b1 = (-(1.0f + cs)) * a0;
		a1 = (-2.0f * cs) * a0;
		a2 = (1.0f - alpha) * a0;

		p->b1 = TIM_FSCALE(b1, 24);
		p->a2 = TIM_FSCALE(a2, 24);
		p->a1 = TIM_FSCALE(a1, 24);
		p->b02 = TIM_FSCALE(b02, 24);
	}
}

static inline void do_filter_biquad(int32 *stream, int32 a1, int32 a2, int32 b1,
									int32 b02, int32 *x1, int32 *x2, int32 *y1, int32 *y2)
{
	int32 t1;
	t1 = imuldiv24(*stream + *x2, b02) + imuldiv24(*x1, b1) - imuldiv24(*y1, a1) - imuldiv24(*y2, a2);
	*x2 = *x1;
	*x1 = *stream;
	*y2 = *y1;
	*y1 = t1;
	*stream = t1;
}

static void do_biquad_filter_stereo(int32* buf, int32 count, filter_biquad *p)
{
	int32 i;
	int32 x1l = p->x1l, x2l = p->x2l, y1l = p->y1l, y2l = p->y2l,
		x1r = p->x1r, x2r = p->x2r, y1r = p->y1r, y2r = p->y2r;
	int32 a1 = p->a1, a2 = p->a2, b02 = p->b02, b1 = p->b1;

	for(i = 0; i < count; i++) {
		do_filter_biquad(&(buf[i]), a1, a2, b1, b02, &x1l, &x2l, &y1l, &y2l);
		++i;
		do_filter_biquad(&(buf[i]), a1, a2, b1, b02, &x1r, &x2r, &y1r, &y2r);
	}
	p->x1l = x1l, p->x2l = x2l, p->y1l = y1l, p->y2l = y2l,
		p->x1r = x1r, p->x2r = x2r, p->y1r = y1r, p->y2r = y2r;
}

void init_filter_shelving(filter_shelving *p)
{
	p->x1l = 0, p->x2l = 0, p->y1l = 0, p->y2l = 0, p->x1r = 0,
		p->x2r = 0, p->y1r = 0, p->y2r = 0;
}

/*! shelving filter */
void calc_filter_shelving_low(filter_shelving *p)
{
	double a0, a1, a2, b0, b1, b2, omega, sn, cs, A, beta;

	init_filter_shelving(p);

	A = pow(10, p->gain / 40);
	omega = 2.0 * M_PI * (double)p->freq / (double)play_mode->rate;
	sn = sin(omega);
	cs = cos(omega);
	if (p->freq < 0 || p->freq > play_mode->rate / 2) {
		p->b0 = TIM_FSCALE(1.0, 24);
		p->a1 = p->b1 = p->a2 = p->b2 = 0;
		return;
	}
	if (p->q == 0) {beta = sqrt(A + A);}
	else {beta = sqrt(A) / p->q;}

	a0 = 1.0 / ((A + 1) + (A - 1) * cs + beta * sn);
	a1 = 2.0 * ((A - 1) + (A + 1) * cs);
	a2 = -((A + 1) + (A - 1) * cs - beta * sn);
	b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
	b1 = 2.0 * A * ((A - 1) - (A + 1) * cs);
	b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);

	a1 *= a0;
	a2 *= a0;
	b1 *= a0;
	b2 *= a0;
	b0 *= a0;

	p->a1 = TIM_FSCALE(a1, 24);
	p->a2 = TIM_FSCALE(a2, 24);
	p->b0 = TIM_FSCALE(b0, 24);
	p->b1 = TIM_FSCALE(b1, 24);
	p->b2 = TIM_FSCALE(b2, 24);
}

void calc_filter_shelving_high(filter_shelving *p)
{
	double a0, a1, a2, b0, b1, b2, omega, sn, cs, A, beta;

	init_filter_shelving(p);

	A = pow(10, p->gain / 40);
	omega = 2.0 * M_PI * (double)p->freq / (double)play_mode->rate;
	sn = sin(omega);
	cs = cos(omega);
	if (p->freq < 0 || p->freq > play_mode->rate / 2) {
		p->b0 = TIM_FSCALE(1.0, 24);
		p->a1 = p->b1 = p->a2 = p->b2 = 0;
		return;
	}
	if (p->q == 0) {beta = sqrt(A + A);}
	else {beta = sqrt(A) / p->q;}

	a0 = 1.0 / ((A + 1) - (A - 1) * cs + beta * sn);
	a1 = (-2 * ((A - 1) - (A + 1) * cs));
	a2 = -((A + 1) - (A - 1) * cs - beta * sn);
	b0 = A * ((A + 1) + (A - 1) * cs + beta * sn);
	b1 = -2 * A * ((A - 1) + (A + 1) * cs);
	b2 = A * ((A + 1) + (A - 1) * cs - beta * sn);

	a1 *= a0;
	a2 *= a0;
	b0 *= a0;
	b1 *= a0;
	b2 *= a0;

	p->a1 = TIM_FSCALE(a1, 24);
	p->a2 = TIM_FSCALE(a2, 24);
	p->b0 = TIM_FSCALE(b0, 24);
	p->b1 = TIM_FSCALE(b1, 24);
	p->b2 = TIM_FSCALE(b2, 24);
}

static void do_shelving_filter_stereo(int32* buf, int32 count, filter_shelving *p)
{
	int32 i;
	int32 x1l = p->x1l, x2l = p->x2l, y1l = p->y1l, y2l = p->y2l,
		x1r = p->x1r, x2r = p->x2r, y1r = p->y1r, y2r = p->y2r, yout;
	int32 a1 = p->a1, a2 = p->a2, b0 = p->b0, b1 = p->b1, b2 = p->b2;

	for(i = 0; i < count; i++) {
		yout = imuldiv24(buf[i], b0) + imuldiv24(x1l, b1) + imuldiv24(x2l, b2) + imuldiv24(y1l, a1) + imuldiv24(y2l, a2);
		x2l = x1l;
		x1l = buf[i];
		y2l = y1l;
		y1l = yout;
		buf[i] = yout;

		yout = imuldiv24(buf[++i], b0) + imuldiv24(x1r, b1) + imuldiv24(x2r, b2) + imuldiv24(y1r, a1) + imuldiv24(y2r, a2);
		x2r = x1r;
		x1r = buf[i];
		y2r = y1r;
		y1r = yout;
		buf[i] = yout;
	}
	p->x1l = x1l, p->x2l = x2l, p->y1l = y1l, p->y2l = y2l,
		p->x1r = x1r, p->x2r = x2r, p->y1r = y1r, p->y2r = y2r;
}

void init_filter_peaking(filter_peaking *p)
{
	p->x1l = 0, p->x2l = 0, p->y1l = 0, p->y2l = 0, p->x1r = 0,
		p->x2r = 0, p->y1r = 0, p->y2r = 0;
}

/*! peaking filter */
void calc_filter_peaking(filter_peaking *p)
{
	double a0, ba1, a2, b0, b2, omega, sn, cs, A, alpha;

	init_filter_peaking(p);

	A = pow(10, p->gain / 40);
	omega = 2.0 * M_PI * (double)p->freq / (double)play_mode->rate;
	sn = sin(omega);
	cs = cos(omega);
	if (p->q == 0 || p->freq < 0 || p->freq > play_mode->rate / 2) {
		p->b0 = TIM_FSCALE(1.0, 24);
		p->ba1 = p->a2 = p->b2 = 0;
		return;
	} else {alpha = sn / (2.0 * p->q);}

	a0 = 1.0 / (1.0 + alpha / A);
	ba1 = -2.0 * cs;
	a2 = 1.0 - alpha / A;
	b0 = 1.0 + alpha * A;
	b2 = 1.0 - alpha * A;

	ba1 *= a0;
	a2 *= a0;
	b0 *= a0;
	b2 *= a0;

	p->ba1 = TIM_FSCALE(ba1, 24);
	p->a2 = TIM_FSCALE(a2, 24);
	p->b0 = TIM_FSCALE(b0, 24);
	p->b2 = TIM_FSCALE(b2, 24);
}

static void do_peaking_filter_stereo(int32* buf, int32 count, filter_peaking *p)
{
	int32 i;
	int32 x1l = p->x1l, x2l = p->x2l, y1l = p->y1l, y2l = p->y2l,
		x1r = p->x1r, x2r = p->x2r, y1r = p->y1r, y2r = p->y2r, yout;