KeplerianAnomalyUtility.java

  1. /* Copyright 2002-2025 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.OrekitInternalError;
  24. import org.orekit.errors.OrekitMessages;

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

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

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

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

  47.     /** Private constructor for utility class. */
  48.     private KeplerianAnomalyUtility() {
  49.         // Nothing to do
  50.     }

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

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

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

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

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

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

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

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

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

  155.         }

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

  160.         return E;
  161.     }

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

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

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

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

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

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

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

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

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

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

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

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

  336.     /**
  337.      * Convert anomaly.
  338.      * @param oldType old position angle type
  339.      * @param anomaly old value for anomaly
  340.      * @param e eccentricity
  341.      * @param newType new position angle type
  342.      * @return converted anomaly
  343.      * @since 12.2
  344.      */
  345.     public static double convertAnomaly(final PositionAngleType oldType, final double anomaly, final double e,
  346.                                         final PositionAngleType newType) {
  347.         if (newType == oldType) {
  348.             return anomaly;

  350.         } else {
  351.             if (e > 1) {
  352.                 switch (newType) {
  353.                     case MEAN:
  354.                         if (oldType == PositionAngleType.ECCENTRIC) {
  355.                             return KeplerianAnomalyUtility.hyperbolicEccentricToMean(e, anomaly);
  356.                         } else {
  357.                             return KeplerianAnomalyUtility.hyperbolicTrueToMean(e, anomaly);
  358.                         }

  360.                     case ECCENTRIC:
  361.                         if (oldType == PositionAngleType.MEAN) {
  362.                             return KeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, anomaly);
  363.                         } else {
  364.                             return KeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, anomaly);
  365.                         }

  367.                     case TRUE:
  368.                         if (oldType == PositionAngleType.ECCENTRIC) {
  369.                             return KeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, anomaly);
  370.                         } else {
  371.                             return KeplerianAnomalyUtility.hyperbolicMeanToTrue(e, anomaly);
  372.                         }

  374.                     default:
  375.                         break;
  376.                 }

  378.             } else {
  379.                 switch (newType) {
  380.                     case MEAN:
  381.                         if (oldType == PositionAngleType.ECCENTRIC) {
  382.                             return KeplerianAnomalyUtility.ellipticEccentricToMean(e, anomaly);
  383.                         } else {
  384.                             return KeplerianAnomalyUtility.ellipticTrueToMean(e, anomaly);
  385.                         }

  387.                     case ECCENTRIC:
  388.                         if (oldType == PositionAngleType.MEAN) {
  389.                             return KeplerianAnomalyUtility.ellipticMeanToEccentric(e, anomaly);
  390.                         } else {
  391.                             return KeplerianAnomalyUtility.ellipticTrueToEccentric(e, anomaly);
  392.                         }

  394.                     case TRUE:
  395.                         if (oldType == PositionAngleType.ECCENTRIC) {
  396.                             return KeplerianAnomalyUtility.ellipticEccentricToTrue(e, anomaly);
  397.                         } else {
  398.                             return KeplerianAnomalyUtility.ellipticMeanToTrue(e, anomaly);
  399.                         }

  401.                     default:
  402.                         break;
  403.                 }
  404.             }
  405.             throw new OrekitInternalError(null);
  406.         }
  407.     }
  408. }
Created with JaCoCo 0.8.12.202403310830