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 ≤ e < 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 ≤ e < 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 ≤ e < 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 ≤ e < 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 ≤ e < 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 ≤ e < 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 > 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 > 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 > 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 > 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 > 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 > 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);
}
}