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