void Cascade::setLayout (const LayoutBase& proto)
{
const int numPoles = proto.getNumPoles();
m_numStages = (numPoles + 1)/ 2;
assert (m_numStages <= m_maxStages);
Biquad* stage = m_stageArray;
for (int i = 0; i < m_numStages; ++i, ++stage)
stage->setPoleZeroPair (proto[i]);
applyScale (proto.getNormalGain() /
std::abs (response (proto.getNormalW() / (2 * doublePi))));
}
void Cascade::setLayout (const LayoutBase& proto)
{
const int numPoles = proto.getNumPoles();
m_numStages = (numPoles + 1)/ 2;
if (m_numStages > m_maxStages)
throw std::invalid_argument("Number of stages is larger than the max stages.");
Biquad* stage = m_stageArray;
for (int i = 0; i < m_numStages; ++i, ++stage)
stage->setPoleZeroPair (proto[i]);
applyScale (proto.getNormalGain() /
std::abs (response (proto.getNormalW() / (2 * doublePi))));
}
HighPassTransform::HighPassTransform (double fc,
LayoutBase& digital,
LayoutBase const& analog)
{
digital.reset ();
// prewarp
f = 1. / tan (doublePi * fc);
const int numPoles = analog.getNumPoles ();
const int pairs = numPoles / 2;
for (int i = 0; i < pairs; ++i)
{
const PoleZeroPair& pair = analog[i];
digital.addPoleZeroConjugatePairs (transform (pair.poles.first),
transform (pair.zeros.first));
}
if (numPoles & 1)
{
const PoleZeroPair& pair = analog[pairs];
digital.add (transform (pair.poles.first),
transform (pair.zeros.first));
}
digital.setNormal (doublePi - analog.getNormalW(),
analog.getNormalGain());
}
BandStopTransform::BandStopTransform (double fc,
double fw,
LayoutBase& digital,
LayoutBase const& analog)
{
digital.reset ();
const double ww = 2 * doublePi * fw;
wc2 = 2 * doublePi * fc - (ww / 2);
wc = wc2 + ww;
// this is crap
if (wc2 < 1e-8)
wc2 = 1e-8;
if (wc > doublePi-1e-8)
wc = doublePi-1e-8;
a = cos ((wc + wc2) * .5) /
cos ((wc - wc2) * .5);
b = tan ((wc - wc2) * .5);
a2 = a * a;
b2 = b * b;
const int numPoles = analog.getNumPoles ();
const int pairs = numPoles / 2;
for (int i = 0; i < pairs; ++i)
{
const PoleZeroPair& pair = analog[i];
ComplexPair p = transform (pair.poles.first);
ComplexPair z = transform (pair.zeros.first);
//
// Optimize out the calculations for conjugates for Release builds
//
#ifdef NDEBUG
// trick to get the conjugate
if (z.second == z.first)
z.second = std::conj (z.first);
#else
// Do the full calculation to verify correctness
ComplexPair pc = transform (analog[i].poles.second);
ComplexPair zc = transform (analog[i].zeros.second);
// get the conjugates into pc and zc
if (zc.first == z.first)
std::swap (zc.first, zc.second);
assert (pc.first == std::conj (p.first));
assert (pc.second == std::conj (p.second));
assert (zc.first == std::conj (z.first));
assert (zc.second == std::conj (z.second));
#endif
digital.addPoleZeroConjugatePairs (p.first, z.first);
digital.addPoleZeroConjugatePairs (p.second, z.second);
}
if (numPoles & 1)
{
ComplexPair poles = transform (analog[pairs].poles.first);
ComplexPair zeros = transform (analog[pairs].zeros.first);
digital.add (poles, zeros);
}
if (fc < 0.25)
digital.setNormal (doublePi, analog.getNormalGain());
else
digital.setNormal (0, analog.getNormalGain());
}
BandPassTransform::BandPassTransform (double fc,
double fw,
LayoutBase& digital,
LayoutBase const& analog)
{
// handle degenerate cases efficiently
// THIS DOESNT WORK because the cascade states won't match
#if 0
const double fw_2 = fw / 2;
if (fc - fw_2 < 0)
{
LowPassTransform::transform (fc + fw_2, digital, analog);
}
else if (fc + fw_2 >= 0.5)
{
HighPassTransform::transform (fc - fw_2, digital, analog);
}
else
#endif
digital.reset ();
const double ww = 2 * doublePi * fw;
// pre-calcs
wc2 = 2 * doublePi * fc - (ww / 2);
wc = wc2 + ww;
// what is this crap?
if (wc2 < 1e-8)
wc2 = 1e-8;
if (wc > doublePi-1e-8)
wc = doublePi-1e-8;
a = cos ((wc + wc2) * 0.5) /
cos ((wc - wc2) * 0.5);
b = 1 / tan ((wc - wc2) * 0.5);
a2 = a * a;
b2 = b * b;
ab = a * b;
ab_2 = 2 * ab;
const int numPoles = analog.getNumPoles ();
const int pairs = numPoles / 2;
for (int i = 0; i < pairs; ++i)
{
const PoleZeroPair& pair = analog[i];
ComplexPair p1 = transform (pair.poles.first);
ComplexPair z1 = transform (pair.zeros.first);
//
// Optimize out the calculations for conjugates for Release builds
//
#ifndef NDEBUG
ComplexPair p2 = transform (pair.poles.second);
ComplexPair z2 = transform (pair.zeros.second);
assert (p2.first == std::conj (p1.first));
assert (p2.second == std::conj (p1.second));
#endif
digital.addPoleZeroConjugatePairs (p1.first, z1.first);
digital.addPoleZeroConjugatePairs (p1.second, z1.second);
}
if (numPoles & 1)
{
ComplexPair poles = transform (analog[pairs].poles.first);
ComplexPair zeros = transform (analog[pairs].zeros.first);
digital.add (poles, zeros);
}
double wn = analog.getNormalW();
digital.setNormal (2 * atan (sqrt (tan ((wc + wn)* 0.5) * tan((wc2 + wn)* 0.5))),
analog.getNormalGain());
}
