FieldKeplerianAnomalyUtility.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.CalculusFieldElement;
  20. import org.hipparchus.Field;
  21. import org.hipparchus.exception.MathIllegalStateException;
  22. import org.hipparchus.util.FastMath;
  23. import org.hipparchus.util.FieldSinCos;
  24. import org.hipparchus.util.MathUtils;
  25. import org.hipparchus.util.Precision;
  26. import org.orekit.errors.OrekitInternalError;
  27. import org.orekit.errors.OrekitMessages;

  29. /**
  30.  * Utility methods for converting between different Keplerian anomalies.
  31.  * @author Luc Maisonobe
  32.  * @author Andrea Antolino
  33.  * @author Andrew Goetz
  34.  */
  35. public class FieldKeplerianAnomalyUtility {

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

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

  43.     static {
  44.         final double k1 = 3 * FastMath.PI + 2;
  45.         final double k2 = FastMath.PI - 1;
  46.         final double k3 = 6 * FastMath.PI - 1;
  47.         A = 3 * k2 * k2 / k1;
  48.         B = k3 * k3 / (6 * k1);
  49.     }

  51.     /** Private constructor for utility class. */
  52.     private FieldKeplerianAnomalyUtility() {
  53.         // Nothing to do
  54.     }

  56.     /**
  57.      * Computes the elliptic true anomaly from the elliptic mean anomaly.
  58.      * @param <T> field type
  59.      * @param e eccentricity such that 0 &le; e &lt; 1
  60.      * @param M elliptic mean anomaly (rad)
  61.      * @return elliptic true anomaly (rad)
  62.      */
  63.     public static <T extends CalculusFieldElement<T>> T ellipticMeanToTrue(final T e, final T M) {
  64.         final T E = ellipticMeanToEccentric(e, M);
  65.         final T v = ellipticEccentricToTrue(e, E);
  66.         return v;
  67.     }

  69.     /**
  70.      * Computes the elliptic mean anomaly from the elliptic true anomaly.
  71.      * @param <T> field type
  72.      * @param e eccentricity such that 0 &le; e &lt; 1
  73.      * @param v elliptic true anomaly (rad)
  74.      * @return elliptic mean anomaly (rad)
  75.      */
  76.     public static <T extends CalculusFieldElement<T>> T ellipticTrueToMean(final T e, final T v) {
  77.         final T E = ellipticTrueToEccentric(e, v);
  78.         return ellipticEccentricToMean(e, E);
  79.     }

  81.     /**
  82.      * Computes the elliptic true anomaly from the elliptic eccentric anomaly.
  83.      * @param <T> field type
  84.      * @param e eccentricity such that 0 &le; e &lt; 1
  85.      * @param E elliptic eccentric anomaly (rad)
  86.      * @return elliptic true anomaly (rad)
  87.      */
  88.     public static <T extends CalculusFieldElement<T>> T ellipticEccentricToTrue(final T e, final T E) {
  89.         final T beta = e.divide(e.square().negate().add(1).sqrt().add(1));
  90.         final FieldSinCos<T> scE = FastMath.sinCos(E);
  91.         return E.add(beta.multiply(scE.sin()).divide(beta.multiply(scE.cos()).subtract(1).negate()).atan().multiply(2));
  92.     }

  94.     /**
  95.      * Computes the elliptic eccentric anomaly from the elliptic true anomaly.
  96.      * @param <T> field type
  97.      * @param e eccentricity such that 0 &le; e &lt; 1
  98.      * @param v elliptic true anomaly (rad)
  99.      * @return elliptic eccentric anomaly (rad)
  100.      */
  101.     public static <T extends CalculusFieldElement<T>> T ellipticTrueToEccentric(final T e, final T v) {
  102.         final T beta = e.divide(e.square().negate().add(1).sqrt().add(1));
  103.         final FieldSinCos<T> scv = FastMath.sinCos(v);
  104.         return v.subtract((beta.multiply(scv.sin()).divide(beta.multiply(scv.cos()).add(1))).atan().multiply(2));
  105.     }

  107.     /**
  108.      * Computes the elliptic eccentric anomaly from the elliptic mean anomaly.
  109.      * <p>
  110.      * The algorithm used here for solving hyperbolic Kepler equation is from Odell,
  111.      * A.W., Gooding, R.H. "Procedures for solving Kepler's equation." Celestial
  112.      * Mechanics 38, 307–334 (1986). https://doi.org/10.1007/BF01238923
  113.      * </p>
  114.      * @param <T> field type
  115.      * @param e eccentricity such that 0 &le; e &lt; 1
  116.      * @param M elliptic mean anomaly (rad)
  117.      * @return elliptic eccentric anomaly (rad)
  118.      */
  119.     public static <T extends CalculusFieldElement<T>> T ellipticMeanToEccentric(final T e, final T M) {
  120.         // reduce M to [-PI PI) interval
  121.         final T reducedM = MathUtils.normalizeAngle(M, M.getField().getZero());

  123.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  124.         T E;
  125.         if (reducedM.abs().getReal() < 1.0 / 6.0) {
  126.             if (FastMath.abs(reducedM.getReal()) < Precision.SAFE_MIN) {
  127.                 // this is an Orekit change to the S12 starter, mainly used when T is some kind
  128.                 // of derivative structure. If reducedM is 0.0, the derivative of cbrt is
  129.                 // infinite which induces NaN appearing later in the computation. As in this
  130.                 // case E and M are almost equal, we initialize E with reducedM
  131.                 E = reducedM;
  132.             } else {
  133.                 // this is the standard S12 starter
  134.                 E = reducedM.add(e.multiply((reducedM.multiply(6).cbrt()).subtract(reducedM)));
  135.             }
  136.         } else {
  137.             final T pi = e.getPi();
  138.             if (reducedM.getReal() < 0) {
  139.                 final T w = reducedM.add(pi);
  140.                 E = reducedM.add(e.multiply(w.multiply(A).divide(w.negate().add(B)).subtract(pi).subtract(reducedM)));
  141.             } else {
  142.                 final T w = reducedM.negate().add(pi);
  143.                 E = reducedM
  144.                         .add(e.multiply(w.multiply(A).divide(w.negate().add(B)).negate().subtract(reducedM).add(pi)));
  145.             }
  146.         }

  148.         final T e1 = e.negate().add(1);
  149.         final boolean noCancellationRisk = (e1.getReal() + E.getReal() * E.getReal() / 6) >= 0.1;

  151.         // perform two iterations, each consisting of one Halley step and one
  152.         // Newton-Raphson step
  153.         for (int j = 0; j < 2; ++j) {

  155.             final T f;
  156.             T fd;
  157.             final FieldSinCos<T> scE = FastMath.sinCos(E);
  158.             final T fdd = e.multiply(scE.sin());
  159.             final T fddd = e.multiply(scE.cos());

  161.             if (noCancellationRisk) {

  163.                 f = (E.subtract(fdd)).subtract(reducedM);
  164.                 fd = fddd.negate().add(1);
  165.             } else {

  167.                 f = eMeSinE(e, E).subtract(reducedM);
  168.                 final T s = E.multiply(0.5).sin();
  169.                 fd = e1.add(e.multiply(s.square()).multiply(2));
  170.             }
  171.             final T dee = f.multiply(fd).divide(f.multiply(fdd).multiply(0.5).subtract(fd.square()));

  173.             // update eccentric anomaly, using expressions that limit underflow problems
  174.             final T w = fd.add(dee.multiply(fdd.add(dee.multiply(fddd.divide(3)))).multiply(0.5));
  175.             fd = fd.add(dee.multiply(fdd.add(dee.multiply(fddd).multiply(0.5))));
  176.             E = E.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd));

  178.         }

  180.         // expand the result back to original range
  181.         E = E.add(M).subtract(reducedM);
  182.         return E;
  183.     }

  185.     /**
  186.      * Accurate computation of E - e sin(E).
  187.      * <p>
  188.      * This method is used when E is close to 0 and e close to 1, i.e. near the
  189.      * perigee of almost parabolic orbits
  190.      * </p>
  191.      * @param <T> field type
  192.      * @param e eccentricity
  193.      * @param E eccentric anomaly (rad)
  194.      * @return E - e sin(E)
  195.      */
  196.     private static <T extends CalculusFieldElement<T>> T eMeSinE(final T e, final T E) {
  197.         T x = (e.negate().add(1)).multiply(E.sin());
  198.         final T mE2 = E.square().negate();
  199.         T term = E;
  200.         double d = 0;
  201.         // the inequality test below IS intentional and should NOT be replaced by a
  202.         // check with a small tolerance
  203.         for (T x0 = E.getField().getZero().add(Double.NaN); !Double.valueOf(x.getReal())
  204.                 .equals(x0.getReal());) {
  205.             d += 2;
  206.             term = term.multiply(mE2.divide(d * (d + 1)));
  207.             x0 = x;
  208.             x = x.subtract(term);
  209.         }
  210.         return x;
  211.     }

  213.     /**
  214.      * Computes the elliptic mean anomaly from the elliptic eccentric anomaly.
  215.      * @param <T> field type
  216.      * @param e eccentricity such that 0 &le; e &lt; 1
  217.      * @param E elliptic eccentric anomaly (rad)
  218.      * @return elliptic mean anomaly (rad)
  219.      */
  220.     public static <T extends CalculusFieldElement<T>> T ellipticEccentricToMean(final T e, final T E) {
  221.         return E.subtract(e.multiply(E.sin()));
  222.     }

  224.     /**
  225.      * Computes the hyperbolic true anomaly from the hyperbolic mean anomaly.
  226.      * @param <T> field type
  227.      * @param e eccentricity &gt; 1
  228.      * @param M hyperbolic mean anomaly
  229.      * @return hyperbolic true anomaly (rad)
  230.      */
  231.     public static <T extends CalculusFieldElement<T>> T hyperbolicMeanToTrue(final T e, final T M) {
  232.         final T H = hyperbolicMeanToEccentric(e, M);
  233.         final T v = hyperbolicEccentricToTrue(e, H);
  234.         return v;
  235.     }

  237.     /**
  238.      * Computes the hyperbolic mean anomaly from the hyperbolic true anomaly.
  239.      * @param <T> field type
  240.      * @param e eccentricity &gt; 1
  241.      * @param v hyperbolic true anomaly (rad)
  242.      * @return hyperbolic mean anomaly
  243.      */
  244.     public static <T extends CalculusFieldElement<T>> T hyperbolicTrueToMean(final T e, final T v) {
  245.         final T H = hyperbolicTrueToEccentric(e, v);
  246.         return hyperbolicEccentricToMean(e, H);
  247.     }

  249.     /**
  250.      * Computes the hyperbolic true anomaly from the hyperbolic eccentric anomaly.
  251.      * @param <T> field type
  252.      * @param e eccentricity &gt; 1
  253.      * @param H hyperbolic eccentric anomaly
  254.      * @return hyperbolic true anomaly (rad)
  255.      */
  256.     public static <T extends CalculusFieldElement<T>> T hyperbolicEccentricToTrue(final T e, final T H) {
  257.         final T s = e.add(1).divide(e.subtract(1)).sqrt();
  258.         final T tanH = H.multiply(0.5).tanh();
  259.         return s.multiply(tanH).atan().multiply(2);
  260.     }

  262.     /**
  263.      * Computes the hyperbolic eccentric anomaly from the hyperbolic true anomaly.
  264.      * @param <T> field type
  265.      * @param e eccentricity &gt; 1
  266.      * @param v hyperbolic true anomaly (rad)
  267.      * @return hyperbolic eccentric anomaly
  268.      */
  269.     public static <T extends CalculusFieldElement<T>> T hyperbolicTrueToEccentric(final T e, final T v) {
  270.         final FieldSinCos<T> scv = FastMath.sinCos(v);
  271.         final T sinhH = e.square().subtract(1).sqrt().multiply(scv.sin()).divide(e.multiply(scv.cos()).add(1));
  272.         return sinhH.asinh();
  273.     }

  275.     /**
  276.      * Computes the hyperbolic eccentric anomaly from the hyperbolic mean anomaly.
  277.      * <p>
  278.      * The algorithm used here for solving hyperbolic Kepler equation is from
  279.      * Gooding, R.H., Odell, A.W. "The hyperbolic Kepler equation (and the elliptic
  280.      * equation revisited)." Celestial Mechanics 44, 267–282 (1988).
  281.      * https://doi.org/10.1007/BF01235540
  282.      * </p>
  283.      * @param <T> field type
  284.      * @param e eccentricity &gt; 1
  285.      * @param M hyperbolic mean anomaly
  286.      * @return hyperbolic eccentric anomaly
  287.      */
  288.     public static <T extends CalculusFieldElement<T>> T hyperbolicMeanToEccentric(final T e, final T M) {
  289.         final Field<T> field = e.getField();
  290.         final T zero = field.getZero();
  291.         final T one = field.getOne();
  292.         final T two = zero.newInstance(2.0);
  293.         final T three = zero.newInstance(3.0);
  294.         final T half = zero.newInstance(0.5);
  295.         final T onePointFive = zero.newInstance(1.5);
  296.         final T fourThirds = zero.newInstance(4.0).divide(3.0);

  298.         // Solve L = S - g * asinh(S) for S = sinh(H).
  299.         final T L = M.divide(e);
  300.         final T g = e.reciprocal();
  301.         final T g1 = one.subtract(g);

  303.         // Starter based on Lagrange's theorem.
  304.         T S = L;
  305.         if (L.isZero()) {
  306.             return M.getField().getZero();
  307.         }
  308.         final T cl = L.square().add(one).sqrt();
  309.         final T al = L.asinh();
  310.         final T w = g.square().multiply(al).divide(cl.multiply(cl).multiply(cl));
  311.         S = one.subtract(g.divide(cl));
  312.         S = L.add(g.multiply(al).divide(S.square().multiply(S)
  313.                 .add(w.multiply(L).multiply(onePointFive.subtract(fourThirds.multiply(g)))).cbrt()));

  315.         // Two iterations (at most) of Halley-then-Newton process.
  316.         for (int i = 0; i < 2; ++i) {
  317.             final T s0 = S.square();
  318.             final T s1 = s0.add(one);
  319.             final T s2 = s1.sqrt();
  320.             final T s3 = s1.multiply(s2);
  321.             final T fdd = g.multiply(S).divide(s3);
  322.             final T fddd = g.multiply(one.subtract(two.multiply(s0))).divide(s1.multiply(s3));

  324.             T f;
  325.             T fd;
  326.             if (s0.divide(6.0).add(g1).getReal() >= 0.5) {
  327.                 f = S.subtract(g.multiply(S.asinh())).subtract(L);
  328.                 fd = one.subtract(g.divide(s2));
  329.             } else {
  330.                 // Accurate computation of S - (1 - g1) * asinh(S)
  331.                 // when (g1, S) is close to (0, 0).
  332.                 final T t = S.divide(one.add(one.add(S.multiply(S)).sqrt()));
  333.                 final T tsq = t.square();
  334.                 T x = S.multiply(g1.add(g.multiply(tsq)));
  335.                 T term = two.multiply(g).multiply(t);
  336.                 T twoI1 = one;
  337.                 T x0;
  338.                 int j = 0;
  339.                 do {
  340.                     if (++j == 1000000) {
  341.                         // This isn't expected to happen, but it protects against an infinite loop.
  342.                         throw new MathIllegalStateException(
  343.                                 OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY, j);
  344.                     }
  345.                     twoI1 = twoI1.add(2.0);
  346.                     term = term.multiply(tsq);
  347.                     x0 = x;
  348.                     x = x.subtract(term.divide(twoI1));
  349.                 } while (x.getReal() != x0.getReal());
  350.                 f = x.subtract(L);
  351.                 fd = s0.divide(s2.add(one)).add(g1).divide(s2);
  352.             }
  353.             final T ds = f.multiply(fd).divide(half.multiply(f).multiply(fdd).subtract(fd.multiply(fd)));
  354.             final T stemp = S.add(ds);
  355.             if (S.getReal() == stemp.getReal()) {
  356.                 break;
  357.             }
  358.             f = f.add(ds.multiply(fd.add(half.multiply(ds.multiply(fdd.add(ds.divide(three).multiply(fddd)))))));
  359.             fd = fd.add(ds.multiply(fdd.add(half.multiply(ds).multiply(fddd))));
  360.             S = stemp.subtract(f.divide(fd));
  361.         }

  363.         final T H = S.asinh();
  364.         return H;
  365.     }

  367.     /**
  368.      * Computes the hyperbolic mean anomaly from the hyperbolic eccentric anomaly.
  369.      * @param <T> field type
  370.      * @param e eccentricity &gt; 1
  371.      * @param H hyperbolic eccentric anomaly
  372.      * @return hyperbolic mean anomaly
  373.      */
  374.     public static <T extends CalculusFieldElement<T>> T hyperbolicEccentricToMean(final T e, final T H) {
  375.         return e.multiply(H.sinh()).subtract(H);
  376.     }

  378.     /**
  379.      * Convert anomaly.
  380.      * @param oldType old position angle type
  381.      * @param anomaly old value for anomaly
  382.      * @param e eccentricity
  383.      * @param newType new position angle type
  384.      * @param <T> field type
  385.      * @return converted anomaly
  386.      * @since 12.2
  387.      */
  388.     public static <T extends CalculusFieldElement<T>> T convertAnomaly(final PositionAngleType oldType, final T anomaly,
  389.                                                                        final T e, final PositionAngleType newType) {
  390.         if (oldType == newType) {
  391.             return anomaly;

  393.         } else {
  394.             if (e.getReal() > 1) {
  395.                 switch (newType) {
  396.                     case MEAN:
  397.                         if (oldType == PositionAngleType.ECCENTRIC) {
  398.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(e, anomaly);
  399.                         } else {
  400.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(e, anomaly);
  401.                         }

  403.                     case ECCENTRIC:
  404.                         if (oldType == PositionAngleType.MEAN) {
  405.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, anomaly);
  406.                         } else {
  407.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, anomaly);
  408.                         }

  410.                     case TRUE:
  411.                         if (oldType == PositionAngleType.ECCENTRIC) {
  412.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, anomaly);
  413.                         } else {
  414.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(e, anomaly);
  415.                         }

  417.                     default:
  418.                         break;
  419.                 }

  421.             } else {
  422.                 switch (newType) {
  423.                     case MEAN:
  424.                         if (oldType == PositionAngleType.ECCENTRIC) {
  425.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, anomaly);
  426.                         } else {
  427.                             return FieldKeplerianAnomalyUtility.ellipticTrueToMean(e, anomaly);
  428.                         }

  430.                     case ECCENTRIC:
  431.                         if (oldType == PositionAngleType.MEAN) {
  432.                             return FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, anomaly);
  433.                         } else {
  434.                             return FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, anomaly);
  435.                         }

  437.                     case TRUE:
  438.                         if (oldType == PositionAngleType.ECCENTRIC) {
  439.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, anomaly);
  440.                         } else {
  441.                             return FieldKeplerianAnomalyUtility.ellipticMeanToTrue(e, anomaly);
  442.                         }

  444.                     default:
  445.                         break;
  446.                 }
  447.             }
  448.             throw new OrekitInternalError(null);
  449.         }
  450.     }
  451. }
Created with JaCoCo 0.8.12.202403310830