libdspl-2.0
fft.c
1 /*
2 * Copyright (c) 2015-2018 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 <stdio.h>
23 #include <string.h>
24 #include "dspl.h"
25 #include "dspl_internal.h"
26 
27 
28 
29 
30 
31 /*******************************************************************************
32 2 points DFT
33 *******************************************************************************/
34 void dft2 (complex_t *x, complex_t* y)
35 {
36  RE(y[0]) = RE(x[0]) + RE(x[1]);
37  IM(y[0]) = IM(x[0]) + IM(x[1]);
38 
39  RE(y[1]) = RE(x[0]) - RE(x[1]);
40  IM(y[1]) = IM(x[0]) - IM(x[1]);
41 }
42 
43 
44 
45 
46 /*******************************************************************************
47 COMPLEX vector IFFT
48 *******************************************************************************/
49 int DSPL_API ifft_cmplx(complex_t *x, int n, fft_t* pfft, complex_t* y)
50 {
51  int err, k;
52  double norm;
53 
54  if(!x || !pfft || !y)
55  return ERROR_PTR;
56  if(n<1)
57  return ERROR_SIZE;
58 
59 
60  err = fft_create(pfft, n);
61  if(err != RES_OK)
62  return err;
63 
64  memcpy(pfft->t1, x, n*sizeof(complex_t));
65  for(k = 0; k < n; k++)
66  IM(pfft->t1[k]) = -IM(pfft->t1[k]);
67 
68  err = fft_dit(pfft, n, y);
69 
70  if(err!=RES_OK)
71  return err;
72 
73  norm = 1.0 / (double)n;
74  for(k = 0; k < n; k++)
75  {
76  RE(y[k]) = RE(y[k])*norm;
77  IM(y[k]) = -IM(y[k])*norm;
78  }
79  return RES_OK;
80 }
81 
82 
83 
84 
85 
86 
87 
88 /*******************************************************************************
89 Real vector FFT
90 *******************************************************************************/
91 int DSPL_API fft(double *x, int n, fft_t* pfft, complex_t* y)
92 {
93  int err;
94 
95  if(!x || !pfft || !y)
96  return ERROR_PTR;
97  if(n<1)
98  return ERROR_SIZE;
99 
100 
101  err = fft_create(pfft, n);
102  if(err != RES_OK)
103  return err;
104 
105  re2cmplx(x, n, pfft->t1);
106 
107  return fft_dit(pfft, n, y);
108 }
109 
110 
111 
112 
113 /*******************************************************************************
114 COMPLEX vector FFT
115 *******************************************************************************/
116 int DSPL_API fft_cmplx(complex_t *x, int n, fft_t* pfft, complex_t* y)
117 {
118  int err;
119 
120  if(!x || !pfft || !y)
121  return ERROR_PTR;
122  if(n<1)
123  return ERROR_SIZE;
124 
125 
126  err = fft_create(pfft, n);
127  if(err != RES_OK)
128  return err;
129 
130  memcpy(pfft->t1, x, n*sizeof(complex_t));
131 
132  return fft_dit(pfft, n, y);
133 }
134 
135 
136 
137 /*******************************************************************************
138 FFT bit reverse
139 *******************************************************************************/
140 int fft_bit_reverse(complex_t* x, complex_t* y, int n, int p2)
141 {
142  static unsigned char rb_table[256] =
143  {
144  0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0,
145  0x10, 0x90, 0x50, 0xD0, 0x30, 0xB0, 0x70, 0xF0,
146  0x08, 0x88, 0x48, 0xC8, 0x28, 0xA8, 0x68, 0xE8,
147  0x18, 0x98, 0x58, 0xD8, 0x38, 0xB8, 0x78, 0xF8,
148  0x04, 0x84, 0x44, 0xC4, 0x24, 0xA4, 0x64, 0xE4,
149  0x14, 0x94, 0x54, 0xD4, 0x34, 0xB4, 0x74, 0xF4,
150  0x0C, 0x8C, 0x4C, 0xCC, 0x2C, 0xAC, 0x6C, 0xEC,
151  0x1C, 0x9C, 0x5C, 0xDC, 0x3C, 0xBC, 0x7C, 0xFC,
152  0x02, 0x82, 0x42, 0xC2, 0x22, 0xA2, 0x62, 0xE2,
153  0x12, 0x92, 0x52, 0xD2, 0x32, 0xB2, 0x72, 0xF2,
154  0x0A, 0x8A, 0x4A, 0xCA, 0x2A, 0xAA, 0x6A, 0xEA,
155  0x1A, 0x9A, 0x5A, 0xDA, 0x3A, 0xBA, 0x7A, 0xFA,
156  0x06, 0x86, 0x46, 0xC6, 0x26, 0xA6, 0x66, 0xE6,
157  0x16, 0x96, 0x56, 0xD6, 0x36, 0xB6, 0x76, 0xF6,
158  0x0E, 0x8E, 0x4E, 0xCE, 0x2E, 0xAE, 0x6E, 0xEE,
159  0x1E, 0x9E, 0x5E, 0xDE, 0x3E, 0xBE, 0x7E, 0xFE,
160  0x01, 0x81, 0x41, 0xC1, 0x21, 0xA1, 0x61, 0xE1,
161  0x11, 0x91, 0x51, 0xD1, 0x31, 0xB1, 0x71, 0xF1,
162  0x09, 0x89, 0x49, 0xC9, 0x29, 0xA9, 0x69, 0xE9,
163  0x19, 0x99, 0x59, 0xD9, 0x39, 0xB9, 0x79, 0xF9,
164  0x05, 0x85, 0x45, 0xC5, 0x25, 0xA5, 0x65, 0xE5,
165  0x15, 0x95, 0x55, 0xD5, 0x35, 0xB5, 0x75, 0xF5,
166  0x0D, 0x8D, 0x4D, 0xCD, 0x2D, 0xAD, 0x6D, 0xED,
167  0x1D, 0x9D, 0x5D, 0xDD, 0x3D, 0xBD, 0x7D, 0xFD,
168  0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3,
169  0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3,
170  0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB,
171  0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB,
172  0x07, 0x87, 0x47, 0xC7, 0x27, 0xA7, 0x67, 0xE7,
173  0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7,
174  0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF,
175  0x1F, 0x9F, 0x5F, 0xDF, 0x3F, 0xBF, 0x7F, 0xFF
176  };
177 
178  if(!x || !y)
179  return ERROR_PTR;
180  if(n<1 || p2 < 1)
181  return ERROR_SIZE;
182 
183  unsigned int v, c;
184 
185  for(v = 0; v < (unsigned int)n; v++)
186  {
187  c = (unsigned int) ((rb_table[ v & 0xff] << 24) |
188  (rb_table[(v >> 8) & 0xff] << 16) |
189  (rb_table[(v >> 16) & 0xff] << 8) |
190  (rb_table[(v >> 24) & 0xff])) >> (32 - p2);
191 
192  RE(y[c]) = RE(x[v]);
193  IM(y[c]) = IM(x[v]);
194  }
195  return RES_OK;
196 }
197 
198 
199 
200 
201 
202 /*******************************************************************************
203 FFT create
204 *******************************************************************************/
205 int DSPL_API fft_create(fft_t *pfft, int n)
206 {
207  int p2, k,r,m,addr,s;
208  double phi;
209 
210  p2 = fft_p2(n);
211  if(p2 < 1)
212  return ERROR_FFT_SIZE;
213  if(n < pfft->n+1)
214  return RES_OK;
215 
216  pfft->n = n;
217  pfft->p2 = p2;
218 
219  pfft->w = pfft->w ? (complex_t*) realloc(pfft->w, n*sizeof(complex_t)):
220  (complex_t*) malloc( n*sizeof(complex_t));
221 
222 
223  pfft->t0 = pfft->t0 ? (complex_t*) realloc(pfft->t0, n*sizeof(complex_t)):
224  (complex_t*) malloc( n*sizeof(complex_t));
225 
226 
227  pfft->t1 = pfft->t1 ? (complex_t*) realloc(pfft->t1, n*sizeof(complex_t)):
228  (complex_t*) malloc( n*sizeof(complex_t));
229 
230  m = 0;
231  addr = 0;
232 
233  for(k = 0; k < p2; k++)
234  {
235  s = 1<<m;
236  for( r = 0; r < s; r++)
237  {
238  phi = -M_2PI *(double)r / (double)(2*s);
239  RE(pfft->w[addr+r]) = cos(phi);
240  IM(pfft->w[addr+r]) = sin(phi);
241  }
242  addr+=s;
243  m++;
244  }
245  return RES_OK;
246 }
247 
248 
249 
250 
251 /*******************************************************************************
252 composite FFT kernel
253 *******************************************************************************/
254 int DSPL_API fftn_krn(complex_t* t0, complex_t* t1, fft_t* p, int n, int addr)
255 {
256  int n1, n2, k;
257  complex_t *pw = p->w+addr;
258 
259  n1 = 1;
260  if(n%2 == 0)
261  {
262  n1 = 2;
263  goto label_size;
264  }
265 
266 label_size:
267  if(n1 == 1)
268  return ERROR_FFT_SIZE;
269  n2 = n / n1;
270  memcpy(t1, t0, n*sizeof(complex_t));
271 
272  transpose_cmplx(t1, n2, n1, t0);
273 
274  if(n1 == 2)
275  {
276  for(k = 0; k < n2; k++)
277  {
278  dft2(t0+2*k, t1+2*k);
279  }
280  }
281 
282  if(n1 > 1)
283  {
284 
285  for(k =0; k < n; k++)
286  {
287  RE(t0[k]) = CMRE(t1[k], pw[k]);
288  IM(t0[k]) = CMIM(t1[k], pw[k]);
289  }
290 
291  transpose_cmplx(t0, n1, n2, t1);
292 
293 
294  for(k = 0; k < n1; k++)
295  {
296  fftn_krn(t1+k*n2, t0+k*n2, p, n2, addr+n);
297  }
298 
299 
300  transpose_cmplx(t0, n2, n1, t1);
301  }
302  return RES_OK;
303 
304 }
305 
306 
307 
308 
309 /*******************************************************************************
310 FFT create for composite N
311 *******************************************************************************/
312 int DSPL_API fftn_create(fft_t *pfft, int n)
313 {
314 
315  int n1, n2, addr, s, k, m, nw;
316  double phi;
317  s = n;
318  nw = addr = 0;
319 
320  while(s > 1)
321  {
322  n2 = 1;
323  if(s%2 == 0)
324  {
325  n2 = 2;
326  goto label_size;
327  }
328 
329 
330 label_size:
331  if(n2 == 1)
332  return ERROR_FFT_SIZE;
333  n1 = s / n2;
334  nw += s;
335  pfft->w = pfft->w ? (complex_t*) realloc(pfft->w, nw*sizeof(complex_t)):
336  (complex_t*) malloc( nw*sizeof(complex_t));
337 
338  for(k = 0; k < n1; k++)
339  {
340  for(m = 0; m < n2; m++)
341  {
342  phi = - M_2PI * (double)(k*m) / (double)s;
343  RE(pfft->w[addr]) = cos(phi);
344  IM(pfft->w[addr]) = sin(phi);
345  addr++;
346  }
347  }
348  s /= n2;
349  }
350 
351  pfft->t0 = pfft->t0 ? (complex_t*) realloc(pfft->t0, n*sizeof(complex_t)):
352  (complex_t*) malloc( n*sizeof(complex_t));
353 
354 
355  pfft->t1 = pfft->t1 ? (complex_t*) realloc(pfft->t1, n*sizeof(complex_t)):
356  (complex_t*) malloc( n*sizeof(complex_t));
357 
358  return RES_OK;
359 }
360 
361 
362 
363 
364 /*******************************************************************************
365 FFT decimation in time
366 *******************************************************************************/
367 int fft_dit(fft_t *pfft, int n, complex_t* y)
368 {
369  int k,s,m,waddr, dm,p2;
370  complex_t *t, *t0, *t1, *w;
371  int err;
372 
373  p2 = fft_p2(n);
374  if(p2<0)
375  return ERROR_FFT_SIZE;
376 
377  t0 = pfft->t0;
378  t1 = pfft->t1;
379  w = pfft->w;
380 
381  s = n>>1;
382  m = 1;
383  waddr = 0;
384 
385  err = fft_bit_reverse(t1, t0, n, p2);
386  if(err!= RES_OK)
387  return err;
388 
389  while(s)
390  {
391  dm = m<<1;
392  if(s > 1)
393  {
394  for(k = 0; k < n; k+=dm)
395  {
396  fft_dit_krn(t0+k, t0+k+m, w+waddr, m, t1+k, t1+k+m);
397  }
398  t = t1;
399  t1 = t0;
400  t0 = t;
401  waddr+=m;
402  m <<= 1;
403  }
404  else
405  {
406  fft_dit_krn(t0, t0+m, w+waddr, m, y, y+m);
407  }
408  s >>= 1;
409  }
410  return RES_OK;
411 }
412 
413 
414 
415 
416 
417 /*******************************************************************************
418 FFT decimation in time kernel
419 *******************************************************************************/
420 void fft_dit_krn(complex_t *x0, complex_t *x1, complex_t *w, int n,
421  complex_t *y0, complex_t *y1)
422 {
423  int k;
424  complex_t mul;
425  for(k = 0; k < n; k++)
426  {
427  RE(mul) = CMRE(x1[k], w[k]);
428  IM(mul) = CMIM(x1[k], w[k]);
429 
430  RE(y0[k]) = RE(x0[k]) + RE(mul);
431  IM(y0[k]) = IM(x0[k]) + IM(mul);
432 
433  RE(y1[k]) = RE(x0[k]) - RE(mul);
434  IM(y1[k]) = IM(x0[k]) - IM(mul);
435  }
436 }
437 
438 
439 
440 
441 
442 /*******************************************************************************
443 FFT free
444 *******************************************************************************/
445 void DSPL_API fft_free(fft_t *pfft)
446 {
447  if(!pfft)
448  return;
449  if(pfft->w)
450  free(pfft->w);
451  if(pfft->t0)
452  free(pfft->t0);
453  if(pfft->t1)
454  free(pfft->t1);
455  memset(pfft, 0, sizeof(fft_t));
456 }
457 
458 
459 
460 
461 
462 /*******************************************************************************
463 FFT power of 2
464 *******************************************************************************/
465 int fft_p2(int n)
466 {
467  int p = (n>0) ? n : 0;
468  int p2 = 0;
469  while((p = p>>1))
470  p2++;
471  if((1<<p2)!=n)
472  return -1;
473  return p2;
474 }
475 
476 
477 
478 
479 
480 
481 
482 /*******************************************************************************
483 FFT shifting
484 *******************************************************************************/
485 int DSPL_API fft_shift(double* x, int n, double* y)
486 {
487  int n2, r;
488  int k;
489  double tmp;
490  double *buf;
491 
492  if(!x || !y)
493  return ERROR_PTR;
494 
495  if(n<1)
496  return ERROR_SIZE;
497 
498  r = n%2;
499  if(!r)
500  {
501  n2 = n>>1;
502  for(k = 0; k < n2; k++)
503  {
504  tmp = x[k];
505  y[k] = x[k+n2];
506  y[k+n2] = tmp;
507  }
508  }
509  else
510  {
511  n2 = (n-1) >> 1;
512  buf = (double*) malloc(n2*sizeof(double));
513  memcpy(buf, x, n2*sizeof(double));
514  memcpy(y, x+n2, (n2+1)*sizeof(double));
515  memcpy(y+n2+1, buf, n2*sizeof(double));
516  free(buf);
517  }
518  return RES_OK;
519 }
520 
521 
522 
523 /*******************************************************************************
524 FFT shifting for complex vector
525 *******************************************************************************/
526 int DSPL_API fft_shift_cmplx(complex_t* x, int n, complex_t* y)
527 {
528  int n2, r;
529  int k;
530  complex_t tmp;
531  complex_t *buf;
532 
533  if(!x || !y)
534  return ERROR_PTR;
535 
536  if(n<1)
537  return ERROR_SIZE;
538 
539  r = n%2;
540  if(!r)
541  {
542  n2 = n>>1;
543  for(k = 0; k < n2; k++)
544  {
545  RE(tmp) = RE(x[k]);
546  IM(tmp) = IM(x[k]);
547 
548  RE(y[k]) = RE(x[k+n2]);
549  IM(y[k]) = IM(x[k+n2]);
550 
551  RE(y[k+n2]) = RE(tmp);
552  IM(y[k+n2]) = IM(tmp);
553  }
554  }
555  else
556  {
557  n2 = (n-1) >> 1;
558  buf = (complex_t*) malloc(n2*sizeof(complex_t));
559  memcpy(buf, x, n2*sizeof(complex_t));
560  memcpy(y, x+n2, (n2+1)*sizeof(complex_t));
561  memcpy(y+n2+1, buf, n2*sizeof(complex_t));
562  free(buf);
563  }
564  return RES_OK;
565 }
566 
complex_t * w
Definition: dspl.h:47
#define ERROR_SIZE
Ошибка при передаче размера массива. Данная ошибка возникает когда помимо указателя на массив входных...
Definition: dspl.h:124
Структура данных объекта быстрого преобразования Фурье
Definition: dspl.h:45
double complex_t[2]
Описание комплексного типа данных.
Definition: dspl.h:41
#define ERROR_PTR
Ошибка указателя. Данная ошибка означает, что один из обязательных указателей (память под который дол...
Definition: dspl.h:117
complex_t * t1
Definition: dspl.h:49
#define IM(x)
Макрос определяющий мнимую часть комплексного числа.
Definition: dspl.h:66
#define RE(x)
Макрос определяющий реальную часть комплексного числа.
Definition: dspl.h:65
int fft_shift(double *x, int n, double *y)
Перестановка спектральных отсчетов дискретного преобразования Фурье
Definition: fft.c:485
complex_t * t0
Definition: dspl.h:48
int re2cmplx(double *x, int n, complex_t *y)
Преобразование массива вещественных данных в массив комплексных данных.
Definition: complex.c:82
int n
Definition: dspl.h:50
void fft_free(fft_t *pfft)
Очистить структуру fft_t алгоритма БПФ
Definition: fft.c:445
int p2
Definition: dspl.h:51
#define ERROR_FFT_SIZE
Неверно задан размер БПФ.
Definition: dspl.h:94
#define RES_OK
Функция завершилась корректно. Ошибки отсутствуют.
Definition: dspl.h:81
int fft_create(fft_t *pfft, int n)
Заполнение структуры fft_t для алгоритма БПФ
Definition: fft.c:205