General Purpose Geodetic Library
SgMathSupport.cpp
Go to the documentation of this file.
1 /*
2  *
3  * This file is a part of Space Geodetic Library. The library is used by
4  * nuSolve, a part of CALC/SOLVE system, and designed to make analysis of
5  * geodetic VLBI observations.
6  * Copyright (C) 2010-2020 Sergei Bolotin.
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <http://www.gnu.org/licenses/>.
20  *
21  */
22 
23 #include <iostream>
24 #include <stdlib.h>
25 #include <complex>
26 
27 
28 
29 //#include <SgLogger.h>
30 #include <SgMathSupport.h>
31 
32 
33 
34 
35 
36 #define SWAP(a, b) {typeof(a) t; t = a; a = b; b = t;}
37 
38 
39 
40 /*=====================================================================================================*/
41 //
42 // just aux functions:
43 //
44 //
45 /*=====================================================================================================*/
46 // https://rosettacode.org/wiki/Fast_Fourier_transform
47 // it is usable too:
48 void _fft(std::complex<double> buf[], std::complex<double> out[], int n, int step)
49 {
50  std::complex<double> zI(0.0, 1.0);
51  if (step < n)
52  {
53  _fft(out, buf, n, step*2);
54  _fft(out + step, buf + step, n, step*2);
55  for (int i=0; i<n; i+=2*step)
56  {
57  std::complex<double> t=std::exp(-zI*M_PI*double(i)/double(n))*out[i + step];
58  buf[i / 2] = out[i] + t;
59  buf[(i + n)/2] = out[i] - t;
60  };
61  };
62 };
63 void fft(std::complex<double> buf[], int n)
64 {
65  std::complex<double> out[n];
66  for (int i=0; i<n; i++)
67  out[i] = buf[i];
68  _fft(buf, out, n, 1);
69 };
70 
71 
72 
73 // This function is originally written by Paul Bourke (http://paulbourke.net/miscellaneous/dft),
74 // then modified a little bit.
75 //
76 /*
77  This computes an in-place complex-to-complex FFT
78  x and y are the real and imaginary arrays of n=2^m points.
79  dir = 1 gives forward transform
80  dir = -1 gives reverse transform
81 */
82 void fft(double *x, double *y, int n, int dir)
83 {
84  int m, i1, j, k, i2, l1, l2;
85  double c1, c2, tx, ty, t1, t2, u1, u2, z;
86 
87  m = 0;
88  while (1<<m<n && m<32)
89  m++;
90 
91  if (1<<m != n)
92  {
93  std::cerr << "FFT: number of points, " << n << ", is not a power of 2.\n";
94  return;
95  };
96 
97  /* Do the bit reversal */
98  i2 = n >> 1;
99  j = 0;
100  for (int i=0; i<n-1; i++)
101  {
102  if (i < j)
103  {
104  tx = x[i];
105  ty = y[i];
106  x[i] = x[j];
107  y[i] = y[j];
108  x[j] = tx;
109  y[j] = ty;
110  };
111  k = i2;
112  while (k <= j)
113  {
114  j -= k;
115  k >>= 1;
116  };
117  j += k;
118  };
119  /* Compute the FFT */
120  c1 = -1.0;
121  c2 = 0.0;
122  l2 = 1;
123  for (int l=0; l<m; l++)
124  {
125  l1 = l2;
126  l2 <<= 1;
127  u1 = 1.0;
128  u2 = 0.0;
129  for (j=0; j<l1; j++)
130  {
131  for (int i=j; i<n; i+=l2)
132  {
133  i1 = i + l1;
134  t1 = u1*x[i1] - u2*y[i1];
135  t2 = u1*y[i1] + u2*x[i1];
136  x[i1] = x[i] - t1;
137  y[i1] = y[i] - t2;
138  x[i] += t1;
139  y[i] += t2;
140  };
141  z = u1*c1 - u2*c2;
142  u2 = u1*c2 + u2*c1;
143  u1 = z;
144  };
145  c2 = sqrt((1.0 - c1)/2.0);
146  if (dir == 1)
147  c2 = -c2;
148  c1 = sqrt((1.0 + c1)/2.0);
149  };
150  /* Scaling for forward transform */
151  if (dir == 1)
152  {
153  for (int i=0; i<n; i++)
154  {
155  x[i] /= n;
156  y[i] /= n;
157  };
158  };
159  //
160  return;
161 };
162 
163 
164 
165 
166 
167 /*=====================================================================================================*/
168 //
169 // statics:
170 //
171 /*=====================================================================================================*/
172 
173 
174 
175 
176 /*=====================================================================================================*/
void _fft(std::complex< double > buf[], std::complex< double > out[], int n, int step)
void fft(std::complex< double > buf[], int n)