snark
fft.h
1 // This file is part of snark, a generic and flexible library
2 // for robotics research.
3 //
4 // Copyright (C) 2011 The University of Sydney
5 //
6 // snark is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // snark is distributed in the hope that it will be useful, but WITHOUT ANY
12 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
13 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
14 // for more details.
15 //
16 // You should have received a copy of the GNU Lesser General Public
17 // License along with snark. If not, see <http://www.gnu.org/licenses/>.
18 
19 #ifndef SNARK_MATH_FFT_H_
20 #define SNARK_MATH_FFT_H_
21 
22 #include <cmath>
23 #include <boost/array.hpp>
24 
25 namespace snark{
26 
33 template < typename T, std::size_t N >
34 void fft( boost::array< T, N >& data );
35 
37 template < typename T >
38 void fft( T* data, std::size_t size );
39 
41 template < typename T, std::size_t N >
42 boost::array< T, N > fft( const boost::array< T, N >& data );
43 
44 template < typename T, std::size_t N >
45 void fft( boost::array< T, N >& data )
46 {
47  fft( &data[0], N );
48 }
49 
50 template < typename T, std::size_t N >
51 inline boost::array< T, N > fft( const boost::array< T, N >& data )
52 {
53  boost::array< T, N > a = data;
54  fft( &a[0], N );
55  return a;
56 }
57 
58 template < typename T >
59 inline void fft( T* data, std::size_t size )
60 {
61  unsigned long n, mmax, m, j, istep, i;
62  double wtemp, wr, wpr, wpi, wi, theta;
63  double tempr, tempi;
64 
65  // reverse-binary reindexing
66  n = size << 1;
67  j = 1;
68  for( i = 1; i < n; i += 2 )
69  {
70  if( j > i )
71  {
72  std::swap( data[ j - 1 ], data[ i - 1 ] );
73  std::swap( data[j], data[i]);
74  }
75  m = size;
76  while( m >= 2 && j > m )
77  {
78  j -= m;
79  m >>= 1;
80  }
81  j += m;
82  };
83 
84  // Danielson-Lanczos method
85  mmax = 2;
86  while( n > mmax )
87  {
88  istep = mmax << 1;
89  theta = -( 2 * M_PI / mmax );
90  wtemp = std::sin( 0.5 * theta );
91  wpr = -2.0 * wtemp * wtemp;
92  wpi = std::sin( theta );
93  wr = 1.0;
94  wi = 0.0;
95  for( m = 1; m < mmax; m += 2 )
96  {
97  for( i = m; i <= n; i += istep )
98  {
99  j = i + mmax;
100  tempr = wr * data[j-1] - wi * data[j];
101  tempi = wr * data[j] + wi * data[j-1];
102  data[j-1] = data[i-1] - tempr;
103  data[j] = data[i] - tempi;
104  data[i-1] += tempr;
105  data[i] += tempi;
106  }
107  wtemp = wr;
108  wr += wr * wpr - wi * wpi;
109  wi += wi * wpr + wtemp * wpi;
110  }
111  mmax = istep;
112  }
113 }
114 
115 } // namespace snark{
116 
117 #endif // SNARK_MATH_FFT_H_