FieldKeplerianAnomalyUtility.java

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

import org.hipparchus.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.exception.MathIllegalStateException;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.FieldSinCos;
import org.hipparchus.util.MathUtils;
import org.hipparchus.util.Precision;
import org.orekit.errors.OrekitMessages;

/**
 * Utility methods for converting between different Keplerian anomalies.
 * @author Luc Maisonobe
 * @author Andrea Antolino
 * @author Andrew Goetz
 */
public class FieldKeplerianAnomalyUtility {

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

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

    static {
        final double k1 = 3 * FastMath.PI + 2;
        final double k2 = FastMath.PI - 1;
        final double k3 = 6 * FastMath.PI - 1;
        A = 3 * k2 * k2 / k1;
        B = k3 * k3 / (6 * k1);
    }

    private FieldKeplerianAnomalyUtility() {
    }

    /**
     * Computes the elliptic true anomaly from the elliptic mean anomaly.
     * @param <T> field type
     * @param e eccentricity such that 0 &le; e &lt; 1
     * @param M elliptic mean anomaly (rad)
     * @return elliptic true anomaly (rad)
     */
    public static <T extends CalculusFieldElement<T>> T ellipticMeanToTrue(final T e, final T M) {
        final T E = ellipticMeanToEccentric(e, M);
        final T v = ellipticEccentricToTrue(e, E);
        return v;
    }

    /**
     * Computes the elliptic mean anomaly from the elliptic true anomaly.
     * @param <T> field type
     * @param e eccentricity such that 0 &le; e &lt; 1
     * @param v elliptic true anomaly (rad)
     * @return elliptic mean anomaly (rad)
     */
    public static <T extends CalculusFieldElement<T>> T ellipticTrueToMean(final T e, final T v) {
        final T E = ellipticTrueToEccentric(e, v);
        final T M = ellipticEccentricToMean(e, E);
        return M;
    }

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

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

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

        // compute start value according to A. W. Odell and R. H. Gooding S12 starter
        T E;
        if (reducedM.abs().getReal() < 1.0 / 6.0) {
            if (FastMath.abs(reducedM.getReal()) < Precision.SAFE_MIN) {
                // this is an Orekit change to the S12 starter, mainly used when T is some kind
                // of derivative structure. If reducedM is 0.0, the derivative of cbrt is
                // infinite which induces NaN appearing later in the computation. As in this
                // case E and M are almost equal, we initialize E with reducedM
                E = reducedM;
            } else {
                // this is the standard S12 starter
                E = reducedM.add(e.multiply((reducedM.multiply(6).cbrt()).subtract(reducedM)));
            }
        } else {
            final T pi = e.getPi();
            if (reducedM.getReal() < 0) {
                final T w = reducedM.add(pi);
                E = reducedM.add(e.multiply(w.multiply(A).divide(w.negate().add(B)).subtract(pi).subtract(reducedM)));
            } else {
                final T w = reducedM.negate().add(pi);
                E = reducedM
                        .add(e.multiply(w.multiply(A).divide(w.negate().add(B)).negate().subtract(reducedM).add(pi)));
            }
        }

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

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

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

            if (noCancellationRisk) {

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

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

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

        }

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

    /**
     * Accurate computation of E - e sin(E).
     * <p>
     * This method is used when E is close to 0 and e close to 1, i.e. near the
     * perigee of almost parabolic orbits
     * </p>
     * @param <T> field type
     * @param e eccentricity
     * @param E eccentric anomaly (rad)
     * @return E - e sin(E)
     */
    private static <T extends CalculusFieldElement<T>> T eMeSinE(final T e, final T E) {
        T x = (e.negate().add(1)).multiply(E.sin());
        final T mE2 = E.negate().multiply(E);
        T term = E;
        double d = 0;
        // the inequality test below IS intentional and should NOT be replaced by a
        // check with a small tolerance
        for (T x0 = E.getField().getZero().add(Double.NaN); !Double.valueOf(x.getReal())
                .equals(Double.valueOf(x0.getReal()));) {
            d += 2;
            term = term.multiply(mE2.divide(d * (d + 1)));
            x0 = x;
            x = x.subtract(term);
        }
        return x;
    }

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

    /**
     * Computes the hyperbolic true anomaly from the hyperbolic mean anomaly.
     * @param <T> field type
     * @param e eccentricity &gt; 1
     * @param M hyperbolic mean anomaly
     * @return hyperbolic true anomaly (rad)
     */
    public static <T extends CalculusFieldElement<T>> T hyperbolicMeanToTrue(final T e, final T M) {
        final T H = hyperbolicMeanToEccentric(e, M);
        final T v = hyperbolicEccentricToTrue(e, H);
        return v;
    }

    /**
     * Computes the hyperbolic mean anomaly from the hyperbolic true anomaly.
     * @param <T> field type
     * @param e eccentricity &gt; 1
     * @param v hyperbolic true anomaly (rad)
     * @return hyperbolic mean anomaly
     */
    public static <T extends CalculusFieldElement<T>> T hyperbolicTrueToMean(final T e, final T v) {
        final T H = hyperbolicTrueToEccentric(e, v);
        final T M = hyperbolicEccentricToMean(e, H);
        return M;
    }

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

    /**
     * Computes the hyperbolic eccentric anomaly from the hyperbolic true anomaly.
     * @param <T> field type
     * @param e eccentricity &gt; 1
     * @param v hyperbolic true anomaly (rad)
     * @return hyperbolic eccentric anomaly
     */
    public static <T extends CalculusFieldElement<T>> T hyperbolicTrueToEccentric(final T e, final T v) {
        final FieldSinCos<T> scv = FastMath.sinCos(v);
        final T sinhH = e.multiply(e).subtract(1).sqrt().multiply(scv.sin()).divide(e.multiply(scv.cos()).add(1));
        return sinhH.asinh();
    }

    /**
     * Computes the hyperbolic eccentric anomaly from the hyperbolic mean anomaly.
     * <p>
     * The algorithm used here for solving hyperbolic Kepler equation is from
     * Gooding, R.H., Odell, A.W. "The hyperbolic Kepler equation (and the elliptic
     * equation revisited)." Celestial Mechanics 44, 267–282 (1988).
     * https://doi.org/10.1007/BF01235540
     * </p>
     * @param <T> field type
     * @param e eccentricity &gt; 1
     * @param M hyperbolic mean anomaly
     * @return hyperbolic eccentric anomaly
     */
    public static <T extends CalculusFieldElement<T>> T hyperbolicMeanToEccentric(final T e, final T M) {
        final Field<T> field = e.getField();
        final T zero = field.getZero();
        final T one = field.getOne();
        final T two = zero.add(2.0);
        final T three = zero.add(3.0);
        final T half = zero.add(0.5);
        final T onePointFive = zero.add(1.5);
        final T fourThirds = zero.add(4.0).divide(zero.add(3.0));

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

        // Starter based on Lagrange's theorem.
        T S = L;
        if (L.isZero()) {
            return M.getField().getZero();
        }
        final T cl = L.multiply(L).add(one).sqrt();
        final T al = L.asinh();
        final T w = g.multiply(g).multiply(al).divide(cl.multiply(cl).multiply(cl));
        S = one.subtract(g.divide(cl));
        S = L.add(g.multiply(al).divide(S.multiply(S).multiply(S)
                .add(w.multiply(L).multiply(onePointFive.subtract(fourThirds.multiply(g)))).cbrt()));

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

            T f;
            T fd;
            if (s0.divide(zero.add(6.0)).add(g1).getReal() >= 0.5) {
                f = S.subtract(g.multiply(S.asinh())).subtract(L);
                fd = one.subtract(g.divide(s2));
            } else {
                // Accurate computation of S - (1 - g1) * asinh(S)
                // when (g1, S) is close to (0, 0).
                final T t = S.divide(one.add(one.add(S.multiply(S)).sqrt()));
                final T tsq = t.multiply(t);
                T x = S.multiply(g1.add(g.multiply(tsq)));
                T term = two.multiply(g).multiply(t);
                T twoI1 = one;
                T x0;
                int j = 0;
                do {
                    if (++j == 1000000) {
                        // This isn't expected to happen, but it protects against an infinite loop.
                        throw new MathIllegalStateException(
                                OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY, j);
                    }
                    twoI1 = twoI1.add(2.0);
                    term = term.multiply(tsq);
                    x0 = x;
                    x = x.subtract(term.divide(twoI1));
                } while (x.getReal() != x0.getReal());
                f = x.subtract(L);
                fd = s0.divide(s2.add(one)).add(g1).divide(s2);
            }
            final T ds = f.multiply(fd).divide(half.multiply(f).multiply(fdd).subtract(fd.multiply(fd)));
            final T stemp = S.add(ds);
            if (S.getReal() == stemp.getReal()) {
                break;
            }
            f = f.add(ds.multiply(fd.add(half.multiply(ds.multiply(fdd.add(ds.divide(three).multiply(fddd)))))));
            fd = fd.add(ds.multiply(fdd.add(half.multiply(ds).multiply(fddd))));
            S = stemp.subtract(f.divide(fd));
        }

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

    /**
     * Computes the hyperbolic mean anomaly from the hyperbolic eccentric anomaly.
     * @param <T> field type
     * @param e eccentricity &gt; 1
     * @param H hyperbolic eccentric anomaly
     * @return hyperbolic mean anomaly
     */
    public static <T extends CalculusFieldElement<T>> T hyperbolicEccentricToMean(final T e, final T H) {
        return e.multiply(H.sinh()).subtract(H);
    }

}