// 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; }