Edinburgh Speech Tools  2.4-release
fft.cc
1 /*************************************************************************/
2 /* */
3 /* Centre for Speech Technology Research */
4 /* University of Edinburgh, UK */
5 /* Copyright (c) 1994,1995,1996 */
6 /* All Rights Reserved. */
7 /* */
8 /* Permission is hereby granted, free of charge, to use and distribute */
9 /* this software and its documentation without restriction, including */
10 /* without limitation the rights to use, copy, modify, merge, publish, */
11 /* distribute, sublicense, and/or sell copies of this work, and to */
12 /* permit persons to whom this work is furnished to do so, subject to */
13 /* the following conditions: */
14 /* 1. The code must retain the above copyright notice, this list of */
15 /* conditions and the following disclaimer. */
16 /* 2. Any modifications must be clearly marked as such. */
17 /* 3. Original authors' names are not deleted. */
18 /* 4. The authors' names are not used to endorse or promote products */
19 /* derived from this software without specific prior written */
20 /* permission. */
21 /* */
22 /* THE UNIVERSITY OF EDINBURGH AND THE CONTRIBUTORS TO THIS WORK */
23 /* DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING */
24 /* ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT */
25 /* SHALL THE UNIVERSITY OF EDINBURGH NOR THE CONTRIBUTORS BE LIABLE */
26 /* FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES */
27 /* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN */
28 /* AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, */
29 /* ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF */
30 /* THIS SOFTWARE. */
31 /* */
32 /*************************************************************************/
33 /* Author : Simon King (taken from Tony Robinson) */
34 /* Date : July 1994 */
35 /*-----------------------------------------------------------------------*/
36 /* FFT functions */
37 /* */
38 /*=======================================================================*/
39 
40 #include <cmath>
41 //#include <iostream>
42 //#include <fstream>
43 #include "sigpr/EST_fft.h"
44 #include "EST_math.h"
45 #include "EST_error.h"
46 
47 #define PI8 0.392699081698724 /* PI / 8.0 */
48 #define RT2 1.4142135623731 /* sqrt(2.0) */
49 #define IRT2 0.707106781186548 /* 1.0/sqrt(2.0) */
50 
51 static void FR2TR(int, float*, float*);
52 static void FR4TR(int, int, float*, float*, float*, float*);
53 static void FORD1(int, float*);
54 static void FORD2(int, float*);
55 
56 /*
57 ** FAST(b,n)
58 ** This routine replaces the float vector b
59 ** of length n with its finite discrete fourier transform.
60 ** DC term is returned in b[0];
61 ** n/2th harmonic float part in b[1].
62 ** jth harmonic is returned as complex number stored as
63 ** b[2*j] + i b[2*j + 1]
64 ** (i.e., remaining coefficients are as a DPCOMPLEX vector).
65 **
66 */
67 
68 
69 static int slowFFTsub(EST_FVector &real, EST_FVector &imag, float f)
70 {
71  // f = -1 for FFT, 1 for IFFT
72  // would be nicer if we used a complex number class,
73  // but we don't, so it isn't
74 
75  // taken from the FORTRAN old chestnut
76  // in various sig proc books
77  // FORTRAN uses 1..n arrays, so subtract 1 all over the place
78 
79 
80  float u_real,u_imag;
81  float w_real,w_imag;
82  float t_real,t_imag;
83  float tmp_real,tmp_imag;
84 
85  int M,N;
86  int i,j,k,l;
87 
88  M = fastlog2(real.n());
89  N = (int)pow(float(2.0),(float)M);
90 
91  if (N != real.n())
92  {
93  EST_warning("Illegal FFT order %d", real.n());
94  return -1;
95  }
96 
97  for(l=1;l<=M;l++){
98 
99  int le = (int)pow(float(2.0),(float)(M+1-l));
100  int le1=le/2;
101 
102  u_real = 1.0;
103  u_imag = 0.0;
104 
105  w_real=cos(PI/le1);
106  w_imag=f * sin(PI/le1);
107 
108  for (j=1;j<=le1;j++)
109  {
110  for (i=j;i<=N-le1;i+=le)
111  {
112  int ip=i+le1;
113  t_real = real.a_no_check(i-1) + real.a_no_check(ip-1);
114  t_imag = imag.a_no_check(i-1) + imag.a_no_check(ip-1);
115 
116  tmp_real = real.a_no_check(i-1) - real.a_no_check(ip-1);
117  tmp_imag = imag.a_no_check(i-1) - imag.a_no_check(ip-1);
118 
119  real.a_no_check(ip-1) = tmp_real*u_real - tmp_imag*u_imag;
120  imag.a_no_check(ip-1) = tmp_real*u_imag + tmp_imag*u_real;
121 
122  real.a_no_check(i-1) = t_real;
123  imag.a_no_check(i-1) = t_imag;
124  }
125 
126  tmp_real = u_real*w_real - u_imag*w_imag;
127  tmp_imag = u_real*w_imag + u_imag*w_real;
128 
129  u_real=tmp_real;
130  u_imag=tmp_imag;
131 
132  }
133 
134  }
135 
136 
137  int NV2=N/2;
138  int NM1=N-1;
139  j=1;
140 
141 
142  for (i=1; i<=NM1;i++)
143  {
144  if (i < j)
145  {
146  t_real=real(j-1);
147  t_imag=imag(j-1);
148 
149  real[j-1] = real(i-1);
150  imag[j-1] = imag(i-1);
151 
152  real[i-1] = t_real;
153  imag[i-1] = t_imag;
154 
155  }
156 
157  k=NV2;
158 
159  while(k < j)
160  {
161  j=j-k;
162  k=k/2;
163  }
164 
165  j=j+k;
166 
167  }
168 
169  return 0;
170 }
171 
172 
173 int slowFFT(EST_FVector &real, EST_FVector &imag)
174 {
175  return slowFFTsub(real,imag,-1.0);
176 }
177 
178 
179 int slowIFFT(EST_FVector &real, EST_FVector &imag)
180 {
181  int N=real.n();
182  if (N <=0 )
183  return -1;
184 
185  if (slowFFTsub(real,imag,1.0) != 0)
186  return -1;
187 
188  for(int i=1;i<=N;i++){
189  real[i-1] /= (float)N;
190  imag[i-1] /= (float)N;
191  }
192 
193  return 0;
194 }
195 
196 
197 int energy_spectrum(EST_FVector &real, EST_FVector &imag)
198 {
199  if (slowFFT(real, imag) != 0)
200  return -1;
201 
202  int i;
203  for(i=0;i<real.n();i++)
204  real[i] = imag[i] = (real(i)*real(i) + imag(i)*imag(i));
205 
206  return 0;
207 }
208 
209 int power_spectrum_slow(EST_FVector &real, EST_FVector &imag)
210 {
211 
212  if (slowFFT(real,imag) != 0)
213  return -1;
214 
215  int i;
216  for(i=0;i<real.n();i++)
217  real[i] = imag[i] = sqrt((real(i)*real(i) + imag(i)*imag(i)));
218 
219  return 0;
220 }
221 
222 int power_spectrum(EST_FVector &real, EST_FVector &imag)
223 {
224 
225  if (fastFFT(real) == 0)
226  return -1;
227 
228  int i,j,k;
229  int n=real.n();
230  for(i=0,j=0, k=1;i<n;i+=2,j++,k+=2)
231  real.a_no_check(j)
232  = imag.a_no_check(j)
233  = sqrt((real.a_no_check(i)*real.a_no_check(i)
234  + real.a_no_check(k)*real.a_no_check(k)));
235 
236  return 0;
237 }
238 
239 // the following code is not by Simon King, as you can see
240 /*
241 ** Discrete Fourier analysis routine
242 ** from IEEE Programs for Digital Signal Processing
243 ** G. D. Bergland and M. T. Dolan, original authors
244 ** Translated from the FORTRAN with some changes by Paul Kube
245 **
246 ** Modified to return the power spectrum by Chuck Wooters
247 **
248 ** Modified again by Tony Robinson (ajr@eng.cam.ac.uk) Dec 92
249 **
250 ** Modified to use EST_FVector class Simon King (simonk@cstr.ed.ac.uk) Nov 96
251 **
252 */
253 
254 #define signum(i) (i < 0 ? -1 : i == 0 ? 0 : 1)
255 
256 int fastFFT(EST_FVector &invec)
257 {
258  // Tony Robinsons
259  int i, in, nn, n2pow, n4pow;
260 
261  // we could modify all the code to use vector classes ....
262  // ... or we could do this:
263 
264  // TO DO
265  // use FSimpleVector::copy_section here
266 
267  // quick fix
268  int n=invec.n(); // order
269 
270 #if 0
271  float *b = new float[n];
272  for(i=0; i<n; i++)
273  b[i] = invec(i);
274 #endif
275  float *b=invec.memory();
276 
277  n2pow = fastlog2(n);
278  if (n2pow <= 0) return 0;
279  n4pow = n2pow / 2;
280 
281  /* radix 2 iteration required; do it now */
282  if (n2pow % 2)
283  {
284  nn = 2;
285  in = n / nn;
286  FR2TR(in, b, b + in );
287  }
288  else nn = 1;
289 
290  /* perform radix 4 iterations */
291  for(i = 1; i <= n4pow; i++) {
292  nn *= 4;
293  in = n / nn;
294  FR4TR(in, nn, b, b + in, b + 2 * in, b + 3 * in);
295  }
296 
297  /* perform inplace reordering */
298  FORD1(n2pow, b);
299  FORD2(n2pow, b);
300 
301  /* take conjugates */
302  for(i = 3; i < n; i += 2) b[i] = -b[i];
303 
304 #if 0
305  // copy back
306  for(i=0; i<n; i++)
307  invec[i] = b[i];
308 #endif
309 
310  return 1;
311 }
312 
313 /* radix 2 subroutine */
314 void FR2TR(int in, float *b0, float *b1)
315 {
316  int k;
317  float t;
318  for (k = 0; k < in; k++)
319  {
320  t = b0[k] + b1[k];
321  b1[k] = b0[k] - b1[k];
322  b0[k] = t;
323  }
324 }
325 
326 /* radix 4 subroutine */
327 void FR4TR(int in, int nn, float *b0, float *b1, float *b2, float* b3) {
328  float arg, piovn, th2;
329  float *b4 = b0, *b5 = b1, *b6 = b2, *b7 = b3;
330  float t0, t1, t2, t3, t4, t5, t6, t7;
331  float r1, r5, pr, pi;
332  float c1, c2, c3, s1, s2, s3;
333 
334  int j, k, jj, kk, jthet, jlast, ji, jl, jr, int4;
335  int L[16], L1, L2, L3, L4, L5, L6, L7, L8, L9, L10, L11, L12, L13, L14, L15;
336  int j0, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14;
337  int k0, kl;
338 
339  L[1] = nn / 4;
340  for(k = 2; k < 16; k++) { /* set up L's */
341  switch (signum(L[k-1] - 2)) {
342  case -1:
343  L[k-1]=2;
344  case 0:
345  L[k]=2;
346  break;
347  case 1:
348  L[k]=L[k-1]/2;
349  }
350  }
351 
352  L15=L[1]; L14=L[2]; L13=L[3]; L12=L[4]; L11=L[5]; L10=L[6]; L9=L[7];
353  L8=L[8]; L7=L[9]; L6=L[10]; L5=L[11]; L4=L[12]; L3=L[13]; L2=L[14];
354  L1=L[15];
355 
356  piovn = PI / nn;
357  ji=3;
358  jl=2;
359  jr=2;
360 
361  for(j1=2;j1<=L1;j1+=2)
362  for(j2=j1;j2<=L2;j2+=L1)
363  for(j3=j2;j3<=L3;j3+=L2)
364  for(j4=j3;j4<=L4;j4+=L3)
365  for(j5=j4;j5<=L5;j5+=L4)
366  for(j6=j5;j6<=L6;j6+=L5)
367  for(j7=j6;j7<=L7;j7+=L6)
368  for(j8=j7;j8<=L8;j8+=L7)
369  for(j9=j8;j9<=L9;j9+=L8)
370  for(j10=j9;j10<=L10;j10+=L9)
371  for(j11=j10;j11<=L11;j11+=L10)
372  for(j12=j11;j12<=L12;j12+=L11)
373  for(j13=j12;j13<=L13;j13+=L12)
374  for(j14=j13;j14<=L14;j14+=L13)
375  for(jthet=j14;jthet<=L15;jthet+=L14)
376  {
377  th2 = jthet - 2;
378  if(th2<=0.0)
379  {
380  for(k=0;k<in;k++)
381  {
382  t0 = b0[k] + b2[k];
383  t1 = b1[k] + b3[k];
384  b2[k] = b0[k] - b2[k];
385  b3[k] = b1[k] - b3[k];
386  b0[k] = t0 + t1;
387  b1[k] = t0 - t1;
388  }
389  if(nn-4>0)
390  {
391  k0 = in*4 + 1;
392  kl = k0 + in - 1;
393  for (k=k0;k<=kl;k++)
394  {
395  kk = k-1;
396  pr = IRT2 * (b1[kk]-b3[kk]);
397  pi = IRT2 * (b1[kk]+b3[kk]);
398  b3[kk] = b2[kk] + pi;
399  b1[kk] = pi - b2[kk];
400  b2[kk] = b0[kk] - pr;
401  b0[kk] = b0[kk] + pr;
402  }
403  }
404  }
405  else
406  {
407  arg = th2*piovn;
408  c1 = cos(arg);
409  s1 = sin(arg);
410  c2 = c1*c1 - s1*s1;
411  s2 = c1*s1 + c1*s1;
412  c3 = c1*c2 - s1*s2;
413  s3 = c2*s1 + s2*c1;
414 
415  int4 = in*4;
416  j0=jr*int4 + 1;
417  k0=ji*int4 + 1;
418  jlast = j0+in-1;
419  for(j=j0;j<=jlast;j++)
420  {
421  k = k0 + j - j0;
422  kk = k-1; jj = j-1;
423  r1 = b1[jj]*c1 - b5[kk]*s1;
424  r5 = b1[jj]*s1 + b5[kk]*c1;
425  t2 = b2[jj]*c2 - b6[kk]*s2;
426  t6 = b2[jj]*s2 + b6[kk]*c2;
427  t3 = b3[jj]*c3 - b7[kk]*s3;
428  t7 = b3[jj]*s3 + b7[kk]*c3;
429  t0 = b0[jj] + t2;
430  t4 = b4[kk] + t6;
431  t2 = b0[jj] - t2;
432  t6 = b4[kk] - t6;
433  t1 = r1 + t3;
434  t5 = r5 + t7;
435  t3 = r1 - t3;
436  t7 = r5 - t7;
437  b0[jj] = t0 + t1;
438  b7[kk] = t4 + t5;
439  b6[kk] = t0 - t1;
440  b1[jj] = t5 - t4;
441  b2[jj] = t2 - t7;
442  b5[kk] = t6 + t3;
443  b4[kk] = t2 + t7;
444  b3[jj] = t3 - t6;
445  }
446  jr += 2;
447  ji -= 2;
448  if(ji-jl <= 0) {
449  ji = 2*jr - 1;
450  jl = jr;
451  }
452  }
453  }
454 }
455 
456 /* an inplace reordering subroutine */
457 void FORD1(int m, float *b) {
458  int j, k = 4, kl = 2, n = 0x1 << m;
459  float t;
460 
461  for(j = 4; j <= n; j += 2) {
462  if (k - j>0) {
463  t = b[j-1];
464  b[j - 1] = b[k - 1];
465  b[k - 1] = t;
466  }
467  k -= 2;
468  if (k - kl <= 0) {
469  k = 2*j;
470  kl = j;
471  }
472  }
473 }
474 
475 /* the other inplace reordering subroutine */
476 void FORD2(int m, float *b)
477 {
478  float t;
479 
480  int n = 0x1<<m, k, ij, ji, ij1, ji1;
481 
482  int l[16], l1, l2, l3, l4, l5, l6, l7, l8, l9, l10, l11, l12, l13, l14, l15;
483  int j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14;
484 
485 
486  l[1] = n;
487  for(k=2;k<=m;k++) l[k]=l[k-1]/2;
488  for(k=m;k<=14;k++) l[k+1]=2;
489 
490  l15=l[1];l14=l[2];l13=l[3];l12=l[4];l11=l[5];l10=l[6];l9=l[7];
491  l8=l[8];l7=l[9];l6=l[10];l5=l[11];l4=l[12];l3=l[13];l2=l[14];l1=l[15];
492 
493  ij = 2;
494 
495  for(j1=2;j1<=l1;j1+=2)
496  for(j2=j1;j2<=l2;j2+=l1)
497  for(j3=j2;j3<=l3;j3+=l2)
498  for(j4=j3;j4<=l4;j4+=l3)
499  for(j5=j4;j5<=l5;j5+=l4)
500  for(j6=j5;j6<=l6;j6+=l5)
501  for(j7=j6;j7<=l7;j7+=l6)
502  for(j8=j7;j8<=l8;j8+=l7)
503  for(j9=j8;j9<=l9;j9+=l8)
504  for(j10=j9;j10<=l10;j10+=l9)
505  for(j11=j10;j11<=l11;j11+=l10)
506  for(j12=j11;j12<=l12;j12+=l11)
507  for(j13=j12;j13<=l13;j13+=l12)
508  for(j14=j13;j14<=l14;j14+=l13)
509  for(ji=j14;ji<=l15;ji+=l14) {
510  ij1 = ij-1; ji1 = ji - 1;
511  if(ij-ji<0) {
512  t = b[ij1-1];
513  b[ij1-1]=b[ji1-1];
514  b[ji1-1] = t;
515 
516  t = b[ij1];
517  b[ij1]=b[ji1];
518  b[ji1] = t;
519  }
520  ij += 2;
521  }
522 }
523 
524 int fastlog2(int n) {
525  int num_bits, power = 0;
526 
527  if ((n < 2) || (n % 2 != 0)) return(0);
528  num_bits = sizeof(int) * 8; /* How big are ints on this machine? */
529 
530  while(power <= num_bits) {
531  n >>= 1;
532  power += 1;
533  if (n & 0x01) {
534  if (n > 1) return(0);
535  else return(power);
536  }
537  }
538  return(0);
539 }
INLINE const T & a_no_check(int n) const
read-only const access operator: without bounds checking
Definition: EST_TVector.h:257
INLINE int n() const
number of items in vector.
Definition: EST_TVector.h:254
const T * memory() const
Definition: EST_TVector.h:241