1 /*
2 * lingot, a musical instrument tuner.
3 *
4 * Copyright (C) 2004-2018 Iban Cereijo.
5 * Copyright (C) 2004-2008 Jairo Chapela.
6
7 *
8 * This file is part of lingot.
9 *
10 * lingot is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * lingot is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU General Public License for more details.
19 *
20 * You should have received a copy of the GNU General Public License
21 * along with lingot; if not, write to the Free Software
22 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 */
24
25 #include <math.h>
26 #include <stdlib.h>
27 #include <string.h>
28
29 #include "lingot-fft.h"
30 #include "lingot-config.h"
31
32 #ifndef LIBFFTW
33 #include "lingot-complex.h"
34 #endif
35
36 /*
37 DTFT functions.
38 */
39
lingot_fft_plan_create(LingotFFTPlan * result,FLT * in,int n)40 void lingot_fft_plan_create(LingotFFTPlan* result, FLT* in, int n) {
41
42 result->n = n;
43 result->in = in;
44
45 #ifdef LIBFFTW
46 result->fft_out = fftw_malloc(n * sizeof(fftw_complex));
47 memset(result->fft_out, 0, n * sizeof(fftw_complex));
48 result->fftwplan = fftw_plan_dft_r2c_1d(n, in, result->fft_out,
49 FFTW_ESTIMATE);
50 #else
51 FLT alpha;
52 unsigned int i;
53
54 // twiddle factors
55 result->wn = (LingotComplex*) malloc((n >> 1) * sizeof(LingotComplex));
56
57 for (i = 0; i < (n >> 1); i++) {
58 alpha = -2.0 * i * M_PI / n;
59 result->wn[i][0] = cos(alpha);
60 result->wn[i][1] = sin(alpha);
61 }
62 result->fft_out = malloc(n * sizeof(LingotComplex)); // complex signal in freq domain.
63 memset(result->fft_out, 0, n * sizeof(LingotComplex));
64 #endif
65
66 }
67
lingot_fft_plan_destroy(LingotFFTPlan * plan)68 void lingot_fft_plan_destroy(LingotFFTPlan* plan) {
69
70 #ifdef LIBFFTW
71 fftw_destroy_plan(plan->fftwplan);
72 fftw_free(plan->fft_out);
73 #else
74 free(plan->fft_out);
75 free(plan->wn);
76 #endif
77 }
78
79 #ifndef LIBFFTW
80
_lingot_fft_fft(FLT * in,LingotComplex * out,LingotComplex * wn,unsigned long int N,unsigned long int offset,unsigned long int d1,unsigned long int step)81 void _lingot_fft_fft(FLT* in, LingotComplex* out, LingotComplex* wn, unsigned long int N,
82 unsigned long int offset, unsigned long int d1, unsigned long int step) {
83 LingotComplex X1, X2;
84 unsigned long int Np2 = (N >> 1); // N/2
85 register unsigned long int a, b, c, q;
86
87 if (N == 2) { // butterfly for N = 2;
88
89 X1[0] = in[offset];
90 X1[1] = 0.0;
91 X2[0] = in[offset + step];
92 X2[1] = 0.0;
93
94 lingot_complex_add(X1, X2, out[d1]);
95 lingot_complex_sub(X1, X2, out[d1 + Np2]);
96
97 return;
98 }
99
100 _lingot_fft_fft(in, out, wn, Np2, offset, d1, step << 1);
101 _lingot_fft_fft(in, out, wn, Np2, offset + step, d1 + Np2, step << 1);
102
103 for (q = 0, c = 0; q < (N >> 1); q++, c += step) {
104
105 a = q + d1;
106 b = a + Np2;
107
108 X1[0] = out[a][0];
109 X1[1] = out[a][1];
110 lingot_complex_mul(out[b], wn[c], X2);
111 lingot_complex_add(X1, X2, out[a]);
112 lingot_complex_sub(X1, X2, out[b]);
113 }
114 }
115
lingot_fft_fft(LingotFFTPlan * plan)116 void lingot_fft_fft(LingotFFTPlan* plan) {
117 _lingot_fft_fft(plan->in, plan->fft_out, plan->wn, plan->n, 0, 0, 1);
118 }
119
120 #endif
121
lingot_fft_compute_dft_and_spd(LingotFFTPlan * plan,FLT * out,int n_out)122 void lingot_fft_compute_dft_and_spd(LingotFFTPlan* plan, FLT* out, int n_out) {
123
124 int i;
125 double _1_N2 = 1.0 / (plan->n * plan->n);
126
127 # ifdef LIBFFTW
128 // transformation.
129 fftw_execute(plan->fftwplan);
130 # else
131 // transformation.
132 lingot_fft_fft(plan);
133 #endif
134
135 // esteem of SPD from FFT. (normalized squared module)
136 for (i = 0; i < n_out; i++) {
137 out[i] = (plan->fft_out[i][0] * plan->fft_out[i][0]
138 + plan->fft_out[i][1] * plan->fft_out[i][1]) * _1_N2;
139 }
140 }
141
142 /* Spectral Power Distribution esteem, selectively in frequency, by DFT.
143 transforms signal in of N1 samples from frequency wi, with sample
144 separation of dw rads, storing the result on buffer out with N2 samples. */
lingot_fft_spd_eval(FLT * in,int N1,FLT wi,FLT dw,FLT * out,int N2)145 void lingot_fft_spd_eval(FLT* in, int N1, FLT wi, FLT dw, FLT* out, int N2) {
146 FLT Xr, Xi;
147 FLT wn;
148 const FLT N1_2 = N1 * N1;
149 int i, n;
150
151 for (i = 0; i < N2; i++) {
152
153 Xr = 0.0;
154 Xi = 0.0;
155
156 for (n = 0; n < N1; n++) { // O(n1*n2) :(
157
158 wn = (wi + dw * i) * n;
159 Xr = Xr + cos(wn) * in[n];
160 Xi = Xi - sin(wn) * in[n];
161 }
162
163 out[i] = (Xr * Xr + Xi * Xi) / N1_2; // normalized squared module.
164 }
165 }
166
lingot_fft_spd_diffs_eval(const FLT * in,int N,FLT w,FLT * out_d0,FLT * out_d1,FLT * out_d2)167 void lingot_fft_spd_diffs_eval(const FLT* in, int N, FLT w, FLT* out_d0,
168 FLT* out_d1, FLT* out_d2) {
169 FLT x_cos_wn;
170 FLT x_sin_wn;
171 const FLT N2 = N * N;
172
173 int n;
174
175 FLT SUM_x_sin_wn = 0.0;
176 FLT SUM_x_cos_wn = 0.0;
177 FLT SUM_x_n_sin_wn = 0.0;
178 FLT SUM_x_n_cos_wn = 0.0;
179 FLT SUM_x_n2_sin_wn = 0.0;
180 FLT SUM_x_n2_cos_wn = 0.0;
181
182 for (n = 0; n < N; n++) {
183
184 x_cos_wn = in[n] * cos(w * n);
185 x_sin_wn = in[n] * sin(w * n);
186
187 SUM_x_sin_wn += x_sin_wn;
188 SUM_x_cos_wn += x_cos_wn;
189 SUM_x_n_sin_wn += x_sin_wn * n;
190 SUM_x_n_cos_wn += x_cos_wn * n;
191 SUM_x_n2_sin_wn += x_sin_wn * n * n;
192 SUM_x_n2_cos_wn += x_cos_wn * n * n;
193 }
194
195 FLT N_2 = N * N;
196 *out_d0 = (SUM_x_cos_wn * SUM_x_cos_wn + SUM_x_sin_wn * SUM_x_sin_wn) / N2;
197 *out_d1 = 2.0
198 * (SUM_x_sin_wn * SUM_x_n_cos_wn - SUM_x_cos_wn * SUM_x_n_sin_wn)
199 / N_2;
200 *out_d2 = 2.0
201 * (SUM_x_n_cos_wn * SUM_x_n_cos_wn - SUM_x_sin_wn * SUM_x_n2_sin_wn
202 + SUM_x_n_sin_wn * SUM_x_n_sin_wn
203 - SUM_x_cos_wn * SUM_x_n2_cos_wn) / N_2;
204 }
205