laplace_zp_filter implemented

.gitignore

@@ -2,3 +2,4 @@ backup/
 *.o
 .depend
 filter_test
+polynom_test

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");
+}

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

filter.c

@@ -1,7 +1,6 @@
 #include <string.h>
 #include <stdlib.h>
 
-#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);
 }

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

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

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

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