001 /*
002 * $Id: SGP4utils.java,v 1.3 2012/02/19 17:35:39 davep Exp $
003 *
004 * This file is part of McIDAS-V
005 *
006 * Copyright 2007-2012
007 * Space Science and Engineering Center (SSEC)
008 * University of Wisconsin - Madison
009 * 1225 W. Dayton Street, Madison, WI 53706, USA
010 * https://www.ssec.wisc.edu/mcidas
011 *
012 * All Rights Reserved
013 *
014 * McIDAS-V is built on Unidata's IDV and SSEC's VisAD libraries, and
015 * some McIDAS-V source code is based on IDV and VisAD source code.
016 *
017 * McIDAS-V is free software; you can redistribute it and/or modify
018 * it under the terms of the GNU Lesser Public License as published by
019 * the Free Software Foundation; either version 3 of the License, or
020 * (at your option) any later version.
021 *
022 * McIDAS-V is distributed in the hope that it will be useful,
023 * but WITHOUT ANY WARRANTY; without even the implied warranty of
024 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
025 * GNU Lesser Public License for more details.
026 *
027 * You should have received a copy of the GNU Lesser Public License
028 * along with this program. If not, see http://www.gnu.org/licenses.
029 */
030 /** ----------------------------------------------------------------
031 * Contains functions to read TLE data files and initalize the SGP4 propogator
032 * as well as other routines from sgp4ext.cpp
033 *
034 * <p>
035 * sgp4ext.cpp header information:
036 * <p>
037 *
038 * sgp4ext.cpp
039 *
040 * this file contains extra routines needed for the main test program for sgp4.
041 * these routines are derived from the astro libraries.
042 *
043 * companion code for
044 * fundamentals of astrodynamics and applications
045 * 2007
046 * by david vallado
047 *
048 * (w) 719-573-2600, email dvallado@agi.com
049 *
050 * current :
051 * 7 may 08 david vallado
052 * fix sgn
053 * changes :
054 * 2 apr 07 david vallado
055 * fix jday floor and str lengths
056 * updates for constants
057 * 14 aug 06 david vallado
058 * original baseline
059 * ---------------------------------------------------------------- */
060
061 package edu.wisc.ssec.mcidasv.data.adde.sgp4;
062
063 import java.text.DecimalFormat;
064 import java.text.DecimalFormatSymbols;
065 import java.text.ParsePosition;
066
067 /**
068 * 19 June 2009
069 * @author Shawn E. Gano, shawn@gano.name
070 */
071 public class SGP4utils
072 {
073 public static double pi = SGP4unit.pi;
074 public static char OPSMODE_AFSPC = 'a';
075 public static char OPSMODE_IMPROVED = 'i';
076
077 /**
078 * Reads the data from the TLE and initializes the SGP4 propogator variables and stores them in the SGP4unit.Gravconsttype object
079 * DOES NOT PERFORM ANY INTERNAL CHECK BEYOND BASICS OF THE TLE DATA use other methods to do that if desired.
080 *
081 * @param satName
082 * @param line1 TLE line 1
083 * @param line2 TLE line 2
084 * @param opsmode
085 * @param whichconst which constants to use in propogation
086 * @param satrec object to store the SGP4 data
087 * @return if the sgp4 propogator was initialized properly
088 */
089 public static boolean readTLEandIniSGP4(String satName, String line1, String line2, char opsmode, SGP4unit.Gravconsttype whichconst, SGP4SatData satrec)
090 {
091 final double deg2rad = pi / 180.0; // 0.0174532925199433
092 final double xpdotp = 1440.0 / (2.0 * pi); // 229.1831180523293
093
094 double sec, tumin;//, mu, radiusearthkm, xke, j2, j3, j4, j3oj2;
095 // double startsec, stopsec, startdayofyr, stopdayofyr, jdstart, jdstop;
096 // int startyear, stopyear, startmon, stopmon, startday, stopday,
097 // starthr, stophr, startmin, stopmin;
098 // int cardnumb, j; // numb,
099 //long revnum = 0, elnum = 0;
100 //char classification, intldesg[11], tmpstr[80];
101 int year = 0;
102 int mon, day, hr, minute;//, nexp, ibexp;
103
104 double[] temp = SGP4unit.getgravconst(whichconst);
105 tumin = temp[0];
106 // mu = temp[1];
107 // radiusearthkm = temp[2];
108 // xke = temp[3];
109 // j2 = temp[4];
110 // j3 = temp[5];
111 // j4 = temp[6];
112 // j3oj2 = temp[7];
113
114 satrec.error = 0;
115
116 satrec.name = satName;
117
118 // SEG -- save gravity - moved to SGP4unit.sgp4init fo consistancy
119 //satrec.gravconsttype = whichconst;
120
121 // get variables from the two lines
122 satrec.line1 = line1;
123 try
124 {
125 readLine1(line1, satrec);
126 }
127 catch(Exception e)
128 {
129 System.out.println("Error Reading TLE line 1: " + e.toString());
130 satrec.tleDataOk = false;
131 satrec.error = 7;
132 return false;
133 }
134
135 satrec.line2 = line2;
136 try
137 {
138 readLine2(line2, satrec);
139 }
140 catch(Exception e)
141 {
142 System.out.println("Error Reading TLE line 2: " + e.toString());
143 satrec.tleDataOk = false;
144 satrec.error = 7;
145 return false;
146 }
147
148
149 // ---- find no, ndot, nddot ----
150 satrec.no = satrec.no / xpdotp; //* rad/min
151 satrec.nddot = satrec.nddot * Math.pow(10.0, satrec.nexp);
152 satrec.bstar = satrec.bstar * Math.pow(10.0, satrec.ibexp);
153
154 // ---- convert to sgp4 units ----
155 satrec.a = Math.pow(satrec.no * tumin, (-2.0 / 3.0));
156 satrec.ndot = satrec.ndot / (xpdotp * 1440.0); //* ? * minperday
157 satrec.nddot = satrec.nddot / (xpdotp * 1440.0 * 1440);
158
159 // ---- find standard orbital elements ----
160 satrec.inclo = satrec.inclo * deg2rad;
161 satrec.nodeo = satrec.nodeo * deg2rad;
162 satrec.argpo = satrec.argpo * deg2rad;
163 satrec.mo = satrec.mo * deg2rad;
164
165 satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0;
166 satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0;
167
168 // ----------------------------------------------------------------
169 // find sgp4epoch time of element set
170 // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch)
171 // and minutes from the epoch (time)
172 // ----------------------------------------------------------------
173
174 // ---------------- temp fix for years from 1957-2056 -------------------
175 // --------- correct fix will occur when year is 4-digit in tle ---------
176 if(satrec.epochyr < 57)
177 {
178 year = satrec.epochyr + 2000;
179 }
180 else
181 {
182 year = satrec.epochyr + 1900;
183 }
184
185 // computes the m/d/hr/min/sec from year and epoch days
186 MDHMS mdhms = days2mdhms(year, satrec.epochdays);
187 mon = mdhms.mon;
188 day = mdhms.day;
189 hr = mdhms.hr;
190 minute = mdhms.minute;
191 sec = mdhms.sec;
192 // computes the jd from m/d/...
193 satrec.jdsatepoch = jday(year, mon, day, hr, minute, sec);
194
195 // ---------------- initialize the orbit at sgp4epoch -------------------
196 boolean result = SGP4unit.sgp4init(whichconst, opsmode, satrec.satnum,
197 satrec.jdsatepoch - 2433281.5, satrec.bstar,
198 satrec.ecco, satrec.argpo, satrec.inclo, satrec.mo, satrec.no,
199 satrec.nodeo, satrec);
200
201 return result;
202
203 } // readTLEandIniSGP4
204
205 private static boolean readLine1(String line1, SGP4SatData satrec) throws Exception
206 {
207 String tleLine1 = line1; // first line
208 if(!tleLine1.startsWith("1 "))
209 {
210 throw new Exception("TLE line 1 not valid first line");
211 }
212
213 // satnum
214 satrec.satnum = (int)readFloatFromString(tleLine1.substring(2, 7));
215 // classification
216 satrec.classification = tleLine1.substring(7, 8); // 1 char
217 // intln designator
218 satrec.intldesg = tleLine1.substring(9, 17); // should be within 8
219
220 // epochyr
221 satrec.epochyr = (int)readFloatFromString(tleLine1.substring(18, 20));
222
223 // epoch days
224 satrec.epochdays = readFloatFromString(tleLine1.substring(20, 32));
225
226 // ndot
227 satrec.ndot = readFloatFromString(tleLine1.substring(33, 43));
228
229 // nddot
230 //nexp
231 if((tleLine1.substring(44, 52)).equals(" "))
232 {
233 satrec.nddot = 0;
234 satrec.nexp = 0;
235 }
236 else
237 {
238 satrec.nddot = readFloatFromString(tleLine1.substring(44, 50)) / 1.0E5;
239 //nexp
240 satrec.nexp = (int)readFloatFromString(tleLine1.substring(50, 52));
241 }
242 //bstar
243 satrec.bstar = readFloatFromString(tleLine1.substring(53, 59)) / 1.0E5;
244 //ibex
245 satrec.ibexp = (int)readFloatFromString(tleLine1.substring(59, 61));
246
247 // these last things are not essential so just try to read them, and give a warning - but no error
248 try
249 {
250 // num b.
251 satrec.numb = (int)readFloatFromString(tleLine1.substring(62, 63));
252
253 // elnum
254 satrec.elnum = (long)readFloatFromString(tleLine1.substring(64, 68));
255 }
256 catch(Exception e)
257 {
258 System.out.println("Warning: Error Reading numb or elnum from TLE line 1 sat#:" + satrec.satnum);
259 }
260
261 // checksum
262 //int checksum1 = (int) readFloatFromString(tleLine1.substring(68));
263
264 // if no errors yet everything went ok
265 return true;
266 } // readLine1
267
268 private static boolean readLine2(String line2, SGP4SatData satrec) throws Exception
269 {
270 /* Read the second line of elements. */
271
272 //theLine = aFile.readLine();
273 String tleLine2 = line2; // second line
274 if(!tleLine2.startsWith("2 "))
275 {
276 throw new Exception("TLE line 2 not valid second line");
277 }
278
279 // satnum
280 int satnum = (int)readFloatFromString(tleLine2.substring(2, 7));
281 if(satnum != satrec.satnum)
282 {
283 System.out.println("Warning TLE line 2 Sat Num doesn't match line1 for sat: " + satrec.name);
284 }
285
286 // inclination
287 satrec.inclo = readFloatFromString(tleLine2.substring(8, 17));
288
289 // nodeo
290 satrec.nodeo = readFloatFromString(tleLine2.substring(17, 26));
291
292 //satrec.ecco
293 satrec.ecco = readFloatFromString(tleLine2.substring(26, 34)) / 1.0E7;
294
295 // satrec.argpo
296 satrec.argpo = readFloatFromString(tleLine2.substring(34, 43));
297
298 // satrec.mo
299 satrec.mo = readFloatFromString(tleLine2.substring(43, 52));
300
301 // no
302 satrec.no = readFloatFromString(tleLine2.substring(52, 63));
303
304 // try to read other data
305 try
306 {
307 // revnum
308 satrec.revnum = (long)readFloatFromString(tleLine2.substring(63, 68));
309 }
310 catch(Exception e)
311 {
312 System.out.println("Warning: Error Reading revnum from TLE line 2 sat#:" + satrec.satnum + "\n" + e.toString());
313 satrec.revnum = -1;
314 }
315
316 // // checksum
317 // int checksum2 = (int) readFloatFromString(tleLine2.substring(68));
318
319 return true;
320 } // readLine1
321
322 /**
323 * Read float data from a string
324 * @param inStr
325 * @return
326 * @throws Exception
327 */
328 protected static double readFloatFromString(String inStr) throws Exception
329 {
330 // make sure decimal sparator is '.' so it works in other countries
331 // because of this can't use Double.parse
332 DecimalFormat dformat = new DecimalFormat("#");
333 DecimalFormatSymbols dfs = new DecimalFormatSymbols();
334 dfs.setDecimalSeparator('.');
335 dformat.setDecimalFormatSymbols(dfs);
336
337 // trim white space and if there is a + at the start
338 String trimStr = inStr.trim();
339 if(trimStr.startsWith("+"))
340 {
341 trimStr = trimStr.substring(1);
342 }
343
344 // parse until we hit the end or invalid char
345 ParsePosition pp = new ParsePosition(0);
346 Number num = dformat.parse(trimStr, pp);
347 if(null == num)
348 {
349 throw new Exception("Invalid Float In TLE");
350 }
351
352 return num.doubleValue();
353 } // readFloatFromString
354
355 /** -----------------------------------------------------------------------------
356 *
357 * procedure jday
358 *
359 * this procedure finds the julian date given the year, month, day, and time.
360 * the julian date is defined by each elapsed day since noon, jan 1, 4713 bc.
361 *
362 * algorithm : calculate the answer in one step for efficiency
363 *
364 * author : david vallado 719-573-2600 1 mar 2001
365 *
366 * inputs description range / units
367 * year - year 1900 .. 2100
368 * mon - month 1 .. 12
369 * day - day 1 .. 28,29,30,31
370 * hr - universal time hour 0 .. 23
371 * min - universal time min 0 .. 59
372 * sec - universal time sec 0.0 .. 59.999
373 *
374 * outputs :
375 * jd - julian date days from 4713 bc
376 *
377 * locals :
378 * none.
379 *
380 * coupling :
381 * none.
382 *
383 * references :
384 * vallado 2007, 189, alg 14, ex 3-14
385 *
386 * ---------------------------------------------------------------------------
387 * @param year
388 * @param mon
389 * @param day
390 * @param hr
391 * @param minute
392 * @param sec
393 * @return
394 */
395 public static double jday(
396 int year, int mon, int day, int hr, int minute, double sec//,
397 //double& jd
398 )
399 {
400 double jd;
401 jd = 367.0 * year -
402 Math.floor((7 * (year + Math.floor((mon + 9) / 12.0))) * 0.25) +
403 Math.floor(275 * mon / 9.0) +
404 day + 1721013.5 +
405 ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days
406 // - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5;
407
408 return jd;
409 } // end jday
410
411 /* -----------------------------------------------------------------------------
412 *
413 * procedure days2mdhms
414 *
415 * this procedure converts the day of the year, days, to the equivalent month
416 * day, hour, minute and second.
417 *
418 *
419 *
420 * algorithm : set up array for the number of days per month
421 * find leap year - use 1900 because 2000 is a leap year
422 * loop through a temp value while the value is < the days
423 * perform int conversions to the correct day and month
424 * convert remainder into h m s using type conversions
425 *
426 * author : david vallado 719-573-2600 1 mar 2001
427 *
428 * inputs description range / units
429 * year - year 1900 .. 2100
430 * days - julian day of the year 0.0 .. 366.0
431 *
432 * outputs :
433 * mon - month 1 .. 12
434 * day - day 1 .. 28,29,30,31
435 * hr - hour 0 .. 23
436 * min - minute 0 .. 59
437 * sec - second 0.0 .. 59.999
438 *
439 * locals :
440 * dayofyr - day of year
441 * temp - temporary extended values
442 * inttemp - temporary int value
443 * i - index
444 * lmonth[12] - int array containing the number of days per month
445 *
446 * coupling :
447 * none.
448 * --------------------------------------------------------------------------- */
449 // returns MDHMS object with the mdhms variables
450 public static MDHMS days2mdhms(
451 int year, double days//,
452 //int& mon, int& day, int& hr, int& minute, double& sec
453 )
454 {
455 // return variables
456 //int mon, day, hr, minute, sec
457 MDHMS mdhms = new MDHMS();
458
459 int i, inttemp, dayofyr;
460 double temp;
461 int lmonth[] =
462 {
463 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31
464 };
465
466 dayofyr = (int)Math.floor(days);
467 /* ----------------- find month and day of month ---------------- */
468 if((year % 4) == 0) // doesn't work for dates starting 2100 and beyond
469 {
470 lmonth[1] = 29;
471 }
472
473 i = 1;
474 inttemp = 0;
475 while((dayofyr > inttemp + lmonth[i - 1]) && (i < 12))
476 {
477 inttemp = inttemp + lmonth[i - 1];
478 i++;
479 }
480 mdhms.mon = i;
481 mdhms.day = dayofyr - inttemp;
482
483 /* ----------------- find hours minutes and seconds ------------- */
484 temp = (days - dayofyr) * 24.0;
485 mdhms.hr = (int)Math.floor(temp);
486 temp = (temp - mdhms.hr) * 60.0;
487 mdhms.minute = (int)Math.floor(temp);
488 mdhms.sec = (temp - mdhms.minute) * 60.0;
489
490 return mdhms;
491 } // end days2mdhms
492
493 // Month Day Hours Min Sec
494 public static class MDHMS
495 {
496 public int mon = 0;
497 ;
498 public int day = 0;
499 ;
500 public int hr = 0;
501 ;
502 public int minute = 0;
503 ;
504 public double sec = 0;
505 }
506
507 /** -----------------------------------------------------------------------------
508 *
509 * procedure invjday
510 *
511 * this procedure finds the year, month, day, hour, minute and second
512 * given the julian date. tu can be ut1, tdt, tdb, etc.
513 *
514 * algorithm : set up starting values
515 * find leap year - use 1900 because 2000 is a leap year
516 * find the elapsed days through the year in a loop
517 * call routine to find each individual value
518 *
519 * author : david vallado 719-573-2600 1 mar 2001
520 *
521 * inputs description range / units
522 * jd - julian date days from 4713 bc
523 *
524 * outputs :
525 * year - year 1900 .. 2100
526 * mon - month 1 .. 12
527 * day - day 1 .. 28,29,30,31
528 * hr - hour 0 .. 23
529 * min - minute 0 .. 59
530 * sec - second 0.0 .. 59.999
531 *
532 * locals :
533 * days - day of year plus fractional
534 * portion of a day days
535 * tu - julian centuries from 0 h
536 * jan 0, 1900
537 * temp - temporary double values
538 * leapyrs - number of leap years from 1900
539 *
540 * coupling :
541 * days2mdhms - finds month, day, hour, minute and second given days and year
542 *
543 * references :
544 * vallado 2007, 208, alg 22, ex 3-13
545 * ---------------------------------------------------------------------------
546 *
547 * @param jd
548 * @return [year,mon,day,hr,minute,sec]
549 */
550 public static double[] invjday(double jd)
551 {
552 // return vars
553 double year, mon, day, hr, minute, sec;
554
555 int leapyrs;
556 double days, tu, temp;
557
558 /* --------------- find year and days of the year --------------- */
559 temp = jd - 2415019.5;
560 tu = temp / 365.25;
561 year = 1900 + (int)Math.floor(tu);
562 leapyrs = (int)Math.floor((year - 1901) * 0.25);
563
564 // optional nudge by 8.64x10-7 sec to get even outputs
565 days = temp - ((year - 1900) * 365.0 + leapyrs) + 0.00000000001;
566
567 /* ------------ check for case of beginning of a year ----------- */
568 if(days < 1.0)
569 {
570 year = year - 1;
571 leapyrs = (int)Math.floor((year - 1901) * 0.25);
572 days = temp - ((year - 1900) * 365.0 + leapyrs);
573 }
574
575 /* ----------------- find remaing data ------------------------- */
576 //days2mdhms(year, days, mon, day, hr, minute, sec);
577 MDHMS mdhms = days2mdhms((int)year, days);
578 mon = mdhms.mon;
579 day = mdhms.day;
580 hr = mdhms.hr;
581 minute = mdhms.minute;
582 sec = mdhms.sec;
583
584 sec = sec - 0.00000086400; // ?
585
586 return new double[]
587 {
588 year, mon, day, hr, minute, sec
589 };
590 } // end invjday
591
592 /** -----------------------------------------------------------------------------
593 *
594 * function rv2coe
595 *
596 * this function finds the classical orbital elements given the geocentric
597 * equatorial position and velocity vectors.
598 *
599 * author : david vallado 719-573-2600 21 jun 2002
600 *
601 * revisions
602 * vallado - fix special cases 5 sep 2002
603 * vallado - delete extra check in inclination code 16 oct 2002
604 * vallado - add constant file use 29 jun 2003
605 * vallado - add mu 2 apr 2007
606 *
607 * inputs description range / units
608 * r - ijk position vector km
609 * v - ijk velocity vector km / s
610 * mu - gravitational parameter km3 / s2
611 *
612 * outputs :
613 * p - semilatus rectum km
614 * a - semimajor axis km
615 * ecc - eccentricity
616 * incl - inclination 0.0 to pi rad
617 * omega - longitude of ascending node 0.0 to 2pi rad
618 * argp - argument of perigee 0.0 to 2pi rad
619 * nu - true anomaly 0.0 to 2pi rad
620 * m - mean anomaly 0.0 to 2pi rad
621 * arglat - argument of latitude (ci) 0.0 to 2pi rad
622 * truelon - true longitude (ce) 0.0 to 2pi rad
623 * lonper - longitude of periapsis (ee) 0.0 to 2pi rad
624 *
625 * locals :
626 * hbar - angular momentum h vector km2 / s
627 * ebar - eccentricity e vector
628 * nbar - line of nodes n vector
629 * c1 - v**2 - u/r
630 * rdotv - r dot v
631 * hk - hk unit vector
632 * sme - specfic mechanical energy km2 / s2
633 * i - index
634 * e - eccentric, parabolic,
635 * hyperbolic anomaly rad
636 * temp - temporary variable
637 * typeorbit - type of orbit ee, ei, ce, ci
638 *
639 * coupling :
640 * mag - magnitude of a vector
641 * cross - cross product of two vectors
642 * angle - find the angle between two vectors
643 * newtonnu - find the mean anomaly
644 *
645 * references :
646 * vallado 2007, 126, alg 9, ex 2-5
647 * ---------------------------------------------------------------------------
648 *
649 * @param r
650 * @param v
651 * @param mu
652 * @return [p, a, ecc, incl, omega, argp, nu, m, arglat, truelon, lonper]
653 */
654 public static double[] rv2coe(
655 double[] r, double[] v, double mu//,
656 // double& p, double& a, double& ecc, double& incl, double& omega, double& argp,
657 // double& nu, double& m, double& arglat, double& truelon, double& lonper
658 )
659 {
660
661 // return variables
662 double p, a, ecc, incl, omega, argp, nu, m, arglat, truelon, lonper;
663
664 // internal
665
666 double undefined, small, magr, magv, magn, sme,
667 rdotv, infinite, temp, c1, hk, twopi, magh, halfpi, e;
668 double[] hbar = new double[3];
669 double[] nbar = new double[3];
670 double[] ebar = new double[3];
671
672 int i;
673 String typeorbit;
674
675 twopi = 2.0 * Math.PI;
676 halfpi = 0.5 * Math.PI;
677 small = 0.00000001;
678 undefined = 999999.1;
679 infinite = 999999.9;
680
681 // SEG m needs to be ini
682 m = undefined;
683
684 // ------------------------- implementation -----------------
685 magr = mag(r);
686 magv = mag(v);
687
688 // ------------------ find h n and e vectors ----------------
689 cross(r, v, hbar);
690 magh = mag(hbar);
691 if(magh > small)
692 {
693 nbar[0] = -hbar[1];
694 nbar[1] = hbar[0];
695 nbar[2] = 0.0;
696 magn = mag(nbar);
697 c1 = magv * magv - mu / magr;
698 rdotv = dot(r, v);
699 for(i = 0; i <= 2; i++)
700 {
701 ebar[i] = (c1 * r[i] - rdotv * v[i]) / mu;
702 }
703 ecc = mag(ebar);
704
705 // ------------ find a e and semi-latus rectum ----------
706 sme = (magv * magv * 0.5) - (mu / magr);
707 if(Math.abs(sme) > small)
708 {
709 a = -mu / (2.0 * sme);
710 }
711 else
712 {
713 a = infinite;
714 }
715 p = magh * magh / mu;
716
717 // ----------------- find inclination -------------------
718 hk = hbar[2] / magh;
719 incl = Math.acos(hk);
720
721 // -------- determine type of orbit for later use --------
722 // ------ elliptical, parabolic, hyperbolic inclined -------
723 typeorbit = "ei";
724 if(ecc < small)
725 {
726 // ---------------- circular equatorial ---------------
727 if((incl < small) | (Math.abs(incl - Math.PI) < small))
728 {
729 typeorbit = "ce";
730 }
731 else
732 // -------------- circular inclined ---------------
733 {
734 typeorbit = "ci";
735 }
736 }
737 else
738 {
739 // - elliptical, parabolic, hyperbolic equatorial --
740 if((incl < small) | (Math.abs(incl - Math.PI) < small))
741 {
742 typeorbit = "ee";
743 }
744 }
745
746 // ---------- find longitude of ascending node ------------
747 if(magn > small)
748 {
749 temp = nbar[0] / magn;
750 if(Math.abs(temp) > 1.0)
751 {
752 temp = Math.signum(temp);
753 }
754 omega = Math.acos(temp);
755 if(nbar[1] < 0.0)
756 {
757 omega = twopi - omega;
758 }
759 }
760 else
761 {
762 omega = undefined;
763 }
764
765 // ---------------- find argument of perigee ---------------
766 if(typeorbit.equalsIgnoreCase("ei") == true) // 0 = true for cpp strcmp
767 {
768 argp = angle(nbar, ebar);
769 if(ebar[2] < 0.0)
770 {
771 argp = twopi - argp;
772 }
773 }
774 else
775 {
776 argp = undefined;
777 }
778
779 // ------------ find true anomaly at epoch -------------
780 if(typeorbit.startsWith("e"))//typeorbit[0] == 'e' )
781 {
782 nu = angle(ebar, r);
783 if(rdotv < 0.0)
784 {
785 nu = twopi - nu;
786 }
787 }
788 else
789 {
790 nu = undefined;
791 }
792
793 // ---- find argument of latitude - circular inclined -----
794 if(typeorbit.equalsIgnoreCase("ci") == true)
795 {
796 arglat = angle(nbar, r);
797 if(r[2] < 0.0)
798 {
799 arglat = twopi - arglat;
800 }
801 m = arglat;
802 }
803 else
804 {
805 arglat = undefined;
806 }
807
808 // -- find longitude of perigee - elliptical equatorial ----
809 if((ecc > small) && (typeorbit.equalsIgnoreCase("ee") == true))
810 {
811 temp = ebar[0] / ecc;
812 if(Math.abs(temp) > 1.0)
813 {
814 temp = Math.signum(temp);
815 }
816 lonper = Math.acos(temp);
817 if(ebar[1] < 0.0)
818 {
819 lonper = twopi - lonper;
820 }
821 if(incl > halfpi)
822 {
823 lonper = twopi - lonper;
824 }
825 }
826 else
827 {
828 lonper = undefined;
829 }
830
831 // -------- find true longitude - circular equatorial ------
832 if((magr > small) && (typeorbit.equalsIgnoreCase("ce") == true))
833 {
834 temp = r[0] / magr;
835 if(Math.abs(temp) > 1.0)
836 {
837 temp = Math.signum(temp);
838 }
839 truelon = Math.acos(temp);
840 if(r[1] < 0.0)
841 {
842 truelon = twopi - truelon;
843 }
844 if(incl > halfpi)
845 {
846 truelon = twopi - truelon;
847 }
848 m = truelon;
849 }
850 else
851 {
852 truelon = undefined;
853 }
854
855 // ------------ find mean anomaly for all orbits -----------
856 if(typeorbit.startsWith("e"))//typeorbit[0] == 'e' )
857 {
858 double[] tt = newtonnu(ecc, nu);
859 e = tt[0];
860 m = tt[1];
861 }
862 }
863 else
864 {
865 p = undefined;
866 a = undefined;
867 ecc = undefined;
868 incl = undefined;
869 omega = undefined;
870 argp = undefined;
871 nu = undefined;
872 m = undefined;
873 arglat = undefined;
874 truelon = undefined;
875 lonper = undefined;
876 }
877
878 return new double[]
879 {
880 p, a, ecc, incl, omega, argp, nu, m, arglat, truelon, lonper
881 };
882 } // end rv2coe
883
884 /** -----------------------------------------------------------------------------
885 *
886 * function newtonnu
887 *
888 * this function solves keplers equation when the true anomaly is known.
889 * the mean and eccentric, parabolic, or hyperbolic anomaly is also found.
890 * the parabolic limit at 168 is arbitrary. the hyperbolic anomaly is also
891 * limited. the hyperbolic sine is used because it's not double valued.
892 *
893 * author : david vallado 719-573-2600 27 may 2002
894 *
895 * revisions
896 * vallado - fix small 24 sep 2002
897 *
898 * inputs description range / units
899 * ecc - eccentricity 0.0 to
900 * nu - true anomaly -2pi to 2pi rad
901 *
902 * outputs :
903 * e0 - eccentric anomaly 0.0 to 2pi rad 153.02
904 * m - mean anomaly 0.0 to 2pi rad 151.7425
905 *
906 * locals :
907 * e1 - eccentric anomaly, next value rad
908 * sine - sine of e
909 * cose - cosine of e
910 * ktr - index
911 *
912 * coupling :
913 * asinh - arc hyperbolic sine
914 *
915 * references :
916 * vallado 2007, 85, alg 5
917 * ---------------------------------------------------------------------------
918 *
919 * @param ecc
920 * @param nu
921 * @return [e0, m]
922 */
923 public static double[] newtonnu(double ecc, double nu)
924 {
925 // return vars
926 double e0, m;
927
928 // internal
929
930 double small, sine, cose;
931
932 // --------------------- implementation ---------------------
933 e0 = 999999.9;
934 m = 999999.9;
935 small = 0.00000001;
936
937 // --------------------------- circular ------------------------
938 if(Math.abs(ecc) < small)
939 {
940 m = nu;
941 e0 = nu;
942 }
943 else // ---------------------- elliptical -----------------------
944 if(ecc < 1.0 - small)
945 {
946 sine = (Math.sqrt(1.0 - ecc * ecc) * Math.sin(nu)) / (1.0 + ecc * Math.cos(nu));
947 cose = (ecc + Math.cos(nu)) / (1.0 + ecc * Math.cos(nu));
948 e0 = Math.atan2(sine, cose);
949 m = e0 - ecc * Math.sin(e0);
950 }
951 else // -------------------- hyperbolic --------------------
952 if(ecc > 1.0 + small)
953 {
954 if((ecc > 1.0) && (Math.abs(nu) + 0.00001 < Math.PI - Math.acos(1.0 / ecc)))
955 {
956 sine = (Math.sqrt(ecc * ecc - 1.0) * Math.sin(nu)) / (1.0 + ecc * Math.cos(nu));
957 e0 = asinh(sine);
958 m = ecc * Math.sinh(e0) - e0;
959 }
960 }
961 else // ----------------- parabolic ---------------------
962 if(Math.abs(nu) < 168.0 * Math.PI / 180.0)
963 {
964 e0 = Math.tan(nu * 0.5);
965 m = e0 + (e0 * e0 * e0) / 3.0;
966 }
967
968 if(ecc < 1.0)
969 {
970 m = (m % (2.0 * Math.PI));
971 if(m < 0.0)
972 {
973 m = m + 2.0 * Math.PI;
974 }
975 e0 = e0 % (2.0 * Math.PI);
976 }
977
978 return new double[]
979 {
980 e0, m
981 };
982 } // end newtonnu
983
984
985 /* -----------------------------------------------------------------------------
986 *
987 * function asinh
988 *
989 * this function evaluates the inverse hyperbolic sine function.
990 *
991 * author : david vallado 719-573-2600 1 mar 2001
992 *
993 * inputs description range / units
994 * xval - angle value any real
995 *
996 * outputs :
997 * arcsinh - result any real
998 *
999 * locals :
1000 * none.
1001 *
1002 * coupling :
1003 * none.
1004 *
1005 * --------------------------------------------------------------------------- */
1006 public static double asinh(double xval)
1007 {
1008 return Math.log(xval + Math.sqrt(xval * xval + 1.0));
1009 } // end asinh
1010
1011 /* -----------------------------------------------------------------------------
1012 *
1013 * function mag
1014 *
1015 * this procedure finds the magnitude of a vector. the tolerance is set to
1016 * 0.000001, thus the 1.0e-12 for the squared test of underflows.
1017 *
1018 * author : david vallado 719-573-2600 1 mar 2001
1019 *
1020 * inputs description range / units
1021 * vec - vector
1022 *
1023 * outputs :
1024 * vec - answer stored in fourth component
1025 *
1026 * locals :
1027 * none.
1028 *
1029 * coupling :
1030 * none.
1031 * --------------------------------------------------------------------------- */
1032 public static double mag(double[] x)
1033 {
1034 return Math.sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
1035 } // end mag
1036
1037 /* -----------------------------------------------------------------------------
1038 *
1039 * procedure cross
1040 *
1041 * this procedure crosses two vectors.
1042 *
1043 * author : david vallado 719-573-2600 1 mar 2001
1044 *
1045 * inputs description range / units
1046 * vec1 - vector number 1
1047 * vec2 - vector number 2
1048 *
1049 * outputs :
1050 * outvec - vector result of a x b
1051 *
1052 * locals :
1053 * none.
1054 *
1055 * coupling :
1056 * mag magnitude of a vector
1057 ---------------------------------------------------------------------------- */
1058 public static void cross(double[] vec1, double[] vec2, double[] outvec)
1059 {
1060 outvec[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
1061 outvec[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
1062 outvec[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
1063 } // end cross
1064
1065 /* -----------------------------------------------------------------------------
1066 *
1067 * function dot
1068 *
1069 * this function finds the dot product of two vectors.
1070 *
1071 * author : david vallado 719-573-2600 1 mar 2001
1072 *
1073 * inputs description range / units
1074 * vec1 - vector number 1
1075 * vec2 - vector number 2
1076 *
1077 * outputs :
1078 * dot - result
1079 *
1080 * locals :
1081 * none.
1082 *
1083 * coupling :
1084 * none.
1085 *
1086 * --------------------------------------------------------------------------- */
1087 public static double dot(double[] x, double[] y)
1088 {
1089 return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
1090 } // end dot
1091
1092 /* -----------------------------------------------------------------------------
1093 *
1094 * procedure angle
1095 *
1096 * this procedure calculates the angle between two vectors. the output is
1097 * set to 999999.1 to indicate an undefined value. be sure to check for
1098 * this at the output phase.
1099 *
1100 * author : david vallado 719-573-2600 1 mar 2001
1101 *
1102 * inputs description range / units
1103 * vec1 - vector number 1
1104 * vec2 - vector number 2
1105 *
1106 * outputs :
1107 * theta - angle between the two vectors -pi to pi
1108 *
1109 * locals :
1110 * temp - temporary real variable
1111 *
1112 * coupling :
1113 * dot dot product of two vectors
1114 * --------------------------------------------------------------------------- */
1115 public static double angle(double[] vec1, double[] vec2)
1116 {
1117 double small, undefined, magv1, magv2, temp;
1118 small = 0.00000001;
1119 undefined = 999999.1;
1120
1121 magv1 = mag(vec1);
1122 magv2 = mag(vec2);
1123
1124 if(magv1 * magv2 > small * small)
1125 {
1126 temp = dot(vec1, vec2) / (magv1 * magv2);
1127 if(Math.abs(temp) > 1.0)
1128 {
1129 temp = Math.signum(temp) * 1.0;
1130 }
1131 return Math.acos(temp);
1132 }
1133 else
1134 {
1135 return undefined;
1136 }
1137 } // end angle
1138 }