plugins/toolame/subs.c

   1 #include <stdlib.h>
   2 #include "common.h"
   3 #include "encoder.h"
   4
   5 /*****************************************************************************
   6  ************************** Start of Subroutines *****************************
   7  *****************************************************************************/
   8
   9 /*****************************************************************************
  10  * FFT computes fast fourier transform of BLKSIZE samples of data            *
  11  *   uses decimation-in-frequency algorithm described in "Digital            *
  12  *   Signal Processing" by Oppenheim and Schafer, refer to pages 304         *
  13  *   (flow graph) and 330-332 (Fortran program in problem 5)                 *
  14  *   to get the inverse fft, change line 20 from                             *
  15  *                 w_imag[L] = -sin(PI/le1);                                 *
  16  *                          to                                               *
  17  *                 w_imag[L] = sin(PI/le1);                                  *
  18  *                                                                           *
  19  *   required constants:                                                     *
  20  *         #define      PI          3.14159265358979                         *
  21  *         #define      BLKSIZE     1024                                     *
  22  *         #define      LOGBLKSIZE  10                                       *
  23  *         #define      BLKSIZE_S   256                                      *
  24  *         #define      LOGBLKSIZE_S 8                                       *
  25  *                                                                           *
  26  *****************************************************************************/
  27 #define      BLKSIZE_S   256
  28 #define      LOGBLKSIZE_S 8
  29
  30 void
  31 fft (x_real, x_imag, energy, phi, N)
  32      FLOAT x_real[BLKSIZE], x_imag[BLKSIZE], energy[BLKSIZE], phi[BLKSIZE];
  33      int N;
  34 {
  35   int M, MM1;
  36   static int init = 0;
  37   int NV2, NM1, MP;
  38   static double w_real[2][LOGBLKSIZE], w_imag[2][LOGBLKSIZE];
  39   int i, j, k, L;
  40   int ip, le, le1;
  41   double t_real, t_imag, u_real, u_imag;
  42
  43   if (init == 0)
  44     {
  45       memset ((char *) w_real, 0, sizeof (w_real));     /* preset statics to 0 */
  46       memset ((char *) w_imag, 0, sizeof (w_imag));     /* preset statics to 0 */
  47       M = LOGBLKSIZE;
  48       for (L = 0; L < M; L++)
  49         {
  50           le = 1 << (M - L);
  51           le1 = le >> 1;
  52           w_real[0][L] = cos (PI / le1);
  53           w_imag[0][L] = -sin (PI / le1);
  54         }
  55       M = LOGBLKSIZE_S;
  56       for (L = 0; L < M; L++)
  57         {
  58           le = 1 << (M - L);
  59           le1 = le >> 1;
  60           w_real[1][L] = cos (PI / le1);
  61           w_imag[1][L] = -sin (PI / le1);
  62         }
  63       init++;
  64     }
  65   switch (N)
  66     {
  67     case BLKSIZE:
  68       M = LOGBLKSIZE;
  69       MP = 0;
  70       break;
  71     case BLKSIZE_S:
  72       M = LOGBLKSIZE_S;
  73       MP = 1;
  74       break;
  75     default:
  76       fprintf (stderr, "Error: Bad FFT Size in subs.c\n");
  77       exit (-1);
  78     }
  79   MM1 = M - 1;
  80   NV2 = N >> 1;
  81   NM1 = N - 1;
  82   for (L = 0; L < MM1; L++)
  83     {
  84       le = 1 << (M - L);
  85       le1 = le >> 1;
  86       u_real = 1;
  87       u_imag = 0;
  88       for (j = 0; j < le1; j++)
  89         {
  90           for (i = j; i < N; i += le)
  91             {
  92               ip = i + le1;
  93               t_real = x_real[i] + x_real[ip];
  94               t_imag = x_imag[i] + x_imag[ip];
  95               x_real[ip] = x_real[i] - x_real[ip];
  96               x_imag[ip] = x_imag[i] - x_imag[ip];
  97               x_real[i] = t_real;
  98               x_imag[i] = t_imag;
  99               t_real = x_real[ip];
 100               x_real[ip] = x_real[ip] * u_real - x_imag[ip] * u_imag;
 101               x_imag[ip] = x_imag[ip] * u_real + t_real * u_imag;
 102             }
 103           t_real = u_real;
 104           u_real = u_real * w_real[MP][L] - u_imag * w_imag[MP][L];
 105           u_imag = u_imag * w_real[MP][L] + t_real * w_imag[MP][L];
 106         }
 107     }
 108   /* special case: L = M-1; all Wn = 1 */
 109   for (i = 0; i < N; i += 2)
 110     {
 111       ip = i + 1;
 112       t_real = x_real[i] + x_real[ip];
 113       t_imag = x_imag[i] + x_imag[ip];
 114       x_real[ip] = x_real[i] - x_real[ip];
 115       x_imag[ip] = x_imag[i] - x_imag[ip];
 116       x_real[i] = t_real;
 117       x_imag[i] = t_imag;
 118       energy[i] = x_real[i] * x_real[i] + x_imag[i] * x_imag[i];
 119       if (energy[i] <= 0.0005)
 120         {
 121           phi[i] = 0;
 122           energy[i] = 0.0005;
 123         }
 124       else
 125         phi[i] = atan2 ((double) x_imag[i], (double) x_real[i]);
 126       energy[ip] = x_real[ip] * x_real[ip] + x_imag[ip] * x_imag[ip];
 127       if (energy[ip] == 0)
 128         phi[ip] = 0;
 129       else
 130         phi[ip] = atan2 ((double) x_imag[ip], (double) x_real[ip]);
 131     }
 132   /* this section reorders the data to the correct ordering */
 133   j = 0;
 134   for (i = 0; i < NM1; i++)
 135     {
 136       if (i < j)
 137         {
 138 /* use this section only if you need the FFT in complex number form *
 139  * (and in the correct ordering)                                    */
 140           t_real = x_real[j];
 141           t_imag = x_imag[j];
 142           x_real[j] = x_real[i];
 143           x_imag[j] = x_imag[i];
 144           x_real[i] = t_real;
 145           x_imag[i] = t_imag;
 146 /* reorder the energy and phase, phi                                        */
 147           t_real = energy[j];
 148           energy[j] = energy[i];
 149           energy[i] = t_real;
 150           t_real = phi[j];
 151           phi[j] = phi[i];
 152           phi[i] = t_real;
 153         }
 154       k = NV2;
 155       while (k <= j)
 156         {
 157           j = j - k;
 158           k = k >> 1;
 159         }
 160       j = j + k;
 161     }
 162 }