home *** CD-ROM | disk | FTP | other *** search
/ Phoenix Heaven Sunny 2 / APPARE2.BIN / oh_towns / tetujin / src.lzh / FFT.C next >
Text File  |  1995-01-05  |  4KB  |  193 lines
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <string.h>
  4. #include <math.h>
  5. #include "fft.h"
  6.  
  7. #define NOERR 0
  8.  
  9. static siftFlg = 1 ;    /* DC成分の位置  0 シフトしない  1 シフトする */
  10.  
  11. static double *sinTable ;
  12. static double *cosTable ;
  13. static double *subBuf ;
  14. static tableFlg = 1 ;    /* 0のときはテーブルを作成しない */
  15.  
  16. int fft_2( char *work, double real[], double imag[], int ex, int inv )
  17. {
  18.     int i, length ;
  19.  
  20.     length = 1 << ex ;
  21.  
  22.     makeTable( work, length, inv ) ;
  23.     tableFlg = 0 ;
  24.  
  25.     for( i=0 ; i<length ; i++ )
  26.     {
  27.         if( inv == -1 )
  28.             sift( &real[i*length], &imag[i*length], length ) ;
  29.         fft_1( work, &real[i*length], &imag[i*length], ex, inv ) ;
  30.         if( inv == 1 )
  31.             sift( &real[i*length], &imag[i*length], length ) ;
  32.     }
  33.     matrixReverce( real, length ) ;
  34.     matrixReverce( imag, length ) ;
  35.     for( i=0 ; i<length ; i++ )
  36.     {
  37.         if( inv == -1 )
  38.             sift( &real[i*length], &imag[i*length], length ) ;
  39.         fft_1( work, &real[i*length], &imag[i*length], ex, inv ) ;
  40.         if( inv == 1 )
  41.             sift( &real[i*length], &imag[i*length], length ) ;
  42.     }
  43.     matrixReverce( real, length ) ;
  44.     matrixReverce( imag, length ) ;
  45.  
  46.     tableFlg = 1 ;
  47.     return NOERR ;
  48. }
  49.  
  50. static int sift( double real[], double imag[], int length )
  51. {
  52.     int i ;
  53.     double temp ;
  54.  
  55.     if( siftFlg == 0 )return NOERR ;
  56.  
  57.     for( i=0 ; i<(length >> 1) ; i++ )
  58.     {
  59.         temp = real[i] ;
  60.         real[i] = real[ i + (length >> 1) ] ;
  61.         real[ i + (length >> 1) ] = temp ;
  62.  
  63.         temp = imag[i] ;
  64.         imag[i] = imag[ i + (length >> 1) ] ;
  65.         imag[ i + (length >> 1) ] = temp ;
  66.     }
  67.  
  68.     return NOERR ;
  69. }
  70.  
  71. static int matrixReverce( double a[], int length )
  72. {
  73.     int i, j ;
  74.     double temp ;
  75.  
  76.     for( i=0 ; i<length ; i++ )
  77.     {
  78.         for( j=0 ; j<length ; j++ )
  79.         {
  80.             if( i < j )
  81.             {
  82.                 temp = a[ j + i*length ] ;
  83.                 a[ j + i*length ] = a[ i + j*length ] ;
  84.                 a[ i + j*length ] = temp ;
  85.             }
  86.         }
  87.     }
  88.  
  89.     return NOERR ;
  90. }
  91.  
  92. int fft_1( char *work, double real[], double imag[], int ex, int inv )
  93. {
  94.     int i, j, k, w, j1, j2 ;
  95.     int length, length2, count, temp ;
  96.     double ar, ai, br, bi, nrml ;
  97.  
  98.     length = 1 << ex ;
  99.  
  100.     if( tableFlg )makeTable( work, length, inv ) ;
  101.  
  102.     count = 1 ;
  103.     length2 = length ;
  104.     for( i=0 ; i<ex ; i++ )
  105.     {
  106.         length2 >>= 1 ;
  107.         temp = 0 ;
  108.         for( j=0 ; j<count ; j++ )
  109.         {
  110.             w = 0 ;
  111.             for( k=0 ; k<length2 ; k++ )
  112.             {
  113.                 j1 = temp + k ;
  114.                 j2 = j1 + length2 ;
  115.                 ar = real[j1] ;
  116.                 ai = imag[j1] ;
  117.                 br = real[j2] ;
  118.                 bi = imag[j2] ;
  119.                 real[j1] = ar + br ;
  120.                 imag[j1] = ai + bi ;
  121.                 ar = ar - br ;
  122.                 ai = ai - bi ;
  123.                 real[j2] = ar*cosTable[w] - ai*sinTable[w] ;
  124.                 imag[j2] = ar*sinTable[w] + ai*cosTable[w] ;
  125.                 w += count ;
  126.             }
  127.             temp += (2*length2) ;
  128.         }
  129.         count <<= 1 ;
  130.     }
  131.     bitReverce( subBuf, real, ex ) ;
  132.     bitReverce( subBuf, imag, ex ) ;
  133.     nrml = 1.0 / sqrt( (double)length ) ;
  134.     for( i=0 ; i<length ; i++ )
  135.     {
  136.         real[i] *= nrml ;
  137.         imag[i] *= nrml ;
  138.     }
  139.  
  140.     return NOERR ;
  141. }
  142.  
  143. static int makeTable( char *work, int length, int inv )
  144. {
  145.     int i ;
  146.  
  147.     sinTable = (double *)work ;
  148.     cosTable = sinTable + length ;
  149.     subBuf   = cosTable + length ;
  150.  
  151.     if( inv == 1 )
  152.     {
  153.         for( i=0 ; i<length ; i++ )
  154.         {
  155.             sinTable[i] = sin( -_PI * 2 * i / length ) ;
  156.             cosTable[i] = cos( -_PI * 2 * i / length ) ;
  157.         }
  158.     }
  159.     else
  160.     {
  161.         for( i=0 ; i<length ; i++ )
  162.         {
  163.             sinTable[i] = sin( _PI * 2 * i / length ) ;
  164.             cosTable[i] = cos( _PI * 2 * i / length ) ;
  165.         }
  166.     }
  167.  
  168.     return NOERR ;
  169. }
  170.  
  171. static bitReverce( double *buf, double a[], int ex )
  172. {
  173.     int i, j, bit, length ;
  174.  
  175.     length = 1 << ex ;
  176.     for( i=0 ; i<length ; i++ )
  177.     {
  178.         bit = 0 ;
  179.         for( j=0 ; j<ex ; j++ )
  180.         {
  181.             if( i & (1 << j) )
  182.             {
  183.                 bit |= (1 << (ex - j - 1)) ;
  184.             }
  185.         }
  186.         buf[i] = a[bit] ;
  187.     }
  188.     for( i=0 ; i<length ; i++ )a[i] = buf[i] ;
  189.  
  190.     return NOERR ;
  191. }
  192.  
  193.