1 /***************************************************************************
2 File : fft2D.cpp
3 Project : QtiPlot
4 --------------------------------------------------------------------
5 Copyright : (C) 2007 - 2011 by Ion Vasilief
6 Email (use @ for *) : ion_vasilief*yahoo.fr
7 Description : FFT for matrices
8
9 ***************************************************************************/
10
11 /***************************************************************************
12 * *
13 * This program is free software; you can redistribute it and/or modify *
14 * it under the terms of the GNU General Public License as published by *
15 * the Free Software Foundation; either version 2 of the License, or *
16 * (at your option) any later version. *
17 * *
18 * This program is distributed in the hope that it will be useful, *
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
21 * GNU General Public License for more details. *
22 * *
23 * You should have received a copy of the GNU General Public License *
24 * along with this program; if not, write to the Free Software *
25 * Foundation, Inc., 51 Franklin Street, Fifth Floor, *
26 * Boston, MA 02110-1301 USA *
27 * *
28 ***************************************************************************/
29 #include "fft2D.h"
30 #include <Matrix.h>
31 #include <math.h>
32
next2Power(int n)33 int next2Power(int n)
34 {
35 int l2n = 0, p = 1; //l2n will become log_2(n)
36 while(p < n){
37 p *= 2;
38 l2n++;
39 }
40 n = 1 << l2n;
41 return n;
42 }
43
isPower2(int n)44 bool isPower2(int n)
45 {
46 return n && !(n & (n - 1));
47 }
48
fft(double * x_int_re,double * x_int_im,int taille)49 void fft(double* x_int_re, double* x_int_im, int taille)
50 {
51 int size_2 = taille >> 1, tmp1 = 0;
52 double tmp, tmpcos, tmpsin, base = 2*M_PI/taille;
53 const double SQ_2=sqrt(2);
54 double pair_re[size_2], pair_im[size_2], impair_re[size_2], impair_im[size_2];
55
56 for(int i=0; i<size_2; i++){
57 tmp1=(i<<1);
58 pair_re[i]=x_int_re[tmp1];
59 pair_im[i]=x_int_im[tmp1];
60 impair_re[i]=x_int_re[tmp1+1];
61 impair_im[i]=x_int_im[tmp1+1];
62 }
63
64 if(taille>2){
65 fft(pair_re,pair_im,size_2);
66 fft(impair_re,impair_im,size_2);
67 }
68
69 for(int i=0; i<size_2; i++){
70 tmp=base*i;
71 tmpcos=cos(tmp);
72 tmpsin=sin(tmp);
73 x_int_re[i]=(pair_re[i]+impair_re[i]*tmpcos+impair_im[i]*tmpsin)/SQ_2;
74 x_int_im[i]=(pair_im[i]+impair_im[i]*tmpcos-impair_re[i]*tmpsin)/SQ_2;
75 x_int_re[i+size_2]=(pair_re[i]-impair_re[i]*tmpcos-impair_im[i]*tmpsin)/SQ_2;
76 x_int_im[i+size_2]=(pair_im[i]-impair_im[i]*tmpcos+impair_re[i]*tmpsin)/SQ_2;
77 }
78 }
79
fft_inv(double * x_int_re,double * x_int_im,int taille)80 void fft_inv(double* x_int_re, double* x_int_im, int taille)
81 {
82 int size_2 = taille >> 1, tmp1 = 0;
83 double tmp, tmpcos, tmpsin, base=2*M_PI/taille;
84 const double SQ_2=sqrt(2);
85 double pair_re[size_2], pair_im[size_2], impair_re[size_2], impair_im[size_2];
86
87 for(int i=0; i<size_2; i++){
88 tmp1=i<<1;
89 pair_re[i]=x_int_re[tmp1];
90 pair_im[i]=x_int_im[tmp1];
91 impair_re[i]=x_int_re[tmp1+1];
92 impair_im[i]=x_int_im[tmp1+1];
93 }
94
95 if(taille>2){
96 fft_inv(pair_re, pair_im,size_2);
97 fft_inv(impair_re, impair_im,size_2);
98 }
99
100 for(int i=0; i<size_2; i++){
101 tmp=base*i;
102 tmpcos=cos(tmp);
103 tmpsin=sin(tmp);
104 x_int_re[i]=(pair_re[i]+impair_re[i]*tmpcos-impair_im[i]*tmpsin)/SQ_2;
105 x_int_im[i]=(pair_im[i]+impair_im[i]*tmpcos+impair_re[i]*tmpsin)/SQ_2;
106 x_int_re[i+size_2]=(pair_re[i]-impair_re[i]*tmpcos+impair_im[i]*tmpsin)/SQ_2;
107 x_int_im[i+size_2]=(pair_im[i]-impair_im[i]*tmpcos-impair_re[i]*tmpsin)/SQ_2;
108 }
109 }
110
fft2d(double ** xtre,double ** xtim,int width,int height,bool shift)111 void fft2d(double **xtre, double **xtim, int width, int height, bool shift)
112 {
113 double **xint_re = Matrix::allocateMatrixData(height, width);
114 if (!xint_re)
115 return;
116 double **xint_im = Matrix::allocateMatrixData(height, width);
117 if (!xint_im){
118 Matrix::freeMatrixData(xint_re, height);
119 return;
120 }
121
122 double x_int_l[width], x_int2_l[width], x_int_c[height], x_int2_c[height];
123 for(int k=0; k<height; k++){
124 for(int j=0; j<width; j++){
125 x_int_l[j] = xtre[k][j];
126 x_int2_l[j] = xtim[k][j];
127 }
128
129 fft(x_int_l, x_int2_l, width);
130
131 for(int j=0; j<width; j++){
132 xint_re[k][j] = x_int_l[j];
133 xint_im[k][j] = x_int2_l[j];
134 }
135 }
136
137 for(int k=0; k<width; k++){
138 for(int i=0; i<height; i++){
139 x_int_c[i] = xint_re[i][k];
140 x_int2_c[i] = xint_im[i][k];
141 }
142
143 fft(x_int_c, x_int2_c, height) ;
144
145 if (shift){
146 int col = (k+(width>>1))%width;
147 for(int i=0; i<height; i++){
148 int row = (i+(height>>1))%height;
149 xtre[row][col] = x_int_c[i];
150 xtim[row][col] = x_int2_c[i];
151 }
152 } else {
153 for(int i=0; i<height; i++){
154 xtre[i][k] = x_int_c[i];
155 xtim[i][k] = x_int2_c[i];
156 }
157 }
158 }
159 Matrix::freeMatrixData(xint_re, height);
160 Matrix::freeMatrixData(xint_im, height);
161 }
162
fft2d_inv(double ** xtre,double ** xtim,double ** xrec_re,double ** xrec_im,int width,int height,bool undoShift)163 void fft2d_inv(double **xtre, double **xtim, double **xrec_re, double **xrec_im, int width, int height, bool undoShift)
164 {
165 double **xint_re = Matrix::allocateMatrixData(height, width);
166 if (!xint_re)
167 return;
168 double **xint_im = Matrix::allocateMatrixData(height, width);
169 if (!xint_im){
170 Matrix::freeMatrixData(xint_re, height);
171 return;
172 }
173
174 double x_int_l[width], x_int2_l[width], x_int_c[height], x_int2_c[height];
175
176 for(int k = 0; k < height; k++){
177 if (undoShift){
178 int row = (k + (height>>1))%height;
179 for(int j = 0; j < width; j++){
180 int col = (j + (width>>1))%width;
181 x_int_l[j] = xtre[row][col];
182 x_int2_l[j] = xtim[row][col];
183 }
184 } else {
185 for(int j = 0; j < width; j++){
186 x_int_l[j] = xtre[k][j];
187 x_int2_l[j] = xtim[k][j];
188 }
189 }
190
191 fft_inv(x_int_l, x_int2_l, width);
192
193 for(int j = 0; j < width; j++){
194 xint_re[k][j] = x_int_l[j];
195 xint_im[k][j] = x_int2_l[j];
196 }
197 }
198 for(int k = 0; k < width; k++){
199 for(int i = 0; i < height; i++){
200 x_int_c[i] = xint_re[i][k];
201 x_int2_c[i] = xint_im[i][k];
202 }
203
204 fft_inv(x_int_c, x_int2_c, height);
205
206 for(int i = 0; i < height; i++){
207 xrec_re[i][k] = x_int_c[i];
208 xrec_im[i][k] = x_int2_c[i];
209 }
210 }
211 Matrix::freeMatrixData(xint_re, height);
212 Matrix::freeMatrixData(xint_im, height);
213 }
214