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


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32