00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "fft.h"
00020
00021 using namespace Marsyas;
00022
00023
00024
00025
00026
00027
00028
00029
00030 void
00031 fft::bitreverse(mrs_real x[], int N )
00032 {
00033 mrs_real rtemp, itemp ;
00034 int i, j, m ;
00035 for ( i = j = 0 ; i < N ; i += 2, j += m ) {
00036 if ( j > i ) {
00037 rtemp = x[j] ; itemp = x[j+1] ;
00038 x[j] = x[i] ; x[j+1] = x[i+1] ;
00039 x[i] = rtemp ; x[i+1] = itemp ;
00040 }
00041 for ( m = N>>1 ; m >= 2 && j >= m ; m >>= 1 )
00042 j -= m ;
00043 }
00044 }
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 void
00059 fft::rfft( mrs_real x[], int N, int forward )
00060 {
00061 mrs_real c1, c2, h1r, h1i, h2r, h2i, wr, wi, wpr, wpi, temp, theta ;
00062 mrs_real xr, xi ;
00063 int i, i1, i2, i3, i4, N2p1 ;
00064 theta = PI/N ;
00065 wr = 1. ;
00066 wi = 0. ;
00067 c1 = 0.5 ;
00068 if ( forward )
00069 {
00070 c2 = -0.5 ;
00071 cfft( x, N, forward ) ;
00072 xr = x[0] ;
00073 xi = x[1] ;
00074 }
00075 else
00076 {
00077 c2 = 0.5 ;
00078 theta = -theta ;
00079 xr = x[1] ;
00080 xi = 0. ;
00081 x[1] = 0. ;
00082 }
00083 wpr = (mrs_real)(-2.*pow( sin( 0.5*theta ), 2. ));
00084 wpi = sin( theta ) ;
00085 N2p1 = (N<<1) + 1 ;
00086 for ( i = 0 ; i <= N>>1 ; ++i )
00087 {
00088 i1 = i<<1 ;
00089 i2 = i1 + 1 ;
00090 i3 = N2p1 - i2 ;
00091 i4 = i3 + 1 ;
00092 if ( i == 0 ) {
00093 h1r = c1*(x[i1] + xr ) ;
00094 h1i = c1*(x[i2] - xi ) ;
00095 h2r = -c2*(x[i2] + xi ) ;
00096 h2i = c2*(x[i1] - xr ) ;
00097 x[i1] = h1r + wr*h2r - wi*h2i ;
00098 x[i2] = h1i + wr*h2i + wi*h2r ;
00099 xr = h1r - wr*h2r + wi*h2i ;
00100 xi = -h1i + wr*h2i + wi*h2r ;
00101 }
00102 else {
00103 h1r = c1*(x[i1] + x[i3] ) ;
00104 h1i = c1*(x[i2] - x[i4] ) ;
00105 h2r = -c2*(x[i2] + x[i4] ) ;
00106 h2i = c2*(x[i1] - x[i3] ) ;
00107 x[i1] = h1r + wr*h2r - wi*h2i ;
00108 x[i2] = h1i + wr*h2i + wi*h2r ;
00109 x[i3] = h1r - wr*h2r + wi*h2i ;
00110 x[i4] = -h1i + wr*h2i + wi*h2r ;
00111 }
00112 wr = (temp = wr)*wpr - wi*wpi + wr ;
00113 wi = wi*wpr + temp*wpi + wi ;
00114 }
00115 if ( forward )
00116 x[1] = xr ;
00117 else
00118 cfft( x, N, forward ) ;
00119 }
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131 void
00132 fft::cfft(mrs_real x[], int NC, int forward )
00133 {
00134 mrs_real wr, wi, wpr, wpi, theta, scale ;
00135 int mmax, ND, m, i, j, delta ;
00136 ND = NC<<1 ;
00137 bitreverse( x, ND ) ;
00138 for ( mmax = 2 ; mmax < ND ; mmax = delta ) {
00139 delta = mmax<<1 ;
00140 theta = TWOPI/( forward? mmax : -mmax ) ;
00141 wpr = (mrs_real)(-2.*pow( sin( 0.5*theta ), 2. ));
00142 wpi = sin( theta ) ;
00143 wr = 1. ;
00144 wi = 0. ;
00145 for ( m = 0 ; m < mmax ; m += 2 ) {
00146 register mrs_real rtemp, itemp ;
00147 for ( i = m ; i < ND ; i += delta )
00148 {
00149 j = i + mmax ;
00150 rtemp = wr*x[j] - wi*x[j+1] ;
00151 itemp = wr*x[j+1] + wi*x[j] ;
00152 x[j] = x[i] - rtemp ;
00153 x[j+1] = x[i+1] - itemp ;
00154 x[i] += rtemp ;
00155 x[i+1] += itemp ;
00156 }
00157 wr = (rtemp = wr)*wpr - wi*wpi + wr ;
00158 wi = wi*wpr + rtemp*wpi + wi ;
00159 }
00160 }
00161
00162
00163
00164
00165 scale = forward ? (mrs_real)1.0/ND : (mrs_real)2.0 ;
00166 { register mrs_real *xi=x, *xe=x+ND ;
00167 while ( xi < xe )
00168 *xi++ *= scale ;
00169 }
00170
00171 }
00172
00173
00174
00175
00176
00177
00178
00179
00180