TEST(DSPDouble, TestVectorScalarMultiply)
{
double out[10];
VectorScalarMultiplyD(out, rampD, 2.0, 10);
for (unsigned i = 0; i < 10; ++i)
{
ASSERT_DOUBLE_EQ(2.0 * rampD[i], out[i]);
}
}
Error_t
UpsamplerProcessD(UpsamplerD* upsampler,
double* outBuffer,
const double* inBuffer,
unsigned n_samples)
{
double tempbuf[n_samples];
if (upsampler && outBuffer)
{
for (unsigned filt = 0; filt < upsampler->factor; ++ filt)
{
FIRFilterProcessD(upsampler->polyphase[filt], tempbuf, inBuffer, n_samples);
CopyBufferStrideD(outBuffer+filt, upsampler->factor, tempbuf, 1, n_samples);
}
VectorScalarMultiplyD(outBuffer, (const double*)outBuffer,
upsampler->factor, n_samples * upsampler->factor);
return NOERR;
}
else
{
return NULL_PTR_ERROR;
}
}
static Error_t
RBJFilterUpdateD(RBJFilterD* filter)
{
filter->cosOmega = cos(filter->omega);
filter->sinOmega = sin(filter->omega);
switch (filter->type)
{
case LOWPASS:
filter->alpha = filter->sinOmega / (2.0 * filter->Q);
filter->b[0] = (1 - filter->cosOmega) / 2;
filter->b[1] = 1 - filter->cosOmega;
filter->b[2] = filter->b[0];
filter->a[0] = 1 + filter->alpha;
filter->a[1] = -2 * filter->cosOmega;
filter->a[2] = 1 - filter->alpha;
break;
case HIGHPASS:
filter->alpha = filter->sinOmega / (2.0 * filter->Q);
filter->b[0] = (1 + filter->cosOmega) / 2;
filter->b[1] = -(1 + filter->cosOmega);
filter->b[2] = filter->b[0];
filter->a[0] = 1 + filter->alpha;
filter->a[1] = -2 * filter->cosOmega;
filter->a[2] = 1 - filter->alpha;
break;
case BANDPASS:
filter->alpha = filter->sinOmega * sinh(logf(2.0) / 2.0 * \
filter->Q * filter->omega/filter->sinOmega);
filter->b[0] = filter->sinOmega / 2;
filter->b[1] = 0;
filter->b[2] = -filter->b[0];
filter->a[0] = 1 + filter->alpha;
filter->a[1] = -2 * filter->cosOmega;
filter->a[2] = 1 - filter->alpha;
break;
case ALLPASS:
filter->alpha = filter->sinOmega / (2.0 * filter->Q);
filter->b[0] = 1 - filter->alpha;
filter->b[1] = -2 * filter->cosOmega;
filter->b[2] = 1 + filter->alpha;
filter->a[0] = filter->b[2];
filter->a[1] = filter->b[1];
filter->a[2] = filter->b[0];
break;
case NOTCH:
filter->alpha = filter->sinOmega * sinh(logf(2.0) / 2.0 * \
filter->Q * filter->omega/filter->sinOmega);
filter->b[0] = 1;
filter->b[1] = -2 * filter->cosOmega;
filter->b[2] = 1;
filter->a[0] = 1 + filter->alpha;
filter->a[1] = filter->b[1];
filter->a[2] = 1 - filter->alpha;
break;
case PEAK:
filter->alpha = filter->sinOmega * sinh(logf(2.0) / 2.0 * \
filter->Q * filter->omega/filter->sinOmega);
filter->b[0] = 1 + (filter->alpha * filter->A);
filter->b[1] = -2 * filter->cosOmega;
filter->b[2] = 1 - (filter->alpha * filter->A);
filter->a[0] = 1 + (filter->alpha / filter->A);
filter->a[1] = filter->b[1];
filter->a[2] = 1 - (filter->alpha / filter->A);
break;
case LOW_SHELF:
filter->alpha = filter->sinOmega / 2.0 * sqrt( (filter->A + 1.0 / \
filter->A) * (1.0 / filter->Q - 1.0) + 2.0);
filter->b[0] = filter->A * ((filter->A + 1) - ((filter->A - 1) * \
filter->cosOmega) + (2 * sqrtf(filter->A) * filter->alpha));
filter->b[1] = 2 * filter->A * ((filter->A - 1) - ((filter->A + 1) * \
filter->cosOmega));
filter->b[2] = filter->A * ((filter->A + 1) - ((filter->A - 1) * \
filter->cosOmega) - (2 * sqrtf(filter->A) * filter->alpha));
filter->a[0] = ((filter->A + 1) + ((filter->A - 1) * \
filter->cosOmega) + (2 * sqrtf(filter->A) * filter->alpha));
filter->a[1] = -2 * ((filter->A - 1) + ((filter->A + 1) * \
filter->cosOmega));
filter->a[2] = ((filter->A + 1) + ((filter->A - 1) * \
filter->cosOmega) - (2 * sqrtf(filter->A) * filter->alpha));
break;
case HIGH_SHELF:
filter->alpha = filter->sinOmega / 2.0 * sqrt( (filter->A + 1.0 / \
filter->A) * (1.0 / filter->Q - 1.0) + 2.0);
filter->b[0] = filter->A * ((filter->A + 1) + ((filter->A - 1) * \
filter->cosOmega) + (2 * sqrtf(filter->A) * filter->alpha));
filter->b[1] = -2 * filter->A * ((filter->A - 1) + ((filter->A + 1) * \
filter->cosOmega));
filter->b[2] = filter->A * ((filter->A + 1) + ((filter->A - 1) * \
filter->cosOmega) - (2 * sqrtf(filter->A) * filter->alpha));
filter->a[0] = ((filter->A + 1) - ((filter->A - 1) * \
filter->cosOmega) + (2 * sqrtf(filter->A) * filter->alpha));
filter->a[1] = 2 * ((filter->A - 1) - ((filter->A + 1) * \
filter->cosOmega));
filter->a[2] = ((filter->A + 1) - ((filter->A - 1) * \
filter->cosOmega) - (2 * sqrtf(filter->A) * filter->alpha));
break;
default:
return ERROR;
break;
}
// Normalize filter coefficients
double factor = 1.0 / filter->a[0];
double norm_a[2];
double norm_b[3];
VectorScalarMultiplyD(norm_a, &filter->a[1], factor, 2);
VectorScalarMultiplyD(norm_b, filter->b, factor, 3);
BiquadFilterUpdateKernelD(filter->biquad, norm_b, norm_a);
return NOERR;
}
