fft.cpp

非常好用的用C编写的矩阵类,可在不同编译器下编译使用.

/// \ingroup newmat
///@{

/// \file fft.cpp
/// \brief Fast Fourier (Carl de Boor) and trig transforms.


// Copyright (C) 1991,2,3,4,8: R B Davies


#define WANT_MATH
// #define WANT_STREAM

#include "include.h"

#include "newmatap.h"

// #include "newmatio.h"

#ifdef use_namespace
namespace NEWMAT {
#endif

#ifdef DO_REPORT
#define REPORT { static ExeCounter ExeCount(__LINE__,19); ++ExeCount; }
#else
#define REPORT {}
#endif

static void cossin(int n, int d, Real& c, Real& s)
// calculate cos(twopi*n/d) and sin(twopi*n/d)
// minimise roundoff error
{
   REPORT
   long n4 = n * 4; int sector = (int)floor( (Real)n4 / (Real)d + 0.5 );
   n4 -= sector * d;
   if (sector < 0) { REPORT sector = 3 - (3 - sector) % 4; }
   else  { REPORT sector %= 4; }
   Real ratio = 1.5707963267948966192 * (Real)n4 / (Real)d;

   switch (sector)
   {
   case 0: REPORT c =  cos(ratio); s =  sin(ratio); break;
   case 1: REPORT c = -sin(ratio); s =  cos(ratio); break;
   case 2: REPORT c = -cos(ratio); s = -sin(ratio); break;
   case 3: REPORT c =  sin(ratio); s = -cos(ratio); break;
   }
}

static void fftstep(ColumnVector& A, ColumnVector& B, ColumnVector& X,
   ColumnVector& Y, int after, int now, int before)
{
   REPORT
   Tracer trace("FFT(step)");
   // const Real twopi = 6.2831853071795864769;
   const int gamma = after * before;  const int delta = now * after;
   // const Real angle = twopi / delta;  Real temp;
   // Real r_omega = cos(angle);  Real i_omega = -sin(angle);
   Real r_arg = 1.0;  Real i_arg = 0.0;
   Real* x = X.Store();  Real* y = Y.Store();   // pointers to array storage
   const int m = A.Nrows() - gamma;

   for (int j = 0; j < now; j++)
   {
      Real* a = A.Store(); Real* b = B.Store(); // pointers to array storage
      Real* x1 = x; Real* y1 = y; x += after; y += after;
      for (int ia = 0; ia < after; ia++)
      {
         // generate sins & cosines explicitly rather than iteratively
         // for more accuracy; but slower
         cossin(-(j*after+ia), delta, r_arg, i_arg);

         Real* a1 = a++; Real* b1 = b++; Real* x2 = x1++; Real* y2 = y1++;
         if (now==2)
         {
            REPORT int ib = before;
            if (ib) for (;;)
            {
               REPORT
               Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
               Real r_value = *a2; Real i_value = *b2;
               *x2 = r_value * r_arg - i_value * i_arg + *(a2-gamma);
               *y2 = r_value * i_arg + i_value * r_arg + *(b2-gamma);
               if (!(--ib)) break;
               x2 += delta; y2 += delta;
            }
         }
         else
         {
            REPORT int ib = before;
            if (ib) for (;;)
            {
               REPORT
               Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
               Real r_value = *a2; Real i_value = *b2;
               int in = now-1; while (in--)
               {
                  // it should be possible to make this faster
                  // hand code for now = 2,3,4,5,8
                  // use symmetry to halve number of operations
                  a2 -= gamma; b2 -= gamma;  Real temp = r_value;
                  r_value = r_value * r_arg - i_value * i_arg + *a2;
                  i_value = temp    * i_arg + i_value * r_arg + *b2;
               }
               *x2 = r_value; *y2 = i_value;
               if (!(--ib)) break;
               x2 += delta; y2 += delta;
            }
         }

         // temp = r_arg;
         // r_arg = r_arg * r_omega - i_arg * i_omega;
         // i_arg = temp  * i_omega + i_arg * r_omega;

      }
   }
}


void FFTI(const ColumnVector& U, const ColumnVector& V,
   ColumnVector& X, ColumnVector& Y)
{
   // Inverse transform
   Tracer trace("FFTI");
   REPORT
   FFT(U,-V,X,Y);
   const Real n = X.Nrows(); X /= n; Y /= (-n);
}

void RealFFT(const ColumnVector& U, ColumnVector& X, ColumnVector& Y)
{
   // Fourier transform of a real series
   Tracer trace("RealFFT");
   REPORT
   const int n = U.Nrows();                     // length of arrays
   const int n2 = n / 2;
   if (n != 2 * n2)
      Throw(ProgramException("Vector length not multiple of 2", U));
   ColumnVector A(n2), B(n2);
   Real* a = A.Store(); Real* b = B.Store(); Real* u = U.Store(); int i = n2;
   while (i--) { *a++ = *u++; *b++ = *u++; }
   FFT(A,B,A,B);
   int n21 = n2 + 1;
   X.resize(n21); Y.resize(n21);
   i = n2 - 1;
   a = A.Store(); b = B.Store();              // first els of A and B
   Real* an = a + i; Real* bn = b + i;        // last els of A and B
   Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
   Real* xn = x + n2; Real* yn = y + n2;      // last els of X and Y

   *x++ = *a + *b; *y++ = 0.0;                // first complex element
   *xn-- = *a++ - *b++; *yn-- = 0.0;          // last complex element

   int j = -1; i = n2/2;
   while (i--)
   {
      Real c,s; cossin(j--,n,c,s);
      Real am = *a - *an; Real ap = *a++ + *an--;
      Real bm = *b - *bn; Real bp = *b++ + *bn--;
      Real samcbp = s * am + c * bp; Real sbpcam = s * bp - c * am;
      *x++  =  0.5 * ( ap + samcbp); *y++  =  0.5 * ( bm + sbpcam);
      *xn-- =  0.5 * ( ap - samcbp); *yn-- =  0.5 * (-bm + sbpcam);
   }
}

void RealFFTI(const ColumnVector& A, const ColumnVector& B, ColumnVector& U)
{
   // inverse of a Fourier transform of a real series
   Tracer trace("RealFFTI");
   REPORT
   const int n21 = A.Nrows();                     // length of arrays
   if (n21 != B.Nrows() || n21 == 0)
      Throw(ProgramException("Vector lengths unequal or zero", A, B));
   const int n2 = n21 - 1;  const int n = 2 * n2;  int i = n2 - 1;

   ColumnVector X(n2), Y(n2);
   Real* a = A.Store(); Real* b = B.Store();  // first els of A and B
   Real* an = a + n2;   Real* bn = b + n2;    // last els of A and B
   Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
   Real* xn = x + i;    Real* yn = y + i;     // last els of X and Y

   Real hn = 0.5 / n2;
   *x++  = hn * (*a + *an);  *y++  = - hn * (*a - *an);
   a++; an--; b++; bn--;
   int j = -1;  i = n2/2;
   while (i--)
   {
      Real c,s; cossin(j--,n,c,s);
      Real am = *a - *an; Real ap = *a++ + *an--;
      Real bm = *b - *bn; Real bp = *b++ + *bn--;
      Real samcbp = s * am - c * bp; Real sbpcam = s * bp + c * am;
      *x++  =  hn * ( ap + samcbp); *y++  =  - hn * ( bm + sbpcam);
      *xn-- =  hn * ( ap - samcbp); *yn-- =  - hn * (-bm + sbpcam);
   }
   FFT(X,Y,X,Y);             // have done inverting elsewhere
   U.resize(n); i = n2;
   x = X.Store(); y = Y.Store(); Real* u = U.Store();
   while (i--) { *u++ = *x++; *u++ = - *y++; }
}

void FFT(const ColumnVector& U, const ColumnVector& V,
   ColumnVector& X, ColumnVector& Y)
{
   // from Carl de Boor (1980), Siam J Sci Stat Comput, 1 173-8
   // but first try Sande and Gentleman
   Tracer trace("FFT");
   REPORT
   const int n = U.Nrows();                     // length of arrays
   if (n != V.Nrows() || n == 0)
      Throw(ProgramException("Vector lengths unequal or zero", U, V));
   if (n == 1) { REPORT X = U; Y = V; return; }

   // see if we can use the newfft routine
   if (!FFT_Controller::OnlyOldFFT && FFT_Controller::CanFactor(n))
   {
      REPORT
      X = U; Y = V;
      if ( FFT_Controller::ar_1d_ft(n,X.Store(),Y.Store()) ) return;
   }

   ColumnVector B = V;
   ColumnVector A = U;
   X.resize(n); Y.resize(n);
   const int nextmx = 8;
   int prime[8] = { 2,3,5,7,11,13,17,19 };
   int after = 1; int before = n; int next = 0; bool inzee = true;
   int now = 0; int b1;             // initialised to keep gnu happy

   do
   {
      for (;;)
      {
	 if (next < nextmx) { REPORT now = prime[next]; }
	 b1 = before / now;  if (b1 * now == before) { REPORT break; }
	 next++; now += 2;
      }
      before = b1;

      if (inzee) { REPORT fftstep(A, B, X, Y, after, now, before); }
      else { REPORT fftstep(X, Y, A, B, after, now, before); }