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

newmat11b
Generated Mon May 9 04:54:17 2016 by Doxygen 1.6.3