libdspl-2.0
Библиотека алгоритмов цифровой обработки сигналов
conv.c
1 /*
2 * Copyright (c) 2015-2019 Sergey Bakhurin
3 * Digital Signal Processing Library [http://dsplib.org]
4 *
5 * This file is part of libdspl-2.0.
6 *
7 * is free software: you can redistribute it and/or modify
8 * it under the terms of the GNU Lesser General Public License as published by
9 * the Free Software Foundation, either version 3 of the License, or
10 * (at your option) any later version.
11 *
12 * DSPL is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public License
18 * along with Foobar. If not, see <http://www.gnu.org/licenses/>.
19 */
20 
21 #include <stdlib.h>
22 #include <string.h>
23 #include "dspl.h"
24 
25 
26 
27 /*******************************************************************************
28 Real vectors linear convolution.
29 --------------------------------------------------------------------------------
30 Documented: RU, EN
31 *******************************************************************************/
32 int DSPL_API conv(double* a, int na, double* b, int nb, double* c)
33 {
34  int k;
35  int n;
36 
37  double *t;
38  size_t bufsize;
39 
40  if(!a || !b || !c)
41  return ERROR_PTR;
42  if(na < 1 || nb < 1)
43  return ERROR_SIZE;
44 
45 
46  bufsize = (na + nb - 1) * sizeof(double);
47 
48  if((a != c) && (b != c))
49  t = c;
50  else
51  t = (double*)malloc(bufsize);
52 
53  memset(t, 0, bufsize);
54 
55  for(k = 0; k < na; k++)
56  for(n = 0; n < nb; n++)
57  t[k+n] += a[k]*b[n];
58 
59  if(t!=c)
60  {
61  memcpy(c, t, bufsize);
62  free(t);
63  }
64  return RES_OK;
65 }
66 
67 
68 
69 
70 
71 
72 /******************************************************************************
73  Complex vectors linear convolution.
74 --------------------------------------------------------------------------------
75 Documented: RU, EN
76 *******************************************************************************/
77 int DSPL_API conv_cmplx(complex_t* a, int na, complex_t* b,
78  int nb, complex_t* c)
79 {
80  int k;
81  int n;
82 
83  complex_t *t;
84  size_t bufsize;
85 
86  if(!a || !b || !c)
87  return ERROR_PTR;
88  if(na < 1 || nb < 1)
89  return ERROR_SIZE;
90 
91  bufsize = (na + nb - 1) * sizeof(complex_t);
92 
93  if((a != c) && (b != c))
94  t = c;
95  else
96  t = (complex_t*)malloc(bufsize);
97 
98  memset(t, 0, bufsize);
99 
100  for(k = 0; k < na; k++)
101  {
102  for(n = 0; n < nb; n++)
103  {
104  RE(t[k+n]) += CMRE(a[k], b[n]);
105  IM(t[k+n]) += CMIM(a[k], b[n]);
106  }
107  }
108 
109  if(t!=c)
110  {
111  memcpy(c, t, bufsize);
112  free(t);
113  }
114 
115  return RES_OK;
116 }
117 
118 
119 
120 
121 
122 /******************************************************************************
123 Real vectors fast linear convolution by using fast Fourier transform
124 --------------------------------------------------------------------------------
125 Documented: RU, EN
126 *******************************************************************************/
127 int DSPL_API conv_fft(double* a, int na, double* b, int nb,
128  fft_t* pfft, int nfft, double* c)
129 {
130  complex_t *pa = NULL, *pb = NULL, *pc = NULL;
131  int err;
132 
133  if(!a || !b || !c || !pfft)
134  return ERROR_PTR;
135  if(na<1 || nb < 1)
136  return ERROR_SIZE;
137  if(nfft<2)
138  return ERROR_FFT_SIZE;
139 
140  pa = (complex_t*) malloc(na*sizeof(complex_t));
141  pb = (complex_t*) malloc(nb*sizeof(complex_t));
142  pc = (complex_t*) malloc((na+nb-1)*sizeof(complex_t));
143 
144  re2cmplx(a, na, pa);
145  re2cmplx(b, nb, pb);
146 
147  err = conv_fft_cmplx(pa, na, pb, nb, pfft, nfft, pc);
148  if(err != RES_OK)
149  goto exit_label;
150 
151  err = cmplx2re(pc, na+nb-1, c, NULL);
152 
153 exit_label:
154  if(pa) free(pa);
155  if(pb) free(pb);
156  if(pc) free(pc);
157 
158  return err;
159 }
160 
161 
162 
163 
164 /******************************************************************************
165 Complex vectors fast linear convolution by using fast Fourier
166 transform algorithms
167 --------------------------------------------------------------------------------
168 Documented: RU, EN
169 *******************************************************************************/
170 int DSPL_API conv_fft_cmplx(complex_t* a, int na, complex_t* b, int nb,
171  fft_t* pfft, int nfft, complex_t* c)
172 {
173 
174  int La, Lb, Lc, Nz, n, p0, p1, ind, err;
175  complex_t *pa, *pb;
176  complex_t *pt, *pA, *pB, *pC;
177 
178  if(!a || !b || !c)
179  return ERROR_PTR;
180  if(na < 1 || nb < 1)
181  return ERROR_SIZE;
182 
183  if(na >= nb)
184  {
185  La = na;
186  Lb = nb;
187  pa = a;
188  pb = b;
189  }
190  else
191  {
192  La = nb;
193  pa = b;
194  Lb = na;
195  pb = a;
196  }
197 
198  Lc = La + Lb - 1;
199  Nz = nfft - Lb;
200 
201  if(Nz <= 0)
202  return ERROR_FFT_SIZE;
203 
204  pt = (complex_t*)malloc(nfft*sizeof(complex_t));
205  pB = (complex_t*)malloc(nfft*sizeof(complex_t));
206  pA = (complex_t*)malloc(nfft*sizeof(complex_t));
207  pC = (complex_t*)malloc(nfft*sizeof(complex_t));
208 
209  memset(pt, 0, nfft*sizeof(complex_t));
210  memcpy(pt+Nz, pb, Lb*sizeof(complex_t));
211 
212  err = fft_cmplx(pt, nfft, pfft, pB);
213  if(err != RES_OK)
214  goto exit_label;
215 
216  p0 = -Lb;
217  p1 = p0 + nfft;
218  ind = 0;
219  while(ind < Lc)
220  {
221  if(p0 >=0)
222  {
223  if(p1 < La)
224  err = fft_cmplx(pa + p0, nfft, pfft, pA);
225  else
226  {
227  memset(pt, 0, nfft*sizeof(complex_t));
228  memcpy(pt, pa+p0, (nfft+La-p1)*sizeof(complex_t));
229  err = fft_cmplx(pt, nfft, pfft, pA);
230  }
231  }
232  else
233  {
234  memset(pt, 0, nfft*sizeof(complex_t));
235  if(p1 < La)
236  memcpy(pt - p0, pa, (nfft+p0)*sizeof(complex_t));
237  else
238  memcpy(pt - p0, pa, La * sizeof(complex_t));
239  err = fft_cmplx(pt, nfft, pfft, pA);
240  }
241 
242  if(err != RES_OK)
243  goto exit_label;
244 
245  for(n = 0; n < nfft; n++)
246  {
247  RE(pC[n]) = CMRE(pA[n], pB[n]);
248  IM(pC[n]) = CMIM(pA[n], pB[n]);
249  }
250 
251 
252  if(ind+nfft < Lc)
253  err = ifft_cmplx(pC, nfft, pfft, c+ind);
254  else
255  {
256  err = ifft_cmplx(pC, nfft, pfft, pt);
257  memcpy(c+ind, pt, (Lc-ind)*sizeof(complex_t));
258  }
259  if(err != RES_OK)
260  goto exit_label;
261 
262  p0 += Nz;
263  p1 += Nz;
264  ind += Nz;
265  }
266 
267 exit_label:
268  if(pt) free(pt);
269  if(pB) free(pB);
270  if(pA) free(pA);
271  if(pC) free(pC);
272 
273  return err;
274 }
275 
276 
277 
278 /*******************************************************************************
279 Real IIR filtration
280 --------------------------------------------------------------------------------
281 Documented: RU, EN
282 *******************************************************************************/
283 int DSPL_API filter_iir(double* b, double* a, int ord,
284  double* x, int n, double* y)
285 {
286  double* buf = NULL;
287  double* an = NULL;
288  double u;
289  int k;
290  int m;
291  int count;
292 
293  if(!b || !x || !y)
294  return ERROR_PTR;
295 
296  if(ord < 1 || n < 1)
297  return ERROR_SIZE;
298 
299  if(a && a[0]==0.0)
300  return ERROR_FILTER_A0;
301 
302  count = ord + 1;
303  buf = (double*) malloc(count*sizeof(double));
304  an = (double*) malloc(count*sizeof(double));
305 
306  memset(buf, 0, count*sizeof(double));
307 
308  if(!a)
309  memset(an, 0, count*sizeof(double));
310  else
311  for(k = 0; k < count; k++)
312  an[k] = a[k] / a[0];
313 
314  for(k = 0; k < n; k++)
315  {
316  for(m = ord; m > 0; m--)
317  buf[m] = buf[m-1];
318  u = 0.0;
319  for(m = ord; m > 0; m--)
320  u += buf[m]*an[m];
321 
322  buf[0] = x[k] - u;
323  y[k] = 0.0;
324  for(m = 0; m < count; m++)
325  y[k] += buf[m] * b[m];
326  }
327 
328  if(buf)
329  free(buf);
330  if(an)
331  free(an);
332  return RES_OK;
333 }
334 
int filter_iir(double *b, double *a, int ord, double *x, int n, double *y)
Фильтрация вещественного сигнала вещественным БИХ-фильтром
Definition: conv.c:283
int conv(double *a, int na, double *b, int nb, double *c)
Линейная свертка двух вещественных векторов
Definition: conv.c:32
#define ERROR_FILTER_A0
Параметр передаточной характеристики цифрового БИХ-фильтра не может быть равен нулю.
Definition: dspl.h:108
#define RE(x)
Макрос определяющий реальную часть комплексного числа.
Definition: dspl.h:77
#define ERROR_PTR
Ошибка указателя. Данная ошибка означает, что один из обязательных указателей (память под который дол...
Definition: dspl.h:142
#define ERROR_SIZE
Ошибка при передаче размера массива. Данная ошибка возникает когда помимо указателя на массив входных...
Definition: dspl.h:150
int fft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Быстрое преобразование Фурье комплексного сигнала
Definition: fft.c:92
int ifft_cmplx(complex_t *x, int n, fft_t *pfft, complex_t *y)
Обратное быстрое преобразование Фурье
Definition: fft.c:30
Структура данных объекта быстрого преобразования Фурье
Definition: dspl.h:47
int re2cmplx(double *x, int n, complex_t *y)
Преобразование массива вещественных данных в массив комплексных данных.
Definition: complex.c:64
int cmplx2re(complex_t *x, int n, double *re, double *im)
Преобразование массива комплексных данных в два массива вещественных данных, содержащих реальную и мн...
Definition: complex.c:34
int conv_cmplx(complex_t *a, int na, complex_t *b, int nb, complex_t *c)
Линейная свертка двух комплексных векторов
Definition: conv.c:77
double complex_t[2]
Описание комплексного типа данных.
Definition: dspl.h:41
#define RES_OK
Функция завершилась корректно. Ошибки отсутствуют.
Definition: dspl.h:94
#define ERROR_FFT_SIZE
Неверно задан размер БПФ. Размер БПФ может быть составным вида , где , а – произвольный простой множ...
Definition: dspl.h:107
#define IM(x)
Макрос определяющий мнимую часть комплексного числа.
Definition: dspl.h:78