FieldKeplerianOrbit.java

  1. /* Copyright 2002-2018 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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;


  20. import java.util.HashMap;
  21. import java.util.List;
  22. import java.util.Map;
  23. import java.util.stream.Collectors;
  24. import java.util.stream.Stream;

  26. import org.hipparchus.Field;
  27. import org.hipparchus.RealFieldElement;
  28. import org.hipparchus.analysis.differentiation.FDSFactory;
  29. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  30. import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
  31. import org.hipparchus.exception.MathIllegalArgumentException;
  32. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  33. import org.hipparchus.util.FastMath;
  34. import org.hipparchus.util.MathArrays;
  35. import org.hipparchus.util.Precision;
  36. import org.orekit.errors.OrekitIllegalArgumentException;
  37. import org.orekit.errors.OrekitInternalError;
  38. import org.orekit.errors.OrekitMessages;
  39. import org.orekit.frames.Frame;
  40. import org.orekit.time.FieldAbsoluteDate;
  41. import org.orekit.utils.FieldPVCoordinates;
  42. import org.orekit.utils.TimeStampedFieldPVCoordinates;


  45. /**
  46.  * This class handles traditional Keplerian orbital parameters.

  48.  * <p>
  49.  * The parameters used internally are the classical Keplerian elements:
  50.  *   <pre>
  51.  *     a
  52.  *     e
  53.  *     i
  54.  *     ω
  55.  *     Ω
  56.  *     v
  57.  *   </pre>
  58.  * where ω stands for the Perigee Argument, Ω stands for the
  59.  * Right Ascension of the Ascending Node and v stands for the true anomaly.
  60.  * </p>
  61.  * <p>
  62.  * This class supports hyperbolic orbits, using the convention that semi major
  63.  * axis is negative for such orbits (and of course eccentricity is greater than 1).
  64.  * </p>
  65.  * <p>
  66.  * When orbit is either equatorial or circular, some Keplerian elements
  67.  * (more precisely ω and Ω) become ambiguous so this class should not
  68.  * be used for such orbits. For this reason, {@link EquinoctialOrbit equinoctial
  69.  * orbits} is the recommended way to represent orbits.
  70.  * </p>

  72.  * <p>
  73.  * The instance <code>KeplerianOrbit</code> is guaranteed to be immutable.
  74.  * </p>
  75.  * @see     Orbit
  76.  * @see    CircularOrbit
  77.  * @see    CartesianOrbit
  78.  * @see    EquinoctialOrbit
  79.  * @author Luc Maisonobe
  80.  * @author Guylaine Prat
  81.  * @author Fabien Maussion
  82.  * @author V&eacute;ronique Pommier-Maurussane
  83.  * @author Andrea Antolino
  84.  * @since 9.0
  85.  */
  86. public class FieldKeplerianOrbit<T extends RealFieldElement<T>> extends FieldOrbit<T> {

  88.     /** Factory for first time derivatives. */
  89.     private static final Map<Field<? extends RealFieldElement<?>>, FDSFactory<? extends RealFieldElement<?>>> FACTORIES =
  90.                     new HashMap<>();

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

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

  98.     static {
  99.         final double k1 = 3 * FastMath.PI + 2;
  100.         final double k2 = FastMath.PI - 1;
  101.         final double k3 = 6 * FastMath.PI - 1;
  102.         A  = 3 * k2 * k2 / k1;
  103.         B  = k3 * k3 / (6 * k1);
  104.     }

  106.     /** Semi-major axis (m). */
  107.     private final T a;

  109.     /** Eccentricity. */
  110.     private final T e;

  112.     /** Inclination (rad). */
  113.     private final T i;

  115.     /** Perigee Argument (rad). */
  116.     private final T pa;

  118.     /** Right Ascension of Ascending Node (rad). */
  119.     private final T raan;

  121.     /** True anomaly (rad). */
  122.     private final T v;

  124.     /** Semi-major axis derivative (m/s). */
  125.     private final T aDot;

  127.     /** Eccentricity derivative. */
  128.     private final T eDot;

  130.     /** Inclination derivative (rad/s). */
  131.     private final T iDot;

  133.     /** Perigee Argument derivative (rad/s). */
  134.     private final T paDot;

  136.     /** Right Ascension of Ascending Node derivative (rad/s). */
  137.     private final T raanDot;

  139.     /** True anomaly derivative (rad/s). */
  140.     private final T vDot;

  142.     /** Partial Cartesian coordinates (position and velocity are valid, acceleration may be missing). */
  143.     private FieldPVCoordinates<T> partialPV;

  145.     /** Identity element. */
  146.     private final T one;

  148.     /** Zero element. */
  149.     private final T zero;

  151.     /** Third Canonical Vector. */
  152.     private final FieldVector3D<T> PLUS_K;

  154.     /** Creates a new instance.
  155.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  156.      * @param e eccentricity
  157.      * @param i inclination (rad)
  158.      * @param pa perigee argument (ω, rad)
  159.      * @param raan right ascension of ascending node (Ω, rad)
  160.      * @param anomaly mean, eccentric or true anomaly (rad)
  161.      * @param type type of anomaly
  162.      * @param frame the frame in which the parameters are defined
  163.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  164.      * @param date date of the orbital parameters
  165.      * @param mu central attraction coefficient (m³/s²)
  166.      * @exception IllegalArgumentException if frame is not a {@link
  167.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  168.      * or v is out of range for hyperbolic orbits
  169.      */
  170.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  171.                                final T pa, final T raan,
  172.                                final T anomaly, final PositionAngle type,
  173.                                final Frame frame, final FieldAbsoluteDate<T> date, final double mu)
  174.         throws IllegalArgumentException {
  175.         this(a, e, i, pa, raan, anomaly,
  176.              null, null, null, null, null, null,
  177.              type, frame, date, mu);
  178.     }

  180.     /** Creates a new instance.
  181.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  182.      * @param e eccentricity
  183.      * @param i inclination (rad)
  184.      * @param pa perigee argument (ω, rad)
  185.      * @param raan right ascension of ascending node (Ω, rad)
  186.      * @param anomaly mean, eccentric or true anomaly (rad)
  187.      * @param aDot  semi-major axis derivative, null if unknown (m/s)
  188.      * @param eDot eccentricity derivative, null if unknown
  189.      * @param iDot inclination derivative, null if unknown (rad/s)
  190.      * @param paDot perigee argument derivative, null if unknown (rad/s)
  191.      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
  192.      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
  193.      * @param type type of anomaly
  194.      * @param frame the frame in which the parameters are defined
  195.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  196.      * @param date date of the orbital parameters
  197.      * @param mu central attraction coefficient (m³/s²)
  198.      * @exception IllegalArgumentException if frame is not a {@link
  199.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  200.      * or v is out of range for hyperbolic orbits
  201.      */
  202.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  203.                                final T pa, final T raan, final T anomaly,
  204.                                final T aDot, final T eDot, final T iDot,
  205.                                final T paDot, final T raanDot, final T anomalyDot,
  206.                                final PositionAngle type,
  207.                                final Frame frame, final FieldAbsoluteDate<T> date, final double mu)
  208.         throws IllegalArgumentException {
  209.         super(frame, date, mu);
  210.         if (a.multiply(e.negate().add(1)).getReal() < 0) {
  211.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a.getReal(), e.getReal());
  212.         }

  214.         if (!FACTORIES.containsKey(a.getField())) {
  215.             FACTORIES.put(a.getField(), new FDSFactory<>(a.getField(), 1, 1));
  216.         }

  218.         this.a       =    a;
  219.         this.aDot    =    aDot;
  220.         this.e       =    e;
  221.         this.eDot    =    eDot;
  222.         this.i       =    i;
  223.         this.iDot    =    iDot;
  224.         this.pa      =   pa;
  225.         this.paDot   =   paDot;
  226.         this.raan    = raan;
  227.         this.raanDot = raanDot;

  229.         /** Identity element. */
  230.         this.one = a.getField().getOne();

  232.         /** Zero element. */
  233.         this.zero = a.getField().getZero();

  235.         /**Third canonical vector. */
  236.         this.PLUS_K = FieldVector3D.getPlusK(a.getField());

  238.         if (hasDerivatives()) {
  239.             @SuppressWarnings("unchecked")
  240.             final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  241.             final FieldDerivativeStructure<T> eDS = factory.build(e, eDot);
  242.             final FieldDerivativeStructure<T> anomalyDS  = factory.build(anomaly,  anomalyDot);
  243.             final FieldDerivativeStructure<T> vDS;
  244.             switch (type) {
  245.                 case MEAN :
  246.                     vDS = (a.getReal() < 0) ?
  247.                           hyperbolicEccentricToTrue(meanToHyperbolicEccentric(anomalyDS, eDS), eDS) :
  248.                           ellipticEccentricToTrue(meanToEllipticEccentric(anomalyDS, eDS), eDS);
  249.                     break;
  250.                 case ECCENTRIC :
  251.                     vDS = (a.getReal() < 0) ?
  252.                           hyperbolicEccentricToTrue(anomalyDS, eDS) :
  253.                           ellipticEccentricToTrue(anomalyDS, eDS);
  254.                     break;
  255.                 case TRUE :
  256.                     vDS = anomalyDS;
  257.                     break;
  258.                 default : // this should never happen
  259.                     throw new OrekitInternalError(null);
  260.             }
  261.             this.v    = vDS.getValue();
  262.             this.vDot = vDS.getPartialDerivative(1);
  263.         } else {
  264.             switch (type) {
  265.                 case MEAN :

  267.                     this.v = (a.getReal() < 0) ?
  268.                              hyperbolicEccentricToTrue(meanToHyperbolicEccentric(anomaly, e), e) :
  269.                              ellipticEccentricToTrue(meanToEllipticEccentric(anomaly, e), e);

  271.                     break;
  272.                 case ECCENTRIC :
  273.                     this.v = (a.getReal() < 0) ?
  274.                              hyperbolicEccentricToTrue(anomaly, e) :
  275.                              ellipticEccentricToTrue(anomaly, e);

  277.                     break;
  278.                 case TRUE :
  279.                     this.v = anomaly;
  280.                     break;
  281.                 default : // this should never happen
  282.                     throw new OrekitInternalError(null);
  283.             }
  284.             this.vDot = null;
  285.         }

  287.         // check true anomaly range
  288.         if (e.multiply(v.cos()).add(1).getReal() <= 0) {
  289.             final double vMax = e.reciprocal().negate().acos().getReal();
  290.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
  291.                                                      v.getReal(), e.getReal(), -vMax, vMax);
  292.         }

  294.         this.partialPV = null;

  296.     }

  298.     /** Constructor from Cartesian parameters.
  299.      *
  300.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  301.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  302.      * use {@code mu} and the position to compute the acceleration, including
  303.      * {@link #shiftedBy(RealFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  304.      *
  305.      * @param pvCoordinates the PVCoordinates of the satellite
  306.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  307.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  308.      * @param mu central attraction coefficient (m³/s²)
  309.      * @exception IllegalArgumentException if frame is not a {@link
  310.      * Frame#isPseudoInertial pseudo-inertial frame}
  311.      */
  312.     public FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  313.                                final Frame frame, final double mu)
  314.         throws IllegalArgumentException {
  315.         this(pvCoordinates, frame, mu, hasNonKeplerianAcceleration(pvCoordinates, mu));
  316.     }

  318.     /** Constructor from Cartesian parameters.
  319.      *
  320.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  321.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  322.      * use {@code mu} and the position to compute the acceleration, including
  323.      * {@link #shiftedBy(RealFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  324.      *
  325.      * @param pvCoordinates the PVCoordinates of the satellite
  326.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  327.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  328.      * @param mu central attraction coefficient (m³/s²)
  329.      * @param reliableAcceleration if true, the acceleration is considered to be reliable
  330.      * @exception IllegalArgumentException if frame is not a {@link
  331.      * Frame#isPseudoInertial pseudo-inertial frame}
  332.      */
  333.     private FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  334.                                 final Frame frame, final double mu,
  335.                                 final boolean reliableAcceleration)
  336.         throws IllegalArgumentException {

  338.         super(pvCoordinates, frame, mu);

  340.         // identity element
  341.         this.one = pvCoordinates.getPosition().getX().getField().getOne();

  343.         // zero element
  344.         this.zero = one.getField().getZero();

  346.         // third canonical vector
  347.         this.PLUS_K = FieldVector3D.getPlusK(one.getField());

  349.         // compute inclination
  350.         final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
  351.         final T m2 = momentum.getNormSq();

  353.         i = FieldVector3D.angle(momentum, PLUS_K);
  354.         // compute right ascension of ascending node
  355.         raan = FieldVector3D.crossProduct(PLUS_K, momentum).getAlpha();
  356.         // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic)
  357.         final FieldVector3D<T> pvP     = pvCoordinates.getPosition();
  358.         final FieldVector3D<T> pvV     = pvCoordinates.getVelocity();
  359.         final FieldVector3D<T> pvA     = pvCoordinates.getAcceleration();

  361.         final T   r2      = pvP.getNormSq();
  362.         final T   r       = r2.sqrt();
  363.         final T   V2      = pvV.getNormSq();
  364.         final T   rV2OnMu = r.multiply(V2).divide(mu);

  366.         // compute semi-major axis (will be negative for hyperbolic orbits)
  367.         a = r.divide(rV2OnMu.negate().add(2.0));
  368.         final T muA = a.multiply(mu);

  370.         // compute true anomaly
  371.         if (a.getReal() > 0) {
  372.             // elliptic or circular orbit
  373.             final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  374.             final T eCE = rV2OnMu.subtract(1);
  375.             e = (eSE.multiply(eSE).add(eCE.multiply(eCE))).sqrt();
  376.             v = ellipticEccentricToTrue(eSE.atan2(eCE), e); //(atan2(eSE, eCE));
  377.         } else {
  378.             // hyperbolic orbit
  379.             final T eSH = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  380.             final T eCH = rV2OnMu.subtract(1);
  381.             e = (m2.negate().divide(muA).add(1)).sqrt();
  382.             v = hyperbolicEccentricToTrue((eCH.add(eSH)).divide(eCH.subtract(eSH)).log().divide(2), e);
  383.         }

  385.         // compute perigee argument
  386.         final FieldVector3D<T> node = new FieldVector3D<>(raan, zero);
  387.         final T px = FieldVector3D.dotProduct(pvP, node);
  388.         final T py = FieldVector3D.dotProduct(pvP, FieldVector3D.crossProduct(momentum, node)).divide(m2.sqrt());
  389.         pa = py.atan2(px).subtract(v);

  391.         partialPV = pvCoordinates;

  393.         if (!FACTORIES.containsKey(a.getField())) {
  394.             FACTORIES.put(a.getField(), new FDSFactory<>(a.getField(), 1, 1));
  395.         }

  397.         if (reliableAcceleration) {
  398.             // we have a relevant acceleration, we can compute derivatives

  400.             final T[][] jacobian = MathArrays.buildArray(a.getField(), 6, 6);
  401.             getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);

  403.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(-mu), pvP);
  404.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  405.             final T   aX                       = nonKeplerianAcceleration.getX();
  406.             final T   aY                       = nonKeplerianAcceleration.getY();
  407.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  408.             aDot    = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  409.             eDot    = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  410.             iDot    = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  411.             paDot   = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  412.             raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  414.             // in order to compute true anomaly derivative, we must compute
  415.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  416.             final T MDot = getKeplerianMeanMotion().
  417.                            add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  418.             @SuppressWarnings("unchecked")
  419.             final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  420.             final FieldDerivativeStructure<T> eDS = factory.build(e, eDot);
  421.             final FieldDerivativeStructure<T> MDS = factory.build(getMeanAnomaly(), MDot);
  422.             final FieldDerivativeStructure<T> vDS = (a.getReal() < 0) ?
  423.                                             FieldKeplerianOrbit.hyperbolicEccentricToTrue(FieldKeplerianOrbit.meanToHyperbolicEccentric(MDS, eDS), eDS) :
  424.                                             FieldKeplerianOrbit.ellipticEccentricToTrue(FieldKeplerianOrbit.meanToEllipticEccentric(MDS, eDS), eDS);
  425.             vDot = vDS.getPartialDerivative(1);

  427.         } else {
  428.             // acceleration is either almost zero or NaN,
  429.             // we assume acceleration was not known
  430.             // we don't set up derivatives
  431.             aDot    = null;
  432.             eDot    = null;
  433.             iDot    = null;
  434.             paDot   = null;
  435.             raanDot = null;
  436.             vDot    = null;
  437.         }

  439.     }

  441.     /** Constructor from Cartesian parameters.
  442.      *
  443.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  444.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  445.      * use {@code mu} and the position to compute the acceleration, including
  446.      * {@link #shiftedBy(RealFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  447.      *
  448.      * @param FieldPVCoordinates the PVCoordinates of the satellite
  449.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  450.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  451.      * @param date date of the orbital parameters
  452.      * @param mu central attraction coefficient (m³/s²)
  453.      * @exception IllegalArgumentException if frame is not a {@link
  454.      * Frame#isPseudoInertial pseudo-inertial frame}
  455.      */
  456.     public FieldKeplerianOrbit(final FieldPVCoordinates<T> FieldPVCoordinates,
  457.                                final Frame frame, final FieldAbsoluteDate<T> date, final double mu)
  458.         throws IllegalArgumentException {
  459.         this(new TimeStampedFieldPVCoordinates<>(date, FieldPVCoordinates), frame, mu);
  460.     }

  462.     /** Constructor from any kind of orbital parameters.
  463.      * @param op orbital parameters to copy
  464.      */
  465.     public FieldKeplerianOrbit(final FieldOrbit<T> op) {
  466.         this(op.getPVCoordinates(), op.getFrame(), op.getMu(), op.hasDerivatives());
  467.     }

  469.     /** {@inheritDoc} */
  470.     public OrbitType getType() {
  471.         return OrbitType.KEPLERIAN;
  472.     }

  474.     /** {@inheritDoc} */
  475.     public T getA() {
  476.         return a;
  477.     }

  479.     /** {@inheritDoc} */
  480.     public T getADot() {
  481.         return aDot;
  482.     }

  484.     /** {@inheritDoc} */
  485.     public T getE() {
  486.         return e;
  487.     }

  489.     /** {@inheritDoc} */
  490.     public T getEDot() {
  491.         return eDot;
  492.     }

  494.     /** {@inheritDoc} */
  495.     public T getI() {
  496.         return i;
  497.     }

  499.     /** {@inheritDoc} */
  500.     public T getIDot() {
  501.         return iDot;
  502.     }

  504.     /** Get the perigee argument.
  505.      * @return perigee argument (rad)
  506.      */
  507.     public T getPerigeeArgument() {
  508.         return pa;
  509.     }

  511.     /** Get the perigee argument derivative.
  512.      * <p>
  513.      * If the orbit was created without derivatives, the value returned is null.
  514.      * </p>
  515.      * @return perigee argument derivative (rad/s)
  516.      */
  517.     public T getPerigeeArgumentDot() {
  518.         return paDot;
  519.     }

  521.     /** Get the right ascension of the ascending node.
  522.      * @return right ascension of the ascending node (rad)
  523.      */
  524.     public T getRightAscensionOfAscendingNode() {
  525.         return raan;
  526.     }

  528.     /** Get the right ascension of the ascending node derivative.
  529.      * <p>
  530.      * If the orbit was created without derivatives, the value returned is null.
  531.      * </p>
  532.      * @return right ascension of the ascending node derivative (rad/s)
  533.      */
  534.     public T getRightAscensionOfAscendingNodeDot() {
  535.         return raanDot;
  536.     }

  538.     /** Get the true anomaly.
  539.      * @return true anomaly (rad)
  540.      */
  541.     public T getTrueAnomaly() {
  542.         return v;
  543.     }

  545.     /** Get the true anomaly derivative.
  546.      * <p>
  547.      * If the orbit was created without derivatives, the value returned is null.
  548.      * </p>
  549.      * @return true anomaly derivative (rad/s)
  550.      */
  551.     public T getTrueAnomalyDot() {
  552.         return vDot;
  553.     }

  555.     /** Get the eccentric anomaly.
  556.      * @return eccentric anomaly (rad)
  557.      */
  558.     public T getEccentricAnomaly() {
  559.         return (a.getReal() < 0) ? trueToHyperbolicEccentric(v, e) : trueToEllipticEccentric(v, e);
  560.     }

  562.     /** Get the eccentric anomaly derivative.
  563.      * <p>
  564.      * If the orbit was created without derivatives, the value returned is null.
  565.      * </p>
  566.      * @return eccentric anomaly derivative (rad/s)
  567.      */
  568.     public T getEccentricAnomalyDot() {

  570.         if (!hasDerivatives()) {
  571.             return null;
  572.         }

  574.         @SuppressWarnings("unchecked")
  575.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  576.         final FieldDerivativeStructure<T> eDS = factory.build(e, eDot);
  577.         final FieldDerivativeStructure<T> vDS = factory.build(v, vDot);
  578.         final FieldDerivativeStructure<T> EDS = (a.getReal() < 0) ?
  579.                                                 trueToHyperbolicEccentric(vDS, eDS) :
  580.                                                 trueToEllipticEccentric(vDS, eDS);
  581.         return EDS.getPartialDerivative(1);

  583.     }

  585.     /** Get the mean anomaly.
  586.      * @return mean anomaly (rad)
  587.      */
  588.     public T getMeanAnomaly() {
  589.         return (a.getReal() < 0) ?
  590.                hyperbolicEccentricToMean(trueToHyperbolicEccentric(v, e), e) :
  591.                ellipticEccentricToMean(trueToEllipticEccentric(v, e), e);
  592.     }

  594.     /** Get the mean anomaly derivative.
  595.      * <p>
  596.      * If the orbit was created without derivatives, the value returned is null.
  597.      * </p>
  598.      * @return mean anomaly derivative (rad/s)
  599.      */
  600.     public T getMeanAnomalyDot() {

  602.         if (!hasDerivatives()) {
  603.             return null;
  604.         }

  606.         @SuppressWarnings("unchecked")
  607.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  608.         final FieldDerivativeStructure<T> eDS = factory.build(e, eDot);
  609.         final FieldDerivativeStructure<T> vDS = factory.build(v, vDot);
  610.         final FieldDerivativeStructure<T> MDS = (a.getReal() < 0) ?
  611.                                                 hyperbolicEccentricToMean(trueToHyperbolicEccentric(vDS, eDS), eDS) :
  612.                                                 ellipticEccentricToMean(trueToEllipticEccentric(vDS, eDS), eDS);
  613.         return MDS.getPartialDerivative(1);

  615.     }

  617.     /** Get the anomaly.
  618.      * @param type type of the angle
  619.      * @return anomaly (rad)
  620.      */
  621.     public T getAnomaly(final PositionAngle type) {
  622.         return (type == PositionAngle.MEAN) ? getMeanAnomaly() :
  623.                                               ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomaly() :
  624.                                                                                    getTrueAnomaly());
  625.     }

  627.     /** Get the anomaly derivative.
  628.      * <p>
  629.      * If the orbit was created without derivatives, the value returned is null.
  630.      * </p>
  631.      * @param type type of the angle
  632.      * @return anomaly derivative (rad/s)
  633.      */
  634.     public T getAnomalyDot(final PositionAngle type) {
  635.         return (type == PositionAngle.MEAN) ? getMeanAnomalyDot() :
  636.                                               ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomalyDot() :
  637.                                                                                    getTrueAnomalyDot());
  638.     }

  640.     /** {@inheritDoc} */
  641.     @Override
  642.     public boolean hasDerivatives() {
  643.         return aDot != null;
  644.     }

  646.     /** Computes the true anomaly from the elliptic eccentric anomaly.
  647.      * @param E eccentric anomaly (rad)
  648.      * @param e eccentricity
  649.      * @param <T> type of the field elements
  650.      * @return v the true anomaly
  651.      */
  652.     public static <T extends RealFieldElement<T>> T ellipticEccentricToTrue(final T E, final T e) {
  653.         final T beta = e.divide(e.multiply(e).negate().add(1).sqrt().add(1));
  654.         return E.add(beta.multiply(E.sin()).divide(beta.multiply(E.cos()).subtract(1).negate()).atan().multiply(2));
  655.     }

  657.     /** Computes the elliptic eccentric anomaly from the true anomaly.
  658.      * @param v true anomaly (rad)
  659.      * @param e eccentricity
  660.      * @param <T> type of the field elements
  661.      * @return E the elliptic eccentric anomaly
  662.      */
  663.     public static <T extends RealFieldElement<T>> T trueToEllipticEccentric(final T v, final T e) {
  664.         final T beta = e.divide(e.multiply(e).negate().add(1).sqrt().add(1));
  665.         return v.subtract((beta.multiply(v.sin()).divide(beta.multiply(v.cos()).add(1))).atan().multiply(2));
  666.     }

  668.     /** Computes the true anomaly from the hyperbolic eccentric anomaly.
  669.      * @param H hyperbolic eccentric anomaly (rad)
  670.      * @param e eccentricity
  671.      * @param <T> type of the field elements
  672.      * @return v the true anomaly
  673.      */
  674.     public static <T extends RealFieldElement<T>> T hyperbolicEccentricToTrue(final T H, final T e) {
  675.         final T s    = e.add(1).divide(e.subtract(1)).sqrt();
  676.         final T tanH = H.multiply(0.5).tanh();
  677.         return s.multiply(tanH).atan().multiply(2);
  678.     }

  680.     /** Computes the hyperbolic eccentric anomaly from the true anomaly.
  681.      * @param v true anomaly (rad)
  682.      * @param e eccentricity
  683.      * @param <T> type of the field elements
  684.      * @return H the hyperbolic eccentric anomaly
  685.      */
  686.     public static <T extends RealFieldElement<T>> T trueToHyperbolicEccentric(final T v, final T e) {
  687.         final T sinhH = e.multiply(e).subtract(1).sqrt().multiply(v.sin()).divide(e.multiply(v.cos()).add(1));
  688.         return sinhH.asinh();
  689.     }

  691.     /** Computes the mean anomaly from the hyperbolic eccentric anomaly.
  692.      * @param H hyperbolic eccentric anomaly (rad)
  693.      * @param e eccentricity
  694.      * @param <T> type of the field elements
  695.      * @return M the mean anomaly
  696.      */
  697.     public static <T extends RealFieldElement<T>> T hyperbolicEccentricToMean(final T H, final T e) {
  698.         return e.multiply(H.sinh()).subtract(H);
  699.     }

  701.     /** Computes the elliptic eccentric anomaly from the mean anomaly.
  702.      * <p>
  703.      * The algorithm used here for solving Kepler equation has been published
  704.      * in: "Procedures for  solving Kepler's Equation", A. W. Odell and
  705.      * R. H. Gooding, Celestial Mechanics 38 (1986) 307-334
  706.      * </p>
  707.      * @param M mean anomaly (rad)
  708.      * @param e eccentricity
  709.      * @param <T> type of the field elements
  710.      * @return E the eccentric anomaly
  711.      */
  712.     public static <T extends RealFieldElement<T>> T meanToEllipticEccentric(final T M, final T e) {
  713.         // reduce M to [-PI PI) interval
  714.         final T reducedM = normalizeAngle(M, M.getField().getZero());

  716.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  717.         T E;
  718.         if (reducedM.abs().getReal() < 1.0 / 6.0) {
  719.             if (FastMath.abs(reducedM.getReal()) < Precision.SAFE_MIN) {
  720.                 // this is an Orekit change to the S12 starter, mainly used when T is some kind of derivative structure.
  721.                 // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in
  722.                 // the computation. As in this case E and M are almost equal, we initialize E with reducedM
  723.                 E = reducedM;
  724.             } else {
  725.                 // this is the standard S12 starter
  726.                 E = reducedM.add(e.multiply( (reducedM.multiply(6).cbrt()).subtract(reducedM)));
  727.             }
  728.         } else {
  729.             if (reducedM.getReal() < 0) {
  730.                 final T w = reducedM.add(FastMath.PI);
  731.                 E = reducedM.add(e.multiply(w.multiply(A).divide(w.negate().add(B)).subtract(FastMath.PI).subtract(reducedM)));
  732.             } else {
  733.                 final T w = reducedM.negate().add(FastMath.PI);
  734.                 E = reducedM.add(e.multiply(w.multiply(A).divide(w.negate().add(B)).negate().subtract(reducedM).add(FastMath.PI)));
  735.             }
  736.         }

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

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

  744.             final T f;
  745.             T fd;
  746.             final T fdd  = e.multiply(E.sin());
  747.             final T fddd = e.multiply(E.cos());

  749.             if (noCancellationRisk) {

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


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

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

  767.         }

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

  774.     /** Accurate computation of E - e sin(E).
  775.      * <p>
  776.      * This method is used when E is close to 0 and e close to 1,
  777.      * i.e. near the perigee of almost parabolic orbits
  778.      * </p>
  779.      * @param E eccentric anomaly
  780.      * @param e eccentricity
  781.      * @param <T> Type of the field elements
  782.      * @return E - e sin(E)
  783.      */
  784.     private static <T extends RealFieldElement<T>> T eMeSinE(final T E, final T e) {

  786.         T x = (e.negate().add(1)).multiply(E.sin());
  787.         final T mE2 = E.negate().multiply(E);
  788.         T term = E;
  789.         double d    = 0;
  790.         // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
  791.         for (T x0 = E.getField().getZero().add(Double.NaN); x.getReal() != x0.getReal();) {
  792.             d += 2;
  793.             term = term.multiply(mE2.divide(d * (d + 1)));
  794.             x0 = x;
  795.             x = x.subtract(term);
  796.         }
  797.         return x;
  798.     }

  800.     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
  801.      * <p>
  802.      * The algorithm used here for solving hyperbolic Kepler equation is
  803.      * Danby's iterative method (3rd order) with Vallado's initial guess.
  804.      * </p>
  805.      * @param M mean anomaly (rad)
  806.      * @param e eccentricity
  807.      * @param <T> Type of the field elements
  808.      * @return H the hyperbolic eccentric anomaly
  809.      */
  810.     public static <T extends RealFieldElement<T>> T meanToHyperbolicEccentric(final T M, final T e) {

  812.         // Resolution of hyperbolic Kepler equation for Keplerian parameters

  814.         // Initial guess
  815.         T H;
  816.         if (e.getReal() < 1.6) {
  817.             if ((-FastMath.PI < M.getReal() && M.getReal() < 0.) || M.getReal() > FastMath.PI) {
  818.                 H = M.subtract(e);
  819.             } else {
  820.                 H = M.add(e);
  821.             }
  822.         } else {
  823.             if (e.getReal() < 3.6 && M.abs().getReal() > FastMath.PI) {
  824.                 H = M.subtract(e.copySign(M));
  825.             } else {
  826.                 H = M.divide(e.subtract(1));
  827.             }
  828.         }

  830.         // Iterative computation
  831.         int iter = 0;
  832.         do {
  833.             final T f3  = e.multiply(H.cosh());
  834.             final T f2  = e.multiply(H.sinh());
  835.             final T f1  = f3.subtract(1);
  836.             final T f0  = f2.subtract(H).subtract(M);
  837.             final T f12 = f1.multiply(2);
  838.             final T d   = f0.divide(f12);
  839.             final T fdf = f1.subtract(d.multiply(f2));
  840.             final T ds  = f0.divide(fdf);

  842.             final T shift = f0.divide(fdf.add(ds.multiply(ds.multiply(f3.divide(6)))));

  844.             H = H.subtract(shift);

  846.             if ((shift.abs().getReal()) <= 1.0e-12) {
  847.                 return H;
  848.             }

  850.         } while (++iter < 50);

  852.         throw new MathIllegalArgumentException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
  853.                                                iter);
  854.     }

  856.     /** Computes the mean anomaly from the elliptic eccentric anomaly.
  857.      * @param E eccentric anomaly (rad)
  858.      * @param e eccentricity
  859.      * @param <T> type of the field elements
  860.      * @return M the mean anomaly
  861.      */
  862.     public static <T extends RealFieldElement<T>> T ellipticEccentricToMean(final T E, final T e) {
  863.         return E.subtract(e.multiply(E.sin()));
  864.     }

  866.     /** Compute position from elliptic Keplerian parameters.
  867.      * @param a semi-major axis (m)
  868.      * @param e eccentricity
  869.      * @param i inclination (rad)
  870.      * @param pa Perigee Argument (rad)
  871.      * @param raan Right Ascension of Ascending Node (rad)
  872.      * @param v true anomaly (rad)
  873.      * @param mu central attraction coefficient (m³/s²)
  874.      * @param <T> type of the fiels elements
  875.      * @return position vector
  876.      */
  877.     public static <T extends RealFieldElement<T>> FieldVector3D<T> ellipticKeplerianToPosition(final T a, final T e, final T i,
  878.                                                                                                final T pa, final T raan, final T v,
  879.                                                                                                final double mu) {

  881.         // preliminary variables
  882.         final T cosRaan = raan.cos();
  883.         final T sinRaan = raan.sin();
  884.         final T cosPa   = pa.cos();
  885.         final T sinPa   = pa.sin();
  886.         final T cosI    = i.cos();
  887.         final T sinI    = i.sin();
  888.         final T crcp    = cosRaan.multiply(cosPa);
  889.         final T crsp    = cosRaan.multiply(sinPa);
  890.         final T srcp    = sinRaan.multiply(cosPa);
  891.         final T srsp    = sinRaan.multiply(sinPa);

  893.         // reference axes defining the orbital plane
  894.         final FieldVector3D<T> p = new FieldVector3D<>(crcp.subtract(cosI.multiply(srsp)),  srcp.add(cosI.multiply(crsp)), sinI.multiply(sinPa));
  895.         final FieldVector3D<T> q = new FieldVector3D<>(crsp.add(cosI.multiply(srcp)).negate(), cosI.multiply(crcp).subtract(srsp), sinI.multiply(cosPa));

  897.         // elliptic eccentric anomaly
  898.         final T uME2   = e.negate().add(1).multiply(e.add(1));
  899.         final T s1Me2  = uME2.sqrt();
  900.         final T E      = (a.getReal() < 0) ? trueToHyperbolicEccentric(v, e) : trueToEllipticEccentric(v, e);
  901.         final T cosE   = E.cos();
  902.         final T sinE   = E.sin();

  904.         // coordinates of position in the orbital plane
  905.         final T x      = a.multiply(cosE.subtract(e));
  906.         final T y      = a.multiply(sinE).multiply(s1Me2);

  908.         return new FieldVector3D<>(x, p, y, q);

  910.     }

  912.     /** Compute position from hyperbolic Keplerian parameters.
  913.      * @param a semi-major axis (m)
  914.      * @param e eccentricity
  915.      * @param i inclination (rad)
  916.      * @param pa Perigee Argument (rad)
  917.      * @param raan Right Ascension of Ascending Node (rad)
  918.      * @param v true anomaly (rad)
  919.      * @param mu central attraction coefficient (m³/s²)
  920.      * @param <T> type of the fiels elements
  921.      * @return position vector
  922.      */
  923.     public static <T extends RealFieldElement<T>> FieldVector3D<T> hyperbolicKeplerianToPosition(final T a, final T e, final T i,
  924.                                                                                                  final T pa, final T raan, final T v,
  925.                                                                                                  final double mu) {

  927.         // preliminary variables
  928.         final T cosRaan = raan.cos();
  929.         final T sinRaan = raan.sin();
  930.         final T cosPa   = pa.cos();
  931.         final T sinPa   = pa.sin();
  932.         final T cosI    = i.cos();
  933.         final T sinI    = i.sin();
  934.         final T crcp    = cosRaan.multiply(cosPa);
  935.         final T crsp    = cosRaan.multiply(sinPa);
  936.         final T srcp    = sinRaan.multiply(cosPa);
  937.         final T srsp    = sinRaan.multiply(sinPa);

  939.         // reference axes defining the orbital plane
  940.         final FieldVector3D<T> p = new FieldVector3D<>(crcp.subtract(cosI.multiply(srsp)),  srcp.add(cosI.multiply(crsp)), sinI.multiply(sinPa));
  941.         final FieldVector3D<T> q = new FieldVector3D<>(crsp.add(cosI.multiply(srcp)).negate(), cosI.multiply(crcp).subtract(srsp), sinI.multiply(cosPa));


  944.         // coordinates of position in the orbital plane
  945.         final T sinV      = v.sin();
  946.         final T cosV      = v.cos();
  947.         final T f         = a.multiply(e.multiply(e).negate().add(1));
  948.         final T posFactor = f.divide(e.multiply(cosV).add(1));
  949.         final T x         = posFactor.multiply(cosV);
  950.         final T y         = posFactor.multiply(sinV);

  952.         return new FieldVector3D<>(x, p, y, q);

  954.     }

  956.     /** {@inheritDoc} */
  957.     public T getEquinoctialEx() {
  958.         return e.multiply(pa.add(raan).cos());
  959.     }

  961.     /** {@inheritDoc} */
  962.     public T getEquinoctialExDot() {

  964.         if (!hasDerivatives()) {
  965.             return null;
  966.         }

  968.         @SuppressWarnings("unchecked")
  969.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  970.         final FieldDerivativeStructure<T> eDS    = factory.build(e,    eDot);
  971.         final FieldDerivativeStructure<T> paDS   = factory.build(pa,   paDot);
  972.         final FieldDerivativeStructure<T> raanDS = factory.build(raan, raanDot);
  973.         return eDS.multiply(paDS.add(raanDS).cos()).getPartialDerivative(1);

  975.     }

  977.     /** {@inheritDoc} */
  978.     public T getEquinoctialEy() {
  979.         return  e.multiply((pa.add(raan)).sin());
  980.     }

  982.     /** {@inheritDoc} */
  983.     public T getEquinoctialEyDot() {

  985.         if (!hasDerivatives()) {
  986.             return null;
  987.         }

  989.         @SuppressWarnings("unchecked")
  990.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  991.         final FieldDerivativeStructure<T> eDS    = factory.build(e,    eDot);
  992.         final FieldDerivativeStructure<T> paDS   = factory.build(pa,   paDot);
  993.         final FieldDerivativeStructure<T> raanDS = factory.build(raan, raanDot);
  994.         return eDS.multiply(paDS.add(raanDS).sin()).getPartialDerivative(1);

  996.     }

  998.     /** {@inheritDoc} */
  999.     public T getHx() {
  1000.         // Check for equatorial retrograde orbit
  1001.         if (FastMath.abs(i.getReal() - FastMath.PI) < 1.0e-10) {
  1002.             return this.zero.add(Double.NaN);
  1003.         }
  1004.         return  raan.cos().multiply(i.divide(2).tan());
  1005.     }

  1007.     /** {@inheritDoc} */
  1008.     public T getHxDot() {

  1010.         if (!hasDerivatives()) {
  1011.             return null;
  1012.         }

  1014.         // Check for equatorial retrograde orbit
  1015.         if (FastMath.abs(i.getReal() - FastMath.PI) < 1.0e-10) {
  1016.             return this.zero.add(Double.NaN);
  1017.         }

  1019.         @SuppressWarnings("unchecked")
  1020.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  1021.         final FieldDerivativeStructure<T> iDS    = factory.build(i,    iDot);
  1022.         final FieldDerivativeStructure<T> raanDS = factory.build(raan, raanDot);
  1023.         return raanDS.cos().multiply(iDS.multiply(0.5).tan()).getPartialDerivative(1);

  1025.     }

  1027.     /** {@inheritDoc} */
  1028.     public T getHy() {
  1029.         // Check for equatorial retrograde orbit
  1030.         if (FastMath.abs(i.getReal() - FastMath.PI) < 1.0e-10) {
  1031.             return this.zero.add(Double.NaN);
  1032.         }
  1033.         return  raan.sin().multiply(i.divide(2).tan());
  1034.     }

  1036.     /** {@inheritDoc} */
  1037.     public T getHyDot() {

  1039.         if (!hasDerivatives()) {
  1040.             return null;
  1041.         }

  1043.         // Check for equatorial retrograde orbit
  1044.         if (FastMath.abs(i.getReal() - FastMath.PI) < 1.0e-10) {
  1045.             return this.zero.add(Double.NaN);
  1046.         }

  1048.         @SuppressWarnings("unchecked")
  1049.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  1050.         final FieldDerivativeStructure<T> iDS    = factory.build(i,    iDot);
  1051.         final FieldDerivativeStructure<T> raanDS = factory.build(raan, raanDot);
  1052.         return raanDS.sin().multiply(iDS.multiply(0.5).tan()).getPartialDerivative(1);

  1054.     }

  1056.     /** {@inheritDoc} */
  1057.     public T getLv() {
  1058.         return pa.add(raan).add(v);
  1059.     }

  1061.     /** {@inheritDoc} */
  1062.     public T getLvDot() {
  1063.         return hasDerivatives() ?
  1064.                paDot.add(raanDot).add(vDot) :
  1065.                null;
  1066.     }

  1068.     /** {@inheritDoc} */
  1069.     public T getLE() {
  1070.         return pa.add(raan).add(getEccentricAnomaly());
  1071.     }

  1073.     /** {@inheritDoc} */
  1074.     public T getLEDot() {
  1075.         return hasDerivatives() ?
  1076.                paDot.add(raanDot).add(getEccentricAnomalyDot()) :
  1077.                null;
  1078.     }

  1080.     /** {@inheritDoc} */
  1081.     public T getLM() {
  1082.         return pa.add(raan).add(getMeanAnomaly());
  1083.     }

  1085.     /** {@inheritDoc} */
  1086.     public T getLMDot() {
  1087.         return hasDerivatives() ?
  1088.                paDot.add(raanDot).add(getMeanAnomalyDot()) :
  1089.                null;
  1090.     }

  1092.     /** Compute position and velocity but not acceleration.
  1093.      */
  1094.     private void computePVWithoutA() {

  1096.         if (partialPV != null) {
  1097.             // already computed
  1098.             return;
  1099.         }

  1101.         // preliminary variables
  1102.         final T cosRaan = raan.cos();
  1103.         final T sinRaan = raan.sin();
  1104.         final T cosPa   = pa.cos();
  1105.         final T sinPa   = pa.sin();
  1106.         final T cosI    = i.cos();
  1107.         final T sinI    = i.sin();
  1108.         final T crcp    = cosRaan.multiply(cosPa);
  1109.         final T crsp    = cosRaan.multiply(sinPa);
  1110.         final T srcp    = sinRaan.multiply(cosPa);
  1111.         final T srsp    = sinRaan.multiply(sinPa);

  1113.         // reference axes defining the orbital plane
  1114.         final FieldVector3D<T> p = new FieldVector3D<>(crcp.subtract(cosI.multiply(srsp)),  srcp.add(cosI.multiply(crsp)), sinI.multiply(sinPa));
  1115.         final FieldVector3D<T> q = new FieldVector3D<>(crsp.add(cosI.multiply(srcp)).negate(), cosI.multiply(crcp).subtract(srsp), sinI.multiply(cosPa));

  1117.         if (a.getReal() > 0) {

  1119.             // elliptical case

  1121.             // elliptic eccentric anomaly
  1122.             final T uME2   = e.negate().add(1).multiply(e.add(1));
  1123.             final T s1Me2  = uME2.sqrt();
  1124.             final T E      = getEccentricAnomaly();
  1125.             final T cosE   = E.cos();
  1126.             final T sinE   = E.sin();

  1128.             // coordinates of position and velocity in the orbital plane
  1129.             final T x      = a.multiply(cosE.subtract(e));
  1130.             final T y      = a.multiply(sinE).multiply(s1Me2);
  1131.             final T factor = (a.reciprocal().multiply(getMu())).sqrt().divide(e.negate().multiply(cosE).add(1));
  1132.             final T xDot   = sinE.negate().multiply(factor);
  1133.             final T yDot   = cosE.multiply(s1Me2).multiply(factor);

  1135.             final FieldVector3D<T> position = new FieldVector3D<>(x, p, y, q);
  1136.             final FieldVector3D<T> velocity = new FieldVector3D<>(xDot, p, yDot, q);
  1137.             partialPV = new FieldPVCoordinates<>(position, velocity);

  1139.         } else {

  1141.             // hyperbolic case

  1143.             // compute position and velocity factors
  1144.             final T sinV      = v.sin();
  1145.             final T cosV      = v.cos();
  1146.             final T f         = a.multiply(e.multiply(e).negate().add(1));
  1147.             final T posFactor = f.divide(e.multiply(cosV).add(1));
  1148.             final T velFactor = f.reciprocal().multiply(getMu()).sqrt();

  1150.             final FieldVector3D<T> position     = new FieldVector3D<>(posFactor.multiply(cosV), p, posFactor.multiply(sinV), q);
  1151.             final FieldVector3D<T> velocity     = new FieldVector3D<>(velFactor.multiply(sinV).negate(), p, velFactor.multiply(e.add(cosV)), q);
  1152.             partialPV = new FieldPVCoordinates<>(position, velocity);

  1154.         }

  1156.     }

  1158.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  1159.      * <p>
  1160.      * This method should be called only when {@link #hasDerivatives()} returns true.
  1161.      * </p>
  1162.      * @return non-Keplerian part of the acceleration
  1163.      */
  1164.     private FieldVector3D<T> nonKeplerianAcceleration() {

  1166.         final T[][] dCdP = MathArrays.buildArray(a.getField(), 6, 6);
  1167.         getJacobianWrtParameters(PositionAngle.MEAN, dCdP);

  1169.         final T nonKeplerianMeanMotion = getMeanAnomalyDot().subtract(getKeplerianMeanMotion());
  1170.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  1171.                                  add(dCdP[3][1].multiply(eDot)).
  1172.                                  add(dCdP[3][2].multiply(iDot)).
  1173.                                  add(dCdP[3][3].multiply(paDot)).
  1174.                                  add(dCdP[3][4].multiply(raanDot)).
  1175.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  1176.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  1177.                                  add(dCdP[4][1].multiply(eDot)).
  1178.                                  add(dCdP[4][2].multiply(iDot)).
  1179.                                  add(dCdP[4][3].multiply(paDot)).
  1180.                                  add(dCdP[4][4].multiply(raanDot)).
  1181.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  1182.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  1183.                                  add(dCdP[5][1].multiply(eDot)).
  1184.                                  add(dCdP[5][2].multiply(iDot)).
  1185.                                  add(dCdP[5][3].multiply(paDot)).
  1186.                                  add(dCdP[5][4].multiply(raanDot)).
  1187.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

  1189.         return new FieldVector3D<>(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);

  1191.     }

  1193.     /** {@inheritDoc} */
  1194.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  1196.         // position and velocity
  1197.         computePVWithoutA();

  1199.         // acceleration
  1200.         final T r2 = partialPV.getPosition().getNormSq();
  1201.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(-getMu()),
  1202.                                                                            partialPV.getPosition());
  1203.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  1204.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  1205.                                               keplerianAcceleration;

  1207.         return new TimeStampedFieldPVCoordinates<>(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);

  1209.     }

  1211.     /** {@inheritDoc} */
  1212.     public FieldKeplerianOrbit<T> shiftedBy(final double dt) {
  1213.         return shiftedBy(getDate().getField().getZero().add(dt));
  1214.     }

  1216.     /** {@inheritDoc} */
  1217.     public FieldKeplerianOrbit<T> shiftedBy(final T dt) {

  1219.         // use Keplerian-only motion
  1220.         final FieldKeplerianOrbit<T> keplerianShifted = new FieldKeplerianOrbit<>(a, e, i, pa, raan,
  1221.                                                                                   getKeplerianMeanMotion().multiply(dt).add(getMeanAnomaly()),
  1222.                                                                                   PositionAngle.MEAN, getFrame(), getDate().shiftedBy(dt), getMu());

  1224.         if (hasDerivatives()) {

  1226.             // extract non-Keplerian acceleration from first time derivatives
  1227.             final FieldVector3D<T> nonKeplerianAcceleration = nonKeplerianAcceleration();

  1229.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  1230.             keplerianShifted.computePVWithoutA();
  1231.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, keplerianShifted.partialPV.getPosition(),
  1232.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  1233.             final T   fixedR2 = fixedP.getNormSq();
  1234.             final T   fixedR  = fixedR2.sqrt();
  1235.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, keplerianShifted.partialPV.getVelocity(),
  1236.                                                                  dt, nonKeplerianAcceleration);
  1237.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(-getMu()),
  1238.                                                                  keplerianShifted.partialPV.getPosition(),
  1239.                                                                  one, nonKeplerianAcceleration);

  1241.             // build a new orbit, taking non-Keplerian acceleration into account
  1242.             return new FieldKeplerianOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  1243.                                                                                  fixedP, fixedV, fixedA),
  1244.                                              keplerianShifted.getFrame(), keplerianShifted.getMu());

  1246.         } else {
  1247.             // Keplerian-only motion is all we can do
  1248.             return keplerianShifted;
  1249.         }

  1251.     }

  1253.     /** {@inheritDoc}
  1254.      * <p>
  1255.      * The interpolated instance is created by polynomial Hermite interpolation
  1256.      * on Keplerian elements, without derivatives (which means the interpolation
  1257.      * falls back to Lagrange interpolation only).
  1258.      * </p>
  1259.      * <p>
  1260.      * As this implementation of interpolation is polynomial, it should be used only
  1261.      * with small samples (about 10-20 points) in order to avoid <a
  1262.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  1263.      * and numerical problems (including NaN appearing).
  1264.      * </p>
  1265.      * <p>
  1266.      * If orbit interpolation on large samples is needed, using the {@link
  1267.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  1268.      * low-level interpolation. The Ephemeris class automatically handles selection of
  1269.      * a neighboring sub-sample with a predefined number of point from a large global sample
  1270.      * in a thread-safe way.
  1271.      * </p>
  1272.      */
  1273.     public FieldKeplerianOrbit<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldOrbit<T>> sample) {

  1275.         // first pass to check if derivatives are available throughout the sample
  1276.         final List<FieldOrbit<T>> list = sample.collect(Collectors.toList());
  1277.         boolean useDerivatives = true;
  1278.         for (final FieldOrbit<T> orbit : list) {
  1279.             useDerivatives = useDerivatives && orbit.hasDerivatives();
  1280.         }

  1282.         // set up an interpolator
  1283.         final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<>();

  1285.         // second pass to feed interpolator
  1286.         FieldAbsoluteDate<T> previousDate = null;
  1287.         T                    previousPA   = zero.add(Double.NaN);
  1288.         T                    previousRAAN = zero.add(Double.NaN);
  1289.         T                    previousM    = zero.add(Double.NaN);
  1290.         for (final FieldOrbit<T> orbit : list) {
  1291.             final FieldKeplerianOrbit<T> kep = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(orbit);
  1292.             final T continuousPA;
  1293.             final T continuousRAAN;
  1294.             final T continuousM;
  1295.             if (previousDate == null) {
  1296.                 continuousPA   = kep.getPerigeeArgument();
  1297.                 continuousRAAN = kep.getRightAscensionOfAscendingNode();
  1298.                 continuousM    = kep.getMeanAnomaly();
  1299.             } else {
  1300.                 final T dt      = kep.getDate().durationFrom(previousDate);
  1301.                 final T keplerM = previousM.add(kep.getKeplerianMeanMotion().multiply(dt));
  1302.                 continuousPA   = normalizeAngle(kep.getPerigeeArgument(), previousPA);
  1303.                 continuousRAAN = normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
  1304.                 continuousM    = normalizeAngle(kep.getMeanAnomaly(), keplerM);
  1305.             }
  1306.             previousDate = kep.getDate();
  1307.             previousPA   = continuousPA;
  1308.             previousRAAN = continuousRAAN;
  1309.             previousM    = continuousM;
  1310.             final T[] toAdd = MathArrays.buildArray(getA().getField(), 6);
  1311.             toAdd[0] = kep.getA();
  1312.             toAdd[1] = kep.getE();
  1313.             toAdd[2] = kep.getI();
  1314.             toAdd[3] = continuousPA;
  1315.             toAdd[4] = continuousRAAN;
  1316.             toAdd[5] = continuousM;
  1317.             if (useDerivatives) {
  1318.                 final T[] toAddDot = MathArrays.buildArray(one.getField(), 6);
  1319.                 toAddDot[0] = kep.getADot();
  1320.                 toAddDot[1] = kep.getEDot();
  1321.                 toAddDot[2] = kep.getIDot();
  1322.                 toAddDot[3] = kep.getPerigeeArgumentDot();
  1323.                 toAddDot[4] = kep.getRightAscensionOfAscendingNodeDot();
  1324.                 toAddDot[5] = kep.getMeanAnomalyDot();
  1325.                 interpolator.addSamplePoint(kep.getDate().durationFrom(date),
  1326.                                             toAdd, toAddDot);
  1327.             } else {
  1328.                 interpolator.addSamplePoint(this.zero.add(kep.getDate().durationFrom(date)),
  1329.                                             toAdd);
  1330.             }
  1331.         }

  1333.         // interpolate
  1334.         final T[][] interpolated = interpolator.derivatives(zero, 1);

  1336.         // build a new interpolated instance
  1337.         return new FieldKeplerianOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  1338.                                          interpolated[0][3], interpolated[0][4], interpolated[0][5],
  1339.                                          interpolated[1][0], interpolated[1][1], interpolated[1][2],
  1340.                                          interpolated[1][3], interpolated[1][4], interpolated[1][5],
  1341.                                          PositionAngle.MEAN, getFrame(), date, getMu());

  1343.     }

  1345.     /** {@inheritDoc} */
  1346.     protected T[][] computeJacobianMeanWrtCartesian() {
  1347.         if (a.getReal() > 0) {
  1348.             return computeJacobianMeanWrtCartesianElliptical();
  1349.         } else {
  1350.             return computeJacobianMeanWrtCartesianHyperbolic();
  1351.         }
  1352.     }

  1354.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1355.      * <p>
  1356.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1357.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1358.      * yDot for j=4, zDot for j=5).
  1359.      * </p>
  1360.      * @return 6x6 Jacobian matrix
  1361.      */
  1362.     private T[][] computeJacobianMeanWrtCartesianElliptical() {

  1364.         final T[][] jacobian = MathArrays.buildArray(getA().getField(), 6, 6);

  1366.         // compute various intermediate parameters
  1367.         computePVWithoutA();
  1368.         final FieldVector3D<T> position = partialPV.getPosition();
  1369.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1370.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  1371.         final T v2         = velocity.getNormSq();
  1372.         final T r2         = position.getNormSq();
  1373.         final T r          = r2.sqrt();
  1374.         final T r3         = r.multiply(r2);

  1376.         final T px         = position.getX();
  1377.         final T py         = position.getY();
  1378.         final T pz         = position.getZ();
  1379.         final T vx         = velocity.getX();
  1380.         final T vy         = velocity.getY();
  1381.         final T vz         = velocity.getZ();
  1382.         final T mx         = momentum.getX();
  1383.         final T my         = momentum.getY();
  1384.         final T mz         = momentum.getZ();

  1386.         final double mu         = getMu();
  1387.         final T sqrtMuA    = a.multiply(mu).sqrt();
  1388.         final T sqrtAoMu   = a.divide(mu).sqrt();
  1389.         final T a2         = a.multiply(a);
  1390.         final T twoA       = a.multiply(2);
  1391.         final T rOnA       = r.divide(a);

  1393.         final T oMe2       = e.multiply(e).negate().add(1);
  1394.         final T epsilon    = oMe2.sqrt();
  1395.         final T sqrtRec    = epsilon.reciprocal();

  1397.         final T cosI       = i.cos();
  1398.         final T sinI       = i.sin();
  1399.         final T cosPA      = pa.cos();
  1400.         final T sinPA      = pa.sin();

  1402.         final T pv         = FieldVector3D.dotProduct(position, velocity);
  1403.         final T cosE       = a.subtract(r).divide(a.multiply(e));
  1404.         final T sinE       = pv.divide(e.multiply(sqrtMuA));

  1406.         // da
  1407.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  1408.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(2 / mu));
  1409.         fillHalfRow(this.one, vectorAR,    jacobian[0], 0);
  1410.         fillHalfRow(this.one, vectorARDot, jacobian[0], 3);

  1412.         // de
  1413.         final T factorER3 = pv.divide(twoA);
  1414.         final FieldVector3D<T> vectorER   = new FieldVector3D<>(cosE.multiply(v2).divide(r.multiply(mu)), position,
  1415.                                                                 sinE.divide(sqrtMuA), velocity,
  1416.                                                                 factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorAR);
  1417.         final FieldVector3D<T> vectorERDot = new FieldVector3D<>(sinE.divide(sqrtMuA), position,
  1418.                                                                  cosE.multiply(2 / mu).multiply(r), velocity,
  1419.                                                                  factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorARDot);
  1420.         fillHalfRow(this.one, vectorER,    jacobian[1], 0);
  1421.         fillHalfRow(this.one, vectorERDot, jacobian[1], 3);

  1423.         // dE / dr (Eccentric anomaly)
  1424.         final T coefE = cosE.divide(e.multiply(sqrtMuA));
  1425.         final FieldVector3D<T>  vectorEAnR =
  1426.             new FieldVector3D<>(sinE.negate().multiply(v2).divide(e.multiply(r).multiply(mu)), position, coefE, velocity,
  1427.                                 factorER3.negate().multiply(coefE), vectorAR);

  1429.         // dE / drDot
  1430.         final FieldVector3D<T>  vectorEAnRDot =
  1431.             new FieldVector3D<>(sinE.multiply(-2).multiply(r).divide(e.multiply(mu)), velocity, coefE, position,
  1432.                                 factorER3.negate().multiply(coefE), vectorARDot);

  1434.         // precomputing some more factors
  1435.         final T s1 = sinE.negate().multiply(pz).divide(r).subtract(cosE.multiply(vz).multiply(sqrtAoMu));
  1436.         final T s2 = cosE.negate().multiply(pz).divide(r3);
  1437.         final T s3 = sinE.multiply(vz).divide(sqrtMuA.multiply(-2));
  1438.         final T t1 = sqrtRec.multiply(cosE.multiply(pz).divide(r).subtract(sinE.multiply(vz).multiply(sqrtAoMu)));
  1439.         final T t2 = sqrtRec.multiply(sinE.negate().multiply(pz).divide(r3));
  1440.         final T t3 = sqrtRec.multiply(cosE.subtract(e)).multiply(vz).divide(sqrtMuA.multiply(2));
  1441.         final T t4 = sqrtRec.multiply(e.multiply(sinI).multiply(cosPA).multiply(sqrtRec).subtract(vz.multiply(sqrtAoMu)));
  1442.         final FieldVector3D<T> s = new FieldVector3D<>(cosE.divide(r), this.PLUS_K,
  1443.                                                        s1,       vectorEAnR,
  1444.                                                        s2,       position,
  1445.                                                        s3,       vectorAR);
  1446.         final FieldVector3D<T> sDot = new FieldVector3D<>(sinE.negate().multiply(sqrtAoMu), this.PLUS_K,
  1447.                                                           s1,               vectorEAnRDot,
  1448.                                                           s3,               vectorARDot);
  1449.         final FieldVector3D<T> t =
  1450.             new FieldVector3D<>(sqrtRec.multiply(sinE).divide(r), this.PLUS_K).add(new FieldVector3D<>(t1, vectorEAnR,
  1451.                                                                                                        t2, position,
  1452.                                                                                                        t3, vectorAR,
  1453.                                                                                                        t4, vectorER));
  1454.         final FieldVector3D<T> tDot = new FieldVector3D<>(sqrtRec.multiply(cosE.subtract(e)).multiply(sqrtAoMu), this.PLUS_K,
  1455.                                                           t1,                                                    vectorEAnRDot,
  1456.                                                           t3,                                                    vectorARDot,
  1457.                                                           t4,                                                    vectorERDot);

  1459.         // di
  1460.         final T factorI1 = sinI.negate().multiply(sqrtRec).divide(sqrtMuA);
  1461.         final T i1 =  factorI1;
  1462.         final T i2 =  factorI1.negate().multiply(mz).divide(twoA);
  1463.         final T i3 =  factorI1.multiply(mz).multiply(e).divide(oMe2);
  1464.         final T i4 = cosI.multiply(sinPA);
  1465.         final T i5 = cosI.multiply(cosPA);
  1466.         fillHalfRow(i1, new FieldVector3D<>(vy, vx.negate(), this.zero), i2, vectorAR, i3, vectorER, i4, s, i5, t,
  1467.                     jacobian[2], 0);
  1468.         fillHalfRow(i1, new FieldVector3D<>(py.negate(), px, this.zero), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
  1469.                     jacobian[2], 3);

  1471.         // dpa
  1472.         fillHalfRow(cosPA.divide(sinI), s,    sinPA.negate().divide(sinI), t,    jacobian[3], 0);
  1473.         fillHalfRow(cosPA.divide(sinI), sDot, sinPA.negate().divide(sinI), tDot, jacobian[3], 3);

  1475.         // dRaan
  1476.         final T factorRaanR = (a.multiply(mu).multiply(oMe2).multiply(sinI).multiply(sinI)).reciprocal();
  1477.         fillHalfRow( factorRaanR.negate().multiply(my), new FieldVector3D<>(zero, vz, vy.negate()),
  1478.                      factorRaanR.multiply(mx), new FieldVector3D<>(vz.negate(), zero,  vx),
  1479.                      jacobian[4], 0);
  1480.         fillHalfRow(factorRaanR.negate().multiply(my), new FieldVector3D<>(zero, pz.negate(),  py),
  1481.                      factorRaanR.multiply(mx), new FieldVector3D<>(pz, zero, px.negate()),
  1482.                      jacobian[4], 3);

  1484.         // dM
  1485.         fillHalfRow(rOnA, vectorEAnR,    sinE.negate(), vectorER,    jacobian[5], 0);
  1486.         fillHalfRow(rOnA, vectorEAnRDot, sinE.negate(), vectorERDot, jacobian[5], 3);

  1488.         return jacobian;

  1490.     }

  1492.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1493.      * <p>
  1494.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1495.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1496.      * yDot for j=4, zDot for j=5).
  1497.      * </p>
  1498.      * @return 6x6 Jacobian matrix
  1499.      */
  1500.     private T[][] computeJacobianMeanWrtCartesianHyperbolic() {

  1502.         final T[][] jacobian = MathArrays.buildArray(getA().getField(), 6, 6);

  1504.         // compute various intermediate parameters
  1505.         computePVWithoutA();
  1506.         final FieldVector3D<T> position = partialPV.getPosition();
  1507.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1508.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  1509.         final T r2         = position.getNormSq();
  1510.         final T r          = r2.sqrt();
  1511.         final T r3         = r.multiply(r2);

  1513.         final T x          = position.getX();
  1514.         final T y          = position.getY();
  1515.         final T z          = position.getZ();
  1516.         final T vx         = velocity.getX();
  1517.         final T vy         = velocity.getY();
  1518.         final T vz         = velocity.getZ();
  1519.         final T mx         = momentum.getX();
  1520.         final T my         = momentum.getY();
  1521.         final T mz         = momentum.getZ();

  1523.         final double mu         = getMu();
  1524.         final T absA       = a.negate();
  1525.         final T sqrtMuA    = absA.multiply(mu).sqrt();
  1526.         final T a2         = a.multiply(a);
  1527.         final T rOa        = r.divide(absA);

  1529.         final T cosI       = i.cos();
  1530.         final T sinI       = i.sin();

  1532.         final T pv         = FieldVector3D.dotProduct(position, velocity);

  1534.         // da
  1535.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(-2).divide(r3), position);
  1536.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(-2 / mu));
  1537.         fillHalfRow(this.one.negate(), vectorAR,    jacobian[0], 0);
  1538.         fillHalfRow(this.one.negate(), vectorARDot, jacobian[0], 3);

  1540.         // differentials of the momentum
  1541.         final T m      = momentum.getNorm();
  1542.         final T oOm    = m.reciprocal();
  1543.         final FieldVector3D<T> dcXP = new FieldVector3D<>(this.zero, vz, vy.negate());
  1544.         final FieldVector3D<T> dcYP = new FieldVector3D<>(vz.negate(),   this.zero,  vx);
  1545.         final FieldVector3D<T> dcZP = new FieldVector3D<>( vy, vx.negate(),   this.zero);
  1546.         final FieldVector3D<T> dcXV = new FieldVector3D<>(  this.zero,  z.negate(),   y);
  1547.         final FieldVector3D<T> dcYV = new FieldVector3D<>(  z,   this.zero,  x.negate());
  1548.         final FieldVector3D<T> dcZV = new FieldVector3D<>( y.negate(),   x,   this.zero);
  1549.         final FieldVector3D<T> dCP  = new FieldVector3D<>(mx.multiply(oOm), dcXP, my.multiply(oOm), dcYP, mz.multiply(oOm), dcZP);
  1550.         final FieldVector3D<T> dCV  = new FieldVector3D<>(mx.multiply(oOm), dcXV, my.multiply(oOm), dcYV, mz.multiply(oOm), dcZV);

  1552.         // dp
  1553.         final T mOMu   = m.divide(mu);
  1554.         final FieldVector3D<T> dpP  = new FieldVector3D<>(mOMu.multiply(2), dCP);
  1555.         final FieldVector3D<T> dpV  = new FieldVector3D<>(mOMu.multiply(2), dCV);

  1557.         // de
  1558.         final T p      = m.multiply(mOMu);
  1559.         final T moO2ae = absA.multiply(2).multiply(e).reciprocal();
  1560.         final T m2OaMu = p.negate().divide(absA);
  1561.         fillHalfRow(moO2ae, dpP, m2OaMu.multiply(moO2ae), vectorAR,    jacobian[1], 0);
  1562.         fillHalfRow(moO2ae, dpV, m2OaMu.multiply(moO2ae), vectorARDot, jacobian[1], 3);

  1564.         // di
  1565.         final T cI1 = m.multiply(sinI).reciprocal();
  1566.         final T cI2 = cosI.multiply(cI1);
  1567.         fillHalfRow(cI2, dCP, cI1.negate(), dcZP, jacobian[2], 0);
  1568.         fillHalfRow(cI2, dCV, cI1.negate(), dcZV, jacobian[2], 3);


  1571.         // dPA
  1572.         final T cP1     =  y.multiply(oOm);
  1573.         final T cP2     =  x.negate().multiply(oOm);
  1574.         final T cP3     =  mx.multiply(cP1).add(my.multiply(cP2)).negate();
  1575.         final T cP4     =  cP3.multiply(oOm);
  1576.         final T cP5     =  r2.multiply(sinI).multiply(sinI).negate().reciprocal();
  1577.         final T cP6     = z.multiply(cP5);
  1578.         final T cP7     = cP3.multiply(cP5);
  1579.         final FieldVector3D<T> dacP  = new FieldVector3D<>(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new FieldVector3D<>(my.negate(), mx, this.zero));
  1580.         final FieldVector3D<T> dacV  = new FieldVector3D<>(cP1, dcXV, cP2, dcYV, cP4, dCV);
  1581.         final FieldVector3D<T> dpoP  = new FieldVector3D<>(cP6, dacP, cP7, this.PLUS_K);
  1582.         final FieldVector3D<T> dpoV  = new FieldVector3D<>(cP6, dacV);

  1584.         final T re2     = r2.multiply(e).multiply(e);
  1585.         final T recOre2 = p.subtract(r).divide(re2);
  1586.         final T resOre2 = pv.multiply(mOMu).divide(re2);
  1587.         final FieldVector3D<T> dreP  = new FieldVector3D<>(mOMu, velocity, pv.divide(mu), dCP);
  1588.         final FieldVector3D<T> dreV  = new FieldVector3D<>(mOMu, position, pv.divide(mu), dCV);
  1589.         final FieldVector3D<T> davP  = new FieldVector3D<>(resOre2.negate(), dpP, recOre2, dreP, resOre2.divide(r), position);
  1590.         final FieldVector3D<T> davV  = new FieldVector3D<>(resOre2.negate(), dpV, recOre2, dreV);
  1591.         fillHalfRow(this.one, dpoP, this.one.negate(), davP, jacobian[3], 0);
  1592.         fillHalfRow(this.one, dpoV, this.one.negate(), davV, jacobian[3], 3);

  1594.         // dRAAN
  1595.         final T cO0 = cI1.multiply(cI1);
  1596.         final T cO1 =  mx.multiply(cO0);
  1597.         final T cO2 =  my.negate().multiply(cO0);
  1598.         fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
  1599.         fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);

  1601.         // dM
  1602.         final T s2a    = pv.divide(absA.multiply(2));
  1603.         final T oObux  = m.multiply(m).add(absA.multiply(mu)).sqrt().reciprocal();
  1604.         final T scasbu = pv.multiply(oObux);
  1605.         final FieldVector3D<T> dauP = new FieldVector3D<>(sqrtMuA.reciprocal(), velocity, s2a.negate().divide(sqrtMuA), vectorAR);
  1606.         final FieldVector3D<T> dauV = new FieldVector3D<>(sqrtMuA.reciprocal(), position, s2a.negate().divide(sqrtMuA), vectorARDot);
  1607.         final FieldVector3D<T> dbuP = new FieldVector3D<>(oObux.multiply(mu / 2), vectorAR,    m.multiply(oObux), dCP);
  1608.         final FieldVector3D<T> dbuV = new FieldVector3D<>(oObux.multiply(mu / 2), vectorARDot, m.multiply(oObux), dCV);
  1609.         final FieldVector3D<T> dcuP = new FieldVector3D<>(oObux, velocity, scasbu.negate().multiply(oObux), dbuP);
  1610.         final FieldVector3D<T> dcuV = new FieldVector3D<>(oObux, position, scasbu.negate().multiply(oObux), dbuV);
  1611.         fillHalfRow(this.one, dauP, e.negate().divide(rOa.add(1)), dcuP, jacobian[5], 0);
  1612.         fillHalfRow(this.one, dauV, e.negate().divide(rOa.add(1)), dcuV, jacobian[5], 3);

  1614.         return jacobian;

  1616.     }

  1618.     /** {@inheritDoc} */
  1619.     protected T[][] computeJacobianEccentricWrtCartesian() {
  1620.         if (a.getReal() > 0) {
  1621.             return computeJacobianEccentricWrtCartesianElliptical();
  1622.         } else {
  1623.             return computeJacobianEccentricWrtCartesianHyperbolic();
  1624.         }
  1625.     }

  1627.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1628.      * <p>
  1629.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1630.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1631.      * yDot for j=4, zDot for j=5).
  1632.      * </p>
  1633.      * @return 6x6 Jacobian matrix
  1634.      */
  1635.     private T[][] computeJacobianEccentricWrtCartesianElliptical() {

  1637.         // start by computing the Jacobian with mean angle
  1638.         final T[][] jacobian = computeJacobianMeanWrtCartesianElliptical();

  1640.         // Differentiating the Kepler equation M = E - e sin E leads to:
  1641.         // dM = (1 - e cos E) dE - sin E de
  1642.         // which is inverted and rewritten as:
  1643.         // dE = a/r dM + sin E a/r de
  1644.         final T eccentricAnomaly = getEccentricAnomaly();
  1645.         final T cosE             = eccentricAnomaly.cos();
  1646.         final T sinE             = eccentricAnomaly.sin();
  1647.         final T aOr              = e.negate().multiply(cosE).add(1).reciprocal();

  1649.         // update anomaly row
  1650.         final T[] eRow           = jacobian[1];
  1651.         final T[] anomalyRow     = jacobian[5];
  1652.         for (int j = 0; j < anomalyRow.length; ++j) {
  1653.             anomalyRow[j] = aOr.multiply(anomalyRow[j].add(sinE.multiply(eRow[j])));
  1654.         }

  1656.         return jacobian;

  1658.     }

  1660.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1661.      * <p>
  1662.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1663.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1664.      * yDot for j=4, zDot for j=5).
  1665.      * </p>
  1666.      * @return 6x6 Jacobian matrix
  1667.      */
  1668.     private T[][] computeJacobianEccentricWrtCartesianHyperbolic() {

  1670.         // start by computing the Jacobian with mean angle
  1671.         final T[][] jacobian = computeJacobianMeanWrtCartesianHyperbolic();

  1673.         // Differentiating the Kepler equation M = e sinh H - H leads to:
  1674.         // dM = (e cosh H - 1) dH + sinh H de
  1675.         // which is inverted and rewritten as:
  1676.         // dH = 1 / (e cosh H - 1) dM - sinh H / (e cosh H - 1) de
  1677.         final T H      = getEccentricAnomaly();
  1678.         final T coshH  = H.cosh();
  1679.         final T sinhH  = H.sinh();
  1680.         final T absaOr = e.multiply(coshH).subtract(1).reciprocal();
  1681.         // update anomaly row
  1682.         final T[] eRow       = jacobian[1];
  1683.         final T[] anomalyRow = jacobian[5];

  1685.         for (int j = 0; j < anomalyRow.length; ++j) {
  1686.             anomalyRow[j] = absaOr.multiply(anomalyRow[j].subtract(sinhH.multiply(eRow[j])));

  1688.         }

  1690.         return jacobian;

  1692.     }

  1694.     /** {@inheritDoc} */
  1695.     protected T[][] computeJacobianTrueWrtCartesian() {
  1696.         if (a.getReal() > 0) {
  1697.             return computeJacobianTrueWrtCartesianElliptical();
  1698.         } else {
  1699.             return computeJacobianTrueWrtCartesianHyperbolic();
  1700.         }
  1701.     }

  1703.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1704.      * <p>
  1705.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1706.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1707.      * yDot for j=4, zDot for j=5).
  1708.      * </p>
  1709.      * @return 6x6 Jacobian matrix
  1710.      */
  1711.     private T[][] computeJacobianTrueWrtCartesianElliptical() {

  1713.         // start by computing the Jacobian with eccentric angle
  1714.         final T[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();
  1715.         // Differentiating the eccentric anomaly equation sin E = sqrt(1-e^2) sin v / (1 + e cos v)
  1716.         // and using cos E = (e + cos v) / (1 + e cos v) to get rid of cos E leads to:
  1717.         // dE = [sqrt (1 - e^2) / (1 + e cos v)] dv - [sin E / (1 - e^2)] de
  1718.         // which is inverted and rewritten as:
  1719.         // dv = sqrt (1 - e^2) a/r dE + [sin E / sqrt (1 - e^2)] a/r de
  1720.         final T e2               = e.multiply(e);
  1721.         final T oMe2             = e2.negate().add(1);
  1722.         final T epsilon          = oMe2.sqrt();
  1723.         final T eccentricAnomaly = getEccentricAnomaly();
  1724.         final T cosE             = eccentricAnomaly.cos();
  1725.         final T sinE             = eccentricAnomaly.sin();
  1726.         final T aOr              = e.multiply(cosE).negate().add(1).reciprocal();
  1727.         final T aFactor          = epsilon.multiply(aOr);
  1728.         final T eFactor          = sinE.multiply(aOr).divide(epsilon);

  1730.         // update anomaly row
  1731.         final T[] eRow           = jacobian[1];
  1732.         final T[] anomalyRow     = jacobian[5];
  1733.         for (int j = 0; j < anomalyRow.length; ++j) {
  1734.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).add(eFactor.multiply(eRow[j]));
  1735.         }
  1736.         return jacobian;

  1738.     }

  1740.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1741.      * <p>
  1742.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1743.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1744.      * yDot for j=4, zDot for j=5).
  1745.      * </p>
  1746.      * @return 6x6 Jacobian matrix
  1747.      */
  1748.     private T[][] computeJacobianTrueWrtCartesianHyperbolic() {

  1750.         // start by computing the Jacobian with eccentric angle
  1751.         final T[][] jacobian = computeJacobianEccentricWrtCartesianHyperbolic();

  1753.         // Differentiating the eccentric anomaly equation sinh H = sqrt(e^2-1) sin v / (1 + e cos v)
  1754.         // and using cosh H = (e + cos v) / (1 + e cos v) to get rid of cosh H leads to:
  1755.         // dH = [sqrt (e^2 - 1) / (1 + e cos v)] dv + [sinh H / (e^2 - 1)] de
  1756.         // which is inverted and rewritten as:
  1757.         // dv = sqrt (1 - e^2) a/r dH - [sinh H / sqrt (e^2 - 1)] a/r de
  1758.         final T e2       = e.multiply(e);
  1759.         final T e2Mo     = e2.subtract(1);
  1760.         final T epsilon  = e2Mo.sqrt();
  1761.         final T H        = getEccentricAnomaly();
  1762.         final T coshH    = H.cosh();
  1763.         final T sinhH    = H.sinh();
  1764.         final T aOr      = e.multiply(coshH).subtract(1).reciprocal();
  1765.         final T aFactor  = epsilon.multiply(aOr);
  1766.         final T eFactor  = sinhH .multiply(aOr).divide(epsilon);

  1768.         // update anomaly row
  1769.         final T[] eRow           = jacobian[1];
  1770.         final T[] anomalyRow     = jacobian[5];
  1771.         for (int j = 0; j < anomalyRow.length; ++j) {
  1772.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).subtract(eFactor.multiply(eRow[j]));
  1773.         }

  1775.         return jacobian;

  1777.     }

  1779.     /** {@inheritDoc} */
  1780.     public void addKeplerContribution(final PositionAngle type, final double gm,
  1781.                                       final T[] pDot) {
  1782.         final T oMe2;
  1783.         final T ksi;
  1784.         final T absA = a.abs();
  1785.         final T n    = absA.reciprocal().multiply(gm).sqrt().divide(absA);
  1786.         switch (type) {
  1787.             case MEAN :
  1788.                 pDot[5] = pDot[5].add(n);
  1789.                 break;
  1790.             case ECCENTRIC :
  1791.                 oMe2 = e.multiply(e).negate().add(1).abs();
  1792.                 ksi  = e.multiply(v.cos()).add(1);
  1793.                 pDot[5] = pDot[5].add( n.multiply(ksi).divide(oMe2));
  1794.                 break;
  1795.             case TRUE :
  1796.                 oMe2 = e.multiply(e).negate().add(1).abs();
  1797.                 ksi  = e.multiply(v.cos()).add(1);
  1798.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1799.                 break;
  1800.             default :
  1801.                 throw new OrekitInternalError(null);
  1802.         }
  1803.     }

  1805.     /**  Returns a string representation of this Keplerian parameters object.
  1806.      * @return a string representation of this object
  1807.      */
  1808.     public String toString() {
  1809.         return new StringBuffer().append("Keplerian parameters: ").append('{').
  1810.                                   append("a: ").append(a.getReal()).
  1811.                                   append("; e: ").append(e.getReal()).
  1812.                                   append("; i: ").append(FastMath.toDegrees(i.getReal())).
  1813.                                   append("; pa: ").append(FastMath.toDegrees(pa.getReal())).
  1814.                                   append("; raan: ").append(FastMath.toDegrees(raan.getReal())).
  1815.                                   append("; v: ").append(FastMath.toDegrees(v.getReal())).
  1816.                                   append(";}").toString();
  1817.     }

  1819.     /**
  1820.      * Normalize an angle in a 2&pi; wide interval around a center value.
  1821.      * <p>This method has three main uses:</p>
  1822.      * <ul>
  1823.      *   <li>normalize an angle between 0 and 2&pi;:<br/>
  1824.      *       {@code a = MathUtils.normalizeAngle(a, FastMath.PI);}</li>
  1825.      *   <li>normalize an angle between -&pi; and +&pi;<br/>
  1826.      *       {@code a = MathUtils.normalizeAngle(a, 0.0);}</li>
  1827.      *   <li>compute the angle between two defining angular positions:<br>
  1828.      *       {@code angle = MathUtils.normalizeAngle(end, start) - start;}</li>
  1829.      * </ul>
  1830.      * <p>Note that due to numerical accuracy and since &pi; cannot be represented
  1831.      * exactly, the result interval is <em>closed</em>, it cannot be half-closed
  1832.      * as would be more satisfactory in a purely mathematical view.</p>
  1833.      * @param a angle to normalize
  1834.      * @param center center of the desired 2&pi; interval for the result
  1835.      * @param <T> the type of the field elements
  1836.      * @return a-2k&pi; with integer k and center-&pi; &lt;= a-2k&pi; &lt;= center+&pi;
  1837.      */
  1838.     public static <T extends RealFieldElement<T>> T normalizeAngle(final T a, final T center) {
  1839.         return a.subtract(2 * FastMath.PI * FastMath.floor((a.getReal() + FastMath.PI - center.getReal()) / (2 * FastMath.PI)));
  1840.     }

  1842.     @Override
  1843.     public KeplerianOrbit toOrbit() {
  1844.         if (hasDerivatives()) {
  1845.             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
  1846.                                       pa.getReal(), raan.getReal(), v.getReal(),
  1847.                                       aDot.getReal(), eDot.getReal(), iDot.getReal(),
  1848.                                       paDot.getReal(), raanDot.getReal(), vDot.getReal(),
  1849.                                       PositionAngle.TRUE,
  1850.                                       getFrame(), getDate().toAbsoluteDate(), getMu());
  1851.         } else {
  1852.             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
  1853.                                       pa.getReal(), raan.getReal(), v.getReal(),
  1854.                                       PositionAngle.TRUE,
  1855.                                       getFrame(), getDate().toAbsoluteDate(), getMu());
  1856.         }
  1857.     }


  1860. }
Created with JaCoCo 0.7.9.201702052155