Diff: SnSigProcUtils.h

Revision:

84:80b15993944e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SnSigProcUtils.h	Fri Oct 30 04:49:40 2015 +0000
@@ -0,0 +1,348 @@
+#ifndef SN_SnSigProcUtils
+#define SN_SnSigProcUtils
+
+#include <cmath>
+#include <stdint.h>
+
+#include "SnConstants.h"
+#include "SnSigProcDataTable.h"
+
+struct SnSigProcUtils {
+    
+    static const double kPi;
+    static const double kTwoPi;
+        
+    
+    template<typename Num_t, typename Res_t>
+    static
+    void GetArraySumAndMaxSingleLoop(const Num_t* const data, const uint32_t n,
+                                     Res_t& sum, Res_t& max) {
+        const Num_t* d = data;
+        max = static_cast<Res_t>(*d);
+        sum = static_cast<Res_t>(*d);
+        ++d; // skip past element 0
+        for (uint32_t i=1; i<n; ++i, ++d) {
+            sum += static_cast<Res_t>(*d);
+            if (*d > max) {
+                max = static_cast<Res_t>(*d);
+            }
+        }
+    }
+    
+    template<typename Dst_t, typename Src_t>
+    static
+    void MemCpyOrLoopCpy(Dst_t* const dst, const Src_t* const src,
+                         const uint32_t n) {
+#ifdef DEBUG_
+        printf("MemCpyOrLoopCpy : loop\r\n");
+#endif
+        Dst_t* d = dst;
+        const Src_t* s = src;
+        for (uint32_t i=0; i<n; ++i, ++d, ++s) {
+            *d = static_cast<Dst_t>(*s);
+        }
+    }
+    
+
+    template<typename Same_t>
+    static
+    void MemCpyOrLoopCpy(Same_t* const dst, const Same_t* const src,
+                         const uint32_t n) {
+        // can only memcpy if the types are the same
+#ifdef DEBUG_
+        printf("MemCpyOrLoopCpy : memcpy\r\n");
+#endif
+        memcpy(dst, src, sizeof(Same_t)*n);
+    }
+    
+    // the number of samples is a template parameter rather than
+    // an input parameter so that we can be sure to use the proper
+    // lookup tables, and that we should be somewhat safe against
+    // using the wrong tables if the number of samples changes in the
+    // future, as the code should no longer compile.
+    template<uint32_t nn, typename Num_t>
+    static
+    bool DiscreteCpxFFT(Num_t* const data,
+                        const bool inverse) {
+        // replace the data in the 'data' array by its discrete, complex-valued FFT
+        // 
+        // if inverse is true, will do the inverse FFT
+        // (unlike the function provided in NumRecC, the inverse FFT will
+        // be provided already be normalized)
+        // so calling DiscreteCpxFFT inverse on the result of
+        // DiscreteCpxFFT forward will yield the original data (within
+        // numerical accuracy)
+        //
+        // See RealFFT for notes on normalization. However, unlike RealFFT,
+        // the factors of 2 and special bin treatments are not necessary
+        // for the complex FFT, since both positive and negative
+        // frequencies are stored in their own bins.
+        // 
+        // NOTE: "data" must be of length 2*nn!! since the calculation is done
+        // in place. the time domain data should be in the first half of "data".
+        // nn must be a power of two
+        // the fft is organized like: [ real_0, img_0, real_1, img_1, ... ]
+        //
+        // NUM REC C - 12.2, pg 507-508
+        //
+        // return true iff FFT succeeded
+        
+        if ( ((nn-1)&nn)!=0 ) {
+#ifdef DEBUG
+            printf("<DiscreteCpxFFT>: nn (=%u) must be a power of 2.",nn);
+            return false;
+#endif
+        } else {
+        
+          static const uint32_t n = nn<<1;  // nn * 2
+          Num_t tempr,tempi;
+          uint32_t m;
+          uint32_t i,j=1;
+          Num_t* di=data, * di1=data+1, * dj, * dj1;
+#ifdef USE_DFFT_LUTS
+          const typename SnSigProcDataTable<n>::DfftDivideIndicies_t* kdi = 
+            SnSigProcDataTable<n>::kDfftDivideIndicies;
+#endif
+          for (i=1; i<n; i+=2, di+=2, di1+=2) {
+             if (j > i) {
+                dj    = data + j - 1;
+                dj1   = dj + 1;
+                tempr = *dj;  *dj  = *di;  *di  = tempr;
+                tempr = *dj1; *dj1 = *di1; *di1 = tempr;
+             }
+#ifdef USE_DFFT_LUTS
+             j = *kdi++;
+             m = *kdi++;
+#else
+             m = nn;
+             while (m>1 && j>m) {
+                j -= m;
+                m >>= 1;
+             }
+             j += m;
+#endif
+          }
+
+#ifdef USE_DFFT_LUTS
+          double wr, wi;
+          const typename SnSigProcDataTable<n>::DfftTwidleFactor_t* ktf = 
+            SnSigProcDataTable<n>::kDfftTwidleFactors;          
+#else
+          double wtemp, wr, wpr, wpi, wi, theta;
+          const int8_t isign = inverse ? -1 : 1;
+#endif
+          uint32_t mmax=2, istep;
+          while (n > mmax) {
+             istep = mmax<<1;
+#ifndef USE_DFFT_LUTS // if not defined!
+             theta = isign * (kTwoPi / mmax);
+             wtemp = std::sin(0.5*theta);
+             wpr   = -2.0 * wtemp * wtemp;
+             wpi   = std::sin(theta);
+             wr    = 1.0;
+             wi    = 0.0;
+#endif
+             for (m=1; m<mmax; m+=2) {
+#ifdef USE_DFFT_LUTS
+                wr = *ktf++;
+                wi = *ktf++;
+                if (inverse) {
+                    wi = -wi;
+                }
+#endif
+                for (i=m; i<=n; i+=istep) {
+                   j     = i + mmax;
+                   dj    = data + j - 1;
+                   dj1   = dj+1;
+                   di    = data + i - 1;
+                   di1   = di+1;
+                   tempr = wr * (*dj)  - wi*(*dj1);
+                   tempi = wr * (*dj1) + wi*(*dj);
+                   *dj   = *di  - tempr;
+                   *dj1  = *di1 - tempi;
+                   *di  += tempr;
+                   *di1 += tempi;
+                }
+#ifndef USE_DFFT_LUTS // if not defined!
+                wtemp = wr;
+                wr = wtemp*wpr - wi*wpi    + wr;
+                wi = wi*wpr    + wtemp*wpi + wi;
+#endif
+             }
+             mmax = istep;
+          }
+          if (inverse) {
+             // scale amplitude back
+             di = data;
+             static const Num_t tn = 2.0/static_cast<Num_t>(n);
+             for (i=0; i<n; ++i, ++di) {
+                *di *= tn;
+             }
+          }
+        }
+        
+        return true;
+    }
+
+    // the number of samples is a template parameter rather than
+    // an input parameter so that we can be sure to use the proper
+    // lookup tables, and that we should be somewhat safe against
+    // using the wrong tables if the number of samples changes in the
+    // future, as the code should no longer compile.
+    template<uint32_t n, typename Num_t>
+    static
+    bool RealFFT(Num_t* const data,
+                 const bool inverse) {
+        // replace the real-valued data with the positive frequency half of its
+        // complex fourier transform
+        //
+        // the real valued first & last components of the complex transform
+        // are stored in data[0] and data[1], respectively
+        //
+        // n must be a power of 2
+        //
+        // data goes from [0..n-1]
+        // 
+        // Note on the normalization convention used:
+        // The 2/n normalization is applied only on the inverse FFT.
+        // * To normalize the *amplitude* of frequencies in the forward
+        //   FFT, multiply by 2/n in bins [2..n-1] (which store two both
+        //   the pos and neg frequencies) and 1/n in bins [0..1]. For
+        //   example, if you have in the time domain (200*sine_50MHz +
+        //   300*sine_230MHz) and you want to see a spike at 50MHz with
+        //   amplitude 200 and a spike at 230MHz with amplitude 300, you
+        //   should multiply bins 0 and 1 by 1/n and the other bins by 2/n.
+        // * To normalize the *total power* of the forward FFT, multiply
+        //   by 2/sqrt(n) for bins [2..n-1] (which store two both
+        //   the pos and neg frequencies) and 1/sqrt(n) in bins [0..1].
+        //   This is the normalization chosen by NewGraphFromRealFFT when its
+        //   "normalize" parameter is true.
+        //
+        // if inverse is true, will perform the inverse FFT
+        // (unlike the function provided in NumRecC, the inverse FFT will
+        // be provided already be mulitplied by 2/n) -- so taking the inverse
+        // FFT of the result of the forward FFT will yield directly the original
+        // data (within numerical accuracy).
+        //
+        // NUM REC C - 12.3, pg 513-514
+        //
+        // return true iff the FFT succeeded
+        
+        if ( ((n-1)&n)!=0 ) {
+#ifdef DEBUG
+            printf("<RealFFT>: n (=%u) must be a power of 2.",n);
+#endif
+            return false;
+        } else {
+       
+            double theta = kPi / static_cast<double>(n>>1);
+            float c1=0.5, c2;
+            if (inverse) {
+                c2 = 0.5;
+                theta = -theta;     // set up for inverse tranform
+            } else {
+                c2 = -0.5;
+                DiscreteCpxFFT<(n/2)>(data,false); // forward FFT
+            }
+        
+#ifdef USE_DFFT_LUTS
+            typename SnSigProcDataTable<n>::RealDfftTwidleFactor_t
+                wr, wi; 
+            const typename SnSigProcDataTable<n>::RealDfftTwidleFactor_t
+                * krtf = SnSigProcDataTable<n>::kRealDfftTwidleFactor;
+#else
+            double wtemp     = std::sin(0.5*theta);
+            const double wpr = -2.0*wtemp*wtemp;
+            const double wpi = std::sin(theta);
+            double wr        = 1.0+wpr;
+            double wi        = wpi;
+#endif
+            float h1r, h1i, h2r, h2i;
+            const uint32_t n4  = n>>2; // n/4
+            uint32_t i;
+            Num_t* d1 = data+2,   * d2 = d1+1,
+                 * d3 = data+n-2, * d4 = d3+1;
+            for (i=1; i<n4; ++i, d1+=2, d2+=2, d3-=2, d4-=2) {
+#ifdef USE_DFFT_LUTS
+                wr = *krtf++;
+                wi = *krtf++;
+                if (inverse) {
+                    wi = -wi;
+                }
+#endif
+                h1r   =  c1 * ((*d1)+(*d3));
+                h1i   =  c1 * ((*d2)-(*d4));
+                h2r   = -c2 * ((*d2)+(*d4));
+                h2i   =  c2 * ((*d1)-(*d3));
+                *d1   =  h1r + wr*h2r - wi*h2i;
+                *d2   =  h1i + wr*h2i + wi*h2r;
+                *d3   =  h1r - wr*h2r + wi*h2i;
+                *d4   = -h1i + wr*h2i + wi*h2r;
+#ifndef USE_DFFT_LUTS // if not defined!
+                wtemp = wr;
+                wr    = wtemp*wpr - wi*wpi    + wr;
+                wi    = wi*wpr    + wtemp*wpi + wi;
+#endif
+            }
+           
+            // now fill in the beginning and end of the array
+            if (inverse) {
+                h1r   = *data;
+                d1    = data+1;
+                *data = c1*(h1r + (*d1));
+                *d1   = c1*(h1r - (*d1));
+                DiscreteCpxFFT<(n/2)>(data,inverse);       // inverse tranform
+            } else {
+                h1r   = *data;
+                d1    = data+1;
+                *data = h1r + (*d1);
+                *d1   = h1r - (*d1);
+            }
+        }
+        return true;
+    }
+
+    template<typename Num_t>
+    static
+    uint32_t ReplaceRfftWithSpectrum(Num_t* const rfft,
+                                     const uint32_t n,
+                                     const bool doSqrt) {
+        // rfft should contain the result of RealFFT and n is the number
+        // of samples.
+        // this routine will replace the first half of the array
+        // with the "spectrum" (frequency magnitude) at each freq bin.
+        // valid freq bins are from [0, (n/2)+1]
+        //
+        // return the number of freq bins filled, or else 0 on error
+        
+        if (rfft!=0) {
+            //const uint32_t hn = n>>1; // n/2
+            //uint32_t j=0;
+            // set the first freq bin
+            rfft[0] = (doSqrt) ? std::abs(rfft[0])
+                : (rfft[0]*rfft[0]);
+            // store the last freq bin
+            const Num_t hibin = 
+                (doSqrt) ? std::abs(rfft[1])
+                         : (rfft[1]*rfft[1]);
+            // loop over other freqs
+            const Num_t* fe=rfft+2; // evens
+            const Num_t* fo=fe+1;   // odds
+            Num_t* fbin=rfft+1;     // current freq bin
+            for (uint32_t i=2; i<n; i+=2, fe+=2, fo+=2, ++fbin) {
+                *fbin = ( ((*fe)*(*fe)) + ((*fo)*(*fo)) );
+                if (doSqrt) {
+                    *fbin = std::sqrt(static_cast<float>(*fbin));
+                }
+            }
+            // set the last freq bin
+            *fbin = hibin;
+
+            return (fbin-rfft+1);
+        }
+        return 0;
+    }
+    
+};
+
+#endif // SN_SnSigProcUtils