List of usage examples for org.apache.commons.math.util MathUtils normalizeAngle
public static double normalizeAngle(double a, double center)
From source file:org.orekit.propagation.Ephemeris.java
/** Get the interpolated orbital parameters. * @param tp time in seconds since previous date * @param tn time in seconds until next date * @param date desired date for the state * @return the new equinoctial parameters *///from w w w . j ava2 s .c om private Orbit getInterpolatedOp(final double tp, final double tn, final AbsoluteDate date) { final double dt = tp + tn; final double cP = tp / dt; final double cN = tn / dt; final double a = cN * previous.getA() + cP * next.getA(); final double ex = cN * previous.getEquinoctialEx() + cP * next.getEquinoctialEx(); final double ey = cN * previous.getEquinoctialEy() + cP * next.getEquinoctialEy(); final double hx = cN * previous.getHx() + cP * next.getHx(); final double hy = cN * previous.getHy() + cP * next.getHy(); final double lv = cN * previous.getLv() + cP * MathUtils.normalizeAngle(next.getLv(), previous.getLv()); return new EquinoctialOrbit(a, ex, ey, hx, hy, lv, EquinoctialOrbit.TRUE_LATITUDE_ARGUMENT, previous.getFrame(), date, previous.getMu()); }
From source file:org.orekit.tle.DeepSDP4.java
/** Computes luni - solar terms from initial coordinates and epoch. * @exception OrekitException when UTC time steps can't be read */// w w w . j a v a2 s . c om protected void luniSolarTermsComputation() throws OrekitException { final double sing = Math.sin(tle.getPerigeeArgument()); final double cosg = Math.cos(tle.getPerigeeArgument()); final double sinq = Math.sin(tle.getRaan()); final double cosq = Math.cos(tle.getRaan()); final double aqnv = 1.0 / a0dp; // Compute julian days since 1900 final double daysSince1900 = (tle.getDate().durationFrom(AbsoluteDate.JULIAN_EPOCH) + tle.getDate().timeScalesOffset(TimeScalesFactory.getUTC(), TimeScalesFactory.getTT())) / Constants.JULIAN_DAY - 2415020; double cc = C1SS; double ze = ZES; double zn = ZNS; double zsinh = sinq; double zcosh = cosq; thgr = thetaG(tle.getDate()); xnq = xn0dp; omegaq = tle.getPerigeeArgument(); final double xnodce = 4.5236020 - 9.2422029e-4 * daysSince1900; final double stem = Math.sin(xnodce); final double ctem = Math.cos(xnodce); final double c_minus_gam = 0.228027132 * daysSince1900 - 1.1151842; final double gam = 5.8351514 + 0.0019443680 * daysSince1900; zcosil = 0.91375164 - 0.03568096 * ctem; zsinil = Math.sqrt(1.0 - zcosil * zcosil); zsinhl = 0.089683511 * stem / zsinil; zcoshl = Math.sqrt(1.0 - zsinhl * zsinhl); zmol = MathUtils.normalizeAngle(c_minus_gam, Math.PI); double zx = 0.39785416 * stem / zsinil; final double zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; zx = Math.atan2(zx, zy) + gam - xnodce; zcosgl = Math.cos(zx); zsingl = Math.sin(zx); zmos = MathUtils.normalizeAngle(6.2565837 + 0.017201977 * daysSince1900, Math.PI); // Do solar terms savtsn = 1e20; double zcosi = 0.91744867; double zsini = 0.39785416; double zsing = -0.98088458; double zcosg = 0.1945905; double se = 0; double sgh = 0; double sh = 0; double si = 0; double sl = 0; // There was previously some convoluted logic here, but it boils // down to this: we compute the solar terms, then the lunar terms. // On a second pass, we recompute the solar terms, taking advantage // of the improved data that resulted from computing lunar terms. for (int iteration = 0; iteration < 2; ++iteration) { final double a1 = zcosg * zcosh + zsing * zcosi * zsinh; final double a3 = -zsing * zcosh + zcosg * zcosi * zsinh; final double a7 = -zcosg * zsinh + zsing * zcosi * zcosh; final double a8 = zsing * zsini; final double a9 = zsing * zsinh + zcosg * zcosi * zcosh; final double a10 = zcosg * zsini; final double a2 = cosi0 * a7 + sini0 * a8; final double a4 = cosi0 * a9 + sini0 * a10; final double a5 = -sini0 * a7 + cosi0 * a8; final double a6 = -sini0 * a9 + cosi0 * a10; final double x1 = a1 * cosg + a2 * sing; final double x2 = a3 * cosg + a4 * sing; final double x3 = -a1 * sing + a2 * cosg; final double x4 = -a3 * sing + a4 * cosg; final double x5 = a5 * sing; final double x6 = a6 * sing; final double x7 = a5 * cosg; final double x8 = a6 * cosg; final double z31 = 12 * x1 * x1 - 3 * x3 * x3; final double z32 = 24 * x1 * x2 - 6 * x3 * x4; final double z33 = 12 * x2 * x2 - 3 * x4 * x4; final double z11 = -6 * a1 * a5 + e0sq * (-24 * x1 * x7 - 6 * x3 * x5); final double z12 = -6 * (a1 * a6 + a3 * a5) + e0sq * (-24 * (x2 * x7 + x1 * x8) - 6 * (x3 * x6 + x4 * x5)); final double z13 = -6 * a3 * a6 + e0sq * (-24 * x2 * x8 - 6 * x4 * x6); final double z21 = 6 * a2 * a5 + e0sq * (24 * x1 * x5 - 6 * x3 * x7); final double z22 = 6 * (a4 * a5 + a2 * a6) + e0sq * (24 * (x2 * x5 + x1 * x6) - 6 * (x4 * x7 + x3 * x8)); final double z23 = 6 * a4 * a6 + e0sq * (24 * x2 * x6 - 6 * x4 * x8); final double s3 = cc / xnq; final double s2 = -0.5 * s3 / beta0; final double s4 = s3 * beta0; final double s1 = -15 * tle.getE() * s4; final double s5 = x1 * x3 + x2 * x4; final double s6 = x2 * x3 + x1 * x4; final double s7 = x2 * x4 - x1 * x3; double z1 = 3 * (a1 * a1 + a2 * a2) + z31 * e0sq; double z2 = 6 * (a1 * a3 + a2 * a4) + z32 * e0sq; double z3 = 3 * (a3 * a3 + a4 * a4) + z33 * e0sq; z1 = z1 + z1 + beta02 * z31; z2 = z2 + z2 + beta02 * z32; z3 = z3 + z3 + beta02 * z33; se = s1 * zn * s5; si = s2 * zn * (z11 + z13); sl = -zn * s3 * (z1 + z3 - 14 - 6 * e0sq); sgh = s4 * zn * (z31 + z33 - 6); if (tle.getI() < (Math.PI / 60.0)) { // inclination smaller than 3 degrees sh = 0; } else { sh = -zn * s2 * (z21 + z23); } ee2 = 2 * s1 * s6; e3 = 2 * s1 * s7; xi2 = 2 * s2 * z12; xi3 = 2 * s2 * (z13 - z11); xl2 = -2 * s3 * z2; xl3 = -2 * s3 * (z3 - z1); xl4 = -2 * s3 * (-21 - 9 * e0sq) * ze; xgh2 = 2 * s4 * z32; xgh3 = 2 * s4 * (z33 - z31); xgh4 = -18 * s4 * ze; xh2 = -2 * s2 * z22; xh3 = -2 * s2 * (z23 - z21); if (iteration == 0) { // we compute lunar terms only on the first pass: sse = se; ssi = si; ssl = sl; ssh = sh / sini0; ssg = sgh - cosi0 * ssh; se2 = ee2; si2 = xi2; sl2 = xl2; sgh2 = xgh2; sh2 = xh2; se3 = e3; si3 = xi3; sl3 = xl3; sgh3 = xgh3; sh3 = xh3; sl4 = xl4; sgh4 = xgh4; zcosg = zcosgl; zsing = zsingl; zcosi = zcosil; zsini = zsinil; zcosh = zcoshl * cosq + zsinhl * sinq; zsinh = sinq * zcoshl - cosq * zsinhl; zn = ZNL; cc = C1L; ze = ZEL; } } // end of solar - lunar - solar terms computation sse += se; ssi += si; ssl += sl; ssg += sgh - cosi0 / sini0 * sh; ssh += sh / sini0; // Start the resonant-synchronous tests and initialization double bfact = 0; // if mean motion is 1.893053 to 2.117652 revs/day, and eccentricity >= 0.5, // start of the 12-hour orbit, e > 0.5 section if ((xnq >= 0.00826) && (xnq <= 0.00924) && (tle.getE() >= 0.5)) { final double g201 = -0.306 - (tle.getE() - 0.64) * 0.440; final double eoc = tle.getE() * e0sq; final double sini2 = sini0 * sini0; final double f220 = 0.75 * (1 + 2 * cosi0 + theta2); final double f221 = 1.5 * sini2; final double f321 = 1.875 * sini0 * (1 - 2 * cosi0 - 3 * theta2); final double f322 = -1.875 * sini0 * (1 + 2 * cosi0 - 3 * theta2); final double f441 = 35 * sini2 * f220; final double f442 = 39.3750 * sini2 * sini2; final double f522 = 9.84375 * sini0 * (sini2 * (1 - 2 * cosi0 - 5 * theta2) + 0.33333333 * (-2 + 4 * cosi0 + 6 * theta2)); final double f523 = sini0 * (4.92187512 * sini2 * (-2 - 4 * cosi0 + 10 * theta2) + 6.56250012 * (1 + 2 * cosi0 - 3 * theta2)); final double f542 = 29.53125 * sini0 * (2 - 8 * cosi0 + theta2 * (-12 + 8 * cosi0 + 10 * theta2)); final double f543 = 29.53125 * sini0 * (-2 - 8 * cosi0 + theta2 * (12 + 8 * cosi0 - 10 * theta2)); double g211; double g310; double g322; double g410; double g422; double g520; resonant = true; // it is resonant... synchronous = false; // but it's not synchronous // Geopotential resonance initialization for 12 hour orbits : if (tle.getE() <= 0.65) { g211 = 3.616 - 13.247 * tle.getE() + 16.290 * e0sq; g310 = -19.302 + 117.390 * tle.getE() - 228.419 * e0sq + 156.591 * eoc; g322 = -18.9068 + 109.7927 * tle.getE() - 214.6334 * e0sq + 146.5816 * eoc; g410 = -41.122 + 242.694 * tle.getE() - 471.094 * e0sq + 313.953 * eoc; g422 = -146.407 + 841.880 * tle.getE() - 1629.014 * e0sq + 1083.435 * eoc; g520 = -532.114 + 3017.977 * tle.getE() - 5740.032 * e0sq + 3708.276 * eoc; } else { g211 = -72.099 + 331.819 * tle.getE() - 508.738 * e0sq + 266.724 * eoc; g310 = -346.844 + 1582.851 * tle.getE() - 2415.925 * e0sq + 1246.113 * eoc; g322 = -342.585 + 1554.908 * tle.getE() - 2366.899 * e0sq + 1215.972 * eoc; g410 = -1052.797 + 4758.686 * tle.getE() - 7193.992 * e0sq + 3651.957 * eoc; g422 = -3581.69 + 16178.11 * tle.getE() - 24462.77 * e0sq + 12422.52 * eoc; if (tle.getE() <= 0.715) { g520 = 1464.74 - 4664.75 * tle.getE() + 3763.64 * e0sq; } else { g520 = -5149.66 + 29936.92 * tle.getE() - 54087.36 * e0sq + 31324.56 * eoc; } } double g533; double g521; double g532; if (tle.getE() < 0.7) { g533 = -919.2277 + 4988.61 * tle.getE() - 9064.77 * e0sq + 5542.21 * eoc; g521 = -822.71072 + 4568.6173 * tle.getE() - 8491.4146 * e0sq + 5337.524 * eoc; g532 = -853.666 + 4690.25 * tle.getE() - 8624.77 * e0sq + 5341.4 * eoc; } else { g533 = -37995.78 + 161616.52 * tle.getE() - 229838.2 * e0sq + 109377.94 * eoc; g521 = -51752.104 + 218913.95 * tle.getE() - 309468.16 * e0sq + 146349.42 * eoc; g532 = -40023.88 + 170470.89 * tle.getE() - 242699.48 * e0sq + 115605.82 * eoc; } double temp1 = 3 * xnq * xnq * aqnv * aqnv; double temp = temp1 * ROOT22; d2201 = temp * f220 * g201; d2211 = temp * f221 * g211; temp1 *= aqnv; temp = temp1 * ROOT32; d3210 = temp * f321 * g310; d3222 = temp * f322 * g322; temp1 *= aqnv; temp = 2 * temp1 * ROOT44; d4410 = temp * f441 * g410; d4422 = temp * f442 * g422; temp1 *= aqnv; temp = temp1 * ROOT52; d5220 = temp * f522 * g520; d5232 = temp * f523 * g532; temp = 2 * temp1 * ROOT54; d5421 = temp * f542 * g521; d5433 = temp * f543 * g533; xlamo = tle.getMeanAnomaly() + tle.getRaan() + tle.getRaan() - thgr - thgr; bfact = xmdot + xnodot + xnodot - THDT - THDT; bfact += ssl + ssh + ssh; } else if ((xnq < 0.0052359877) && (xnq > 0.0034906585)) { // if mean motion is .8 to 1.2 revs/day : (geosynch) final double cosio_plus_1 = 1.0 + cosi0; final double g200 = 1 + e0sq * (-2.5 + 0.8125 * e0sq); final double g300 = 1 + e0sq * (-6 + 6.60937 * e0sq); final double f311 = 0.9375 * sini0 * sini0 * (1 + 3 * cosi0) - 0.75 * cosio_plus_1; final double g310 = 1 + 2 * e0sq; final double f220 = 0.75 * cosio_plus_1 * cosio_plus_1; final double f330 = 2.5 * f220 * cosio_plus_1; resonant = true; synchronous = true; // Synchronous resonance terms initialization del1 = 3 * xnq * xnq * aqnv * aqnv; del2 = 2 * del1 * f220 * g200 * Q22; del3 = 3 * del1 * f330 * g300 * Q33 * aqnv; del1 = del1 * f311 * g310 * Q31 * aqnv; xlamo = tle.getMeanAnomaly() + tle.getRaan() + tle.getPerigeeArgument() - thgr; bfact = xmdot + omgdot + xnodot - THDT; bfact = bfact + ssl + ssg + ssh; } else { // it's neither a high-e 12-hours orbit nor a geosynchronous: resonant = false; synchronous = false; } if (resonant) { xfact = bfact - xnq; // Initialize integrator xli = xlamo; xni = xnq; atime = 0; } derivs = new double[SECULAR_INTEGRATION_ORDER]; }
From source file:org.orekit.tle.DeepSDP4.java
/** Computes periodic terms from current coordinates and epoch. * @param t offset from initial epoch (min) *///from w w w . jav a 2 s. c o m protected void deepPeriodicEffects(final double t) { // If the time didn't change by more than 30 minutes, // there's no good reason to recompute the perturbations; // they don't change enough over so short a time span. // However, the Dundee code _always_ recomputes, so if // we're attempting to replicate its results, we've gotta // recompute everything, too. if ((Math.abs(savtsn - t) >= 30.0) || isDundeeCompliant) { savtsn = t; // Update solar perturbations for time T double zm = zmos + ZNS * t; double zf = zm + 2 * ZES * Math.sin(zm); double sinzf = Math.sin(zf); double f2 = 0.5 * sinzf * sinzf - 0.25; double f3 = -0.5 * sinzf * Math.cos(zf); final double ses = se2 * f2 + se3 * f3; final double sis = si2 * f2 + si3 * f3; final double sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; final double sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; final double shs = sh2 * f2 + sh3 * f3; // Update lunar perturbations for time T zm = zmol + ZNL * t; zf = zm + 2 * ZEL * Math.sin(zm); sinzf = Math.sin(zf); f2 = 0.5 * sinzf * sinzf - 0.25; f3 = -0.5 * sinzf * Math.cos(zf); final double sel = ee2 * f2 + e3 * f3; final double sil = xi2 * f2 + xi3 * f3; final double sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; final double sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; final double sh1 = xh2 * f2 + xh3 * f3; // Sum the solar and lunar contributions pe = ses + sel; pinc = sis + sil; pl = sls + sll; pgh = sghs + sghl; ph = shs + sh1; } xinc += pinc; final double sinis = Math.sin(xinc); final double cosis = Math.cos(xinc); /* Add solar/lunar perturbation correction to eccentricity: */ em += pe; xll += pl; omgadf += pgh; xinc = MathUtils.normalizeAngle(xinc, 0); if (Math.abs(xinc) >= 0.2) { // Apply periodics directly final double temp_val = ph / sinis; omgadf -= cosis * temp_val; xnode += temp_val; } else { // Apply periodics with Lyddane modification final double sinok = Math.sin(xnode); final double cosok = Math.cos(xnode); final double alfdp = ph * cosok + (pinc * cosis + sinis) * sinok; final double betdp = -ph * sinok + (pinc * cosis + sinis) * cosok; final double delta_xnode = MathUtils.normalizeAngle(Math.atan2(alfdp, betdp) - xnode, 0); final double dls = -xnode * sinis * pinc; omgadf += dls - cosis * delta_xnode; xnode += delta_xnode; } }
From source file:org.orekit.tle.TLEPropagator.java
/** Retrieves the position and velocity. * @return the computed PVCoordinates./*w w w . ja v a 2 s.c o m*/ * @exception OrekitException if current orbit is out of supported range * (too large eccentricity, too low perigee ...) */ private PVCoordinates computePVCoordinates() throws OrekitException { // Long period periodics final double axn = e * Math.cos(omega); double temp = 1.0 / (a * (1.0 - e * e)); final double xlcof = 0.125 * TLEConstants.A3OVK2 * sini0 * (3.0 + 5.0 * cosi0) / (1.0 + cosi0); final double aycof = 0.25 * TLEConstants.A3OVK2 * sini0; final double xll = temp * xlcof * axn; final double aynl = temp * aycof; final double xlt = xl + xll; final double ayn = e * Math.sin(omega) + aynl; final double elsq = axn * axn + ayn * ayn; final double capu = MathUtils.normalizeAngle(xlt - xnode, Math.PI); double epw = capu; double ecosE = 0; double esinE = 0; double sinEPW = 0; double cosEPW = 0; // Dundee changes: items dependent on cosio get recomputed: final double cosi0Sq = cosi0 * cosi0; final double x3thm1 = 3.0 * cosi0Sq - 1.0; final double x1mth2 = 1.0 - cosi0Sq; final double x7thm1 = 7.0 * cosi0Sq - 1.0; if (e > (1 - 1e-6)) { throw new OrekitException( "eccentricity becomes too large for TLE propagation " + "(e: {0}, satellite number: {1})", e, tle.getSatelliteNumber()); } if ((a * (1.0 - e) < 1.0) || (a * (1.0 + e) < 1.0)) { throw new OrekitException( "too small perigee radius for TLE propagation " + "(r: {0}, satellite number: {1})", a * (1. - e), tle.getSatelliteNumber()); } // Solve Kepler's' Equation. final double newtonRaphsonEpsilon = 1e-12; for (int j = 0; j < 10; j++) { boolean doSecondOrderNewtonRaphson = true; sinEPW = Math.sin(epw); cosEPW = Math.cos(epw); ecosE = axn * cosEPW + ayn * sinEPW; esinE = axn * sinEPW - ayn * cosEPW; final double f = capu - epw + esinE; if (Math.abs(f) < newtonRaphsonEpsilon) { break; } final double fdot = 1.0 - ecosE; double delta_epw = f / fdot; if (j == 0) { final double maxNewtonRaphson = 1.25 * Math.abs(e); doSecondOrderNewtonRaphson = false; if (delta_epw > maxNewtonRaphson) { delta_epw = maxNewtonRaphson; } else if (delta_epw < -maxNewtonRaphson) { delta_epw = -maxNewtonRaphson; } else { doSecondOrderNewtonRaphson = true; } } if (doSecondOrderNewtonRaphson) { delta_epw = f / (fdot + 0.5 * esinE * delta_epw); } epw += delta_epw; } // Short period preliminary quantities temp = 1.0 - elsq; final double pl = a * temp; final double r = a * (1.0 - ecosE); double temp2 = a / r; final double betal = Math.sqrt(temp); temp = esinE / (1.0 + betal); final double cosu = temp2 * (cosEPW - axn + ayn * temp); final double sinu = temp2 * (sinEPW - ayn - axn * temp); final double u = Math.atan2(sinu, cosu); final double sin2u = 2.0 * sinu * cosu; final double cos2u = 2.0 * cosu * cosu - 1.0; final double temp1 = TLEConstants.CK2 / pl; temp2 = temp1 / pl; // Update for short periodics final double rk = r * (1.0 - 1.5 * temp2 * betal * x3thm1) + 0.5 * temp1 * x1mth2 * cos2u; final double uk = u - 0.25 * temp2 * x7thm1 * sin2u; final double xnodek = xnode + 1.5 * temp2 * cosi0 * sin2u; final double xinck = i + 1.5 * temp2 * cosi0 * sini0 * cos2u; // Orientation vectors final double sinuk = Math.sin(uk); final double cosuk = Math.cos(uk); final double sinik = Math.sin(xinck); final double cosik = Math.cos(xinck); final double sinnok = Math.sin(xnodek); final double cosnok = Math.cos(xnodek); final double xmx = -sinnok * cosik; final double xmy = cosnok * cosik; final double ux = xmx * sinuk + cosnok * cosuk; final double uy = xmy * sinuk + sinnok * cosuk; final double uz = sinik * sinuk; // Position and velocity final double cr = 1000 * rk * TLEConstants.EARTH_RADIUS; final Vector3D pos = new Vector3D(cr * ux, cr * uy, cr * uz); final double rdot = TLEConstants.XKE * Math.sqrt(a) * esinE / r; final double rfdot = TLEConstants.XKE * Math.sqrt(pl) / r; final double xn = TLEConstants.XKE / (a * Math.sqrt(a)); final double rdotk = rdot - xn * temp1 * x1mth2 * sin2u; final double rfdotk = rfdot + xn * temp1 * (x1mth2 * cos2u + 1.5 * x3thm1); final double vx = xmx * cosuk - cosnok * sinuk; final double vy = xmy * cosuk - sinnok * sinuk; final double vz = sinik * cosuk; final double cv = 1000.0 * TLEConstants.EARTH_RADIUS / 60.0; final Vector3D vel = new Vector3D(cv * (rdotk * ux + rfdotk * vx), cv * (rdotk * uy + rfdotk * vy), cv * (rdotk * uz + rfdotk * vz)); return new PVCoordinates(pos, vel); }