soft/ffttool/fft.c

   1 \r
   2 #include <stdlib.h>\r
   3 #include <math.h>\r
   4 #include "fft.h"\r
   5 \r
   6 /* y = fft_log2(x) */\r
   7 static int fft_log2(int x)\r
   8 {\r
   9     int y;\r
  10 \r
  11     y = 0;\r
  12     while (x > 1) {\r
  13         x >>= 1;\r
  14         y++;\r
  15     }\r
  16 \r
  17     return y;\r
  18 }\r
  19 \r
  20 /* y = 2 ^ x */\r
  21 static int fft_pow2(int x)\r
  22 {\r
  23     int y;\r
  24 \r
  25     if (x == 0) {\r
  26         y = 1;\r
  27     } else {\r
  28         y = 2 << (x - 1);\r
  29     }\r
  30 \r
  31     return y;\r
  32 }\r
  33 \r
  34 void FFT(double x_real[], double x_imag[], int N)\r
  35 {\r
  36     int i, j, k, n, m, r, stage, number_of_stage, *index;\r
  37     double a_real, a_imag, b_real, b_imag, c_real, c_imag, real, imag;\r
  38 \r
  39     /* FFT\82Ì\92i\90\94 */\r
  40     number_of_stage = fft_log2(N);\r
  41 \r
  42     /* \83o\83^\83t\83\89\83C\8cv\8eZ */\r
  43     for (stage = 1; stage <= number_of_stage; stage++) {\r
  44         for (i = 0; i < fft_pow2(stage - 1); i++) {\r
  45             for (j = 0; j < fft_pow2(number_of_stage - stage); j++) {\r
  46                 n = fft_pow2(number_of_stage - stage + 1) * i + j;\r
  47                 m = fft_pow2(number_of_stage - stage) + n;\r
  48                 r = fft_pow2(stage - 1) * j;\r
  49                 a_real = x_real[n];\r
  50                 a_imag = x_imag[n];\r
  51                 b_real = x_real[m];\r
  52                 b_imag = x_imag[m];\r
  53                 c_real = cos((2.0 * M_PI * r) / N);\r
  54                 c_imag = -sin((2.0 * M_PI * r) / N);\r
  55                 if (stage < number_of_stage) {\r
  56                     x_real[n] = a_real + b_real;\r
  57                     x_imag[n] = a_imag + b_imag;\r
  58                     x_real[m] = (a_real - b_real) * c_real - (a_imag - b_imag) * c_imag;\r
  59                     x_imag[m] = (a_imag - b_imag) * c_real + (a_real - b_real) * c_imag;\r
  60                 } else {\r
  61                     x_real[n] = a_real + b_real;\r
  62                     x_imag[n] = a_imag + b_imag;\r
  63                     x_real[m] = a_real - b_real;\r
  64                     x_imag[m] = a_imag - b_imag;\r
  65                 }\r
  66             }\r
  67         }\r
  68     }\r
  69 \r
  70     /* \83C\83\93\83f\83b\83N\83X\82Ì\95À\82Ñ\91Ö\82¦\82Ì\82½\82ß\82Ì\83e\81[\83u\83\8b\82Ì\8dì\90¬ */\r
  71     index = calloc(N, sizeof(int));\r
  72     for (stage = 1; stage <= number_of_stage; stage++) {\r
  73         for (i = 0; i < fft_pow2(stage - 1); i++) {\r
  74             index[fft_pow2(stage - 1) + i] = index[i] + fft_pow2(number_of_stage - stage);\r
  75         }\r
  76     }\r
  77 \r
  78     /* \83C\83\93\83f\83b\83N\83X\82Ì\95À\82Ñ\91Ö\82¦ */\r
  79     for (k = 0; k < N; k++) {\r
  80         if (index[k] > k) {\r
  81             real = x_real[index[k]];\r
  82             imag = x_imag[index[k]];\r
  83             x_real[index[k]] = x_real[k];\r
  84             x_imag[index[k]] = x_imag[k];\r
  85             x_real[k] = real;\r
  86             x_imag[k] = imag;\r
  87         }\r
  88     }\r
  89 \r
  90     free(index);\r
  91 }\r
  92 \r
  93 void IFFT(double x_real[], double x_imag[], int N)\r
  94 {\r
  95     int i, j, k, n, m, r, stage, number_of_stage, *index;\r
  96     double a_real, a_imag, b_real, b_imag, c_real, c_imag, real, imag;\r
  97 \r
  98     /* IFFT\82Ì\92i\90\94 */\r
  99     number_of_stage = fft_log2(N);\r
 100 \r
 101     /* \83o\83^\83t\83\89\83C\8cv\8eZ */\r
 102     for (stage = 1; stage <= number_of_stage; stage++) {\r
 103         for (i = 0; i < fft_pow2(stage - 1); i++) {\r
 104             for (j = 0; j < fft_pow2(number_of_stage - stage); j++) {\r
 105                 n = fft_pow2(number_of_stage - stage + 1) * i + j;\r
 106                 m = fft_pow2(number_of_stage - stage) + n;\r
 107                 r = fft_pow2(stage - 1) * j;\r
 108                 a_real = x_real[n];\r
 109                 a_imag = x_imag[n];\r
 110                 b_real = x_real[m];\r
 111                 b_imag = x_imag[m];\r
 112                 c_real = cos((2.0 * M_PI * r) / N);\r
 113                 c_imag = sin((2.0 * M_PI * r) / N);\r
 114                 if (stage < number_of_stage) {\r
 115                     x_real[n] = a_real + b_real;\r
 116                     x_imag[n] = a_imag + b_imag;\r
 117                     x_real[m] = (a_real - b_real) * c_real - (a_imag - b_imag) * c_imag;\r
 118                     x_imag[m] = (a_imag - b_imag) * c_real + (a_real - b_real) * c_imag;\r
 119                 } else {\r
 120                     x_real[n] = a_real + b_real;\r
 121                     x_imag[n] = a_imag + b_imag;\r
 122                     x_real[m] = a_real - b_real;\r
 123                     x_imag[m] = a_imag - b_imag;\r
 124                 }\r
 125             }\r
 126         }\r
 127     }\r
 128 \r
 129     /* \83C\83\93\83f\83b\83N\83X\82Ì\95À\82Ñ\91Ö\82¦\82Ì\82½\82ß\82Ì\83e\81[\83u\83\8b\82Ì\8dì\90¬ */\r
 130     index = calloc(N, sizeof(int));\r
 131     for (stage = 1; stage <= number_of_stage; stage++) {\r
 132         for (i = 0; i < fft_pow2(stage - 1); i++) {\r
 133             index[fft_pow2(stage - 1) + i] = index[i] + fft_pow2(number_of_stage - stage);\r
 134         }\r
 135     }\r
 136 \r
 137     /* \83C\83\93\83f\83b\83N\83X\82Ì\95À\82Ñ\91Ö\82¦ */\r
 138     for (k = 0; k < N; k++) {\r
 139         if (index[k] > k) {\r
 140             real = x_real[index[k]];\r
 141             imag = x_imag[index[k]];\r
 142             x_real[index[k]] = x_real[k];\r
 143             x_imag[index[k]] = x_imag[k];\r
 144             x_real[k] = real;\r
 145             x_imag[k] = imag;\r
 146         }\r
 147     }\r
 148 \r
 149     /* \8cv\8eZ\8c\8b\89Ê\82ðN\82Å\8a\84\82é */\r
 150     for (k = 0; k < N; k++) {\r
 151         x_real[k] /= N;\r
 152         x_imag[k] /= N;\r
 153     }\r
 154 \r
 155     free(index);\r
 156 }\r
 157 \r