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