1/* ------------------------------------------------------------------ * 2 * * 3 * Copyright (C) 1995 Roger Lindell * 4 * * 5 * All rights reserved. * 6 * * 7 * Adapted by Roger Lindell at the Dept. of Speech, Music and Hearing, * 8 * KTH (Royal Institute of Technology), Sweden, from an original algorithm * 9 * described in "Programs for Digital Signal Processing", Bergland & Dolan * 10 * (1979) Ed. by the DSP committee IEEE Acoustics, Speech and Signal * 11 * Processing Society. Wiley, New York. * 12 * Roger Lindell, rog@speech.kth.se * 13 * * 14 * KTH * 15 * Institutionen foer Tal, musik och hoersel * 16 * Box 700 14 * 17 * 100 44 STOCKHOLM * 18 * SWEDEN * 19 * * 20 *---------------------------------------------------------------------------* 21 * Function Snack_DBPowerSpectrum * 22 * Fast Fourier Transform for N=2**M * 23 * Complex Input * 24 *---------------------------------------------------------------------------* 25 * * 26 * This program replaces the vector z=x+iy by its finite * 27 * discrete, complex fourier transform if in=0. The inverse * 28 * transform is calculated for in=1. It performs as many base * 29 * 8 iterations as possible and then finishes with a base * 30 * 4 iteration or a base 2 iteration if needed. * 31 * * 32 *---------------------------------------------------------------------------* 33 * The functions are called as * 34 * Snack_InitFFT(int n) ; / * n must be a power of 2 * / * 35 * Snack_DBPowerSpectrum (float *x) ; * 36 *---------------------------------------------------------------------------* 37 * */ 38 39#include <stdio.h> 40#include <stdlib.h> 41#include <math.h> 42#include <string.h> 43#include "snack.h" 44 45#ifdef __cplusplus 46extern "C" { 47#endif 48int Snack_InitFFT(int n); 49void Snack_DBPowerSpectrum(float *x); 50#ifdef __cplusplus 51} 52#endif 53static void r2tx(int nthpo, float *cr0, float *cr1, float *ci0, float *ci1); 54static void r4tx(int nthpo, float *cr0, float *cr1, float *cr2, float *cr3, 55 float *ci0, float *ci1, float *ci2, float *ci3); 56static void r8tx(int nxtlt, int nthpo, int lengt, 57 float *cr0, float *cr1, float *cr2, float *cr3, float *cr4, 58 float *cr5, float *cr6, float *cr7, float *ci0, float *ci1, 59 float *ci2, float *ci3, float *ci4, float *ci5, float *ci6, 60 float *ci7); 61 62static unsigned int Pow2[17] = 63 {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768, 64 65536}; 65static float *sint, *cost, *x, *y; 66static int sint_init = 0; 67static int n2pow,nthpo; 68static double theta, wpr, wpi; 69 70#define SNACK_PI2 6.283185307179586 71#define P7 0.707106781186548 72#define LN2 0.6931471805599453 73 74int 75Snack_InitFFT(int n) 76{ 77 int k, l, m; 78 double a, p, wtemp; 79 80 n = n>>1; 81 m = (int) (log ((float) (n)) / LN2 + .5); 82 l = Pow2[m]; 83 p = SNACK_PI2 / l; 84 if (sint_init != 0) { 85 ckfree ((char *) sint); 86 ckfree ((char *) cost); 87 ckfree ((char *) x); 88 ckfree ((char *) y); 89 } 90 sint = (float *) ckalloc (l * sizeof(float)); 91 cost = (float *) ckalloc (l * sizeof(float)); 92 x = (float *) ckalloc (l * sizeof(float)); 93 y = (float *) ckalloc (l * sizeof(float)); 94 memset(sint, 0, l * sizeof(float)); 95 memset(cost, 0, l * sizeof(float)); 96 memset(x, 0, l * sizeof(float)); 97 memset(y, 0, l * sizeof(float)); 98 99 sint_init = 1; 100 for (k = 0; k < l; k++) { 101 a = k * p; 102 sint[k] = (float) sin (a); 103 cost[k] = (float) cos (a); 104 } 105 nthpo = l; 106 n2pow = m; 107 108 theta = 3.141592653589793/(double) (nthpo); 109 wtemp = sin(0.5*theta); 110 wpr = -2.0*wtemp*wtemp; 111 wpi = sin(theta); 112 113 return (l<<1); 114} 115 116void 117Snack_DBPowerSpectrum(float *z) 118{ 119 int l[17]; 120 int i, in, n8pow, fn; 121 int lengt; 122 register int ij, ji, nxtlt; 123 int i1, i2, i3, i4, i5, i6, i7, i8, i9, i10, i11, i12, i13, i14; 124 float r, fi; 125 float h1r, h1i, h2r, h2i; 126 double wr, wi, wtemp; 127 128 fn = nthpo; 129 for (i = 0; i < nthpo; i++) { 130 y[i] = -z[(i<<1)+1]; 131 x[i] = z[i<<1]; 132 } 133 134 n8pow = n2pow / 3; 135 if (n8pow != 0) { 136 for (i = 0; i < n8pow; i++) { 137 lengt = n2pow - 3 * i; 138 nxtlt = Pow2[lengt - 3]; 139 r8tx (nxtlt, nthpo, lengt, 140 &x[0], &x[nxtlt], &x[2 * nxtlt], &x[3 * nxtlt], 141 &x[4 * nxtlt], &x[5 * nxtlt], &x[6 * nxtlt], &x[7 * nxtlt], 142 &y[0], &y[nxtlt], &y[2 * nxtlt], &y[3 * nxtlt], 143 &y[4 * nxtlt], &y[5 * nxtlt], &y[6 * nxtlt], &y[7 * nxtlt]); 144 } 145 } 146 147 switch (n2pow - 3 * n8pow) { 148 case 0: 149 break; 150 case 1: 151 r2tx (nthpo, &x[0], &x[1], &y[0], &y[1]); 152 break; 153 case 2: 154 r4tx (nthpo, &x[0], &x[1], &x[2], &x[3], 155 &y[0], &y[1], &y[2], &y[3]); 156 break; 157 default: 158 /* fprintf (stderr, "-- Algorithm Error Snack_DBPowerSpectrum\n");*/ 159 exit (1); 160 } 161 for (i = 0; i < 17; i++) { 162 if (i < n2pow) { 163 l[i] = Pow2[n2pow - i]; 164 } else { 165 l[i] = 1; 166 } 167 } 168 ij = 0; 169 for (i1 = 0; i1 < l[14]; i1++) { 170 for (i2 = i1; i2 < l[13]; i2 += l[14]) { 171 for (i3 = i2; i3 < l[12]; i3 += l[13]) { 172 for (i4 = i3; i4 < l[11]; i4 += l[12]) { 173 for (i5 = i4; i5 < l[10]; i5 += l[11]) { 174 for (i6 = i5; i6 < l[9]; i6 += l[10]) { 175 for (i7 = i6; i7 < l[8]; i7 += l[9]) { 176 for (i8 = i7; i8 < l[7]; i8 += l[8]) { 177 for (i9 = i8; i9 < l[6]; i9 += l[7]) { 178 for (i10 = i9; i10 < l[5]; i10 += l[6]) { 179 for (i11 = i10; i11 < l[4]; i11 += l[5]) { 180 for (i12 = i11; i12 < l[3]; i12 += l[4]) { 181 for (i13 = i12; i13 < l[2]; i13 += l[3]) { 182 for (i14 = i13; i14 < l[1]; i14 += l[2]) { 183 for (ji = i14; ji < l[0]; ji += l[1]) { 184 if (ij < ji) { 185 r = x[ij]; 186 x[ij] = x[ji]; 187 x[ji] = r; 188 fi = y[ij]; 189 y[ij] = y[ji]; 190 y[ji] = fi; 191 } 192 ij++; 193 } 194 } 195 } 196 } 197 } 198 } 199 } 200 } 201 } 202 } 203 } 204 } 205 } 206 } 207 } 208 209 wr = 1.0+wpr; 210 wi = wpi; 211 for (i = 1; i <= (nthpo>>1); i++) { 212 in = nthpo-i; 213 h1r = (x[i]+x[in]); 214 h1i = (y[i]-y[in]); 215 h2r = (y[i]+y[in]); 216 h2i = (x[in]-x[i]); 217 x[in] = (float) (h1r+wr*h2r-wi*h2i); 218 y[in] = (float) (h1i+wr*h2i+wi*h2r); 219 wtemp = x[in]*x[in]+y[in]*y[in]; 220 if (wtemp < SNACK_INTLOGARGMIN) 221 wtemp = SNACK_INTLOGARGMIN; 222 z[in] = (float) (SNACK_DB * log(wtemp) - SNACK_CORRN); 223 224 x[i] = (float) (h1r-wr*h2r+wi*h2i); 225 y[i] = (float) (-h1i+wr*h2i+wi*h2r); 226 wtemp = x[i]*x[i]+y[i]*y[i]; 227 if (wtemp < SNACK_INTLOGARGMIN) 228 wtemp = SNACK_INTLOGARGMIN; 229 z[i] = (float) (SNACK_DB * log(wtemp) - SNACK_CORRN); 230 231 wtemp = wr; 232 wr = wr*wpr-wi*wpi+wr; 233 wi = wi*wpr+wtemp*wpi+wi; 234 } 235 wtemp = x[0]-y[0]; 236 wtemp = wtemp*wtemp; 237 if (wtemp < SNACK_INTLOGARGMIN) 238 wtemp = SNACK_INTLOGARGMIN; 239 z[0] = (float) (SNACK_DB * log(wtemp) - SNACK_CORR0); 240} 241 242/*--------------------------------------------------------------------* 243 * Function: r2tx * 244 * RADIX 2 ITERATION SUBROUTINE * 245 *--------------------------------------------------------------------*/ 246static void 247r2tx(int nthpo, float *cr0, float *cr1, float *ci0, float *ci1) 248{ 249 register int k; 250 register float r1, fi1; 251 252 for (k = 0; k < nthpo; k += 2) { 253 r1 = cr0[k] + cr1[k]; 254 cr1[k] = cr0[k] - cr1[k]; 255 cr0[k] = r1; 256 fi1 = ci0[k] + ci1[k]; 257 ci1[k] = ci0[k] - ci1[k]; 258 ci0[k] = fi1; 259 } 260} 261 262/*--------------------------------------------------------------------* 263 * Function: r4tx * 264 * RADIX 4 ITERATION SUBROUTINE * 265 *--------------------------------------------------------------------*/ 266static void 267r4tx(int nthpo, float *cr0, float *cr1, float *cr2, float *cr3, float *ci0, float *ci1, float *ci2, float *ci3) 268{ 269 register int k; 270 register float r1, r2, r3, r4; 271 register float fi1, fi2, fi3, fi4; 272 273 for (k = 0; k < nthpo; k += 4) { 274 r1 = cr0[k] + cr2[k]; 275 r2 = cr0[k] - cr2[k]; 276 r3 = cr1[k] + cr3[k]; 277 r4 = cr1[k] - cr3[k]; 278 fi1 = ci0[k] + ci2[k]; 279 fi2 = ci0[k] - ci2[k]; 280 fi3 = ci1[k] + ci3[k]; 281 fi4 = ci1[k] - ci3[k]; 282 cr0[k] = r1 + r3; 283 ci0[k] = fi1 + fi3; 284 cr1[k] = r1 - r3; 285 ci1[k] = fi1 - fi3; 286 cr2[k] = r2 - fi4; 287 ci2[k] = fi2 + r4; 288 cr3[k] = r2 + fi4; 289 ci3[k] = fi2 - r4; 290 } 291} 292 293/*--------------------------------------------------------------------* 294 * Function: r8tx * 295 * RADIX 8 ITERATION SUBROUTINE * 296 *--------------------------------------------------------------------*/ 297static void 298r8tx(int nxtlt, int nthpo, int lengt, 299 float *cr0, float *cr1, float *cr2, float *cr3, 300 float *cr4, float *cr5, float *cr6, float *cr7, 301 float *ci0, float *ci1, float *ci2, float *ci3, 302 float *ci4, float *ci5, float *ci6, float *ci7) 303{ 304 float c1, c2, c3, c4, c5, c6, c7; 305 float s1, s2, s3, s4, s5, s6, s7; 306 float ar0, ar1, ar2, ar3, ar4, ar5, ar6, ar7; 307 float ai0, ai1, ai2, ai3, ai4, ai5, ai6, ai7; 308 float br0, br1, br2, br3, br4, br5, br6, br7; 309 float bi0, bi1, bi2, bi3, bi4, bi5, bi6, bi7; 310 register float tr, ti; 311 register int j, k; 312 313 for (j = 0; j < nxtlt; j++) { 314 c1 = cost[(j * nthpo) >> lengt]; 315 s1 = sint[(j * nthpo) >> lengt]; 316 c2 = c1 * c1 - s1 * s1; 317 s2 = c1 * s1 + c1 * s1; 318 c3 = c1 * c2 - s1 * s2; 319 s3 = c2 * s1 + s2 * c1; 320 c4 = c2 * c2 - s2 * s2; 321 s4 = c2 * s2 + c2 * s2; 322 c5 = c2 * c3 - s2 * s3; 323 s5 = c3 * s2 + s3 * c2; 324 c6 = c3 * c3 - s3 * s3; 325 s6 = c3 * s3 + c3 * s3; 326 c7 = c3 * c4 - s3 * s4; 327 s7 = c4 * s3 + s4 * c3; 328 for (k = j; k < nthpo; k += Pow2[lengt]) { 329 ar0 = cr0[k] + cr4[k]; 330 ar1 = cr1[k] + cr5[k]; 331 ar2 = cr2[k] + cr6[k]; 332 ar3 = cr3[k] + cr7[k]; 333 ar4 = cr0[k] - cr4[k]; 334 ar5 = cr1[k] - cr5[k]; 335 ar6 = cr2[k] - cr6[k]; 336 ar7 = cr3[k] - cr7[k]; 337 ai0 = ci0[k] + ci4[k]; 338 ai1 = ci1[k] + ci5[k]; 339 ai2 = ci2[k] + ci6[k]; 340 ai3 = ci3[k] + ci7[k]; 341 ai4 = ci0[k] - ci4[k]; 342 ai5 = ci1[k] - ci5[k]; 343 ai6 = ci2[k] - ci6[k]; 344 ai7 = ci3[k] - ci7[k]; 345 br0 = ar0 + ar2; 346 br1 = ar1 + ar3; 347 br2 = ar0 - ar2; 348 br3 = ar1 - ar3; 349 br4 = ar4 - ai6; 350 br5 = ar5 - ai7; 351 br6 = ar4 + ai6; 352 br7 = ar5 + ai7; 353 bi0 = ai0 + ai2; 354 bi1 = ai1 + ai3; 355 bi2 = ai0 - ai2; 356 bi3 = ai1 - ai3; 357 bi4 = ai4 + ar6; 358 bi5 = ai5 + ar7; 359 bi6 = ai4 - ar6; 360 bi7 = ai5 - ar7; 361 cr0[k] = br0 + br1; 362 ci0[k] = bi0 + bi1; 363 if (j > 0) { 364 cr1[k] = c4 * (br0 - br1) - s4 * (bi0 - bi1); 365 ci1[k] = c4 * (bi0 - bi1) + s4 * (br0 - br1); 366 cr2[k] = c2 * (br2 - bi3) - s2 * (bi2 + br3); 367 ci2[k] = c2 * (bi2 + br3) + s2 * (br2 - bi3); 368 cr3[k] = c6 * (br2 + bi3) - s6 * (bi2 - br3); 369 ci3[k] = c6 * (bi2 - br3) + s6 * (br2 + bi3); 370 tr = (float) (P7 * (br5 - bi5)); 371 ti = (float) (P7 * (br5 + bi5)); 372 cr4[k] = c1 * (br4 + tr) - s1 * (bi4 + ti); 373 ci4[k] = c1 * (bi4 + ti) + s1 * (br4 + tr); 374 cr5[k] = c5 * (br4 - tr) - s5 * (bi4 - ti); 375 ci5[k] = c5 * (bi4 - ti) + s5 * (br4 - tr); 376 tr = (float) (-P7 * (br7 + bi7)); 377 ti = (float) (P7 * (br7 - bi7)); 378 cr6[k] = c3 * (br6 + tr) - s3 * (bi6 + ti); 379 ci6[k] = c3 * (bi6 + ti) + s3 * (br6 + tr); 380 cr7[k] = c7 * (br6 - tr) - s7 * (bi6 - ti); 381 ci7[k] = c7 * (bi6 - ti) + s7 * (br6 - tr); 382 } else { 383 cr1[k] = br0 - br1; 384 ci1[k] = bi0 - bi1; 385 cr2[k] = br2 - bi3; 386 ci2[k] = bi2 + br3; 387 cr3[k] = br2 + bi3; 388 ci3[k] = bi2 - br3; 389 tr = (float) (P7 * (br5 - bi5)); 390 ti = (float) (P7 * (br5 + bi5)); 391 cr4[k] = br4 + tr; 392 ci4[k] = bi4 + ti; 393 cr5[k] = br4 - tr; 394 ci5[k] = bi4 - ti; 395 tr = (float) (-P7 * (br7 + bi7)); 396 ti = (float) (P7 * (br7 - bi7)); 397 cr6[k] = br6 + tr; 398 ci6[k] = bi6 + ti; 399 cr7[k] = br6 - tr; 400 ci7[k] = bi6 - ti; 401 } 402 } 403 } 404} 405 406void 407Snack_PowerSpectrum(float *z) 408{ 409 int l[17]; 410 int i, in, n8pow, fn; 411 int lengt; 412 register int ij, ji, nxtlt; 413 int i1, i2, i3, i4, i5, i6, i7, i8, i9, i10, i11, i12, i13, i14; 414 float r, fi; 415 float h1r, h1i, h2r, h2i; 416 double wr, wi, wtemp; 417 418 fn = nthpo; 419 for (i = 0; i < nthpo; i++) { 420 y[i] = -z[(i<<1)+1]; 421 x[i] = z[i<<1]; 422 } 423 424 n8pow = n2pow / 3; 425 if (n8pow != 0) { 426 for (i = 0; i < n8pow; i++) { 427 lengt = n2pow - 3 * i; 428 nxtlt = Pow2[lengt - 3]; 429 r8tx (nxtlt, nthpo, lengt, 430 &x[0], &x[nxtlt], &x[2 * nxtlt], &x[3 * nxtlt], 431 &x[4 * nxtlt], &x[5 * nxtlt], &x[6 * nxtlt], &x[7 * nxtlt], 432 &y[0], &y[nxtlt], &y[2 * nxtlt], &y[3 * nxtlt], 433 &y[4 * nxtlt], &y[5 * nxtlt], &y[6 * nxtlt], &y[7 * nxtlt]); 434 } 435 } 436 437 switch (n2pow - 3 * n8pow) { 438 case 0: 439 break; 440 case 1: 441 r2tx (nthpo, &x[0], &x[1], &y[0], &y[1]); 442 break; 443 case 2: 444 r4tx (nthpo, &x[0], &x[1], &x[2], &x[3], 445 &y[0], &y[1], &y[2], &y[3]); 446 break; 447 default: 448 /* fprintf (stderr, "-- Algorithm Error Snack_DBPowerSpectrum\n");*/ 449 exit (1); 450 } 451 for (i = 0; i < 17; i++) { 452 if (i < n2pow) { 453 l[i] = Pow2[n2pow - i]; 454 } else { 455 l[i] = 1; 456 } 457 } 458 ij = 0; 459 for (i1 = 0; i1 < l[14]; i1++) { 460 for (i2 = i1; i2 < l[13]; i2 += l[14]) { 461 for (i3 = i2; i3 < l[12]; i3 += l[13]) { 462 for (i4 = i3; i4 < l[11]; i4 += l[12]) { 463 for (i5 = i4; i5 < l[10]; i5 += l[11]) { 464 for (i6 = i5; i6 < l[9]; i6 += l[10]) { 465 for (i7 = i6; i7 < l[8]; i7 += l[9]) { 466 for (i8 = i7; i8 < l[7]; i8 += l[8]) { 467 for (i9 = i8; i9 < l[6]; i9 += l[7]) { 468 for (i10 = i9; i10 < l[5]; i10 += l[6]) { 469 for (i11 = i10; i11 < l[4]; i11 += l[5]) { 470 for (i12 = i11; i12 < l[3]; i12 += l[4]) { 471 for (i13 = i12; i13 < l[2]; i13 += l[3]) { 472 for (i14 = i13; i14 < l[1]; i14 += l[2]) { 473 for (ji = i14; ji < l[0]; ji += l[1]) { 474 if (ij < ji) { 475 r = x[ij]; 476 x[ij] = x[ji]; 477 x[ji] = r; 478 fi = y[ij]; 479 y[ij] = y[ji]; 480 y[ji] = fi; 481 } 482 ij++; 483 } 484 } 485 } 486 } 487 } 488 } 489 } 490 } 491 } 492 } 493 } 494 } 495 } 496 } 497 } 498 499 wr = 1.0+wpr; 500 wi = wpi; 501 for (i = 1; i <= (nthpo>>1); i++) { 502 in = nthpo-i; 503 h1r = (x[i]+x[in]); 504 h1i = (y[i]-y[in]); 505 h2r = (y[i]+y[in]); 506 h2i = (x[in]-x[i]); 507 x[in] = (float) (h1r+wr*h2r-wi*h2i); 508 y[in] = (float) (h1i+wr*h2i+wi*h2r); 509 wtemp = x[in]*x[in]+y[in]*y[in]; 510 z[in] = (float) wtemp; 511 512 x[i] = (float) (h1r-wr*h2r+wi*h2i); 513 y[i] = (float) (-h1i+wr*h2i+wi*h2r); 514 wtemp = x[i]*x[i]+y[i]*y[i]; 515 z[i] = (float) wtemp; 516 517 wtemp = wr; 518 wr = wr*wpr-wi*wpi+wr; 519 wi = wi*wpr+wtemp*wpi+wi; 520 } 521 wtemp = x[0]-y[0]; 522 wtemp = wtemp*wtemp; 523 z[0] = (float) wtemp; 524} 525