satellite-sgp4ext.cc
Go to the documentation of this file.
1 /* ----------------------------------------------------------------
2  *
3  * sgp4ext.h
4  *
5  * this file contains extra routines needed for the main test program for sgp4.
6  * these routines are derived from the astro libraries.
7  *
8  * companion code for
9  * fundamentals of astrodynamics and applications
10  * 2007
11  * by david vallado
12  *
13  * (w) 719-573-2600, email dvallado@agi.com
14  *
15  * current :
16  * 20 apr 07 david vallado
17  * misc documentation updates
18  * changes :
19  * 14 aug 06 david vallado
20  * original baseline
21  *
22  * code from https://gitlab.inesctec.pt/pmms/ns3-satellite
23  * ---------------------------------------------------------------- */
24 
25 #include "satellite-sgp4ext.h"
26 
27 double
28 sgn(double x)
29 {
30  if (x < 0.0)
31  {
32  return -1.0;
33  }
34  else
35  {
36  return 1.0;
37  }
38 } // end sgn
39 
40 /* -----------------------------------------------------------------------------
41  *
42  * function mag
43  *
44  * this procedure finds the magnitude of a vector. the tolerance is set to
45  * 0.000001, thus the 1.0e-12 for the squared test of underflows.
46  *
47  * author : david vallado 719-573-2600 1 mar 2001
48  *
49  * inputs description range / units
50  * vec - vector
51  *
52  * outputs :
53  * vec - answer stored in fourth component
54  *
55  * locals :
56  * none.
57  *
58  * coupling :
59  * none.
60  * --------------------------------------------------------------------------- */
61 
62 double
63 mag(double x[3])
64 {
65  return sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
66 } // end mag
67 
68 /* -----------------------------------------------------------------------------
69 *
70 * procedure cross
71 *
72 * this procedure crosses two vectors.
73 *
74 * author : david vallado 719-573-2600 1 mar 2001
75 *
76 * inputs description range / units
77 * vec1 - vector number 1
78 * vec2 - vector number 2
79 *
80 * outputs :
81 * outvec - vector result of a x b
82 *
83 * locals :
84 * none.
85 *
86 * coupling :
87 * mag magnitude of a vector
88  ---------------------------------------------------------------------------- */
89 
90 void
91 cross(double vec1[3], double vec2[3], double outvec[3])
92 {
93  outvec[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
94  outvec[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
95  outvec[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
96 } // end cross
97 
98 /* -----------------------------------------------------------------------------
99  *
100  * function dot
101  *
102  * this function finds the dot product of two vectors.
103  *
104  * author : david vallado 719-573-2600 1 mar 2001
105  *
106  * inputs description range / units
107  * vec1 - vector number 1
108  * vec2 - vector number 2
109  *
110  * outputs :
111  * dot - result
112  *
113  * locals :
114  * none.
115  *
116  * coupling :
117  * none.
118  *
119  * --------------------------------------------------------------------------- */
120 
121 double
122 dot(double x[3], double y[3])
123 {
124  return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
125 } // end dot
126 
127 /* -----------------------------------------------------------------------------
128  *
129  * procedure angle
130  *
131  * this procedure calculates the angle between two vectors. the output is
132  * set to 999999.1 to indicate an undefined value. be sure to check for
133  * this at the output phase.
134  *
135  * author : david vallado 719-573-2600 1 mar 2001
136  *
137  * inputs description range / units
138  * vec1 - vector number 1
139  * vec2 - vector number 2
140  *
141  * outputs :
142  * theta - angle between the two vectors -pi to pi
143  *
144  * locals :
145  * temp - temporary real variable
146  *
147  * coupling :
148  * dot dot product of two vectors
149  * --------------------------------------------------------------------------- */
150 
151 double
152 angle(double vec1[3], double vec2[3])
153 {
154  double small, undefined, magv1, magv2, temp;
155  small = 0.00000001;
156  undefined = 999999.1;
157 
158  magv1 = mag(vec1);
159  magv2 = mag(vec2);
160 
161  if (magv1 * magv2 > small * small)
162  {
163  temp = dot(vec1, vec2) / (magv1 * magv2);
164  if (fabs(temp) > 1.0)
165  temp = sgn(temp) * 1.0;
166  return acos(temp);
167  }
168  else
169  return undefined;
170 } // end angle
171 
172 /* -----------------------------------------------------------------------------
173  *
174  * function asinh
175  *
176  * this function evaluates the inverse hyperbolic sine function.
177  *
178  * author : david vallado 719-573-2600 1 mar 2001
179  *
180  * inputs description range / units
181  * xval - angle value any real
182  *
183  * outputs :
184  * arcsinh - result any real
185  *
186  * locals :
187  * none.
188  *
189  * coupling :
190  * none.
191  *
192  * --------------------------------------------------------------------------- */
193 
194 // double asinh (double xval)
195 //{
196 // return log( xval + sqrt( xval*xval + 1.0 ) );
197 // } // end asinh
198 
199 /* -----------------------------------------------------------------------------
200  *
201  * function newtonnu
202  *
203  * this function solves keplers equation when the true anomaly is known.
204  * the mean and eccentric, parabolic, or hyperbolic anomaly is also found.
205  * the parabolic limit at 168ø is arbitrary. the hyperbolic anomaly is also
206  * limited. the hyperbolic sine is used because it's not double valued.
207  *
208  * author : david vallado 719-573-2600 27 may 2002
209  *
210  * revisions
211  * vallado - fix small 24 sep 2002
212  *
213  * inputs description range / units
214  * ecc - eccentricity 0.0 to
215  * nu - true anomaly -2pi to 2pi rad
216  *
217  * outputs :
218  * e0 - eccentric anomaly 0.0 to 2pi rad 153.02 ø
219  * m - mean anomaly 0.0 to 2pi rad 151.7425 ø
220  *
221  * locals :
222  * e1 - eccentric anomaly, next value rad
223  * sine - sine of e
224  * cose - cosine of e
225  * ktr - index
226  *
227  * coupling :
228  * asinh - arc hyperbolic sine
229  *
230  * references :
231  * vallado 2007, 85, alg 5
232  * --------------------------------------------------------------------------- */
233 
234 void
235 newtonnu(double ecc, double nu, double& e0, double& m)
236 {
237  double small, sine, cose;
238 
239  // --------------------- implementation ---------------------
240  e0 = 999999.9;
241  m = 999999.9;
242  small = 0.00000001;
243 
244  // --------------------------- circular ------------------------
245  if (fabs(ecc) < small)
246  {
247  m = nu;
248  e0 = nu;
249  }
250  else
251  // ---------------------- elliptical -----------------------
252  if (ecc < 1.0 - small)
253  {
254  sine = (sqrt(1.0 - ecc * ecc) * sin(nu)) / (1.0 + ecc * cos(nu));
255  cose = (ecc + cos(nu)) / (1.0 + ecc * cos(nu));
256  e0 = atan2(sine, cose);
257  m = e0 - ecc * sin(e0);
258  }
259  else
260  // -------------------- hyperbolic --------------------
261  if (ecc > 1.0 + small)
262  {
263  if ((ecc > 1.0) && (fabs(nu) + 0.00001 < pi - acos(1.0 / ecc)))
264  {
265  sine = (sqrt(ecc * ecc - 1.0) * sin(nu)) / (1.0 + ecc * cos(nu));
266  e0 = asinh(sine);
267  m = ecc * sinh(e0) - e0;
268  }
269  }
270  else
271  // ----------------- parabolic ---------------------
272  if (fabs(nu) < 168.0 * pi / 180.0)
273  {
274  e0 = tan(nu * 0.5);
275  m = e0 + (e0 * e0 * e0) / 3.0;
276  }
277 
278  if (ecc < 1.0)
279  {
280  m = fmod(m, 2.0 * pi);
281  if (m < 0.0)
282  m = m + 2.0 * pi;
283  e0 = fmod(e0, 2.0 * pi);
284  }
285 } // end newtonnu
286 
287 /* -----------------------------------------------------------------------------
288  *
289  * function rv2coe
290  *
291  * this function finds the classical orbital elements given the geocentric
292  * equatorial position and velocity vectors.
293  *
294  * author : david vallado 719-573-2600 21 jun 2002
295  *
296  * revisions
297  * vallado - fix special cases 5 sep 2002
298  * vallado - delete extra check in inclination code 16 oct 2002
299  * vallado - add constant file use 29 jun 2003
300  * vallado - add mu 2 apr 2007
301  *
302  * inputs description range / units
303  * r - ijk position vector km
304  * v - ijk velocity vector km / s
305  * mu - gravitational parameter km3 / s2
306  *
307  * outputs :
308  * p - semilatus rectum km
309  * a - semimajor axis km
310  * ecc - eccentricity
311  * incl - inclination 0.0 to pi rad
312  * omega - longitude of ascending node 0.0 to 2pi rad
313  * argp - argument of perigee 0.0 to 2pi rad
314  * nu - true anomaly 0.0 to 2pi rad
315  * m - mean anomaly 0.0 to 2pi rad
316  * arglat - argument of latitude (ci) 0.0 to 2pi rad
317  * truelon - true longitude (ce) 0.0 to 2pi rad
318  * lonper - longitude of periapsis (ee) 0.0 to 2pi rad
319  *
320  * locals :
321  * hbar - angular momentum h vector km2 / s
322  * ebar - eccentricity e vector
323  * nbar - line of nodes n vector
324  * c1 - v**2 - u/r
325  * rdotv - r dot v
326  * hk - hk unit vector
327  * sme - specfic mechanical energy km2 / s2
328  * i - index
329  * e - eccentric, parabolic,
330  * hyperbolic anomaly rad
331  * temp - temporary variable
332  * typeorbit - type of orbit ee, ei, ce, ci
333  *
334  * coupling :
335  * mag - magnitude of a vector
336  * cross - cross product of two vectors
337  * angle - find the angle between two vectors
338  * newtonnu - find the mean anomaly
339  *
340  * references :
341  * vallado 2007, 126, alg 9, ex 2-5
342  * --------------------------------------------------------------------------- */
343 
344 void
345 rv2coe(double r[3],
346  double v[3],
347  double mu,
348  double& p,
349  double& a,
350  double& ecc,
351  double& incl,
352  double& omega,
353  double& argp,
354  double& nu,
355  double& m,
356  double& arglat,
357  double& truelon,
358  double& lonper)
359 {
360  double undefined, small, hbar[3], nbar[3], magr, magv, magn, ebar[3], sme, rdotv, infinite,
361  temp, c1, hk, twopi, magh, halfpi, e;
362 
363  int i;
364  char typeorbit[3];
365 
366  twopi = 2.0 * pi;
367  halfpi = 0.5 * pi;
368  small = 0.00000001;
369  undefined = 999999.1;
370  infinite = 999999.9;
371 
372  // ------------------------- implementation -----------------
373  magr = mag(r);
374  magv = mag(v);
375 
376  // ------------------ find h n and e vectors ----------------
377  cross(r, v, hbar);
378  magh = mag(hbar);
379  if (magh > small)
380  {
381  nbar[0] = -hbar[1];
382  nbar[1] = hbar[0];
383  nbar[2] = 0.0;
384  magn = mag(nbar);
385  c1 = magv * magv - mu / magr;
386  rdotv = dot(r, v);
387  for (i = 0; i <= 2; i++)
388  ebar[i] = (c1 * r[i] - rdotv * v[i]) / mu;
389  ecc = mag(ebar);
390 
391  // ------------ find a e and semi-latus rectum ----------
392  sme = (magv * magv * 0.5) - (mu / magr);
393  if (fabs(sme) > small)
394  a = -mu / (2.0 * sme);
395  else
396  a = infinite;
397  p = magh * magh / mu;
398 
399  // ----------------- find inclination -------------------
400  hk = hbar[2] / magh;
401  incl = acos(hk);
402 
403  // -------- determine type of orbit for later use --------
404  // ------ elliptical, parabolic, hyperbolic inclined -------
405  strcpy(typeorbit, "ei");
406  if (ecc < small)
407  {
408  // ---------------- circular equatorial ---------------
409  if ((incl < small) | (fabs(incl - pi) < small))
410  strcpy(typeorbit, "ce");
411  else
412  // -------------- circular inclined ---------------
413  strcpy(typeorbit, "ci");
414  }
415  else
416  {
417  // - elliptical, parabolic, hyperbolic equatorial --
418  if ((incl < small) | (fabs(incl - pi) < small))
419  strcpy(typeorbit, "ee");
420  }
421 
422  // ---------- find longitude of ascending node ------------
423  if (magn > small)
424  {
425  temp = nbar[0] / magn;
426  if (fabs(temp) > 1.0)
427  temp = sgn(temp);
428  omega = acos(temp);
429  if (nbar[1] < 0.0)
430  omega = twopi - omega;
431  }
432  else
433  omega = undefined;
434 
435  // ---------------- find argument of perigee ---------------
436  if (strcmp(typeorbit, "ei") == 0)
437  {
438  argp = angle(nbar, ebar);
439  if (ebar[2] < 0.0)
440  argp = twopi - argp;
441  }
442  else
443  argp = undefined;
444 
445  // ------------ find true anomaly at epoch -------------
446  if (typeorbit[0] == 'e')
447  {
448  nu = angle(ebar, r);
449  if (rdotv < 0.0)
450  nu = twopi - nu;
451  }
452  else
453  nu = undefined;
454 
455  // ---- find argument of latitude - circular inclined -----
456  if (strcmp(typeorbit, "ci") == 0)
457  {
458  arglat = angle(nbar, r);
459  if (r[2] < 0.0)
460  arglat = twopi - arglat;
461  m = arglat;
462  }
463  else
464  arglat = undefined;
465 
466  // -- find longitude of perigee - elliptical equatorial ----
467  if ((ecc > small) && (strcmp(typeorbit, "ee") == 0))
468  {
469  temp = ebar[0] / ecc;
470  if (fabs(temp) > 1.0)
471  temp = sgn(temp);
472  lonper = acos(temp);
473  if (ebar[1] < 0.0)
474  lonper = twopi - lonper;
475  if (incl > halfpi)
476  lonper = twopi - lonper;
477  }
478  else
479  lonper = undefined;
480 
481  // -------- find true longitude - circular equatorial ------
482  if ((magr > small) && (strcmp(typeorbit, "ce") == 0))
483  {
484  temp = r[0] / magr;
485  if (fabs(temp) > 1.0)
486  temp = sgn(temp);
487  truelon = acos(temp);
488  if (r[1] < 0.0)
489  truelon = twopi - truelon;
490  if (incl > halfpi)
491  truelon = twopi - truelon;
492  m = truelon;
493  }
494  else
495  truelon = undefined;
496 
497  // ------------ find mean anomaly for all orbits -----------
498  if (typeorbit[0] == 'e')
499  newtonnu(ecc, nu, e, m);
500  }
501  else
502  {
503  p = undefined;
504  a = undefined;
505  ecc = undefined;
506  incl = undefined;
507  omega = undefined;
508  argp = undefined;
509  nu = undefined;
510  m = undefined;
511  arglat = undefined;
512  truelon = undefined;
513  lonper = undefined;
514  }
515 } // end rv2coe
516 
517 /* -----------------------------------------------------------------------------
518  *
519  * procedure jday
520  *
521  * this procedure finds the julian date given the year, month, day, and time.
522  * the julian date is defined by each elapsed day since noon, jan 1, 4713 bc.
523  *
524  * algorithm : calculate the answer in one step for efficiency
525  *
526  * author : david vallado 719-573-2600 1 mar 2001
527  *
528  * inputs description range / units
529  * year - year 1900 .. 2100
530  * mon - month 1 .. 12
531  * day - day 1 .. 28,29,30,31
532  * hr - universal time hour 0 .. 23
533  * min - universal time min 0 .. 59
534  * sec - universal time sec 0.0 .. 59.999
535  *
536  * outputs :
537  * jd - julian date days from 4713 bc
538  *
539  * locals :
540  * none.
541  *
542  * coupling :
543  * none.
544  *
545  * references :
546  * vallado 2007, 189, alg 14, ex 3-14
547  *
548  * --------------------------------------------------------------------------- */
549 
550 void
551 jday(int year, int mon, int day, int hr, int minute, double sec, double& jd)
552 {
553  jd = 367.0 * year - floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) +
554  floor(275 * mon / 9.0) + day + 1721013.5 +
555  ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days
556 } // end jday
557 
558 /* -----------------------------------------------------------------------------
559  *
560  * procedure days2mdhms
561  *
562  * this procedure converts the day of the year, days, to the equivalent month
563  * day, hour, minute and second.
564  *
565  * algorithm : set up array for the number of days per month
566  * find leap year - use 1900 because 2000 is a leap year
567  * loop through a temp value while the value is < the days
568  * perform int conversions to the correct day and month
569  * convert remainder into h m s using type conversions
570  *
571  * author : david vallado 719-573-2600 1 mar 2001
572  *
573  * inputs description range / units
574  * year - year 1900 .. 2100
575  * days - julian day of the year 0.0 .. 366.0
576  *
577  * outputs :
578  * mon - month 1 .. 12
579  * day - day 1 .. 28,29,30,31
580  * hr - hour 0 .. 23
581  * min - minute 0 .. 59
582  * sec - second 0.0 .. 59.999
583  *
584  * locals :
585  * dayofyr - day of year
586  * temp - temporary extended values
587  * inttemp - temporary int value
588  * i - index
589  * lmonth[12] - int array containing the number of days per month
590  *
591  * coupling :
592  * none.
593  * --------------------------------------------------------------------------- */
594 
595 void
596 days2mdhms(int year, double days, int& mon, int& day, int& hr, int& minute, double& sec)
597 {
598  int i, inttemp, dayofyr;
599  double temp;
600  int lmonth[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
601 
602  dayofyr = (int)floor(days);
603  /* ----------------- find month and day of month ---------------- */
604  if ((year % 4) == 0)
605  lmonth[1] = 29;
606 
607  i = 1;
608  inttemp = 0;
609  while ((dayofyr > inttemp + lmonth[i - 1]) && (i < 12))
610  {
611  inttemp = inttemp + lmonth[i - 1];
612  i++;
613  }
614  mon = i;
615  day = dayofyr - inttemp;
616 
617  /* ----------------- find hours minutes and seconds ------------- */
618  temp = (days - dayofyr) * 24.0;
619  hr = (int)floor(temp);
620  temp = (temp - hr) * 60.0;
621  minute = (int)floor(temp);
622  sec = (temp - minute) * 60.0;
623 } // end days2mdhms
624 
625 /* -----------------------------------------------------------------------------
626  *
627  * procedure invjday
628  *
629  * this procedure finds the year, month, day, hour, minute and second
630  * given the julian date. tu can be ut1, tdt, tdb, etc.
631  *
632  * algorithm : set up starting values
633  * find leap year - use 1900 because 2000 is a leap year
634  * find the elapsed days through the year in a loop
635  * call routine to find each individual value
636  *
637  * author : david vallado 719-573-2600 1 mar 2001
638  *
639  * inputs description range / units
640  * jd - julian date days from 4713 bc
641  *
642  * outputs :
643  * year - year 1900 .. 2100
644  * mon - month 1 .. 12
645  * day - day 1 .. 28,29,30,31
646  * hr - hour 0 .. 23
647  * min - minute 0 .. 59
648  * sec - second 0.0 .. 59.999
649  *
650  * locals :
651  * days - day of year plus fractional
652  * portion of a day days
653  * tu - julian centuries from 0 h
654  * jan 0, 1900
655  * temp - temporary double values
656  * leapyrs - number of leap years from 1900
657  *
658  * coupling :
659  * days2mdhms - finds month, day, hour, minute and second given days and year
660  *
661  * references :
662  * vallado 2007, 208, alg 22, ex 3-13
663  * --------------------------------------------------------------------------- */
664 
665 void
666 invjday(double jd, int& year, int& mon, int& day, int& hr, int& minute, double& sec)
667 {
668  int leapyrs;
669  double days, tu, temp;
670 
671  /* --------------- find year and days of the year --------------- */
672  temp = jd - 2415019.5;
673  tu = temp / 365.25;
674  year = 1900 + (int)floor(tu);
675  leapyrs = (int)floor((year - 1901) * 0.25);
676 
677  // optional nudge by 8.64x10-7 sec to get even outputs
678  days = temp - ((year - 1900) * 365.0 + leapyrs) + 0.00000000001;
679 
680  /* ------------ check for case of beginning of a year ----------- */
681  if (days < 1.0)
682  {
683  year = year - 1;
684  leapyrs = (int)floor((year - 1901) * 0.25);
685  days = temp - ((year - 1900) * 365.0 + leapyrs);
686  }
687 
688  /* ----------------- find remaing data ------------------------- */
689  days2mdhms(year, days, mon, day, hr, minute, sec);
690  sec = sec - 0.00000086400;
691 } // end invjday
invjday
void invjday(double jd, int &year, int &mon, int &day, int &hr, int &minute, double &sec)
Definition: satellite-sgp4ext.cc:666
cross
void cross(double vec1[3], double vec2[3], double outvec[3])
Definition: satellite-sgp4ext.cc:91
sgn
double sgn(double x)
Definition: satellite-sgp4ext.cc:28
days2mdhms
void days2mdhms(int year, double days, int &mon, int &day, int &hr, int &minute, double &sec)
Definition: satellite-sgp4ext.cc:596
newtonnu
void newtonnu(double ecc, double nu, double &e0, double &m)
Definition: satellite-sgp4ext.cc:235
angle
double angle(double vec1[3], double vec2[3])
Definition: satellite-sgp4ext.cc:152
rv2coe
void rv2coe(double r[3], double v[3], double mu, double &p, double &a, double &ecc, double &incl, double &omega, double &argp, double &nu, double &m, double &arglat, double &truelon, double &lonper)
Definition: satellite-sgp4ext.cc:345
dot
double dot(double x[3], double y[3])
Definition: satellite-sgp4ext.cc:122
mag
double mag(double x[3])
Definition: satellite-sgp4ext.cc:63
jday
void jday(int year, int mon, int day, int hr, int minute, double sec, double &jd)
Definition: satellite-sgp4ext.cc:551
pi
#define pi
Definition: satellite-sgp4unit.h:59