Homepage Demos Overview Downloads Tutorials Reference
Credits

fft.cpp

Go to the documentation of this file.
00001 //$$ fft.cpp                         Fast fourier transform
00002 
00003 // Copyright (C) 1991,2,3,4,8: R B Davies
00004 
00005 
00006 #define WANT_MATH
00007 // #define WANT_STREAM
00008 
00009 #include "include.h"
00010 
00011 #include "newmatap.h"
00012 
00013 // #include "newmatio.h"
00014 
00015 #ifdef use_namespace
00016 namespace NEWMAT {
00017 #endif
00018 
00019 #ifdef DO_REPORT
00020 #define REPORT { static ExeCounter ExeCount(__LINE__,19); ++ExeCount; }
00021 #else
00022 #define REPORT {}
00023 #endif
00024 
00025 static void cossin(int n, int d, Real& c, Real& s)
00026 // calculate cos(twopi*n/d) and sin(twopi*n/d)
00027 // minimise roundoff error
00028 {
00029    REPORT
00030    long n4 = n * 4; int sector = (int)floor( (Real)n4 / (Real)d + 0.5 );
00031    n4 -= sector * d;
00032    if (sector < 0) { REPORT sector = 3 - (3 - sector) % 4; }
00033    else  { REPORT sector %= 4; }
00034    Real ratio = 1.5707963267948966192 * (Real)n4 / (Real)d;
00035 
00036    switch (sector)
00037    {
00038    case 0: REPORT c =  cos(ratio); s =  sin(ratio); break;
00039    case 1: REPORT c = -sin(ratio); s =  cos(ratio); break;
00040    case 2: REPORT c = -cos(ratio); s = -sin(ratio); break;
00041    case 3: REPORT c =  sin(ratio); s = -cos(ratio); break;
00042    }
00043 }
00044 
00045 static void fftstep(ColumnVector& A, ColumnVector& B, ColumnVector& X,
00046    ColumnVector& Y, int after, int now, int before)
00047 {
00048    REPORT
00049    Tracer trace("FFT(step)");
00050    // const Real twopi = 6.2831853071795864769;
00051    const int gamma = after * before;  const int delta = now * after;
00052    // const Real angle = twopi / delta;  Real temp;
00053    // Real r_omega = cos(angle);  Real i_omega = -sin(angle);
00054    Real r_arg = 1.0;  Real i_arg = 0.0;
00055    Real* x = X.Store();  Real* y = Y.Store();   // pointers to array storage
00056    const int m = A.Nrows() - gamma;
00057 
00058    for (int j = 0; j < now; j++)
00059    {
00060       Real* a = A.Store(); Real* b = B.Store(); // pointers to array storage
00061       Real* x1 = x; Real* y1 = y; x += after; y += after;
00062       for (int ia = 0; ia < after; ia++)
00063       {
00064          // generate sins & cosines explicitly rather than iteratively
00065          // for more accuracy; but slower
00066          cossin(-(j*after+ia), delta, r_arg, i_arg);
00067 
00068          Real* a1 = a++; Real* b1 = b++; Real* x2 = x1++; Real* y2 = y1++;
00069          if (now==2)
00070          {
00071             REPORT int ib = before;
00072             if (ib) for (;;)
00073             {
00074                REPORT
00075                Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
00076                Real r_value = *a2; Real i_value = *b2;
00077                *x2 = r_value * r_arg - i_value * i_arg + *(a2-gamma);
00078                *y2 = r_value * i_arg + i_value * r_arg + *(b2-gamma);
00079                if (!(--ib)) break;
00080                x2 += delta; y2 += delta;
00081             }
00082          }
00083          else
00084          {
00085             REPORT int ib = before;
00086             if (ib) for (;;)
00087             {
00088                REPORT
00089                Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
00090                Real r_value = *a2; Real i_value = *b2;
00091                int in = now-1; while (in--)
00092                {
00093                   // it should be possible to make this faster
00094                   // hand code for now = 2,3,4,5,8
00095                   // use symmetry to halve number of operations
00096                   a2 -= gamma; b2 -= gamma;  Real temp = r_value;
00097                   r_value = r_value * r_arg - i_value * i_arg + *a2;
00098                   i_value = temp    * i_arg + i_value * r_arg + *b2;
00099                }
00100                *x2 = r_value; *y2 = i_value;
00101                if (!(--ib)) break;
00102                x2 += delta; y2 += delta;
00103             }
00104          }
00105 
00106          // temp = r_arg;
00107          // r_arg = r_arg * r_omega - i_arg * i_omega;
00108          // i_arg = temp  * i_omega + i_arg * r_omega;
00109 
00110       }
00111    }
00112 }
00113 
00114 
00115 void FFTI(const ColumnVector& U, const ColumnVector& V,
00116    ColumnVector& X, ColumnVector& Y)
00117 {
00118    // Inverse transform
00119    Tracer trace("FFTI");
00120    REPORT
00121    FFT(U,-V,X,Y);
00122    const Real n = X.Nrows(); X /= n; Y /= (-n);
00123 }
00124 
00125 void RealFFT(const ColumnVector& U, ColumnVector& X, ColumnVector& Y)
00126 {
00127    // Fourier transform of a real series
00128    Tracer trace("RealFFT");
00129    REPORT
00130    const int n = U.Nrows();                     // length of arrays
00131    const int n2 = n / 2;
00132    if (n != 2 * n2)
00133       Throw(ProgramException("Vector length not multiple of 2", U));
00134    ColumnVector A(n2), B(n2);
00135    Real* a = A.Store(); Real* b = B.Store(); Real* u = U.Store(); int i = n2;
00136    while (i--) { *a++ = *u++; *b++ = *u++; }
00137    FFT(A,B,A,B);
00138    int n21 = n2 + 1;
00139    X.ReSize(n21); Y.ReSize(n21);
00140    i = n2 - 1;
00141    a = A.Store(); b = B.Store();              // first els of A and B
00142    Real* an = a + i; Real* bn = b + i;        // last els of A and B
00143    Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
00144    Real* xn = x + n2; Real* yn = y + n2;      // last els of X and Y
00145 
00146    *x++ = *a + *b; *y++ = 0.0;                // first complex element
00147    *xn-- = *a++ - *b++; *yn-- = 0.0;          // last complex element
00148 
00149    int j = -1; i = n2/2;
00150    while (i--)
00151    {
00152       Real c,s; cossin(j--,n,c,s);
00153       Real am = *a - *an; Real ap = *a++ + *an--;
00154       Real bm = *b - *bn; Real bp = *b++ + *bn--;
00155       Real samcbp = s * am + c * bp; Real sbpcam = s * bp - c * am;
00156       *x++  =  0.5 * ( ap + samcbp); *y++  =  0.5 * ( bm + sbpcam);
00157       *xn-- =  0.5 * ( ap - samcbp); *yn-- =  0.5 * (-bm + sbpcam);
00158    }
00159 }
00160 
00161 void RealFFTI(const ColumnVector& A, const ColumnVector& B, ColumnVector& U)
00162 {
00163    // inverse of a Fourier transform of a real series
00164    Tracer trace("RealFFTI");
00165    REPORT
00166    const int n21 = A.Nrows();                     // length of arrays
00167    if (n21 != B.Nrows() || n21 == 0)
00168       Throw(ProgramException("Vector lengths unequal or zero", A, B));
00169    const int n2 = n21 - 1;  const int n = 2 * n2;  int i = n2 - 1;
00170 
00171    ColumnVector X(n2), Y(n2);
00172    Real* a = A.Store(); Real* b = B.Store();  // first els of A and B
00173    Real* an = a + n2;   Real* bn = b + n2;    // last els of A and B
00174    Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
00175    Real* xn = x + i;    Real* yn = y + i;     // last els of X and Y
00176 
00177    Real hn = 0.5 / n2;
00178    *x++  = hn * (*a + *an);  *y++  = - hn * (*a - *an);
00179    a++; an--; b++; bn--;
00180    int j = -1;  i = n2/2;
00181    while (i--)
00182    {
00183       Real c,s; cossin(j--,n,c,s);
00184       Real am = *a - *an; Real ap = *a++ + *an--;
00185       Real bm = *b - *bn; Real bp = *b++ + *bn--;
00186       Real samcbp = s * am - c * bp; Real sbpcam = s * bp + c * am;
00187       *x++  =  hn * ( ap + samcbp); *y++  =  - hn * ( bm + sbpcam);
00188       *xn-- =  hn * ( ap - samcbp); *yn-- =  - hn * (-bm + sbpcam);
00189    }
00190    FFT(X,Y,X,Y);             // have done inverting elsewhere
00191    U.ReSize(n); i = n2;
00192    x = X.Store(); y = Y.Store(); Real* u = U.Store();
00193    while (i--) { *u++ = *x++; *u++ = - *y++; }
00194 }
00195 
00196 void FFT(const ColumnVector& U, const ColumnVector& V,
00197    ColumnVector& X, ColumnVector& Y)
00198 {
00199    // from Carl de Boor (1980), Siam J Sci Stat Comput, 1 173-8
00200    // but first try Sande and Gentleman
00201    Tracer trace("FFT");
00202    REPORT
00203    const int n = U.Nrows();                     // length of arrays
00204    if (n != V.Nrows() || n == 0)
00205       Throw(ProgramException("Vector lengths unequal or zero", U, V));
00206    if (n == 1) { REPORT X = U; Y = V; return; }
00207 
00208    // see if we can use the newfft routine
00209    if (!FFT_Controller::OnlyOldFFT && FFT_Controller::CanFactor(n))
00210    {
00211       REPORT
00212       X = U; Y = V;
00213       if ( FFT_Controller::ar_1d_ft(n,X.Store(),Y.Store()) ) return;
00214    }
00215 
00216    ColumnVector B = V;
00217    ColumnVector A = U;
00218    X.ReSize(n); Y.ReSize(n);
00219    const int nextmx = 8;
00220    int prime[8] = { 2,3,5,7,11,13,17,19 };
00221    int after = 1; int before = n; int next = 0; bool inzee = true;
00222    int now = 0; int b1;             // initialised to keep gnu happy
00223 
00224    do
00225    {
00226       for (;;)
00227       {
00228    if (next < nextmx) { REPORT now = prime[next]; }
00229    b1 = before / now;  if (b1 * now == before) { REPORT break; }
00230    next++; now += 2;
00231       }
00232       before = b1;
00233 
00234       if (inzee) { REPORT fftstep(A, B, X, Y, after, now, before); }
00235       else { REPORT fftstep(X, Y, A, B, after, now, before); }
00236 
00237       inzee = !inzee; after *= now;
00238    }
00239    while (before != 1);
00240 
00241    if (inzee) { REPORT A.Release(); X = A; B.Release(); Y = B; }
00242 }
00243 
00244 // Trigonometric transforms
00245 // see Charles Van Loan (1992) "Computational frameworks for the fast
00246 // Fourier transform" published by SIAM; section 4.4.
00247 
00248 void DCT_II(const ColumnVector& U, ColumnVector& V)
00249 {
00250    // Discrete cosine transform, type II, of a real series
00251    Tracer trace("DCT_II");
00252    REPORT
00253    const int n = U.Nrows();                     // length of arrays
00254    const int n2 = n / 2; const int n4 = n * 4;
00255    if (n != 2 * n2)
00256       Throw(ProgramException("Vector length not multiple of 2", U));
00257    ColumnVector A(n);
00258    Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00259    int i = n2;
00260    while (i--) { *a++ = *u++; *(--b) = *u++; }
00261    ColumnVector X, Y;
00262    RealFFT(A, X, Y); A.CleanUp();
00263    V.ReSize(n);
00264    Real* x = X.Store(); Real* y = Y.Store();
00265    Real* v = V.Store(); Real* w = v + n;
00266    *v = *x;
00267    int k = 0; i = n2;
00268    while (i--)
00269    {
00270       Real c, s; cossin(++k, n4, c, s);
00271       Real xi = *(++x); Real yi = *(++y);
00272       *(++v) = xi * c + yi * s; *(--w) = xi * s - yi * c;
00273    }
00274 }
00275 
00276 void DCT_II_inverse(const ColumnVector& V, ColumnVector& U)
00277 {
00278    // Inverse of discrete cosine transform, type II
00279    Tracer trace("DCT_II_inverse");
00280    REPORT
00281    const int n = V.Nrows();                     // length of array
00282    const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
00283    if (n != 2 * n2)
00284       Throw(ProgramException("Vector length not multiple of 2", V));
00285    ColumnVector X(n21), Y(n21);
00286    Real* x = X.Store(); Real* y = Y.Store();
00287    Real* v = V.Store(); Real* w = v + n;
00288    *x = *v; *y = 0.0;
00289    int i = n2; int k = 0;
00290    while (i--)
00291    {
00292       Real c, s; cossin(++k, n4, c, s);
00293       Real vi = *(++v); Real wi = *(--w);
00294       *(++x) = vi * c + wi * s; *(++y) = vi * s - wi * c;
00295    }
00296    ColumnVector A; RealFFTI(X, Y, A);
00297    X.CleanUp(); Y.CleanUp(); U.ReSize(n);
00298    Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00299    i = n2;
00300    while (i--) { *u++ = *a++; *u++ = *(--b); }
00301 }
00302 
00303 void DST_II(const ColumnVector& U, ColumnVector& V)
00304 {
00305    // Discrete sine transform, type II, of a real series
00306    Tracer trace("DST_II");
00307    REPORT
00308    const int n = U.Nrows();                     // length of arrays
00309    const int n2 = n / 2; const int n4 = n * 4;
00310    if (n != 2 * n2)
00311       Throw(ProgramException("Vector length not multiple of 2", U));
00312    ColumnVector A(n);
00313    Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00314    int i = n2;
00315    while (i--) { *a++ = *u++; *(--b) = -(*u++); }
00316    ColumnVector X, Y;
00317    RealFFT(A, X, Y); A.CleanUp();
00318    V.ReSize(n);
00319    Real* x = X.Store(); Real* y = Y.Store();
00320    Real* v = V.Store(); Real* w = v + n;
00321    *(--w) = *x;
00322    int k = 0; i = n2;
00323    while (i--)
00324    {
00325       Real c, s; cossin(++k, n4, c, s);
00326       Real xi = *(++x); Real yi = *(++y);
00327       *v++ = xi * s - yi * c; *(--w) = xi * c + yi * s;
00328    }
00329 }
00330 
00331 void DST_II_inverse(const ColumnVector& V, ColumnVector& U)
00332 {
00333    // Inverse of discrete sine transform, type II
00334    Tracer trace("DST_II_inverse");
00335    REPORT
00336    const int n = V.Nrows();                     // length of array
00337    const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
00338    if (n != 2 * n2)
00339       Throw(ProgramException("Vector length not multiple of 2", V));
00340    ColumnVector X(n21), Y(n21);
00341    Real* x = X.Store(); Real* y = Y.Store();
00342    Real* v = V.Store(); Real* w = v + n;
00343    *x = *(--w); *y = 0.0;
00344    int i = n2; int k = 0;
00345    while (i--)
00346    {
00347       Real c, s; cossin(++k, n4, c, s);
00348       Real vi = *v++; Real wi = *(--w);
00349       *(++x) = vi * s + wi * c; *(++y) = - vi * c + wi * s;
00350    }
00351    ColumnVector A; RealFFTI(X, Y, A);
00352    X.CleanUp(); Y.CleanUp(); U.ReSize(n);
00353    Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00354    i = n2;
00355    while (i--) { *u++ = *a++; *u++ = -(*(--b)); }
00356 }
00357 
00358 void DCT_inverse(const ColumnVector& V, ColumnVector& U)
00359 {
00360    // Inverse of discrete cosine transform, type I
00361    Tracer trace("DCT_inverse");
00362    REPORT
00363    const int n = V.Nrows()-1;                     // length of transform
00364    const int n2 = n / 2; const int n21 = n2 + 1;
00365    if (n != 2 * n2)
00366       Throw(ProgramException("Vector length not multiple of 2", V));
00367    ColumnVector X(n21), Y(n21);
00368    Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
00369    Real vi = *v++; *x++ = vi; *y++ = 0.0;
00370    Real sum1 = vi / 2.0; Real sum2 = sum1; vi = *v++;
00371    int i = n2-1;
00372    while (i--)
00373    {
00374       Real vi2 = *v++; sum1 += vi2 + vi; sum2 += vi2 - vi;
00375       *x++ = vi2; vi2 = *v++; *y++ = vi - vi2; vi = vi2;
00376    }
00377    sum1 += vi; sum2 -= vi;
00378    vi = *v; *x = vi; *y = 0.0; vi /= 2.0; sum1 += vi; sum2 += vi;
00379    ColumnVector A; RealFFTI(X, Y, A);
00380    X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
00381    Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
00382    i = n2; int k = 0; *u++ = sum1 / n2; *v-- = sum2 / n2;
00383    while (i--)
00384    {
00385       Real s = sin(1.5707963267948966192 * (++k) / n2);
00386       Real ai = *(++a); Real bi = *(--b);
00387       Real bz = (ai - bi) / 4 / s; Real az = (ai + bi) / 2;
00388       *u++ = az - bz; *v-- = az + bz;
00389    }
00390 }
00391 
00392 void DCT(const ColumnVector& U, ColumnVector& V)
00393 {
00394    // Discrete cosine transform, type I
00395    Tracer trace("DCT");
00396    REPORT
00397    DCT_inverse(U, V);
00398    V *= (V.Nrows()-1)/2;
00399 }
00400 
00401 void DST_inverse(const ColumnVector& V, ColumnVector& U)
00402 {
00403    // Inverse of discrete sine transform, type I
00404    Tracer trace("DST_inverse");
00405    REPORT
00406    const int n = V.Nrows()-1;                     // length of transform
00407    const int n2 = n / 2; const int n21 = n2 + 1;
00408    if (n != 2 * n2)
00409       Throw(ProgramException("Vector length not multiple of 2", V));
00410    ColumnVector X(n21), Y(n21);
00411    Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
00412    Real vi = *(++v); *x++ = 2 * vi; *y++ = 0.0;
00413    int i = n2-1;
00414    while (i--) { *y++ = *(++v); Real vi2 = *(++v); *x++ = vi2 - vi; vi = vi2; }
00415    *x = -2 * vi; *y = 0.0;
00416    ColumnVector A; RealFFTI(X, Y, A);
00417    X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
00418    Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
00419    i = n2; int k = 0; *u++ = 0.0; *v-- = 0.0;
00420    while (i--)
00421    {
00422       Real s = sin(1.5707963267948966192 * (++k) / n2);
00423       Real ai = *(++a); Real bi = *(--b);
00424       Real az = (ai + bi) / 4 / s; Real bz = (ai - bi) / 2;
00425       *u++ = az - bz; *v-- = az + bz;
00426    }
00427 }
00428 
00429 void DST(const ColumnVector& U, ColumnVector& V)
00430 {
00431    // Discrete sine transform, type I
00432    Tracer trace("DST");
00433    REPORT
00434    DST_inverse(U, V);
00435    V *= (V.Nrows()-1)/2;
00436 }
00437 
00438 // Two dimensional FFT
00439 void FFT2(const Matrix& U, const Matrix& V, Matrix& X, Matrix& Y)
00440 {
00441    Tracer trace("FFT2");
00442    REPORT
00443    int m = U.Nrows(); int n = U.Ncols();
00444    if (m != V.Nrows() || n != V.Ncols() || m == 0 || n == 0)
00445       Throw(ProgramException("Matrix dimensions unequal or zero", U, V));
00446    X = U; Y = V;
00447    int i; ColumnVector CVR; ColumnVector CVI;
00448    for (i = 1; i <= m; ++i)
00449    {
00450       FFT(X.Row(i).t(), Y.Row(i).t(), CVR, CVI);
00451       X.Row(i) = CVR.t(); Y.Row(i) = CVI.t();
00452    }
00453    for (i = 1; i <= n; ++i)
00454    {
00455       FFT(X.Column(i), Y.Column(i), CVR, CVI);
00456       X.Column(i) = CVR; Y.Column(i) = CVI;
00457    }
00458 }
00459 
00460 void FFT2I(const Matrix& U, const Matrix& V, Matrix& X, Matrix& Y)
00461 {
00462    // Inverse transform
00463    Tracer trace("FFT2I");
00464    REPORT
00465    FFT2(U,-V,X,Y);
00466    const Real n = X.Nrows() * X.Ncols(); X /= n; Y /= (-n);
00467 }
00468 
00469 
00470 #ifdef use_namespace
00471 }
00472 #endif
00473 
00474 

newmat11b
Generated Tue Nov 23 16:35:39 2004 by Doxygen 1.3.9.1