1 #include "Python.h"
2 #include <math.h>
3 #include <stddef.h>
4 #include <stdlib.h>
5 #include <string.h>
6 #include <stdio.h>
7 #define NO_IMPORT_ARRAY
8 #include "numpy/arrayobject.h"
9
10 void compute_root_from_lambda(double, double *, double *);
11
12 #define CONJ(a) (~(a))
13 #define ABSQ(a) (__real__ (a*CONJ(a)))
14 #ifdef __GNUC__
15
16 void Z_IIR_order1 (__complex__ double,__complex__ double,__complex__ double*,__complex__ double*,int,int,int);
17 void Z_IIR_order2 (__complex__ double,__complex__ double,__complex__ double,__complex__ double*,__complex__ double*,int,int,int);
18 void Z_IIR_order2_cascade (__complex__ double,__complex__ double,__complex__ double,__complex__ double,__complex__ double*,__complex__ double*,int,int,int);
19 int Z_IIR_forback1(__complex__ double,__complex__ double,__complex__ double*,__complex__ double*,int,int,int,double);
20 void Z_FIR_mirror_symmetric(__complex__ double*,__complex__ double*,int,__complex__ double*,int,int,int);
21 int Z_separable_2Dconvolve_mirror(__complex__ double*,__complex__ double*,int,int,__complex__ double*,__complex__ double*,int,int,npy_intp*,npy_intp*);
22
23 /* Implement the following difference equation */
24 /* y[n] = a1 * x[n] + a2 * y[n-1] */
25 /* with a given starting value loaded into the array */
26
27 void
Z_IIR_order1(__complex__ double a1,__complex__ double a2,__complex__ double * x,__complex__ double * y,int N,int stridex,int stridey)28 Z_IIR_order1(__complex__ double a1, __complex__ double a2,
29 __complex__ double *x, __complex__ double *y,
30 int N, int stridex, int stridey)
31 {
32 __complex__ double *yvec = y+stridey;
33 __complex__ double *xvec = x+stridex;
34 int n;
35
36 for (n=1; n < N; n++) {
37 *yvec = *xvec * a1 + *(yvec-stridey) * a2;
38 yvec += stridey;
39 xvec += stridex;
40 }
41 }
42
43
44 /* Implement the following difference equation */
45 /* y[n] = a1 * x[n] + a2 * y[n-1] + a3 * y[n-2] */
46 /* with two starting values loaded into the array */
47
48 void
Z_IIR_order2(__complex__ double a1,__complex__ double a2,__complex__ double a3,__complex__ double * x,__complex__ double * y,int N,int stridex,int stridey)49 Z_IIR_order2(__complex__ double a1, __complex__ double a2, __complex__ double a3,
50 __complex__ double *x, __complex__ double *y,
51 int N, int stridex, int stridey)
52 {
53 __complex__ double *yvec = y+2*stridey;
54 __complex__ double *xvec = x+2*stridex;
55 int n;
56
57 for (n=2; n < N; n++) {
58 *yvec = *xvec * a1 + *(yvec-stridey) * a2 + *(yvec-2*stridey) * a3;
59 yvec += stridey;
60 xvec += stridex;
61 }
62 }
63
64 /* Implement a second order IIR difference equation using a cascade
65 of first order sections. Suppose the transfer function is
66 cs
67 H(z) = -------------------
68 (1-z1/z) ( 1-z2/z)
69
70 then the following pair is implemented with one starting value loaded in
71 the output array and the starting value for the intermediate array
72 passed in as yp0.
73
74 y1[n] = x[n] + z1 y1[n-1]
75 yp[n] = cs y1[n] + z2 yp[n-1]
76
77 */
78
79 void
Z_IIR_order2_cascade(__complex__ double cs,__complex__ double z1,__complex__ double z2,__complex__ double y1_0,__complex__ double * x,__complex__ double * yp,int N,int stridex,int stridey)80 Z_IIR_order2_cascade(__complex__ double cs,
81 __complex__ double z1, __complex__ double z2,
82 __complex__ double y1_0, __complex__ double *x,
83 __complex__ double *yp,
84 int N, int stridex, int stridey)
85 {
86 __complex__ double *yvec = yp+stridey;
87 __complex__ double *xvec = x+stridex;
88 int n;
89
90 for (n=1; n < N; n++) {
91 y1_0 = *xvec + y1_0 * z1;
92 *yvec = cs * y1_0 + *(yvec-stridey) * z2;
93 yvec += stridey;
94 xvec += stridex;
95 }
96 }
97
98
99 /* Implement a smoothing IIR filter with mirror-symmetric boundary conditions
100 using a cascade of first-order sections. The second section uses a
101 reversed sequence. This implements the following transfer function:
102 c0
103 H(z) = ---------------------------
104 (1-z1/z) (1 - z1 z)
105
106 with the following difference equations:
107
108 yp[n] = x[n] + z1 yp[n-1]
109 with starting condition:
110 yp[0] = x[0] + Sum(z1^(k+1) x[k],k=0..Infinity)
111
112 and
113
114 y[n] = z1 y[n+1] + c0 yp[n]
115 with starting condition:
116 y[N-1] = z1 / (z1-1) yp[N-1]
117
118 The resulting signal will have mirror symmetric boundary conditions as well.
119
120 If memory could not be allocated for the temporary vector yp, the
121 function returns -1 otherwise it returns 0.
122
123 z1 should be less than 1;
124
125 */
126
127 int
Z_IIR_forback1(__complex__ double c0,__complex__ double z1,__complex__ double * x,__complex__ double * y,int N,int stridex,int stridey,double precision)128 Z_IIR_forback1(__complex__ double c0, __complex__ double z1,
129 __complex__ double *x, __complex__ double *y,
130 int N, int stridex, int stridey, double precision)
131 {
132 __complex__ double *yp = NULL;
133 __complex__ double *xptr = x;
134 __complex__ double yp0;
135 __complex__ double powz1;
136 __complex__ double diff;
137 double err;
138 int k;
139
140 if (ABSQ(z1) >= 1.0) return -2; /* z1 not less than 1 */
141
142 /* Initialize memory for loop */
143 if ((yp = malloc(N*sizeof(__complex__ double)))==NULL) return -1;
144
145 /* Fix starting value assuming mirror-symmetric boundary conditions. */
146 yp0 = x[0];
147 powz1 = 1.0;
148 k = 0;
149 precision *= precision;
150 do {
151 yp[0] = yp0;
152 powz1 *= z1;
153 yp0 += powz1 * (*xptr);
154 diff = powz1;
155 err = ABSQ(diff);
156 xptr += stridex;
157 k++;
158 } while((err > precision) && (k < N));
159 if (k >= N) return -3; /* sum did not converge */
160 yp[0] = yp0;
161
162 Z_IIR_order1(1.0, z1, x, yp, N, stridex, 1);
163
164 *(y + (N-1)*stridey) = -c0 / (z1 - 1.0) * yp[N-1];
165
166 Z_IIR_order1(c0, z1, yp+N-1, y+(N-1)*stridey, N, -1, -stridey);
167
168 free(yp);
169 return 0;
170 }
171
172
173 /* h must be odd length */
174 /* strides in units of sizeof(__complex__ double) bytes */
175
176 void
Z_FIR_mirror_symmetric(__complex__ double * in,__complex__ double * out,int N,__complex__ double * h,int Nh,int instride,int outstride)177 Z_FIR_mirror_symmetric(__complex__ double *in, __complex__ double *out,
178 int N, __complex__ double *h, int Nh,
179 int instride, int outstride)
180 {
181 int n, k;
182 int Nhdiv2 = Nh >> 1;
183 __complex__ double *outptr;
184 __complex__ double *inptr;
185 __complex__ double *hptr;
186
187 /* first part boundary conditions */
188 outptr = out;
189 for (n=0; n < Nhdiv2; n++) {
190 *outptr = 0.0;
191 hptr = h;
192 inptr = in + (n+Nhdiv2)*instride;
193 for (k=-Nhdiv2; k <= n; k++) {
194 *outptr += *hptr++ * *inptr;
195 inptr -= instride;
196 }
197 inptr += instride;
198 for (k=n+1; k <= Nhdiv2; k++) {
199 *outptr += *hptr++ * *inptr;
200 inptr += instride;
201 }
202 outptr += outstride;
203 }
204
205 /* middle section */
206 outptr = out + Nhdiv2*outstride;
207 for (n=Nhdiv2; n < N-Nhdiv2; n++) {
208 *outptr = 0.0;
209 hptr = h;
210 inptr = in + (n+Nhdiv2)*instride;
211 for (k=-Nhdiv2; k <= Nhdiv2; k++) {
212 *outptr += *hptr++ * *inptr;
213 inptr -= instride;
214 }
215 outptr += outstride;
216 }
217
218 /* end boundary conditions */
219 outptr = out + (N-Nhdiv2)*outstride;
220 for (n=N-Nhdiv2; n < N; n++) {
221 *outptr = 0.0;
222 hptr = h;
223 inptr = in + (2*N-1-n-Nhdiv2)*instride;
224 for (k=-Nhdiv2; k <= n-N; k++) {
225 *outptr += *hptr++ * *inptr;
226 inptr += instride;
227 }
228 inptr -= instride;
229 for (k=n+1-N; k <= Nhdiv2; k++) {
230 *outptr += *hptr++ * *inptr;
231 inptr -= instride;
232 }
233 outptr += outstride;
234 }
235
236 }
237
238 int
Z_separable_2Dconvolve_mirror(__complex__ double * in,__complex__ double * out,int M,int N,__complex__ double * hr,__complex__ double * hc,int Nhr,int Nhc,npy_intp * instrides,npy_intp * outstrides)239 Z_separable_2Dconvolve_mirror(__complex__ double *in, __complex__ double *out,
240 int M, int N,
241 __complex__ double *hr, __complex__ double *hc,
242 int Nhr, int Nhc,
243 npy_intp *instrides, npy_intp *outstrides)
244 {
245 int m, n;
246 __complex__ double *tmpmem;
247 __complex__ double *inptr=NULL, *outptr=NULL;
248
249 tmpmem = malloc(M*N*sizeof(__complex__ double));
250 if (tmpmem == NULL) return -1;
251
252 if (Nhr > 0) {
253 /* filter across rows */
254 inptr = in;
255 outptr = tmpmem;
256 for (m = 0; m < M; m++) {
257 Z_FIR_mirror_symmetric (inptr, outptr, N, hr, Nhr, instrides[1], 1);
258 inptr += instrides[0];
259 outptr += N;
260 }
261 }
262 else
263 memmove(tmpmem, inptr, M*N*sizeof(__complex__ double));
264
265 if (Nhc > 0) {
266 /* filter down columns */
267 inptr = tmpmem;
268 outptr = out;
269 for (n = 0; n < N; n++) {
270 Z_FIR_mirror_symmetric (inptr, outptr, M, hc, Nhc, N, outstrides[0]);
271 outptr += outstrides[1];
272 inptr += 1;
273 }
274 }
275 else
276 memmove(outptr, tmpmem, M*N*sizeof(__complex__ double));
277
278 free(tmpmem);
279 return 0;
280 }
281 #endif
282