File indexing completed on 2024-04-21 07:34:54
0001 /* ---------------------------------------------------------------- 0002 * 0003 * sgp4ext.cpp 0004 * 0005 * this file contains extra routines needed for the main test program for sgp4. 0006 * these routines are derived from the astro libraries. 0007 * 0008 * companion code for 0009 * fundamentals of astrodynamics and applications 0010 * 2007 0011 * by david vallado 0012 * 0013 * (w) 719-573-2600, email dvallado@agi.com 0014 * 0015 * current : 0016 * 7 may 08 david vallado 0017 * fix sgn 0018 * changes : 0019 * 2 apr 07 david vallado 0020 * fix jday floor and str lengths 0021 * updates for constants 0022 * 14 aug 06 david vallado 0023 * original baseline 0024 * ---------------------------------------------------------------- */ 0025 0026 #include "sgp4ext.h" 0027 0028 0029 double sgn 0030 ( 0031 double x 0032 ) 0033 { 0034 if (x < 0.0) 0035 { 0036 return -1.0; 0037 } 0038 else 0039 { 0040 return 1.0; 0041 } 0042 0043 } // end sgn 0044 0045 /* ----------------------------------------------------------------------------- 0046 * 0047 * function mag 0048 * 0049 * this procedure finds the magnitude of a vector. the tolerance is set to 0050 * 0.000001, thus the 1.0e-12 for the squared test of underflows. 0051 * 0052 * author : david vallado 719-573-2600 1 mar 2001 0053 * 0054 * inputs description range / units 0055 * vec - vector 0056 * 0057 * outputs : 0058 * vec - answer stored in fourth component 0059 * 0060 * locals : 0061 * none. 0062 * 0063 * coupling : 0064 * none. 0065 * --------------------------------------------------------------------------- */ 0066 0067 double mag 0068 ( 0069 double x[3] 0070 ) 0071 { 0072 return sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]); 0073 } // end mag 0074 0075 /* ----------------------------------------------------------------------------- 0076 * 0077 * procedure cross 0078 * 0079 * this procedure crosses two vectors. 0080 * 0081 * author : david vallado 719-573-2600 1 mar 2001 0082 * 0083 * inputs description range / units 0084 * vec1 - vector number 1 0085 * vec2 - vector number 2 0086 * 0087 * outputs : 0088 * outvec - vector result of a x b 0089 * 0090 * locals : 0091 * none. 0092 * 0093 * coupling : 0094 * mag magnitude of a vector 0095 ---------------------------------------------------------------------------- */ 0096 0097 void cross 0098 ( 0099 double vec1[3], double vec2[3], double outvec[3] 0100 ) 0101 { 0102 outvec[0]= vec1[1]*vec2[2] - vec1[2]*vec2[1]; 0103 outvec[1]= vec1[2]*vec2[0] - vec1[0]*vec2[2]; 0104 outvec[2]= vec1[0]*vec2[1] - vec1[1]*vec2[0]; 0105 } // end cross 0106 0107 0108 /* ----------------------------------------------------------------------------- 0109 * 0110 * function dot 0111 * 0112 * this function finds the dot product of two vectors. 0113 * 0114 * author : david vallado 719-573-2600 1 mar 2001 0115 * 0116 * inputs description range / units 0117 * vec1 - vector number 1 0118 * vec2 - vector number 2 0119 * 0120 * outputs : 0121 * dot - result 0122 * 0123 * locals : 0124 * none. 0125 * 0126 * coupling : 0127 * none. 0128 * 0129 * --------------------------------------------------------------------------- */ 0130 0131 double dot 0132 ( 0133 double x[3], double y[3] 0134 ) 0135 { 0136 return (x[0]*y[0] + x[1]*y[1] + x[2]*y[2]); 0137 } // end dot 0138 0139 /* ----------------------------------------------------------------------------- 0140 * 0141 * procedure angle 0142 * 0143 * this procedure calculates the angle between two vectors. the output is 0144 * set to 999999.1 to indicate an undefined value. be sure to check for 0145 * this at the output phase. 0146 * 0147 * author : david vallado 719-573-2600 1 mar 2001 0148 * 0149 * inputs description range / units 0150 * vec1 - vector number 1 0151 * vec2 - vector number 2 0152 * 0153 * outputs : 0154 * theta - angle between the two vectors -pi to pi 0155 * 0156 * locals : 0157 * temp - temporary real variable 0158 * 0159 * coupling : 0160 * dot dot product of two vectors 0161 * --------------------------------------------------------------------------- */ 0162 0163 double angle 0164 ( 0165 double vec1[3], 0166 double vec2[3] 0167 ) 0168 { 0169 double small, undefined, magv1, magv2, temp; 0170 small = 0.00000001; 0171 undefined = 999999.1; 0172 0173 magv1 = mag(vec1); 0174 magv2 = mag(vec2); 0175 0176 if (magv1*magv2 > small*small) 0177 { 0178 temp= dot(vec1,vec2) / (magv1*magv2); 0179 if (fabs( temp ) > 1.0) 0180 temp= sgn(temp) * 1.0; 0181 return acos( temp ); 0182 } 0183 else 0184 return undefined; 0185 } // end angle 0186 0187 0188 /* ----------------------------------------------------------------------------- 0189 * 0190 * function newtonnu 0191 * 0192 * this function solves keplers equation when the true anomaly is known. 0193 * the mean and eccentric, parabolic, or hyperbolic anomaly is also found. 0194 * the parabolic limit at 168ø is arbitrary. the hyperbolic anomaly is also 0195 * limited. the hyperbolic sine is used because it's not double valued. 0196 * 0197 * author : david vallado 719-573-2600 27 may 2002 0198 * 0199 * revisions 0200 * vallado - fix small 24 sep 2002 0201 * 0202 * inputs description range / units 0203 * ecc - eccentricity 0.0 to 0204 * nu - true anomaly -2pi to 2pi rad 0205 * 0206 * outputs : 0207 * e0 - eccentric anomaly 0.0 to 2pi rad 153.02 ø 0208 * m - mean anomaly 0.0 to 2pi rad 151.7425 ø 0209 * 0210 * locals : 0211 * e1 - eccentric anomaly, next value rad 0212 * sine - sine of e 0213 * cose - cosine of e 0214 * ktr - index 0215 * 0216 * coupling : 0217 * asinh - arc hyperbolic sine 0218 * 0219 * references : 0220 * vallado 2007, 85, alg 5 0221 * --------------------------------------------------------------------------- */ 0222 0223 void newtonnu 0224 ( 0225 double ecc, double nu, 0226 double& e0, double& m 0227 ) 0228 { 0229 double small, sine, cose; 0230 0231 // --------------------- implementation --------------------- 0232 e0= 999999.9; 0233 m = 999999.9; 0234 small = 0.00000001; 0235 0236 // --------------------------- circular ------------------------ 0237 if ( fabs( ecc ) < small ) 0238 { 0239 m = nu; 0240 e0= nu; 0241 } 0242 else 0243 // ---------------------- elliptical ----------------------- 0244 if ( ecc < 1.0-small ) 0245 { 0246 sine= ( sqrt( 1.0 -ecc*ecc ) * sin(nu) ) / ( 1.0 +ecc*cos(nu) ); 0247 cose= ( ecc + cos(nu) ) / ( 1.0 + ecc*cos(nu) ); 0248 e0 = atan2( sine,cose ); 0249 m = e0 - ecc*sin(e0); 0250 } 0251 else 0252 // -------------------- hyperbolic -------------------- 0253 if ( ecc > 1.0 + small ) 0254 { 0255 if ((ecc > 1.0 ) && (fabs(nu)+0.00001 < M_PI-acos(1.0 /ecc))) 0256 { 0257 sine= ( sqrt( ecc*ecc-1.0 ) * sin(nu) ) / ( 1.0 + ecc*cos(nu) ); 0258 e0 = asinh( sine ); 0259 m = ecc*sinh(e0) - e0; 0260 } 0261 } 0262 else 0263 // ----------------- parabolic --------------------- 0264 if ( fabs(nu) < 168.0*M_PI/180.0 ) 0265 { 0266 e0= tan( nu*0.5 ); 0267 m = e0 + (e0*e0*e0)/3.0; 0268 } 0269 0270 if ( ecc < 1.0 ) 0271 { 0272 m = fmod( m,2.0 *M_PI ); 0273 if ( m < 0.0 ) 0274 m = m + 2.0 *M_PI; 0275 e0 = fmod( e0,2.0 *M_PI ); 0276 } 0277 } // end newtonnu 0278 0279 0280 /* ----------------------------------------------------------------------------- 0281 * 0282 * function rv2coe 0283 * 0284 * this function finds the classical orbital elements given the geocentric 0285 * equatorial position and velocity vectors. 0286 * 0287 * author : david vallado 719-573-2600 21 jun 2002 0288 * 0289 * revisions 0290 * vallado - fix special cases 5 sep 2002 0291 * vallado - delete extra check in inclination code 16 oct 2002 0292 * vallado - add constant file use 29 jun 2003 0293 * vallado - add mu 2 apr 2007 0294 * 0295 * inputs description range / units 0296 * r - ijk position vector km 0297 * v - ijk velocity vector km / s 0298 * mu - gravitational parameter km3 / s2 0299 * 0300 * outputs : 0301 * p - semilatus rectum km 0302 * a - semimajor axis km 0303 * ecc - eccentricity 0304 * incl - inclination 0.0 to pi rad 0305 * omega - longitude of ascending node 0.0 to 2pi rad 0306 * argp - argument of perigee 0.0 to 2pi rad 0307 * nu - true anomaly 0.0 to 2pi rad 0308 * m - mean anomaly 0.0 to 2pi rad 0309 * arglat - argument of latitude (ci) 0.0 to 2pi rad 0310 * truelon - true longitude (ce) 0.0 to 2pi rad 0311 * lonper - longitude of periapsis (ee) 0.0 to 2pi rad 0312 * 0313 * locals : 0314 * hbar - angular momentum h vector km2 / s 0315 * ebar - eccentricity e vector 0316 * nbar - line of nodes n vector 0317 * c1 - v**2 - u/r 0318 * rdotv - r dot v 0319 * hk - hk unit vector 0320 * sme - specific mechanical energy km2 / s2 0321 * i - index 0322 * e - eccentric, parabolic, 0323 * hyperbolic anomaly rad 0324 * temp - temporary variable 0325 * typeorbit - type of orbit ee, ei, ce, ci 0326 * 0327 * coupling : 0328 * mag - magnitude of a vector 0329 * cross - cross product of two vectors 0330 * angle - find the angle between two vectors 0331 * newtonnu - find the mean anomaly 0332 * 0333 * references : 0334 * vallado 2007, 126, alg 9, ex 2-5 0335 * --------------------------------------------------------------------------- */ 0336 0337 void rv2coe 0338 ( 0339 double r[3], double v[3], double mu, 0340 double& p, double& a, double& ecc, double& incl, double& omega, double& argp, 0341 double& nu, double& m, double& arglat, double& truelon, double& lonper 0342 ) 0343 { 0344 double undefined, small, hbar[3], nbar[3], magr, magv, magn, ebar[3], sme, 0345 rdotv, infinite, temp, c1, hk, twopi, magh, halfpi, e; 0346 0347 int i; 0348 char typeorbit[3]; 0349 0350 twopi = 2.0 * M_PI; 0351 halfpi = 0.5 * M_PI; 0352 small = 0.00000001; 0353 undefined = 999999.1; 0354 infinite = 999999.9; 0355 0356 // ------------------------- implementation ----------------- 0357 magr = mag( r ); 0358 magv = mag( v ); 0359 0360 // ------------------ find h n and e vectors ---------------- 0361 cross( r,v, hbar ); 0362 magh = mag( hbar ); 0363 if ( magh > small ) 0364 { 0365 nbar[0]= -hbar[1]; 0366 nbar[1]= hbar[0]; 0367 nbar[2]= 0.0; 0368 magn = mag( nbar ); 0369 c1 = magv*magv - mu /magr; 0370 rdotv = dot( r,v ); 0371 for (i= 0; i <= 2; i++) 0372 ebar[i]= (c1*r[i] - rdotv*v[i])/mu; 0373 ecc = mag( ebar ); 0374 0375 // ------------ find a e and semi-latus rectum ---------- 0376 sme= ( magv*magv*0.5 ) - ( mu /magr ); 0377 if ( fabs( sme ) > small ) 0378 a= -mu / (2.0 *sme); 0379 else 0380 a= infinite; 0381 p = magh*magh/mu; 0382 0383 // ----------------- find inclination ------------------- 0384 hk= hbar[2]/magh; 0385 incl= acos( hk ); 0386 0387 // -------- determine type of orbit for later use -------- 0388 // ------ elliptical, parabolic, hyperbolic inclined ------- 0389 strcpy(typeorbit,"ei"); 0390 if ( ecc < small ) 0391 { 0392 // ---------------- circular equatorial --------------- 0393 if ((incl<small) | (fabs(incl-M_PI)<small)) 0394 strcpy(typeorbit,"ce"); 0395 else 0396 // -------------- circular inclined --------------- 0397 strcpy(typeorbit,"ci"); 0398 } 0399 else 0400 { 0401 // - elliptical, parabolic, hyperbolic equatorial -- 0402 if ((incl<small) | (fabs(incl-M_PI)<small)) 0403 strcpy(typeorbit,"ee"); 0404 } 0405 0406 // ---------- find longitude of ascending node ------------ 0407 if ( magn > small ) 0408 { 0409 temp= nbar[0] / magn; 0410 if ( fabs(temp) > 1.0 ) 0411 temp= sgn(temp); 0412 omega= acos( temp ); 0413 if ( nbar[1] < 0.0 ) 0414 omega= twopi - omega; 0415 } 0416 else 0417 omega= undefined; 0418 0419 // ---------------- find argument of perigee --------------- 0420 if ( strcmp(typeorbit,"ei") == 0 ) 0421 { 0422 argp = angle( nbar,ebar); 0423 if ( ebar[2] < 0.0 ) 0424 argp= twopi - argp; 0425 } 0426 else 0427 argp= undefined; 0428 0429 // ------------ find true anomaly at epoch ------------- 0430 if ( typeorbit[0] == 'e' ) 0431 { 0432 nu = angle( ebar,r); 0433 if ( rdotv < 0.0 ) 0434 nu= twopi - nu; 0435 } 0436 else 0437 nu= undefined; 0438 0439 // ---- find argument of latitude - circular inclined ----- 0440 if ( strcmp(typeorbit,"ci") == 0 ) 0441 { 0442 arglat = angle( nbar,r ); 0443 if ( r[2] < 0.0 ) 0444 arglat= twopi - arglat; 0445 m = arglat; 0446 } 0447 else 0448 arglat= undefined; 0449 0450 // -- find longitude of perigee - elliptical equatorial ---- 0451 if (( ecc>small ) && (strcmp(typeorbit,"ee") == 0)) 0452 { 0453 temp= ebar[0]/ecc; 0454 if ( fabs(temp) > 1.0 ) 0455 temp= sgn(temp); 0456 lonper= acos( temp ); 0457 if ( ebar[1] < 0.0 ) 0458 lonper= twopi - lonper; 0459 if ( incl > halfpi ) 0460 lonper= twopi - lonper; 0461 } 0462 else 0463 lonper= undefined; 0464 0465 // -------- find true longitude - circular equatorial ------ 0466 if (( magr>small ) && ( strcmp(typeorbit,"ce") == 0 )) 0467 { 0468 temp= r[0]/magr; 0469 if ( fabs(temp) > 1.0 ) 0470 temp= sgn(temp); 0471 truelon= acos( temp ); 0472 if ( r[1] < 0.0 ) 0473 truelon= twopi - truelon; 0474 if ( incl > halfpi ) 0475 truelon= twopi - truelon; 0476 m = truelon; 0477 } 0478 else 0479 truelon= undefined; 0480 0481 // ------------ find mean anomaly for all orbits ----------- 0482 if ( typeorbit[0] == 'e' ) 0483 newtonnu(ecc,nu, e, m); 0484 } 0485 else 0486 { 0487 p = undefined; 0488 a = undefined; 0489 ecc = undefined; 0490 incl = undefined; 0491 omega= undefined; 0492 argp = undefined; 0493 nu = undefined; 0494 m = undefined; 0495 arglat = undefined; 0496 truelon= undefined; 0497 lonper = undefined; 0498 } 0499 } // end rv2coe 0500 0501 /* ----------------------------------------------------------------------------- 0502 * 0503 * procedure jday 0504 * 0505 * this procedure finds the julian date given the year, month, day, and time. 0506 * the julian date is defined by each elapsed day since noon, jan 1, 4713 bc. 0507 * 0508 * algorithm : calculate the answer in one step for efficiency 0509 * 0510 * author : david vallado 719-573-2600 1 mar 2001 0511 * 0512 * inputs description range / units 0513 * year - year 1900 .. 2100 0514 * mon - month 1 .. 12 0515 * day - day 1 .. 28,29,30,31 0516 * hr - universal time hour 0 .. 23 0517 * min - universal time min 0 .. 59 0518 * sec - universal time sec 0.0 .. 59.999 0519 * 0520 * outputs : 0521 * jd - julian date days from 4713 bc 0522 * 0523 * locals : 0524 * none. 0525 * 0526 * coupling : 0527 * none. 0528 * 0529 * references : 0530 * vallado 2007, 189, alg 14, ex 3-14 0531 * 0532 * --------------------------------------------------------------------------- */ 0533 0534 void jday 0535 ( 0536 int year, int mon, int day, int hr, int minute, double sec, 0537 double& jd 0538 ) 0539 { 0540 jd = 367.0 * year - 0541 floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) + 0542 floor( 275 * mon / 9.0 ) + 0543 day + 1721013.5 + 0544 ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days 0545 // - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5; 0546 } // end jday 0547 0548 /* ----------------------------------------------------------------------------- 0549 * 0550 * procedure days2mdhms 0551 * 0552 * this procedure converts the day of the year, days, to the equivalent month 0553 * day, hour, minute and second. 0554 * 0555 * algorithm : set up array for the number of days per month 0556 * find leap year - use 1900 because 2000 is a leap year 0557 * loop through a temp value while the value is < the days 0558 * perform int conversions to the correct day and month 0559 * convert remainder into h m s using type conversions 0560 * 0561 * author : david vallado 719-573-2600 1 mar 2001 0562 * 0563 * inputs description range / units 0564 * year - year 1900 .. 2100 0565 * days - julian day of the year 0.0 .. 366.0 0566 * 0567 * outputs : 0568 * mon - month 1 .. 12 0569 * day - day 1 .. 28,29,30,31 0570 * hr - hour 0 .. 23 0571 * min - minute 0 .. 59 0572 * sec - second 0.0 .. 59.999 0573 * 0574 * locals : 0575 * dayofyr - day of year 0576 * temp - temporary extended values 0577 * inttemp - temporary int value 0578 * i - index 0579 * lmonth[12] - int array containing the number of days per month 0580 * 0581 * coupling : 0582 * none. 0583 * --------------------------------------------------------------------------- */ 0584 0585 void days2mdhms 0586 ( 0587 int year, double days, 0588 int& mon, int& day, int& hr, int& minute, double& sec 0589 ) 0590 { 0591 int i, inttemp, dayofyr; 0592 double temp; 0593 int lmonth[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; 0594 0595 dayofyr = (int)floor(days); 0596 /* ----------------- find month and day of month ---------------- */ 0597 if ( (year % 4) == 0 ) 0598 lmonth[1] = 29; 0599 0600 i = 1; 0601 inttemp = 0; 0602 while ((dayofyr > inttemp + lmonth[i-1]) && (i < 12)) 0603 { 0604 inttemp = inttemp + lmonth[i-1]; 0605 i++; 0606 } 0607 mon = i; 0608 day = dayofyr - inttemp; 0609 0610 /* ----------------- find hours minutes and seconds ------------- */ 0611 temp = (days - dayofyr) * 24.0; 0612 hr = (int)floor(temp); 0613 temp = (temp - hr) * 60.0; 0614 minute = (int)floor(temp); 0615 sec = (temp - minute) * 60.0; 0616 } // end days2mdhms 0617 0618 /* ----------------------------------------------------------------------------- 0619 * 0620 * procedure invjday 0621 * 0622 * this procedure finds the year, month, day, hour, minute and second 0623 * given the julian date. tu can be ut1, tdt, tdb, etc. 0624 * 0625 * algorithm : set up starting values 0626 * find leap year - use 1900 because 2000 is a leap year 0627 * find the elapsed days through the year in a loop 0628 * call routine to find each individual value 0629 * 0630 * author : david vallado 719-573-2600 1 mar 2001 0631 * 0632 * inputs description range / units 0633 * jd - julian date days from 4713 bc 0634 * 0635 * outputs : 0636 * year - year 1900 .. 2100 0637 * mon - month 1 .. 12 0638 * day - day 1 .. 28,29,30,31 0639 * hr - hour 0 .. 23 0640 * min - minute 0 .. 59 0641 * sec - second 0.0 .. 59.999 0642 * 0643 * locals : 0644 * days - day of year plus fractional 0645 * portion of a day days 0646 * tu - julian centuries from 0 h 0647 * jan 0, 1900 0648 * temp - temporary double values 0649 * leapyrs - number of leap years from 1900 0650 * 0651 * coupling : 0652 * days2mdhms - finds month, day, hour, minute and second given days and year 0653 * 0654 * references : 0655 * vallado 2007, 208, alg 22, ex 3-13 0656 * --------------------------------------------------------------------------- */ 0657 0658 void invjday 0659 ( 0660 double jd, 0661 int& year, int& mon, int& day, 0662 int& hr, int& minute, double& sec 0663 ) 0664 { 0665 int leapyrs; 0666 double days, tu, temp; 0667 0668 /* --------------- find year and days of the year --------------- */ 0669 temp = jd - 2415019.5; 0670 tu = temp / 365.25; 0671 year = 1900 + (int)floor(tu); 0672 leapyrs = (int)floor((year - 1901) * 0.25); 0673 0674 // optional nudge by 8.64x10-7 sec to get even outputs 0675 days = temp - ((year - 1900) * 365.0 + leapyrs) + 0.00000000001; 0676 0677 /* ------------ check for case of beginning of a year ----------- */ 0678 if (days < 1.0) 0679 { 0680 year = year - 1; 0681 leapyrs = (int)floor((year - 1901) * 0.25); 0682 days = temp - ((year - 1900) * 365.0 + leapyrs); 0683 } 0684 0685 /* ----------------- find residual data ------------------------- */ 0686 days2mdhms(year, days, mon, day, hr, minute, sec); 0687 sec = sec - 0.00000086400; 0688 } // end invjday 0689 0690 0691 0692 0693