KeplerianAnomalyUtility.java

  1. /* Copyright 2002-2024 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 constructor for utility class. */
  47.     private KeplerianAnomalyUtility() {
  48.         // Nothing to do
  49.     }

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

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

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

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

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

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

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

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

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

  154.         }

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

  159.         return E;
  160.     }

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

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

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

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

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

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

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

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

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

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

  321.         final double H = FastMath.asinh(S);
  322.         return H;
  323.     }

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

  335. }
Created with JaCoCo 0.8.11.202310140853