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 
26 
27 
28 //#include <SgLogger.h>
29 #include <SgMathSupport.h>
30 #include <SgMJD.h>
31 #include <Sg3dVector.h>
32 
33 
34 
35 
36 
37 #define SWAP(a, b) {typeof(a) t; t = a; a = b; b = t;}
38 
39 
40 
41 
42 /*=====================================================================================================*/
43 //
44 // just aux functions:
45 //
46 //
47 /*=====================================================================================================*/
48 //
49 unsigned int reverseBitOrder(unsigned int n, unsigned int k) // n = 0...2^k
50 {
51  unsigned int x=0;
52  const unsigned int mask=1;
53  while (k > 0)
54  {
55  x = x << 1;
56  if ( mask & n )
57  x = x | mask;
58  n = n >> 1;
59  k--;
60  }
61  return x;
62 };
63 
64 
65 
66 // FFT:
67 // Radix-2 Cooley-Tukey algorythm is available at:
68 // https://en.wikipedia.org/wiki/Cooley%E2%80%93Tukey_FFT_algorithm
69 void fft(std::complex<double> x_a[], std::complex<double> x_A[], unsigned int n, FFT_Direction dir)
70 {
71  unsigned int log2n; // log2(n)
72  unsigned int m, hm;
73  std::complex<double> omega, omega_m;
74  std::complex<double> t, u;
75  std::complex<double> a;
76  a = dir==FFT_Forward?-2.0*M_PI*zI:2.0*M_PI*zI;
77 
78  log2n = 0;
79  while (1u<<log2n<n && log2n<32u)
80  log2n++;
81 
82  if (1u<<log2n != n)
83  {
84  std::cerr << "iterative_fft: number of points, " << n << ", is not a power of 2.\n";
85  return;
86  };
87 
88  for (unsigned int i=0; i<n; i++)
89  x_A[reverseBitOrder(i, log2n)] = x_a[i];
90 
91  for (unsigned int s=1; s<=log2n; s++)
92  {
93  m = 1 << s;
94  hm = m >> 1;
95  omega_m = std::exp(a/((double)m));
96  for (unsigned int k=0; k<n; k+=m)
97  {
98  omega = std::complex<double>(1.0, 0.0);
99  for (unsigned int j=0; j<hm; j++)
100  {
101  t = omega*x_A[k + j + hm];
102  u = x_A[k + j];
103  x_A[k + j] = u + t;
104  x_A[k + j + hm] = u - t;
105  omega *= omega_m;
106  };
107  };
108  };
109  if (dir == FFT_Inverse)
110  for (unsigned int i=0; i<n; i++)
111  x_A[i] /= n;
112 };
113 
114 
115 
116 // The algorithm is from
117 // T. Fukushima, "Transformation from Cartesian to geodetic coordinates accelerated by Halley’s method",
118 // February 2006 Journal of Geodesy 79(12):689-693
119 //
120 void geocentric2geodetic(const Sg3dVector& r, double& latitude, double& longitude, double& height,
121  bool useOldEllipsoid)
122 {
123 //useOldEllipsoid = false;
124 //useOldEllipsoid = true;
125 
126 //Set up the geoid parameters:
127  double a, f;
128  a = 6378137.0;
129  f = 0.00335281068118;
130 
131  if (useOldEllipsoid)
132  {
133  a = 6378136.3;
134  f = 0.0033528918690;
135  };
136  double p, dP, ec, b, z, dZ, dE;
137  double dC0, dC1, dS0, dS1;
138  double dA0, dA1, dB0, dD0, dF0;
139 
140  dE = (2.0 - f)*f;
141  ec = sqrt(1.0 - dE);
142  b = a*ec;
143  z = fabs(r.at(Z_AXIS));
144  p = hypot(r.at(X_AXIS), r.at(Y_AXIS));
145  if (1.0e-3 < p)
146  {
147  dZ = z*ec/a;
148  dP = p/a;
149 // Initial values for C0 and S0, eq. (17):
150  dC0 = ec*dP;
151  dS0 = dZ;
152 // The first iteration:
153  dA0 = hypot(dC0, dS0); // eq. (14)
154  dB0 = 1.5*dE*dS0*dC0*dC0*( (dP*dS0 - dZ*dC0)*dA0 - dE*dS0*dC0); // eq. (15)
155  dD0 = dZ*dA0*dA0*dA0 + dE*dS0*dS0*dS0; // eq. (12)
156  dF0 = dP*dA0*dA0*dA0 - dE*dC0*dC0*dC0; // eq. (13)
157 // Next iteration:
158  dS1 = dD0*dF0 - dB0*dS0; // eq. (10)
159  dC1 = dF0*dF0 - dB0*dC0; // eq. (11)
160  dA1 = hypot(dC1, dS1); // eq. (14)
161 // Calculate geodetic coordinates:
162 // longitude:
163  longitude = atan2(r.at(Y_AXIS), r.at(X_AXIS));
164  if (longitude < 0.0)
165  longitude += M_PI*2.0;
166 // latitude:
167  latitude = signum(r.at(Z_AXIS))*atan(dS1/dC1/ec); // eq. (19)
168 // height above geoid:
169  height = (p*dC1*ec + z*dS1 - b*dA1)/hypot(ec*dC1, dS1); // eq. (20)
170  }
171  else // is it pole?
172  {
173  longitude = 0.0;
174  latitude = M_PI_2*signum(r.at(Z_AXIS));
175  height = z - a*ec;
176  };
177 };
178 
179 
180 
181 //
182 void calcCip2IAU1980(const SgMJD& epoch, double dX, double dY, double dPsi_1980, double dEps_1980,
183  double dPsi_2000, double dEps_2000, double& diffPsi, double& diffEps)
184 {
185  double psi_A, chi_A, eps_A, eps_0;
186  double dt1, dt2, dt3, dt4, dt5;
187  double coseps_0;
188  double sineps_A;
189  double f, f2;
190  dt1 = (epoch - tEphem)/36525.0;
191  dt2 = dt1*dt1;
192  dt3 = dt2*dt1;
193  dt4 = dt2*dt2;
194  dt5 = dt3*dt2;
195  // Obliquity of the ecliptic at J2000.0:
196  eps_0 = 84381.406;
197  // IERS Conventions (2010), IERS TN #36 eqs. (5.39):
198  psi_A = 5038.481507*dt1 - 1.0790069*dt2 - 0.00114045*dt3 + 0.000132851*dt4 - 0.0000000951*dt5;
199  // IERS Conventions (2010), IERS TN #36 eqs. (5.40):
200  chi_A = 10.556403*dt1 - 2.3814292*dt2 - 0.00121197*dt3 + 0.000170663*dt4 - 0.000000056*dt5;
201  eps_A = eps_0 - 46.836769*dt1 - 0.0001831*dt2 + 0.0020034*dt3 - 0.000000576*dt4 - 0.0000000434*dt5;
202  // convert to radians:
203  eps_0 *= SEC2RAD;
204  psi_A *= SEC2RAD;
205  chi_A *= SEC2RAD;
206  eps_A *= SEC2RAD;
207  coseps_0 = cos(eps_0);
208  sineps_A = sin(eps_A);
209 
210  f = f2 = psi_A*coseps_0 - chi_A;
211  f2 *= f;
212 
213  diffPsi = dPsi_2000 - dPsi_1980;
214  diffEps = dEps_2000 - dEps_1980;
215 
216  // IERS Conventions (2010), IERS TN #36, page 50 eqs. (5.25),
217  // Express delta Psi and delta Eps as functions of dX and dY:
218  diffPsi+= (dX - f*dY)/(f2 + 1.0)/sineps_A;
219  diffEps+= (f*dX + dY)/(f2 + 1.0);
220 
221  // Adjust for differences in precession rates IAU 1976 -> IAU 2006:
222  // IERS Conventions (2010), IERS TN #36, page 55:
223  diffPsi+= -0.29965*dt1*SEC2RAD;
224  diffEps+= -0.02524*dt1*SEC2RAD;
225 
226  // Adjust offset of CIP with respect to IAU 1976 Precession/Nutation origin:
227  // N. Capitaine et al., "Expressions for IAU 2000 precession quantities",
228  // A&A 412, 567–586 (2003), DOI: 10.1051/0004-6361:20031539
229  // Eq (11):
230  diffPsi+= (-0.0417750)*SEC2RAD;
231  diffEps+= (-0.0068192)*SEC2RAD;
232 };
233 
234 
235 
236 //
237 // Calculates the five nutation fundamental arguments according to the IERS Conventions 2003 / IERS TN32:
238 // (The same coeffs are in IERS Conventions 2010 / IERS TN36):
239 void calcNutationFundArgs_IersConv2003(const SgMJD& tEpoch, double args[5])
240 {
241  // Page 48, eq. (40):
242  const double cL [5] = {134.96340251*DEG2SEC, 1717915923.2178, 31.8792, 0.051635, -0.00024470};
243  const double cL0 [5] = {357.52910918*DEG2SEC, 129596581.0481, -0.5532, 0.000136, -0.00001149};
244  const double cF [5] = { 93.27209062*DEG2SEC, 1739527262.8478,-12.7512,-0.001037, 0.00000417};
245  const double cD [5] = {297.85019547*DEG2SEC, 1602961601.2090, -6.3706, 0.006593, -0.00003169};
246  const double cOm [5] = {125.04455501*DEG2SEC, -6962890.5431, 7.4722, 0.007702, -0.00005939};
247  double t((tEpoch - tEphem)/36525.0); // centuries since Tephem
248  double t2, t3, t4;
249  double dL, dL0, dF, dD, dOm;
250  t2 = t*t;
251  t3 = t2*t;
252  t4 = t2*t2;
253  dL = cL [0] + cL [1]*t + cL [2]*t2 + cL [3]*t3 + cL [4]*t4;
254  dL0 = cL0[0] + cL0[1]*t + cL0[2]*t2 + cL0[3]*t3 + cL0[4]*t4;
255  dF = cF [0] + cF [1]*t + cF [2]*t2 + cF [3]*t3 + cF [4]*t4;
256  dD = cD [0] + cD [1]*t + cD [2]*t2 + cD [3]*t3 + cD [4]*t4;
257  dOm = cOm[0] + cOm[1]*t + cOm[2]*t2 + cOm[3]*t3 + cOm[4]*t4;
258 
259  args[0] = fmod(dL, 360.0*DEG2SEC)*SEC2RAD;
260  args[1] = fmod(dL0, 360.0*DEG2SEC)*SEC2RAD;
261  args[2] = fmod(dF, 360.0*DEG2SEC)*SEC2RAD;
262  args[3] = fmod(dD, 360.0*DEG2SEC)*SEC2RAD;
263  args[4] = fmod(dOm, 360.0*DEG2SEC)*SEC2RAD;
264 };
265 
266 
267 
268 //
269 //
270 // Calculates the five nutation fundamental arguments according to the IERS Conventions 1996 / IERS TN21:
271 void calcNutationFundArgs_IersConv1996(const SgMJD& tEpoch, double args[5])
272 {
273 // Page 23:
274  const double cL [5] = {134.96340251*DEG2SEC, 1717915923.2178, 31.8792, 0.051635, -0.00024470};
275  const double cL0 [5] = {357.52910918*DEG2SEC, 129596581.0481, -0.5532,-0.000136, -0.00001149};
276  const double cF [5] = { 93.27209062*DEG2SEC, 1739527262.8478,-12.7512,-0.001037, 0.00000417};
277  const double cD [5] = {297.85019547*DEG2SEC, 1602961601.2090, -6.3706, 0.006593, -0.00003169};
278  const double cOm [5] = {125.04455501*DEG2SEC, -6962890.2665, 7.4722, 0.007702, -0.00005939};
279 
280  double t((tEpoch - tEphem)/36525.0); // centuries since Tephem
281  double t2, t3, t4;
282  double dL, dL0, dF, dD, dOm;
283  t2 = t*t;
284  t3 = t2*t;
285  t4 = t2*t2;
286  dL = cL [0] + cL [1]*t + cL [2]*t2 + cL [3]*t3 + cL [4]*t4;
287  dL0 = cL0[0] + cL0[1]*t + cL0[2]*t2 + cL0[3]*t3 + cL0[4]*t4;
288  dF = cF [0] + cF [1]*t + cF [2]*t2 + cF [3]*t3 + cF [4]*t4;
289  dD = cD [0] + cD [1]*t + cD [2]*t2 + cD [3]*t3 + cD [4]*t4;
290  dOm = cOm[0] + cOm[1]*t + cOm[2]*t2 + cOm[3]*t3 + cOm[4]*t4;
291 
292  args[0] = fmod(dL, 360.0*DEG2SEC)*SEC2RAD;
293  args[1] = fmod(dL0, 360.0*DEG2SEC)*SEC2RAD;
294  args[2] = fmod(dF, 360.0*DEG2SEC)*SEC2RAD;
295  args[3] = fmod(dD, 360.0*DEG2SEC)*SEC2RAD;
296  args[4] = fmod(dOm, 360.0*DEG2SEC)*SEC2RAD;
297 };
298 
299 
300 
301 //
302 //
303 // Calculates the five nutation fundamental arguments according to the IERS Standards 1992 / IERS TN13:
304 void calcNutationFundArgs_IersStds1992(const SgMJD& tEpoch, double args[5])
305 {
306 // Page 32:
307  double t((tEpoch - tEphem)/36525.0); // centuries since Tephem
308  double t2, t3, r;
309  double dL, dL0, dF, dD, dOm;
310 
311  r = 1296000.0; // sec in 2*M_PI
312  t2 = t*t;
313  t3 = t2*t;
314 
315  // Mean Anomaly of the Moon:
316  dL = (134.0*3600.0 + 57.0*60.0 + 46.733) +
317  fmod((1325.0*r + 198.0*3600.0 + 52.0*60.0 + 2.633)*t, r) + 31.310*t2 + 0.064*t3;
318 
319  // Mean Anomaly of the Sun:
320  dL0 = (357.0*3600.0 + 31.0*60.0 + 39.804) +
321  fmod(( 99.0*r + 359.0*3600.0 + 3.0*60.0 + 1.224)*t, r) - 0.577*t2 - 0.012*t3;
322 
323  // L - Omega, L is Mean Longitude of the Moon:
324  dF = ( 93.0*3600.0 + 16.0*60.0 + 18.877) +
325  fmod((1342.0*r + 82.0*3600.0 + 1.0*60.0 + 3.137)*t, r) - 13.257*t2 + 0.011*t3;
326 
327  //Mean Elongation of the Moon from the Sun:
328  dD = (297.0*3600.0 + 51.0*60.0 + 1.307) +
329  fmod((1236.0*r + 307.0*3600.0 + 6.0*60.0 + 41.328)*t, r) - 6.891*t2 + 0.019*t3;
330 
331  // Mean Longitude of the Ascending Node of the Moon:
332  dOm = (125.0*3600.0 + 2.0*60.0 + 40.280) -
333  fmod(( 5.0*r + 134.0*3600.0 + 8.0*60.0 + 10.539)*t, r) + 7.455*t2 + 0.008*t3;
334 
335  args[0] = fmod(dL, 360.0*DEG2SEC)*SEC2RAD;
336  args[1] = fmod(dL0, 360.0*DEG2SEC)*SEC2RAD;
337  args[2] = fmod(dF, 360.0*DEG2SEC)*SEC2RAD;
338  args[3] = fmod(dD, 360.0*DEG2SEC)*SEC2RAD;
339  args[4] = fmod(dOm, 360.0*DEG2SEC)*SEC2RAD;
340 };
341 /*=====================================================================================================*/
342 
343 
344 
345 
346 /*=====================================================================================================*/
347 //
348 // statics:
349 //
350 /*=====================================================================================================*/
351 
352 
353 
354 
355 /*=====================================================================================================*/
356 const std::complex<double> zI(0.0, 1.0);
357 
358 
359 
360 
361 
362 
363 
364 
365 
366 
367 
368 
369 
370 
371 
372 
const SgMJD tEphem(51544.5)
void geocentric2geodetic(const Sg3dVector &r, double &latitude, double &longitude, double &height, bool useOldEllipsoid)
void calcNutationFundArgs_IersConv1996(const SgMJD &tEpoch, double args[5])
void fft(std::complex< double > x_a[], std::complex< double > x_A[], unsigned int n, FFT_Direction dir)
void calcNutationFundArgs_IersConv2003(const SgMJD &tEpoch, double args[5])
unsigned int reverseBitOrder(unsigned int n, unsigned int k)
void calcCip2IAU1980(const SgMJD &epoch, double dX, double dY, double dPsi_1980, double dEps_1980, double dPsi_2000, double dEps_2000, double &diffPsi, double &diffEps)
const std::complex< double > zI(0.0, 1.0)
void calcNutationFundArgs_IersStds1992(const SgMJD &tEpoch, double args[5])
#define SEC2RAD
radians to arc seconds:
Definition: SgMathSupport.h:61
double signum(const double x)
Definition: SgMathSupport.h:89
#define DEG2SEC
hours to radians:
Definition: SgMathSupport.h:53
@ Z_AXIS
Definition: SgMathSupport.h:81
@ X_AXIS
Definition: SgMathSupport.h:81
@ Y_AXIS
Definition: SgMathSupport.h:81
FFT_Direction
Definition: SgMathSupport.h:82
@ FFT_Inverse
Definition: SgMathSupport.h:82
@ FFT_Forward
Definition: SgMathSupport.h:82
double at(DIRECTION i) const
Definition: Sg3dVector.h:91
Definition: SgMJD.h:59