Wei Chang Shen / Motor_Embedded_3rd_emwin

Dependencies:   DMSupport DMemWin

embedded/fft.cpp@0:08606a13a816, 2016-06-02 (annotated)

Committer:
destinyXfate
Date:
Thu Jun 02 05:04:57 2016 +0000
Revision:
0:08606a13a816
;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
destinyXfate 0:08606a13a816 1 // Include declaration file
destinyXfate 0:08606a13a816 2 #include "fft.h"
destinyXfate 0:08606a13a816 3
destinyXfate 0:08606a13a816 4 // FORWARD FOURIER TRANSFORM
destinyXfate 0:08606a13a816 5 // Input - input data
destinyXfate 0:08606a13a816 6 // Output - transform result
destinyXfate 0:08606a13a816 7 // N - length of both input data and result
destinyXfate 0:08606a13a816 8 bool CFFT::Forward(const complex *const Input, complex *const Output, const unsigned int N)
destinyXfate 0:08606a13a816 9 {
destinyXfate 0:08606a13a816 10 // Check input parameters
destinyXfate 0:08606a13a816 11 if (!Input || !Output || N < 1 || N & (N - 1))
destinyXfate 0:08606a13a816 12 return false;
destinyXfate 0:08606a13a816 13 // Initialize data
destinyXfate 0:08606a13a816 14 Rearrange(Input, Output, N);
destinyXfate 0:08606a13a816 15 // Call FFT implementation
destinyXfate 0:08606a13a816 16 Perform(Output, N);
destinyXfate 0:08606a13a816 17 // Succeeded
destinyXfate 0:08606a13a816 18 return true;
destinyXfate 0:08606a13a816 19 }
destinyXfate 0:08606a13a816 20
destinyXfate 0:08606a13a816 21 // FORWARD FOURIER TRANSFORM, INPLACE VERSION
destinyXfate 0:08606a13a816 22 // Data - both input data and output
destinyXfate 0:08606a13a816 23 // N - length of input data
destinyXfate 0:08606a13a816 24 bool CFFT::Forward(complex *const Data, const unsigned int N)
destinyXfate 0:08606a13a816 25 {
destinyXfate 0:08606a13a816 26 // Check input parameters
destinyXfate 0:08606a13a816 27 if (!Data || N < 1 || N & (N - 1))
destinyXfate 0:08606a13a816 28 return false;
destinyXfate 0:08606a13a816 29 // Rearrange
destinyXfate 0:08606a13a816 30 Rearrange(Data, N);
destinyXfate 0:08606a13a816 31 // Call FFT implementation
destinyXfate 0:08606a13a816 32 Perform(Data, N);
destinyXfate 0:08606a13a816 33 // Succeeded
destinyXfate 0:08606a13a816 34 return true;
destinyXfate 0:08606a13a816 35 }
destinyXfate 0:08606a13a816 36
destinyXfate 0:08606a13a816 37 // INVERSE FOURIER TRANSFORM
destinyXfate 0:08606a13a816 38 // Input - input data
destinyXfate 0:08606a13a816 39 // Output - transform result
destinyXfate 0:08606a13a816 40 // N - length of both input data and result
destinyXfate 0:08606a13a816 41 // Scale - if to scale result
destinyXfate 0:08606a13a816 42 bool CFFT::Inverse(const complex *const Input, complex *const Output, const unsigned int N, const bool Scale /* = true */)
destinyXfate 0:08606a13a816 43 {
destinyXfate 0:08606a13a816 44 // Check input parameters
destinyXfate 0:08606a13a816 45 if (!Input || !Output || N < 1 || N & (N - 1))
destinyXfate 0:08606a13a816 46 return false;
destinyXfate 0:08606a13a816 47 // Initialize data
destinyXfate 0:08606a13a816 48 Rearrange(Input, Output, N);
destinyXfate 0:08606a13a816 49 // Call FFT implementation
destinyXfate 0:08606a13a816 50 Perform(Output, N, true);
destinyXfate 0:08606a13a816 51 // Scale if necessary
destinyXfate 0:08606a13a816 52 if (Scale)
destinyXfate 0:08606a13a816 53 CFFT::Scale(Output, N);
destinyXfate 0:08606a13a816 54 // Succeeded
destinyXfate 0:08606a13a816 55 return true;
destinyXfate 0:08606a13a816 56 }
destinyXfate 0:08606a13a816 57
destinyXfate 0:08606a13a816 58 // INVERSE FOURIER TRANSFORM, INPLACE VERSION
destinyXfate 0:08606a13a816 59 // Data - both input data and output
destinyXfate 0:08606a13a816 60 // N - length of both input data and result
destinyXfate 0:08606a13a816 61 // Scale - if to scale result
destinyXfate 0:08606a13a816 62 bool CFFT::Inverse(complex *const Data, const unsigned int N, const bool Scale /* = true */)
destinyXfate 0:08606a13a816 63 {
destinyXfate 0:08606a13a816 64 // Check input parameters
destinyXfate 0:08606a13a816 65 if (!Data || N < 1 || N & (N - 1))
destinyXfate 0:08606a13a816 66 return false;
destinyXfate 0:08606a13a816 67 // Rearrange
destinyXfate 0:08606a13a816 68 Rearrange(Data, N);
destinyXfate 0:08606a13a816 69 // Call FFT implementation
destinyXfate 0:08606a13a816 70 Perform(Data, N, true);
destinyXfate 0:08606a13a816 71 // Scale if necessary
destinyXfate 0:08606a13a816 72 if (Scale)
destinyXfate 0:08606a13a816 73 CFFT::Scale(Data, N);
destinyXfate 0:08606a13a816 74 // Succeeded
destinyXfate 0:08606a13a816 75 return true;
destinyXfate 0:08606a13a816 76 }
destinyXfate 0:08606a13a816 77
destinyXfate 0:08606a13a816 78 // Rearrange function
destinyXfate 0:08606a13a816 79 void CFFT::Rearrange(const complex *const Input, complex *const Output, const unsigned int N)
destinyXfate 0:08606a13a816 80 {
destinyXfate 0:08606a13a816 81 // Data entry position
destinyXfate 0:08606a13a816 82 unsigned int Target = 0;
destinyXfate 0:08606a13a816 83 // Process all positions of input signal
destinyXfate 0:08606a13a816 84 for (unsigned int Position = 0; Position < N; ++Position)
destinyXfate 0:08606a13a816 85 {
destinyXfate 0:08606a13a816 86 // Set data entry
destinyXfate 0:08606a13a816 87 Output[Target] = Input[Position];
destinyXfate 0:08606a13a816 88 // Bit mask
destinyXfate 0:08606a13a816 89 unsigned int Mask = N;
destinyXfate 0:08606a13a816 90 // While bit is set
destinyXfate 0:08606a13a816 91 while (Target & (Mask >>= 1))
destinyXfate 0:08606a13a816 92 // Drop bit
destinyXfate 0:08606a13a816 93 Target &= ~Mask;
destinyXfate 0:08606a13a816 94 // The current bit is 0 - set it
destinyXfate 0:08606a13a816 95 Target |= Mask;
destinyXfate 0:08606a13a816 96 }
destinyXfate 0:08606a13a816 97 }
destinyXfate 0:08606a13a816 98
destinyXfate 0:08606a13a816 99 // Inplace version of rearrange function
destinyXfate 0:08606a13a816 100 void CFFT::Rearrange(complex *const Data, const unsigned int N)
destinyXfate 0:08606a13a816 101 {
destinyXfate 0:08606a13a816 102 // Swap position
destinyXfate 0:08606a13a816 103 unsigned int Target = 0;
destinyXfate 0:08606a13a816 104 // Process all positions of input signal
destinyXfate 0:08606a13a816 105 for (unsigned int Position = 0; Position < N; ++Position)
destinyXfate 0:08606a13a816 106 {
destinyXfate 0:08606a13a816 107 // Only for not yet swapped entries
destinyXfate 0:08606a13a816 108 if (Target > Position)
destinyXfate 0:08606a13a816 109 {
destinyXfate 0:08606a13a816 110 // Swap entries
destinyXfate 0:08606a13a816 111 const complex Temp(Data[Target]);
destinyXfate 0:08606a13a816 112 Data[Target] = Data[Position];
destinyXfate 0:08606a13a816 113 Data[Position] = Temp;
destinyXfate 0:08606a13a816 114 }
destinyXfate 0:08606a13a816 115 // Bit mask
destinyXfate 0:08606a13a816 116 unsigned int Mask = N;
destinyXfate 0:08606a13a816 117 // While bit is set
destinyXfate 0:08606a13a816 118 while (Target & (Mask >>= 1))
destinyXfate 0:08606a13a816 119 // Drop bit
destinyXfate 0:08606a13a816 120 Target &= ~Mask;
destinyXfate 0:08606a13a816 121 // The current bit is 0 - set it
destinyXfate 0:08606a13a816 122 Target |= Mask;
destinyXfate 0:08606a13a816 123 }
destinyXfate 0:08606a13a816 124 }
destinyXfate 0:08606a13a816 125
destinyXfate 0:08606a13a816 126 // FFT implementation
destinyXfate 0:08606a13a816 127 void CFFT::Perform(complex *const Data, const unsigned int N, const bool Inverse /* = false */)
destinyXfate 0:08606a13a816 128 {
destinyXfate 0:08606a13a816 129 const double pi = Inverse ? 3.14159265358979323846 : -3.14159265358979323846;
destinyXfate 0:08606a13a816 130 // Iteration through dyads, quadruples, octads and so on...
destinyXfate 0:08606a13a816 131 for (unsigned int Step = 1; Step < N; Step <<= 1)
destinyXfate 0:08606a13a816 132 {
destinyXfate 0:08606a13a816 133 // Jump to the next entry of the same transform factor
destinyXfate 0:08606a13a816 134 const unsigned int Jump = Step << 1;
destinyXfate 0:08606a13a816 135 // Angle increment
destinyXfate 0:08606a13a816 136 const double delta = pi / double(Step);
destinyXfate 0:08606a13a816 137 // Auxiliary sin(delta / 2)
destinyXfate 0:08606a13a816 138 const double Sine = sin(delta * .5);
destinyXfate 0:08606a13a816 139 // Multiplier for trigonometric recurrence
destinyXfate 0:08606a13a816 140 const complex Multiplier(-2. * Sine * Sine, sin(delta));
destinyXfate 0:08606a13a816 141 // Start value for transform factor, fi = 0
destinyXfate 0:08606a13a816 142 complex Factor(1.);
destinyXfate 0:08606a13a816 143 // Iteration through groups of different transform factor
destinyXfate 0:08606a13a816 144 for (unsigned int Group = 0; Group < Step; ++Group)
destinyXfate 0:08606a13a816 145 {
destinyXfate 0:08606a13a816 146 // Iteration within group
destinyXfate 0:08606a13a816 147 for (unsigned int Pair = Group; Pair < N; Pair += Jump)
destinyXfate 0:08606a13a816 148 {
destinyXfate 0:08606a13a816 149 // Match position
destinyXfate 0:08606a13a816 150 const unsigned int Match = Pair + Step;
destinyXfate 0:08606a13a816 151 // Second term of two-point transform
destinyXfate 0:08606a13a816 152 const complex Product(Factor * Data[Match]);
destinyXfate 0:08606a13a816 153 // Transform for fi + pi
destinyXfate 0:08606a13a816 154 Data[Match] = Data[Pair] - Product;
destinyXfate 0:08606a13a816 155 // Transform for fi
destinyXfate 0:08606a13a816 156 Data[Pair] += Product;
destinyXfate 0:08606a13a816 157 }
destinyXfate 0:08606a13a816 158 // Successive transform factor via trigonometric recurrence
destinyXfate 0:08606a13a816 159 Factor = Multiplier * Factor + Factor;
destinyXfate 0:08606a13a816 160 }
destinyXfate 0:08606a13a816 161 }
destinyXfate 0:08606a13a816 162 }
destinyXfate 0:08606a13a816 163
destinyXfate 0:08606a13a816 164 // Scaling of inverse FFT result
destinyXfate 0:08606a13a816 165 void CFFT::Scale(complex *const Data, const unsigned int N)
destinyXfate 0:08606a13a816 166 {
destinyXfate 0:08606a13a816 167 const double Factor = 1. / double(N);
destinyXfate 0:08606a13a816 168 // Scale all data entries
destinyXfate 0:08606a13a816 169 for (unsigned int Position = 0; Position < N; ++Position)
destinyXfate 0:08606a13a816 170 Data[Position] *= Factor;
destinyXfate 0:08606a13a816 171 }
destinyXfate 0:08606a13a816 172