KeplerianAnomalyUtility.java

  1. /* Copyright 2002-2022 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.orbits;

  19. import org.hipparchus.exception.MathIllegalStateException;
  20. import org.hipparchus.util.FastMath;
  21. import org.hipparchus.util.MathUtils;
  22. import org.hipparchus.util.SinCos;
  23. import org.orekit.errors.OrekitMessages;

  25. /**
  26.  * Utility methods for converting between different Keplerian anomalies.
  27.  * @author Luc Maisonobe
  28.  * @author Andrew Goetz
  29.  */
  30. public final class KeplerianAnomalyUtility {

  32.     /** First coefficient to compute elliptic Kepler equation solver starter. */
  33.     private static final double A;

  35.     /** Second coefficient to compute elliptic Kepler equation solver starter. */
  36.     private static final double B;

  38.     static {
  39.         final double k1 = 3 * FastMath.PI + 2;
  40.         final double k2 = FastMath.PI - 1;
  41.         final double k3 = 6 * FastMath.PI - 1;
  42.         A = 3 * k2 * k2 / k1;
  43.         B = k3 * k3 / (6 * k1);
  44.     }

  46.     private KeplerianAnomalyUtility() {
  47.     }

  49.     /**
  50.      * Computes the elliptic true anomaly from the elliptic mean anomaly.
  51.      * @param e eccentricity such that 0 ≤ e < 1
  52.      * @param M elliptic mean anomaly (rad)
  53.      * @return elliptic true anomaly (rad)
  54.      */
  55.     public static double ellipticMeanToTrue(final double e, final double M) {
  56.         final double E = ellipticMeanToEccentric(e, M);
  57.         final double v = ellipticEccentricToTrue(e, E);
  58.         return v;
  59.     }

  61.     /**
  62.      * Computes the elliptic mean anomaly from the elliptic true anomaly.
  63.      * @param e eccentricity such that 0 ≤ e < 1
  64.      * @param v elliptic true anomaly (rad)
  65.      * @return elliptic mean anomaly (rad)
  66.      */
  67.     public static double ellipticTrueToMean(final double e, final double v) {
  68.         final double E = ellipticTrueToEccentric(e, v);
  69.         final double M = ellipticEccentricToMean(e, E);
  70.         return M;
  71.     }

  73.     /**
  74.      * Computes the elliptic true anomaly from the elliptic eccentric anomaly.
  75.      * @param e eccentricity such that 0 ≤ e < 1
  76.      * @param E elliptic eccentric anomaly (rad)
  77.      * @return elliptic true anomaly (rad)
  78.      */
  79.     public static double ellipticEccentricToTrue(final double e, final double E) {
  80.         final double beta = e / (1 + FastMath.sqrt((1 - e) * (1 + e)));
  81.         final SinCos scE = FastMath.sinCos(E);
  82.         return E + 2 * FastMath.atan(beta * scE.sin() / (1 - beta * scE.cos()));
  83.     }

  85.     /**
  86.      * Computes the elliptic eccentric anomaly from the elliptic true anomaly.
  87.      * @param e eccentricity such that 0 ≤ e < 1
  88.      * @param v elliptic true anomaly (rad)
  89.      * @return elliptic eccentric anomaly (rad)
  90.      */
  91.     public static double ellipticTrueToEccentric(final double e, final double v) {
  92.         final double beta = e / (1 + FastMath.sqrt(1 - e * e));
  93.         final SinCos scv = FastMath.sinCos(v);
  94.         return v - 2 * FastMath.atan(beta * scv.sin() / (1 + beta * scv.cos()));
  95.     }

  97.     /**
  98.      * Computes the elliptic eccentric anomaly from the elliptic mean anomaly.
  99.      * <p>
  100.      * The algorithm used here for solving hyperbolic Kepler equation is from Odell,
  101.      * A.W., Gooding, R.H. "Procedures for solving Kepler's equation." Celestial
  102.      * Mechanics 38, 307–334 (1986). https://doi.org/10.1007/BF01238923
  103.      * </p>
  104.      * @param e eccentricity such that 0 &le; e &lt; 1
  105.      * @param M elliptic mean anomaly (rad)
  106.      * @return elliptic eccentric anomaly (rad)
  107.      */
  108.     public static double ellipticMeanToEccentric(final double e, final double M) {
  109.         // reduce M to [-PI PI) interval
  110.         final double reducedM = MathUtils.normalizeAngle(M, 0.0);

  112.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  113.         double E;
  114.         if (FastMath.abs(reducedM) < 1.0 / 6.0) {
  115.             E = reducedM + e * (FastMath.cbrt(6 * reducedM) - reducedM);
  116.         } else {
  117.             if (reducedM < 0) {
  118.                 final double w = FastMath.PI + reducedM;
  119.                 E = reducedM + e * (A * w / (B - w) - FastMath.PI - reducedM);
  120.             } else {
  121.                 final double w = FastMath.PI - reducedM;
  122.                 E = reducedM + e * (FastMath.PI - A * w / (B - w) - reducedM);
  123.             }
  124.         }

  126.         final double e1 = 1 - e;
  127.         final boolean noCancellationRisk = (e1 + E * E / 6) >= 0.1;

  129.         // perform two iterations, each consisting of one Halley step and one
  130.         // Newton-Raphson step
  131.         for (int j = 0; j < 2; ++j) {
  132.             final double f;
  133.             double fd;
  134.             final SinCos sc = FastMath.sinCos(E);
  135.             final double fdd = e * sc.sin();
  136.             final double fddd = e * sc.cos();
  137.             if (noCancellationRisk) {
  138.                 f = (E - fdd) - reducedM;
  139.                 fd = 1 - fddd;
  140.             } else {
  141.                 f = eMeSinE(e, E) - reducedM;
  142.                 final double s = FastMath.sin(0.5 * E);
  143.                 fd = e1 + 2 * e * s * s;
  144.             }
  145.             final double dee = f * fd / (0.5 * f * fdd - fd * fd);

  147.             // update eccentric anomaly, using expressions that limit underflow problems
  148.             final double w = fd + 0.5 * dee * (fdd + dee * fddd / 3);
  149.             fd += dee * (fdd + 0.5 * dee * fddd);
  150.             E -= (f - dee * (fd - w)) / fd;

  152.         }

  154.         // expand the result back to original range
  155.         E += M - reducedM;

  157.         return E;
  158.     }

  160.     /**
  161.      * Accurate computation of E - e sin(E).
  162.      * <p>
  163.      * This method is used when E is close to 0 and e close to 1, i.e. near the
  164.      * perigee of almost parabolic orbits
  165.      * </p>
  166.      * @param e eccentricity
  167.      * @param E eccentric anomaly (rad)
  168.      * @return E - e sin(E)
  169.      */
  170.     private static double eMeSinE(final double e, final double E) {
  171.         double x = (1 - e) * FastMath.sin(E);
  172.         final double mE2 = -E * E;
  173.         double term = E;
  174.         double d = 0;
  175.         // the inequality test below IS intentional and should NOT be replaced by a
  176.         // check with a small tolerance
  177.         for (double x0 = Double.NaN; !Double.valueOf(x).equals(Double.valueOf(x0));) {
  178.             d += 2;
  179.             term *= mE2 / (d * (d + 1));
  180.             x0 = x;
  181.             x = x - term;
  182.         }
  183.         return x;
  184.     }

  186.     /**
  187.      * Computes the elliptic mean anomaly from the elliptic eccentric anomaly.
  188.      * @param e eccentricity such that 0 &le; e &lt; 1
  189.      * @param E elliptic eccentric anomaly (rad)
  190.      * @return elliptic mean anomaly (rad)
  191.      */
  192.     public static double ellipticEccentricToMean(final double e, final double E) {
  193.         return E - e * FastMath.sin(E);
  194.     }

  196.     /**
  197.      * Computes the hyperbolic true anomaly from the hyperbolic mean anomaly.
  198.      * @param e eccentricity &gt; 1
  199.      * @param M hyperbolic mean anomaly
  200.      * @return hyperbolic true anomaly (rad)
  201.      */
  202.     public static double hyperbolicMeanToTrue(final double e, final double M) {
  203.         final double H = hyperbolicMeanToEccentric(e, M);
  204.         final double v = hyperbolicEccentricToTrue(e, H);
  205.         return v;
  206.     }

  208.     /**
  209.      * Computes the hyperbolic mean anomaly from the hyperbolic true anomaly.
  210.      * @param e eccentricity &gt; 1
  211.      * @param v hyperbolic true anomaly (rad)
  212.      * @return hyperbolic mean anomaly
  213.      */
  214.     public static double hyperbolicTrueToMean(final double e, final double v) {
  215.         final double H = hyperbolicTrueToEccentric(e, v);
  216.         final double M = hyperbolicEccentricToMean(e, H);
  217.         return M;
  218.     }

  220.     /**
  221.      * Computes the hyperbolic true anomaly from the hyperbolic eccentric anomaly.
  222.      * @param e eccentricity &gt; 1
  223.      * @param H hyperbolic eccentric anomaly
  224.      * @return hyperbolic true anomaly (rad)
  225.      */
  226.     public static double hyperbolicEccentricToTrue(final double e, final double H) {
  227.         return 2 * FastMath.atan(FastMath.sqrt((e + 1) / (e - 1)) * FastMath.tanh(0.5 * H));
  228.     }

  230.     /**
  231.      * Computes the hyperbolic eccentric anomaly from the hyperbolic true anomaly.
  232.      * @param e eccentricity &gt; 1
  233.      * @param v hyperbolic true anomaly (rad)
  234.      * @return hyperbolic eccentric anomaly
  235.      */
  236.     public static double hyperbolicTrueToEccentric(final double e, final double v) {
  237.         final SinCos scv = FastMath.sinCos(v);
  238.         final double sinhH = FastMath.sqrt(e * e - 1) * scv.sin() / (1 + e * scv.cos());
  239.         return FastMath.asinh(sinhH);
  240.     }

  242.     /**
  243.      * Computes the hyperbolic eccentric anomaly from the hyperbolic mean anomaly.
  244.      * <p>
  245.      * The algorithm used here for solving hyperbolic Kepler equation is from
  246.      * Gooding, R.H., Odell, A.W. "The hyperbolic Kepler equation (and the elliptic
  247.      * equation revisited)." Celestial Mechanics 44, 267–282 (1988).
  248.      * https://doi.org/10.1007/BF01235540
  249.      * </p>
  250.      * @param e eccentricity &gt; 1
  251.      * @param M hyperbolic mean anomaly
  252.      * @return hyperbolic eccentric anomaly
  253.      */
  254.     public static double hyperbolicMeanToEccentric(final double e, final double M) {
  255.         // Solve L = S - g * asinh(S) for S = sinh(H).
  256.         final double L = M / e;
  257.         final double g = 1.0 / e;
  258.         final double g1 = 1.0 - g;

  260.         // Starter based on Lagrange's theorem.
  261.         double S = L;
  262.         if (L == 0.0) {
  263.             return 0.0;
  264.         }
  265.         final double cl = FastMath.sqrt(1.0 + L * L);
  266.         final double al = FastMath.asinh(L);
  267.         final double w = g * g * al / (cl * cl * cl);
  268.         S = 1.0 - g / cl;
  269.         S = L + g * al / FastMath.cbrt(S * S * S + w * L * (1.5 - (4.0 / 3.0) * g));

  271.         // Two iterations (at most) of Halley-then-Newton process.
  272.         for (int i = 0; i < 2; ++i) {
  273.             final double s0 = S * S;
  274.             final double s1 = s0 + 1.0;
  275.             final double s2 = FastMath.sqrt(s1);
  276.             final double s3 = s1 * s2;
  277.             final double fdd = g * S / s3;
  278.             final double fddd = g * (1.0 - 2.0 * s0) / (s1 * s3);

  280.             double f;
  281.             double fd;
  282.             if (s0 / 6.0 + g1 >= 0.5) {
  283.                 f = (S - g * FastMath.asinh(S)) - L;
  284.                 fd = 1.0 - g / s2;
  285.             } else {
  286.                 // Accurate computation of S - (1 - g1) * asinh(S)
  287.                 // when (g1, S) is close to (0, 0).
  288.                 final double t = S / (1.0 + FastMath.sqrt(1.0 + S * S));
  289.                 final double tsq = t * t;
  290.                 double x = S * (g1 + g * tsq);
  291.                 double term = 2.0 * g * t;
  292.                 double twoI1 = 1.0;
  293.                 double x0;
  294.                 int j = 0;
  295.                 do {
  296.                     if (++j == 1000000) {
  297.                         // This isn't expected to happen, but it protects against an infinite loop.
  298.                         throw new MathIllegalStateException(
  299.                                 OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY, j);
  300.                     }
  301.                     twoI1 += 2.0;
  302.                     term *= tsq;
  303.                     x0 = x;
  304.                     x -= term / twoI1;
  305.                 } while (x != x0);
  306.                 f = x - L;
  307.                 fd = (s0 / (s2 + 1.0) + g1) / s2;
  308.             }
  309.             final double ds = f * fd / (0.5 * f * fdd - fd * fd);
  310.             final double stemp = S + ds;
  311.             if (S == stemp) {
  312.                 break;
  313.             }
  314.             f += ds * (fd + 0.5 * ds * (fdd + ds / 3.0 * fddd));
  315.             fd += ds * (fdd + 0.5 * ds * fddd);
  316.             S = stemp - f / fd;
  317.         }

  319.         final double H = FastMath.asinh(S);
  320.         return H;
  321.     }

  323.     /**
  324.      * Computes the hyperbolic mean anomaly from the hyperbolic eccentric anomaly.
  325.      * @param e eccentricity &gt; 1
  326.      * @param H hyperbolic eccentric anomaly
  327.      * @return hyperbolic mean anomaly
  328.      */
  329.     public static double hyperbolicEccentricToMean(final double e, final double H) {
  330.         return e * FastMath.sinh(H) - H;
  331.     }

  333. }
Created with JaCoCo 0.8.8.202204050719