// LOWPASS
pzkContainer * t2lp(pzkContainer * pzk, real w0) {
pzkContainer * f = createPzkContainer(pzk->nextPole, pzk->nextZero);
uint i;
complex tmp;
f->no_wz = pzk->no_wz;
f->amp *= pzk->amp * pow(w0,(real)(-f->no_wz));
for( i = 0; i < pzk->nextPole; i++ ) {
tmp = cmul2(w0, pzk->poles[i]);
addPole(f, tmp);
f->amp *= w0;
if( !cisreal(tmp) ) {
f->amp *= w0;
}
}
for( i = 0; i < pzk->nextZero; i++ ) {
tmp = cmul2(w0, pzk->zeros[i]);
addZero(f, tmp);
if( cisreal(tmp) ) {
f->amp /= w0;
} else {
f->amp /= w0*w0;
}
}
f->type = lowpass;
return f;
}
/*
* hash_complex - hash a COMPLEX
*
* given:
* type - hash type (see hash.h)
* c - the COMPLEX
* state - the state to hash or NULL
*
* returns:
* the new state
*/
HASH *
hash_complex(int type, void *c, HASH *state)
{
COMPLEX *complex = (COMPLEX *)c; /* c as a COMPLEX pointer */
/*
* initialize if state is NULL
*/
if (state == NULL) {
state = hash_init(type, NULL);
}
/*
* setup for the COMPLEX hash
*/
(state->chkpt)(state);
state->bytes = FALSE;
/*
* catch the zero special case
*/
if (ciszero(complex)) {
/* note a zero numeric value and return */
(state->note)(HASH_ZERO(state->base), state);
return state;
}
/*
* process the real value if not pure imaginary
*
* We will ignore the real part if the value is of the form 0+xi.
*/
if (!qiszero(complex->real)) {
state = hash_number(type, complex->real, state);
}
/*
* if the NUMBER is not real, process the imaginary value
*
* We will ignore the imaginary part of the value is of the form x+0i.
*/
if (!cisreal(complex)) {
/* note the sqrt(-1) */
(state->note)(HASH_COMPLEX(state->base), state);
/* hash the imaginary value */
state = hash_number(type, complex->imag, state);
}
/*
* all done
*/
return state;
}
// HIGHPASS
pzkContainer * t2hp(pzkContainer * pzk, real w0) {
pzkContainer * f = createPzkContainer(pzk->nextPole, pzk->nextZero);
uint i;
complex tmp;
f->amp = pzk->amp * pow(w0,(real)pzk->no_wz);
f->no_wz = -pzk->no_wz;
for( i = 0; i < pzk->nextPole; i++ ) {
tmp.re = w0;
tmp.im = 0;
f->no_wz++;
if( cisreal(pzk->poles[i]) ) {
f->amp /= -pzk->poles[i].re;
tmp.re /= pzk->poles[i].re;
} else {
tmp = cdiv(tmp, pzk->poles[i]);
f->amp /= cabs2(pzk->poles[i]);
f->no_wz++;
}
addPole(f, tmp);
}
for( i = 0; i < pzk->nextZero; i++ ) {
tmp.re = w0;
tmp.im = 0;
f->no_wz--;
if( cisreal(pzk->zeros[i]) ) {
f->amp *= -pzk->zeros[i].re;
tmp.re /= pzk->zeros[i].re;
} else {
tmp = cdiv(tmp, pzk->zeros[i]);
f->amp *= cabs2(pzk->zeros[i]);
f->no_wz--;
}
addZero(f, tmp);
}
f->type = highpass;
return f;
}
/*
* Return the imaginary part of a complex number as a real.
*/
COMPLEX *
c_imag(COMPLEX *c)
{
COMPLEX *r;
if (cisreal(c))
return clink(&_czero_);
r = comalloc();
qfree(r->real);
r->real = qlink(c->imag);
return r;
}
/*
* Multiply two complex numbers.
* This saves one multiplication over the obvious algorithm by
* trading it for several extra additions, as follows. Let
* q1 = (a + b) * (c + d)
* q2 = a * c
* q3 = b * d
* Then (a+bi) * (c+di) = (q2 - q3) + (q1 - q2 - q3)i.
*/
COMPLEX *
cmul(COMPLEX *c1, COMPLEX *c2)
{
COMPLEX *r;
NUMBER *q1, *q2, *q3, *q4;
if (ciszero(c1) || ciszero(c2))
return clink(&_czero_);
if (cisone(c1))
return clink(c2);
if (cisone(c2))
return clink(c1);
if (cisreal(c2))
return cmulq(c1, c2->real);
if (cisreal(c1))
return cmulq(c2, c1->real);
/*
* Need to do the full calculation.
*/
r = comalloc();
q2 = qqadd(c1->real, c1->imag);
q3 = qqadd(c2->real, c2->imag);
q1 = qmul(q2, q3);
qfree(q2);
qfree(q3);
q2 = qmul(c1->real, c2->real);
q3 = qmul(c1->imag, c2->imag);
q4 = qqadd(q2, q3);
qfree(r->real);
r->real = qsub(q2, q3);
qfree(r->imag);
r->imag = qsub(q1, q4);
qfree(q1);
qfree(q2);
qfree(q3);
qfree(q4);
return r;
}
/*
* Return the real part of a complex number.
*/
COMPLEX *
c_real(COMPLEX *c)
{
COMPLEX *r;
if (cisreal(c))
return clink(c);
r = comalloc();
if (!qiszero(c->real)) {
qfree(r->real);
r->real = qlink(c->real);
}
return r;
}
/*
* Take the conjugate of a complex number.
* This negates the complex part.
*/
COMPLEX *
cconj(COMPLEX *c)
{
COMPLEX *r;
if (cisreal(c))
return clink(c);
r = comalloc();
if (!qiszero(c->real)) {
qfree(r->real);
r->real = qlink(c->real);
}
qfree(r->imag);
r->imag = qneg(c->imag);
return r;
}
/*
* Invert a complex number.
*/
COMPLEX *
cinv(COMPLEX *c)
{
COMPLEX *r;
NUMBER *q1, *q2, *den;
if (ciszero(c)) {
math_error("Inverting zero");
/*NOTREACHED*/
}
r = comalloc();
if (cisreal(c)) {
qfree(r->real);
r->real = qinv(c->real);
return r;
}
if (cisimag(c)) {
q1 = qinv(c->imag);
qfree(r->imag);
r->imag = qneg(q1);
qfree(q1);
return r;
}
q1 = qsquare(c->real);
q2 = qsquare(c->imag);
den = qqadd(q1, q2);
qfree(q1);
qfree(q2);
qfree(r->real);
r->real = qqdiv(c->real, den);
q1 = qqdiv(c->imag, den);
qfree(r->imag);
r->imag = qneg(q1);
qfree(q1);
qfree(den);
return r;
}
/*
* Square a complex number.
*/
COMPLEX *
csquare(COMPLEX *c)
{
COMPLEX *r;
NUMBER *q1, *q2;
if (ciszero(c))
return clink(&_czero_);
if (cisrunit(c))
return clink(&_cone_);
if (cisiunit(c))
return clink(&_cnegone_);
r = comalloc();
if (cisreal(c)) {
qfree(r->real);
r->real = qsquare(c->real);
return r;
}
if (cisimag(c)) {
qfree(r->real);
q1 = qsquare(c->imag);
r->real = qneg(q1);
qfree(q1);
return r;
}
q1 = qsquare(c->real);
q2 = qsquare(c->imag);
qfree(r->real);
r->real = qsub(q1, q2);
qfree(q1);
qfree(q2);
qfree(r->imag);
q1 = qmul(c->real, c->imag);
r->imag = qscale(q1, 1L);
qfree(q1);
return r;
}
/*
* Divide two complex numbers.
*/
COMPLEX *
cdiv(COMPLEX *c1, COMPLEX *c2)
{
COMPLEX *r;
NUMBER *q1, *q2, *q3, *den;
if (ciszero(c2)) {
math_error("Division by zero");
/*NOTREACHED*/
}
if ((c1->real == c2->real) && (c1->imag == c2->imag))
return clink(&_cone_);
r = comalloc();
if (cisreal(c1) && cisreal(c2)) {
qfree(r->real);
r->real = qqdiv(c1->real, c2->real);
return r;
}
if (cisimag(c1) && cisimag(c2)) {
qfree(r->real);
r->real = qqdiv(c1->imag, c2->imag);
return r;
}
if (cisimag(c1) && cisreal(c2)) {
qfree(r->imag);
r->imag = qqdiv(c1->imag, c2->real);
return r;
}
if (cisreal(c1) && cisimag(c2)) {
qfree(r->imag);
q1 = qqdiv(c1->real, c2->imag);
r->imag = qneg(q1);
qfree(q1);
return r;
}
if (cisreal(c2)) {
qfree(r->real);
qfree(r->imag);
r->real = qqdiv(c1->real, c2->real);
r->imag = qqdiv(c1->imag, c2->real);
return r;
}
q1 = qsquare(c2->real);
q2 = qsquare(c2->imag);
den = qqadd(q1, q2);
qfree(q1);
qfree(q2);
q1 = qmul(c1->real, c2->real);
q2 = qmul(c1->imag, c2->imag);
q3 = qqadd(q1, q2);
qfree(q1);
qfree(q2);
qfree(r->real);
r->real = qqdiv(q3, den);
qfree(q3);
q1 = qmul(c1->real, c2->imag);
q2 = qmul(c1->imag, c2->real);
q3 = qsub(q2, q1);
qfree(q1);
qfree(q2);
qfree(r->imag);
r->imag = qqdiv(q3, den);
qfree(q3);
qfree(den);
return r;
}
// BANDSTOP
pzkContainer * t2bs(pzkContainer * pzk, real w0, real dw) {
uint nop, noz, i;
pzkContainer * f;
const real w02 = w0*w0;
complex tmp, beta, gamma, cw0, cdwp2;
cw0.re = w0;
cw0.im = 0;
cdwp2.re = dw / 2;
cdwp2.im = 0;
noz = 2*pzk->nextZero;
nop = 2*pzk->nextPole;
if( pzk->no_wz > 0 ) {
noz += pzk->no_wz;
}
if( pzk->no_wz < 0 ) {
nop -= pzk->no_wz;
}
f = createPzkContainer(nop, noz);
f->wz = w0;
f->no_wz = -pzk->no_wz;
f->amp = pzk->amp * pow(dw, (real)(pzk->no_wz));
gamma.re = 0;
gamma.im = 0;
if( pzk->no_wz > 0 ) {
for( i = 0; i < pzk->no_wz; i++ ) {
addZero(f, gamma);
}
}
if( pzk->no_wz < 0 ) {
for( i = 0; i < -pzk->no_wz; i++ ) {
addPole(f, gamma);
}
}
for( i = 0; i < pzk->nextPole; i++ ) {
if( cisreal(pzk->poles[i]) ) {
beta.re = cdwp2.re / pzk->poles[i].re;
tmp.re = 1 - ( w02/(beta.re*beta.re) );
if( tmp.re >= 0 ) {
tmp.re = sqrt( tmp.re );
gamma.im = 0;
gamma.re = beta.re * (1 + tmp.re);
addPole(f, gamma);
gamma.re = beta.re * (1 - tmp.re);
addPole(f, gamma);
} else {
tmp.im = sqrt( -tmp.re );
tmp.re = 1;
gamma = cmul2( beta.re, tmp );
addPole(f, gamma);
}
f->amp /= -pzk->poles[i].re;
f->no_wz++;
}
else {
beta = cdiv(cdwp2, pzk->poles[i]);
tmp = cdiv(cw0, beta);
tmp = csqrt( csub2(1, cmlt(tmp, tmp)) );
gamma = cmlt(beta, cadd2(1, tmp));
addPole(f, gamma);
gamma = cmlt(beta, csub2(1, tmp));
addPole(f, gamma);
f->amp /= cabs2(pzk->poles[i]);
f->no_wz += 2;
}
}
for( i = 0; i < pzk->nextZero; i++ ) {
if( cisreal(pzk->zeros[i]) ) {
beta.re = cdwp2.re / pzk->zeros[i].re;
tmp.re = 1 - ( w02/(beta.re*beta.re) );
if( tmp.re >= 0 ) {
tmp.re = sqrt( tmp.re );
gamma.im = 0;
gamma.re = beta.re * (1 + tmp.re);
addZero(f, gamma);
gamma.re = beta.re * (1 - tmp.re);
addZero(f, gamma);
} else {
tmp.im = sqrt( -tmp.re );
tmp.re = 1;
gamma = cmul2( beta.re, tmp );
addZero(f, gamma);
}
f->amp *= -pzk->zeros[i].re;
f->no_wz--;
}
else {
beta = cdiv(cdwp2, pzk->zeros[i]);
tmp = cdiv(cw0, beta);
tmp = csqrt( csub2(1, cmlt(tmp, tmp)) );
gamma = cmlt(beta, cadd2(1, tmp));
addZero(f, gamma);
gamma = cmlt(beta, csub2(1, tmp));
addZero(f, gamma);
f->amp *= cabs2(pzk->zeros[i]);
f->no_wz -= 2;
}
}
return f;
}
