diff --git a/.gitignore b/.gitignore index 360adbc..80a5325 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ backup/ *.o .depend filter_test +polynom_test diff --git a/dumper.c b/dumper.c index db1a167..fcd0a4f 100644 --- a/dumper.c +++ b/dumper.c @@ -5,18 +5,19 @@ void print_polynom(const double *p, const int np, const char v) { int i, j = 0; + for (i = 0; i < np; ++i) { double pi = p[i]; if (pi == 0) continue; - if (j == 0) { - if (pi < 0) - printf("- "); - } else { + if (j > 0) { if (pi < 0) printf(" - "); else printf(" + "); + } else { + if (pi < 0) + printf("- "); } if (pi < 0) pi = -pi; @@ -27,10 +28,33 @@ void print_polynom(const double *p, const int np, const char v) printf("^%d", i); ++j; } + printf("\n"); } -void print_polynom_nd(const double *num, const int nnum, const double *den, const int nden, const char v, const char *title) +void print_polynom_c(const complex *p, const int np, const char v) +{ + int i, j = 0; + + for (i = 0; i < np; ++i) { + complex pi = p[i]; + if ((pi.i == 0) && (pi.r == 0)) + continue; + if (j > 0) + printf(" + "); + printf("[%f,%f]", pi.r, pi.i); + if (i > 0) + printf("%c", v); + if (i > 1) + printf("^%d", i); + ++j; + } + + printf("\n"); +} + +void print_polynom_nd(const double *num, const int nnum, const double *den, const int nden, + const char v, const char *title) { if (title) printf("\n%s:\n", title); @@ -39,3 +63,70 @@ void print_polynom_nd(const double *num, const int nnum, const double *den, cons print_polynom(den, nden, v); printf("\n"); } + +void print_polynom_nd_c(const complex *num, const int nnum, const complex *den, const int nden, + const char v, const char *title) +{ + if (title) + printf("\n%s:\n", title); + print_polynom_c(num, nnum, v); + printf("----------\n"); + print_polynom_c(den, nden, v); + printf("\n"); +} + +void print_zerolist(const double *z, const int nz, const char v) +{ + int i; + + for (i = 0; i < nz; ++i) { + if (i > 0) + printf(" * "); + printf("(%f + %c", z[i], v); + if (i > 1) + printf("^%d", i); + printf(")"); + } + + printf("\n"); +} + +void print_zerolist_c(const complex *z, const int nz, const char v) +{ + int i; + + for (i = 0; i < nz; ++i) { + if (i > 0) + printf(" * "); + printf("([%f,%f] + %c", z[i].r, z[i].i, v); + if (i > 1) + printf("^%d", i); + printf(")"); + } + + printf("\n"); +} + +void print_polynom_zp(const double *zero, const int nzero, const double *pole, const int npole, + const double k, const char v, const char *title) +{ + if (title) + printf("\n%s:\n", title); + printf("%f *", k); + print_zerolist(zero, nzero, v); + printf("----------\n"); + print_zerolist(pole, npole, v); + printf("\n"); +} + +void print_polynom_zp_c(const complex *zero, const int nzero, const complex *pole, const int npole, + const complex k, const char v, const char *title) +{ + if (title) + printf("\n%s:\n", title); + printf("[%f,%f] *", k.r, k.i); + print_zerolist_c(zero, nzero, v); + printf("----------\n"); + print_zerolist_c(pole, npole, v); + printf("\n"); +} diff --git a/dumper.h b/dumper.h index c2f8d9d..088d2b9 100644 --- a/dumper.h +++ b/dumper.h @@ -1,2 +1,19 @@ +#ifndef DUMPER_H +#define DUMPER_H + +#include "polynom.h" + void print_polynom(const double *p, const int np, const char v); -void print_polynom_nd(const double *num, const int nnum, const double *den, const int nden, const char v, const char *title); +void print_polynom_c(const complex *p, const int np, const char v); +void print_polynom_nd(const double *num, const int nnum, const double *den, const int nden, + const char v, const char *title); +void print_polynom_nd_c(const complex *num, const int nnum, const complex *den, const int nden, + const char v, const char *title); +void print_zerolist(const double *z, const int nz, const char v); +void print_zerolist_c(const complex *z, const int nz, const char v); +void print_polynom_zp(const double *zero, const int nzero, const double *pole, const int npole, + const double k, const char v, const char *title); +void print_polynom_zp_c(const complex *zero, const int nzero, const complex *pole, const int npole, + const complex k, const char v, const char *title); + +#endif // DUMPER_H diff --git a/filter.c b/filter.c index 853be78..86e022e 100644 --- a/filter.c +++ b/filter.c @@ -1,7 +1,6 @@ #include #include -#include "polynom.h" #include "filter.h" @@ -48,7 +47,8 @@ void bilinear_transform_n(const double *s, const int ns, const double t, double } -int bilinear_transform_nd(const double *snum, const int nsnum, const double *sden, const int nsden, const double t, double *znum, double *zden) +int bilinear_transform_nd(const double *snum, const int nsnum, const double *sden, const int nsden, + const double t, double *znum, double *zden) { double znumnum[nsnum], znumden[nsnum], zdennum[nsden], zdenden[nsden]; int nz = (nsnum > nsden ? nsnum : nsden); @@ -61,10 +61,8 @@ int bilinear_transform_nd(const double *snum, const int nsnum, const double *sde bilinear_transform_n(sden, nsden, t, zdennum, zdenden); /* - printf("zn:\n"); - print_polynom_nd(znumnum, nsnum, znumden, nsnum, 'z'); - printf("zd:\n"); - print_polynom_nd(zdennum, nsden, zdenden, nsden, 'z'); + print_polynom_nd(znumnum, nsnum, znumden, nsnum, 'z', "zn"); + print_polynom_nd(zdennum, nsden, zdenden, nsden, 'z', "zd"); */ clear_polynom(znumden, nsnum); @@ -162,7 +160,9 @@ double iir_filter_next(IIR_FILTER *iir, const double x) } -IIR_FILTER *laplace_nd_filter(const double *num, const int nnum, const double *den, const int nden, const double dt, const int L) +IIR_FILTER *laplace_nd_filter(const double *num, const int nnum, + const double *den, const int nden, + const double dt, const int L) { double interp_znum[] = { 0.0221824, -0.0134342, -0.0134342, 0.0221824 }; int ninterp_znum = sizeof(interp_znum) / sizeof(interp_znum[0]); @@ -198,7 +198,25 @@ IIR_FILTER *laplace_nd_filter(const double *num, const int nnum, const double *d } -IIR_FILTER *laplace_zp_filter(const double *num[2], const int nnum, const double *den[2], const int nden, const double dt, const int L) +IIR_FILTER *laplace_zp_filter(const complex *zlc, const int nzlc, + const complex *plc, const int nplc, + const complex zkc, const double dt, const int L) { - return NULL; + int nnum = nzlc + 1; + complex num_c[nnum]; + double num[nnum]; + + complex pkc = { 1, 0 }; + int nden = nplc + 1; + complex den_c[nden]; + double den[nden]; + + expand_zerolist_c(zlc, nzlc, zkc, num_c); + copy_polynom_cr(num_c, nnum, num); + expand_zerolist_c(plc, nplc, pkc, den_c); + copy_polynom_cr(den_c, nden, den); + + //print_polynom_nd(num, nnum, den, nden, 's', "zp nd"); + + return laplace_nd_filter(num, nnum, den, nden, dt, L); } diff --git a/filter.h b/filter.h index 7e2abf6..92f2f07 100644 --- a/filter.h +++ b/filter.h @@ -1,3 +1,8 @@ +#ifndef FILTER_H +#define FILTER_H + +#include "polynom.h" + typedef struct { double *b; int m; @@ -10,11 +15,18 @@ typedef struct { } IIR_FILTER; void bilinear_transform_n(const double *s, const int ns, const double t, double *znum, double *zden); -int bilinear_transform_nd(const double *snum, const int nsnum, const double *sden, const int nsden, const double t, double *znum, double *zden); +int bilinear_transform_nd(const double *snum, const int nsnum, const double *sden, const int nsden, + const double t, double *znum, double *zden); IIR_FILTER *iir_filter_init(const double *b, const int m, const double *a, const int n); void iir_filter_free(IIR_FILTER *iir); double iir_filter_next(IIR_FILTER *iir, const double x); -IIR_FILTER *laplace_nd_filter(const double *num, const int nnum, const double *den, const int nden, const double dt, const int L); -IIR_FILTER *laplace_zp_filter(const double *num[2], const int nnum, const double *den[2], const int nden, const double dt, const int L); +IIR_FILTER *laplace_nd_filter(const double *num, const int nnum, + const double *den, const int nden, + const double dt, const int L); +IIR_FILTER *laplace_zp_filter(const complex *zlc, const int nzlc, + const complex *plc, const int nplc, + const complex zkc, const double dt, const int L); + +#endif // FILTER_H diff --git a/filter_test.c b/filter_test.c index fdf9a9a..13bfda9 100644 --- a/filter_test.c +++ b/filter_test.c @@ -12,21 +12,29 @@ double f(const double t) int main() { double snum[] = { 1 }; - double sden[] = { 1 }; + double sden[] = { 1, 1 }; + int nsnum = sizeof(snum) / sizeof(snum[0]); + int nsden = sizeof(sden) / sizeof(sden[0]); + + complex szero[] = { { 1, 2 }, { 3, 4 } }; + complex spole[] = { { 5, 6 }, { 7, 8 } }; + complex sk = { 9, 0 }; + int nszero = sizeof(szero) / sizeof(szero[0]); + int nspole = sizeof(spole) / sizeof(spole[0]); double dT = 0.1; int L = 10; double tmax = 0.2; - int nsnum = sizeof(snum) / sizeof(snum[0]); - int nsden = sizeof(sden) / sizeof(sden[0]); double dt = dT / L; - IIR_FILTER *iir = laplace_nd_filter(snum, nsnum, sden, nsden, dt, L); + IIR_FILTER *iir_nd = laplace_nd_filter(snum, nsnum, sden, nsden, dt, L); + IIR_FILTER *iir_zp = laplace_zp_filter(szero, nszero, spole, nspole, sk, dt, L); - print_polynom_nd(snum, nsnum, sden, nsden, 's', "Laplace H(s)"); printf("dT: %f, L: %d, dt: %f\n", dT, L, dt); - print_polynom_nd(iir->b, iir->m + 1, iir->a, iir->n + 1, 'z', "IIR H(z)"); + + print_polynom_nd(snum, nsnum, sden, nsden, 's', "Laplace ND H(s)"); + print_polynom_nd(iir_nd->b, iir_nd->m + 1, iir_nd->a, iir_nd->n + 1, 'z', "IIR H(z)"); printf("t\tx\ty\n"); double t, x = 0, y; @@ -36,11 +44,27 @@ int main() x = f(t); l = L; } - y = iir_filter_next(iir, x); + y = iir_filter_next(iir_nd, x); + printf("%f\t%f\t%f\n", t, x, y); + } + + print_polynom_zp_c(szero, nszero, spole, nspole, sk, 's', "Laplace ZP H(s)"); + print_polynom_nd(iir_zp->b, iir_zp->m + 1, iir_zp->a, iir_zp->n + 1, 'z', "IIR H(z)"); + + printf("t\tx\ty\n"); + x = 0; + l = 0; + for (t = 0; t < tmax; t += dt) { + if (--l <= 0) { + x = f(t); + l = L; + } + y = iir_filter_next(iir_zp, x); printf("%f\t%f\t%f\n", t, x, y); } - iir_filter_free(iir); + iir_filter_free(iir_nd); + iir_filter_free(iir_zp); return 0; } diff --git a/junk/readme.txt b/junk/readme.txt new file mode 100644 index 0000000..f5d8a14 --- /dev/null +++ b/junk/readme.txt @@ -0,0 +1,3 @@ +https://reference.wolfram.com/language/guide/PolynomialAlgebra.html +https://reference.wolfram.com/language/guide/PolynomialFactoring.html +http://www.mathsisfun.com/algebra/partial-fractions.html diff --git a/makefile.polynom b/makefile.polynom new file mode 100644 index 0000000..a7286f5 --- /dev/null +++ b/makefile.polynom @@ -0,0 +1,28 @@ +SRC=polynom_test.c polynom.c dumper.c +OBJ=$(SRC:.c=.o) +EXE=polynom_test + +CC=gcc +CFLAGS=-Wall -O3 -std=c99 +#LDFLAGS=-static-libgcc -static-libstdc++ -static +LDFLAGS=-lm +RM=rm + +all: $(EXE) + +%.o: %.c + $(CC) $(CFLAGS) -o $@ -c $< + +$(EXE): $(OBJ) + $(CC) $(OBJ) $(LDFLAGS) -o $@ + +clean: + $(RM) $(OBJ) $(EXE) + +depend: $(SRCS) + rm -f ./.depend + $(CC) $(CFLAGS) -MM $(SRC) >> ./.depend + +-include .depend + +.PHONY: all clean diff --git a/polynom.c b/polynom.c index 864ceee..5588703 100644 --- a/polynom.c +++ b/polynom.c @@ -1,16 +1,42 @@ #include #include +#include "polynom.h" + void clear_polynom(double *a, const int na) { memset(a, 0, na * sizeof(a[0])); } +void clear_polynom_c(complex *a, const int na) +{ + memset(a, 0, na * sizeof(a[0])); +} + void copy_polynom(const double *a, const int nb, double *b) { memcpy(b, a, nb * sizeof(b[0])); } +void copy_polynom_c(const complex *a, const int nb, complex *b) +{ + memcpy(b, a, nb * sizeof(b[0])); +} + +void copy_polynom_cr(const complex *a, const int nb, double *b) +{ + int i; + for (i = 0; i < nb; ++i) + b[i] = a[i].r; +} + +void copy_polynom_ci(const complex *a, const int nb, double *b) +{ + int i; + for (i = 0; i < nb; ++i) + b[i] = a[i].i; +} + void expand_polynom(const double *a, const int na, const double *b, const int nb, double *c) { int i, j, nc = na + nb - 1; @@ -22,7 +48,50 @@ void expand_polynom(const double *a, const int na, const double *b, const int nb c[i + j] += a[i] * b[j]; } -void pow_polynom(const double *a, const int na, int m, double *c) +void expand_polynom_c(const complex *a, const int na, const complex *b, const int nb, complex *c) +{ + int i, j, nc = na + nb - 1; + + if (nc > 0) + clear_polynom_c(c, nc); + for (i = 0; i < na; ++i) + for (j = 0; j < nb; ++j) { + c[i + j].r += a[i].r * b[j].r - a[i].i * b[j].i; + c[i + j].i += a[i].r * b[j].i + a[i].i * b[j].r; + } +} + +void expand_zerolist(const double *zl, const int nzl, const double zk, double *r) +{ + double ri[nzl + 1]; + double zi[2] = { 0, 1 }; + int i; + + clear_polynom(r, nzl + 1); + r[0] = zk; + for (i = 0; i < nzl; ++i) { + zi[0] = zl[i]; + copy_polynom(r, i + 1, ri); + expand_polynom(ri, i + 1, zi, 2, r); + } +} + +void expand_zerolist_c(const complex *zl, const int nzl, const complex zk, complex *r) +{ + complex ri[nzl + 1]; + complex zi[2] = { { 0, 0 }, { 1, 0 } }; + int i; + + clear_polynom_c(r, nzl + 1); + r[0] = zk; + for (i = 0; i < nzl; ++i) { + zi[0] = zl[i]; + copy_polynom_c(r, i + 1, ri); + expand_polynom_c(ri, i + 1, zi, 2, r); + } +} + +void pow_polynom(const double *a, const int na, const int m, double *c) { double b[na * m]; int i, nb = na; @@ -43,18 +112,74 @@ void pow_polynom(const double *a, const int na, int m, double *c) } } +void pow_polynom_c(const complex *a, const int na, const int m, complex *c) +{ + complex b[na * m]; + int i, nb = na; -void add_polynom(double *a, const int na, const double k, double *b) + if (m < 0) + return; + + if (m == 0) { + c[0].r = 1; + c[0].i = 0; + return; + } + + copy_polynom_c(a, nb, c); + for (i = 1; i < m; ++i) { + copy_polynom_c(c, nb, b); + expand_polynom_c(a, na, b, nb, c); + nb += na; + } +} + +void add_polynom(double *a, const int na, const double k, const double *b) { int i; for (i = 0; i < na; ++i) a[i] += b[i] * k; } +void add_polynom_c(complex *a, const int na, const double k, const complex *b) +{ + int i; + for (i = 0; i < na; ++i) { + a[i].r += b[i].r * k; + a[i].i += b[i].i * k; + } +} + +void add_polynom_cc(complex *a, const int na, const complex k, const complex *b) +{ + int i; + for (i = 0; i < na; ++i) { + a[i].r += b[i].r * k.r - b[i].i * k.i; + a[i].i += b[i].r * k.i + b[i].i * k.r; + } +} void mul_polynom(double *a, const int na, const double k) { int i; for (i = 0; i < na; ++i) - a[i] *= k; + a[i] = a[i] * k; +} + +void mul_polynom_c(complex *a, const int na, const double k) +{ + int i; + for (i = 0; i < na; ++i) { + a[i].r = a[i].r * k; + a[i].i = a[i].i * k; + } +} + +void mul_polynom_cc(complex *a, const int na, const complex k) +{ + int i; + for (i = 0; i < na; ++i) { + a[i].r = a[i].r * k.r - a[i].i * k.i; + a[i].i = a[i].r * k.i + a[i].i * k.r; + } } diff --git a/polynom.h b/polynom.h index ab928cd..73f9895 100644 --- a/polynom.h +++ b/polynom.h @@ -1,6 +1,25 @@ +#ifndef POLYNOM_H +#define POLYNOM_H + +typedef struct { double r; double i; } complex; + void clear_polynom(double *a, const int na); +void clear_polynom_c(complex *a, const int na); void copy_polynom(const double *a, const int nb, double *b); +void copy_polynom_c(const complex *a, const int nb, complex *b); +void copy_polynom_cr(const complex *a, const int nb, double *b); +void copy_polynom_ci(const complex *a, const int nb, double *b); void expand_polynom(const double *a, const int na, const double *b, const int nb, double *c); -void pow_polynom(const double *a, const int na, int m, double *c); -void add_polynom(double *a, const int na, const double k, double *b); +void expand_polynom_c(const complex *a, const int na, const complex *b, const int nb, complex *c); +void expand_zerolist(const double *zl, const int nzl, const double zk, double *r); +void expand_zerolist_c(const complex *zl, const int nzl, const complex zk, complex *r); +void pow_polynom(const double *a, const int na, const int m, double *c); +void pow_polynom_c(const complex *a, const int na, const int m, complex *c); +void add_polynom(double *a, const int na, const double k, const double *b); +void add_polynom_c(complex *a, const int na, const double k, const complex *b); +void add_polynom_cc(complex *a, const int na, const complex k, const complex *b); void mul_polynom(double *a, const int na, const double k); +void mul_polynom_c(complex *a, const int na, const double k); +void mul_polynom_cc(complex *a, const int na, const complex k); + +#endif // POLYNOM_H diff --git a/polynom_test.c b/polynom_test.c index ebe871a..a47fc79 100644 --- a/polynom_test.c +++ b/polynom_test.c @@ -12,6 +12,23 @@ int main() int nc = 20; double c[nc]; + double zl[] = { 1, 2, 3, 4 }; + int nzl = sizeof(zl) / sizeof(zl[0]); + double zk = 1; + int nr = nzl + 1; + double r[nr]; + + complex szero[] = { { 1, 2 }, { 3, 4 } }; + complex spole[] = { { 5, 6 }, { 7, 8 } }; + complex szerok = { 9, 0 }; + complex spolek = { 1, 0 }; + int nszero = sizeof(szero) / sizeof(szero[0]); + int nspole = sizeof(spole) / sizeof(spole[0]); + int nrzero = nszero + 1; + complex rzero[nrzero]; + int nrpole = nspole + 1; + complex rpole[nrpole]; + printf("a (%d):\n", na); print_polynom(a, na, 'x'); printf("b (%d):\n", nb); @@ -26,4 +43,14 @@ int main() pow_polynom(a, na, 2, c); printf("c (%d):\n", nc); print_polynom(c, nc, 'x'); + + expand_zerolist(zl, nzl, zk, r); + printf("r (%d):\n", nr); + print_polynom(r, nr, 'x'); + + expand_zerolist_c(szero, nszero, szerok, rzero); + expand_zerolist_c(spole, nspole, spolek, rpole); + print_polynom_nd_c(rzero, nrzero, rpole, nrpole, 's', "zp nd"); + + return 0; }