|
749
|
1 /*
|
|
|
2 * Realfftf
|
|
|
3 * Originally (C) Copyright 1993 Philib Van Baren
|
|
|
4 * Cleaned up and refactored to be re-entrant by
|
|
|
5 * Matti 'ccr' Hamalainen <ccr@tnsp.org>, 2013
|
|
|
6 *
|
|
|
7 * Note: Output is BIT-REVERSED! so you must use the BitReversed to
|
|
|
8 * get legible output, (i.e. Real_i = buffer[ BitReversed[i] ]
|
|
|
9 * Imag_i = buffer[ BitReversed[i]+1 ] )
|
|
|
10 * Input is in normal order.
|
|
|
11 */
|
|
|
12 #include "dmfft.h"
|
|
|
13 #include <math.h>
|
|
|
14
|
|
|
15
|
|
|
16 int dmInitializeFFT(DMFFTContext * ctx, const int fftlen)
|
|
|
17 {
|
|
|
18 int i;
|
|
|
19
|
|
|
20 if (ctx == NULL)
|
|
|
21 return DMERR_NULLPTR;
|
|
|
22
|
|
|
23 if (fftlen <= 16)
|
|
|
24 return DMERR_INVALID_ARGS;
|
|
|
25
|
|
|
26 // Allocate necessary tables
|
|
|
27 ctx->npoints = fftlen / 2;
|
|
|
28 ctx->sinTable = (DMFFTType *) dmMalloc(fftlen * sizeof(DMFFTType));
|
|
|
29 if (ctx->sinTable == NULL)
|
|
|
30 return DMERR_MALLOC;
|
|
|
31
|
|
|
32 // Bitreversion buffer
|
|
|
33 ctx->breversed = (int *) dmMalloc(ctx->npoints * sizeof(int));
|
|
|
34 if (ctx->breversed == NULL)
|
|
|
35 {
|
|
|
36 dmFree(ctx->sinTable);
|
|
|
37 return DMERR_MALLOC;
|
|
|
38 }
|
|
|
39
|
|
|
40 // Calculate data
|
|
|
41 for (i = 0; i < ctx->npoints; i++)
|
|
|
42 {
|
|
|
43 int mask, tmp;
|
|
|
44 for (tmp = 0, mask = ctx->npoints / 2; mask > 0; mask >>= 1)
|
|
|
45 {
|
|
|
46 tmp = (tmp >> 1) + ((i & mask) ? ctx->npoints : 0);
|
|
|
47 }
|
|
|
48
|
|
|
49 ctx->breversed[i] = tmp;
|
|
|
50 }
|
|
|
51
|
|
|
52 for (i = 0; i < ctx->npoints; i++)
|
|
|
53 {
|
|
|
54 ctx->sinTable[ctx->breversed[i]] = -sin((2.0f * M_PI * i) / fftlen);
|
|
|
55 ctx->sinTable[ctx->breversed[i] + 1] =
|
|
|
56 -cos((2.0f * M_PI * i) / fftlen);
|
|
|
57 }
|
|
|
58
|
|
|
59 return DMERR_OK;
|
|
|
60 }
|
|
|
61
|
|
|
62
|
|
|
63 void dmEndFFT(DMFFTContext * ctx)
|
|
|
64 {
|
|
|
65 if (ctx != NULL)
|
|
|
66 {
|
|
|
67 dmFree(ctx->breversed);
|
|
|
68 dmFree(ctx->sinTable);
|
|
|
69 memset(ctx, 0, sizeof(DMFFTContext));
|
|
|
70 }
|
|
|
71 }
|
|
|
72
|
|
|
73
|
|
|
74 int dmRealFFT(DMFFTContext * ctx, DMFFTType * buffer)
|
|
|
75 {
|
|
|
76 if (ctx == NULL || buffer == NULL)
|
|
|
77 return DMERR_NULLPTR;
|
|
|
78
|
|
|
79 int rounds = ctx->npoints / 2;
|
|
|
80 DMFFTType *endptr1 = buffer + ctx->npoints * 2;
|
|
|
81
|
|
|
82 while (rounds > 0)
|
|
|
83 {
|
|
|
84 DMFFTType *sptr = ctx->sinTable;
|
|
|
85 DMFFTType *A = buffer,
|
|
|
86 *B = buffer + rounds * 2;
|
|
|
87
|
|
|
88 while (A < endptr1)
|
|
|
89 {
|
|
|
90 DMFFTType qsin = *sptr;
|
|
|
91 DMFFTType qcos = *(sptr + 1);
|
|
|
92 DMFFTType *endptr2 = B;
|
|
|
93
|
|
|
94 while (A < endptr2)
|
|
|
95 {
|
|
|
96 DMFFTType v1 = (*B) * qcos + (*(B + 1)) * qsin;
|
|
|
97 DMFFTType v2 = (*B) * qsin - (*(B + 1)) * qcos;
|
|
|
98 *B = (*A + v1) * 0.5;
|
|
|
99 *(A++) = *(B++) - v1;
|
|
|
100 *B = (*A - v2) * 0.5;
|
|
|
101 *(A++) = *(B++) + v2;
|
|
|
102 }
|
|
|
103 A = B;
|
|
|
104 B += rounds * 2;
|
|
|
105 sptr += 2;
|
|
|
106 }
|
|
|
107
|
|
|
108 rounds >>= 1;
|
|
|
109 }
|
|
|
110
|
|
|
111 /*
|
|
|
112 * Massage output to get the output for a real input sequence.
|
|
|
113 */
|
|
|
114 int *br1 = ctx->breversed + 1;
|
|
|
115 int *br2 = ctx->breversed + ctx->npoints - 1;
|
|
|
116
|
|
|
117 while (br1 <= br2)
|
|
|
118 {
|
|
|
119 const DMFFTType vsin = ctx->sinTable[*br1];
|
|
|
120 const DMFFTType vcos = ctx->sinTable[*br1 + 1];
|
|
|
121 DMFFTType *A = buffer + *br1,
|
|
|
122 *B = buffer + *br2,
|
|
|
123 HRplus, HRminus,
|
|
|
124 HIplus, HIminus,
|
|
|
125 tmp1, tmp2;
|
|
|
126
|
|
|
127 HRminus = *A - *B;
|
|
|
128 HRplus = HRminus + (*B * 2);
|
|
|
129
|
|
|
130 HIminus = *(A + 1) - *(B + 1);
|
|
|
131 HIplus = HIminus + (*(B + 1) * 2);
|
|
|
132
|
|
|
133 tmp1 = (vsin * HRminus) - (vcos * HIplus);
|
|
|
134 tmp2 = (vcos * HRminus) + (vsin * HIplus);
|
|
|
135
|
|
|
136 *A = (HRplus + tmp1) * 0.5f;
|
|
|
137 *B = *A - tmp1;
|
|
|
138
|
|
|
139 *(A + 1) = (HIminus + tmp2) * 0.5f;
|
|
|
140 *(B + 1) = (*(A + 1)) - HIminus;
|
|
|
141
|
|
|
142 br1++;
|
|
|
143 br2--;
|
|
|
144 }
|
|
|
145
|
|
|
146 // Handle DC bin separately
|
|
|
147 buffer[0] += buffer[1];
|
|
|
148 buffer[1] = 0;
|
|
|
149
|
|
|
150 return DMERR_OK;
|
|
|
151 }
|
