FieldKeplerianOrbit.java

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


  18. import java.lang.reflect.Array;

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.Field;
  21. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.util.FastMath;
  24. import org.hipparchus.util.FieldSinCos;
  25. import org.hipparchus.util.MathArrays;
  26. import org.orekit.errors.OrekitException;
  27. import org.orekit.errors.OrekitIllegalArgumentException;
  28. import org.orekit.errors.OrekitInternalError;
  29. import org.orekit.errors.OrekitMessages;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.time.FieldAbsoluteDate;
  32. import org.orekit.utils.FieldPVCoordinates;
  33. import org.orekit.utils.TimeStampedFieldPVCoordinates;


  34. /**
  35.  * This class handles traditional Keplerian orbital parameters.

  36.  * <p>
  37.  * The parameters used internally are the classical Keplerian elements:
  38.  *   <pre>
  39.  *     a
  40.  *     e
  41.  *     i
  42.  *     ω
  43.  *     Ω
  44.  *     v
  45.  *   </pre>
  46.  * where ω stands for the Perigee Argument, Ω stands for the
  47.  * Right Ascension of the Ascending Node and v stands for the true anomaly.
  48.  * <p>
  49.  * This class supports hyperbolic orbits, using the convention that semi major
  50.  * axis is negative for such orbits (and of course eccentricity is greater than 1).
  51.  * </p>
  52.  * <p>
  53.  * When orbit is either equatorial or circular, some Keplerian elements
  54.  * (more precisely ω and Ω) become ambiguous so this class should not
  55.  * be used for such orbits. For this reason, {@link EquinoctialOrbit equinoctial
  56.  * orbits} is the recommended way to represent orbits.
  57.  * </p>

  58.  * <p>
  59.  * The instance <code>KeplerianOrbit</code> is guaranteed to be immutable.
  60.  * </p>
  61.  * @see     Orbit
  62.  * @see    CircularOrbit
  63.  * @see    CartesianOrbit
  64.  * @see    EquinoctialOrbit
  65.  * @author Luc Maisonobe
  66.  * @author Guylaine Prat
  67.  * @author Fabien Maussion
  68.  * @author V&eacute;ronique Pommier-Maurussane
  69.  * @author Andrea Antolino
  70.  * @since 9.0
  71.  * @param <T> type of the field elements
  72.  */
  73. public class FieldKeplerianOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T>
  74.         implements PositionAngleBased {

  75.     /** Name of the eccentricity parameter. */
  76.     private static final String ECCENTRICITY = "eccentricity";

  77.     /** Semi-major axis (m). */
  78.     private final T a;

  79.     /** Eccentricity. */
  80.     private final T e;

  81.     /** Inclination (rad). */
  82.     private final T i;

  83.     /** Perigee Argument (rad). */
  84.     private final T pa;

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

  87.     /** Cached anomaly (rad). */
  88.     private final T cachedAnomaly;

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

  91.     /** Eccentricity derivative. */
  92.     private final T eDot;

  93.     /** Inclination derivative (rad/s). */
  94.     private final T iDot;

  95.     /** Perigee Argument derivative (rad/s). */
  96.     private final T paDot;

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

  99.     /** Derivative of cached anomaly (rad/s). */
  100.     private final T cachedAnomalyDot;

  101.     /** Cached type of position angle. */
  102.     private final PositionAngleType cachedPositionAngleType;

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

  105.     /** PThird Canonical Vector. */
  106.     private final FieldVector3D<T> PLUS_K;

  107.     /** Creates a new instance.
  108.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  109.      * @param e eccentricity (positive or equal to 0)
  110.      * @param i inclination (rad)
  111.      * @param pa perigee argument (ω, rad)
  112.      * @param raan right ascension of ascending node (Ω, rad)
  113.      * @param anomaly mean, eccentric or true anomaly (rad)
  114.      * @param type type of anomaly
  115.      * @param cachedPositionAngleType type of cached anomaly
  116.      * @param frame the frame in which the parameters are defined
  117.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  118.      * @param date date of the orbital parameters
  119.      * @param mu central attraction coefficient (m³/s²)
  120.      * @exception IllegalArgumentException if frame is not a {@link
  121.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  122.      * or v is out of range for hyperbolic orbits
  123.      * @since 12.1
  124.      */
  125.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  126.                                final T pa, final T raan,
  127.                                final T anomaly, final PositionAngleType type,
  128.                                final PositionAngleType cachedPositionAngleType,
  129.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  130.         throws IllegalArgumentException {
  131.         this(a, e, i, pa, raan, anomaly,
  132.              null, null, null, null, null, null,
  133.              type, cachedPositionAngleType, frame, date, mu);
  134.     }

  135.     /** Creates a new instance.
  136.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  137.      * @param e eccentricity (positive or equal to 0)
  138.      * @param i inclination (rad)
  139.      * @param pa perigee argument (ω, rad)
  140.      * @param raan right ascension of ascending node (Ω, rad)
  141.      * @param anomaly mean, eccentric or true anomaly (rad)
  142.      * @param type type of anomaly
  143.      * @param frame the frame in which the parameters are defined
  144.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  145.      * @param date date of the orbital parameters
  146.      * @param mu central attraction coefficient (m³/s²)
  147.      * @exception IllegalArgumentException if frame is not a {@link
  148.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  149.      * or v is out of range for hyperbolic orbits
  150.      * @since 12.1
  151.      */
  152.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  153.                                final T pa, final T raan,
  154.                                final T anomaly, final PositionAngleType type,
  155.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  156.             throws IllegalArgumentException {
  157.         this(a, e, i, pa, raan, anomaly, type, type, frame, date, mu);
  158.     }

  159.     /** Creates a new instance.
  160.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  161.      * @param e eccentricity (positive or equal to 0)
  162.      * @param i inclination (rad)
  163.      * @param pa perigee argument (ω, rad)
  164.      * @param raan right ascension of ascending node (Ω, rad)
  165.      * @param anomaly mean, eccentric or true anomaly (rad)
  166.      * @param aDot  semi-major axis derivative, null if unknown (m/s)
  167.      * @param eDot eccentricity derivative, null if unknown
  168.      * @param iDot inclination derivative, null if unknown (rad/s)
  169.      * @param paDot perigee argument derivative, null if unknown (rad/s)
  170.      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
  171.      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
  172.      * @param type type of anomaly
  173.      * @param cachedPositionAngleType type of cached anomaly
  174.      * @param frame the frame in which the parameters are defined
  175.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  176.      * @param date date of the orbital parameters
  177.      * @param mu central attraction coefficient (m³/s²)
  178.      * @exception IllegalArgumentException if frame is not a {@link
  179.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  180.      * or v is out of range for hyperbolic orbits
  181.      * @since 12.1
  182.      */
  183.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  184.                                final T pa, final T raan, final T anomaly,
  185.                                final T aDot, final T eDot, final T iDot,
  186.                                final T paDot, final T raanDot, final T anomalyDot,
  187.                                final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
  188.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  189.         throws IllegalArgumentException {
  190.         super(frame, date, mu);
  191.         this.cachedPositionAngleType = cachedPositionAngleType;

  192.         if (a.multiply(e.negate().add(1)).getReal() < 0) {
  193.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a.getReal(), e.getReal());
  194.         }

  195.         // Checking eccentricity range
  196.         checkParameterRangeInclusive(ECCENTRICITY, e.getReal(), 0.0, Double.POSITIVE_INFINITY);

  197.         this.a       =    a;
  198.         this.aDot    =    aDot;
  199.         this.e       =    e;
  200.         this.eDot    =    eDot;
  201.         this.i       =    i;
  202.         this.iDot    =    iDot;
  203.         this.pa      =    pa;
  204.         this.paDot   =    paDot;
  205.         this.raan    =    raan;
  206.         this.raanDot =    raanDot;

  207.         this.PLUS_K = FieldVector3D.getPlusK(a.getField());  // third canonical vector

  208.         if (hasDerivatives()) {
  209.             final FieldUnivariateDerivative1<T> cachedAnomalyUD = initializeCachedAnomaly(anomaly, anomalyDot, type);
  210.             this.cachedAnomaly = cachedAnomalyUD.getValue();
  211.             this.cachedAnomalyDot = cachedAnomalyUD.getFirstDerivative();
  212.         } else {
  213.             this.cachedAnomaly = initializeCachedAnomaly(anomaly, type);
  214.             this.cachedAnomalyDot = null;
  215.         }

  216.         // check true anomaly range
  217.         if (!isElliptical()) {
  218.             final T trueAnomaly = getTrueAnomaly();
  219.             if (e.multiply(trueAnomaly.cos()).add(1).getReal() <= 0) {
  220.                 final double vMax = e.reciprocal().negate().acos().getReal();
  221.                 throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
  222.                         trueAnomaly.getReal(), e.getReal(), -vMax, vMax);
  223.             }
  224.         }

  225.         this.partialPV = null;

  226.     }

  227.     /** Creates a new instance.
  228.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  229.      * @param e eccentricity (positive or equal to 0)
  230.      * @param i inclination (rad)
  231.      * @param pa perigee argument (ω, rad)
  232.      * @param raan right ascension of ascending node (Ω, rad)
  233.      * @param anomaly mean, eccentric or true anomaly (rad)
  234.      * @param aDot  semi-major axis derivative, null if unknown (m/s)
  235.      * @param eDot eccentricity derivative, null if unknown
  236.      * @param iDot inclination derivative, null if unknown (rad/s)
  237.      * @param paDot perigee argument derivative, null if unknown (rad/s)
  238.      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
  239.      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
  240.      * @param type type of anomaly
  241.      * @param frame the frame in which the parameters are defined
  242.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  243.      * @param date date of the orbital parameters
  244.      * @param mu central attraction coefficient (m³/s²)
  245.      * @exception IllegalArgumentException if frame is not a {@link
  246.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  247.      * or v is out of range for hyperbolic orbits
  248.      */
  249.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  250.                                final T pa, final T raan, final T anomaly,
  251.                                final T aDot, final T eDot, final T iDot,
  252.                                final T paDot, final T raanDot, final T anomalyDot,
  253.                                final PositionAngleType type,
  254.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  255.             throws IllegalArgumentException {
  256.         this(a, e, i, pa, raan, anomaly, aDot, eDot, iDot, paDot, raanDot, anomalyDot,
  257.                 type, type, frame, date, mu);
  258.     }

  259.     /** Constructor from Cartesian parameters.
  260.      *
  261.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  262.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  263.      * use {@code mu} and the position to compute the acceleration, including
  264.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  265.      *
  266.      * @param pvCoordinates the PVCoordinates of the satellite
  267.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  268.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  269.      * @param mu central attraction coefficient (m³/s²)
  270.      * @exception IllegalArgumentException if frame is not a {@link
  271.      * Frame#isPseudoInertial pseudo-inertial frame}
  272.      */
  273.     public FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  274.                                final Frame frame, final T mu)
  275.         throws IllegalArgumentException {
  276.         this(pvCoordinates, frame, mu, hasNonKeplerianAcceleration(pvCoordinates, mu));
  277.     }

  278.     /** Constructor from Cartesian parameters.
  279.      *
  280.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  281.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  282.      * use {@code mu} and the position to compute the acceleration, including
  283.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  284.      *
  285.      * @param pvCoordinates the PVCoordinates of the satellite
  286.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  287.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  288.      * @param mu central attraction coefficient (m³/s²)
  289.      * @param reliableAcceleration if true, the acceleration is considered to be reliable
  290.      * @exception IllegalArgumentException if frame is not a {@link
  291.      * Frame#isPseudoInertial pseudo-inertial frame}
  292.      */
  293.     private FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  294.                                 final Frame frame, final T mu,
  295.                                 final boolean reliableAcceleration)
  296.         throws IllegalArgumentException {

  297.         super(pvCoordinates, frame, mu);

  298.         // third canonical vector
  299.         this.PLUS_K = FieldVector3D.getPlusK(getOne().getField());

  300.         // compute inclination
  301.         final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
  302.         final T m2 = momentum.getNormSq();

  303.         i = FieldVector3D.angle(momentum, PLUS_K);
  304.         // compute right ascension of ascending node
  305.         raan = FieldVector3D.crossProduct(PLUS_K, momentum).getAlpha();
  306.         // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic)
  307.         final FieldVector3D<T> pvP     = pvCoordinates.getPosition();
  308.         final FieldVector3D<T> pvV     = pvCoordinates.getVelocity();
  309.         final FieldVector3D<T> pvA     = pvCoordinates.getAcceleration();

  310.         final T   r2      = pvP.getNormSq();
  311.         final T   r       = r2.sqrt();
  312.         final T   V2      = pvV.getNormSq();
  313.         final T   rV2OnMu = r.multiply(V2).divide(mu);

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

  317.         // compute true anomaly
  318.         if (isElliptical()) {
  319.             // elliptic or circular orbit
  320.             final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  321.             final T eCE = rV2OnMu.subtract(1);
  322.             e = (eSE.multiply(eSE).add(eCE.multiply(eCE))).sqrt();
  323.             this.cachedPositionAngleType = PositionAngleType.ECCENTRIC;
  324.             cachedAnomaly = eSE.atan2(eCE);
  325.         } else {
  326.             // hyperbolic orbit
  327.             final T eSH = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  328.             final T eCH = rV2OnMu.subtract(1);
  329.             e = (m2.negate().divide(muA).add(1)).sqrt();
  330.             this.cachedPositionAngleType = PositionAngleType.TRUE;
  331.             cachedAnomaly = FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, (eCH.add(eSH)).divide(eCH.subtract(eSH)).log().divide(2));
  332.         }

  333.         // Checking eccentricity range
  334.         checkParameterRangeInclusive(ECCENTRICITY, e.getReal(), 0.0, Double.POSITIVE_INFINITY);

  335.         // compute perigee argument
  336.         final FieldVector3D<T> node = new FieldVector3D<>(raan, getZero());
  337.         final T px = FieldVector3D.dotProduct(pvP, node);
  338.         final T py = FieldVector3D.dotProduct(pvP, FieldVector3D.crossProduct(momentum, node)).divide(m2.sqrt());
  339.         pa = py.atan2(px).subtract(getTrueAnomaly());

  340.         partialPV = pvCoordinates;

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

  343.             final T[][] jacobian = MathArrays.buildArray(a.getField(), 6, 6);
  344.             getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);

  345.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  346.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  347.             final T   aX                       = nonKeplerianAcceleration.getX();
  348.             final T   aY                       = nonKeplerianAcceleration.getY();
  349.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  350.             aDot    = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  351.             eDot    = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  352.             iDot    = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  353.             paDot   = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  354.             raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  355.             // in order to compute true anomaly derivative, we must compute
  356.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  357.             final T MDot = getKeplerianMeanMotion().
  358.                            add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  359.             final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  360.             final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(getMeanAnomaly(), MDot);
  361.             if (cachedPositionAngleType == PositionAngleType.ECCENTRIC) {
  362.                 final FieldUnivariateDerivative1<T> EUD = (a.getReal() < 0) ?
  363.                          FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD, MUD) :
  364.                          FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD, MUD);
  365.                 cachedAnomalyDot = EUD.getFirstDerivative();
  366.             } else { // TRUE
  367.                 final FieldUnivariateDerivative1<T> vUD = (a.getReal() < 0) ?
  368.                          FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, MUD) :
  369.                          FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, MUD);
  370.                 cachedAnomalyDot = vUD.getFirstDerivative();
  371.             }

  372.         } else {
  373.             // acceleration is either almost zero or NaN,
  374.             // we assume acceleration was not known
  375.             // we don't set up derivatives
  376.             aDot    = null;
  377.             eDot    = null;
  378.             iDot    = null;
  379.             paDot   = null;
  380.             raanDot = null;
  381.             cachedAnomalyDot    = null;
  382.         }

  383.     }

  384.     /** Constructor from Cartesian parameters.
  385.      *
  386.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  387.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  388.      * use {@code mu} and the position to compute the acceleration, including
  389.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  390.      *
  391.      * @param FieldPVCoordinates the PVCoordinates of the satellite
  392.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  393.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  394.      * @param date date of the orbital parameters
  395.      * @param mu central attraction coefficient (m³/s²)
  396.      * @exception IllegalArgumentException if frame is not a {@link
  397.      * Frame#isPseudoInertial pseudo-inertial frame}
  398.      */
  399.     public FieldKeplerianOrbit(final FieldPVCoordinates<T> FieldPVCoordinates,
  400.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  401.         throws IllegalArgumentException {
  402.         this(new TimeStampedFieldPVCoordinates<>(date, FieldPVCoordinates), frame, mu);
  403.     }

  404.     /** Constructor from any kind of orbital parameters.
  405.      * @param op orbital parameters to copy
  406.      */
  407.     public FieldKeplerianOrbit(final FieldOrbit<T> op) {
  408.         this(op.getPVCoordinates(), op.getFrame(), op.getMu(), op.hasDerivatives());
  409.     }

  410.     /** Constructor from Field and KeplerianOrbit.
  411.      * <p>Build a FieldKeplerianOrbit from non-Field KeplerianOrbit.</p>
  412.      * @param field CalculusField to base object on
  413.      * @param op non-field orbit with only "constant" terms
  414.      * @since 12.0
  415.      */
  416.     public FieldKeplerianOrbit(final Field<T> field, final KeplerianOrbit op) {
  417.         this(field.getZero().newInstance(op.getA()), field.getZero().newInstance(op.getE()), field.getZero().newInstance(op.getI()),
  418.                 field.getZero().newInstance(op.getPerigeeArgument()), field.getZero().newInstance(op.getRightAscensionOfAscendingNode()),
  419.                 field.getZero().newInstance(op.getAnomaly(op.getCachedPositionAngleType())),
  420.                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getADot()) : null,
  421.                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getEDot()) : null,
  422.                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getIDot()) : null,
  423.                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getPerigeeArgumentDot()) : null,
  424.                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getRightAscensionOfAscendingNodeDot()) : null,
  425.                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getAnomalyDot(op.getCachedPositionAngleType())) : null,
  426.                 op.getCachedPositionAngleType(), op.getFrame(),
  427.                 new FieldAbsoluteDate<>(field, op.getDate()), field.getZero().newInstance(op.getMu()));
  428.     }

  429.     /** Constructor from Field and Orbit.
  430.      * <p>Build a FieldKeplerianOrbit from any non-Field Orbit.</p>
  431.      * @param field CalculusField to base object on
  432.      * @param op non-field orbit with only "constant" terms
  433.      * @since 12.0
  434.      */
  435.     public FieldKeplerianOrbit(final Field<T> field, final Orbit op) {
  436.         this(field, (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(op));
  437.     }

  438.     /** {@inheritDoc} */
  439.     @Override
  440.     public OrbitType getType() {
  441.         return OrbitType.KEPLERIAN;
  442.     }

  443.     /** {@inheritDoc} */
  444.     @Override
  445.     public T getA() {
  446.         return a;
  447.     }

  448.     /** {@inheritDoc} */
  449.     @Override
  450.     public T getADot() {
  451.         return aDot;
  452.     }

  453.     /** {@inheritDoc} */
  454.     @Override
  455.     public T getE() {
  456.         return e;
  457.     }

  458.     /** {@inheritDoc} */
  459.     @Override
  460.     public T getEDot() {
  461.         return eDot;
  462.     }

  463.     /** {@inheritDoc} */
  464.     @Override
  465.     public T getI() {
  466.         return i;
  467.     }

  468.     /** {@inheritDoc} */
  469.     @Override
  470.     public T getIDot() {
  471.         return iDot;
  472.     }

  473.     /** Get the perigee argument.
  474.      * @return perigee argument (rad)
  475.      */
  476.     public T getPerigeeArgument() {
  477.         return pa;
  478.     }

  479.     /** Get the perigee argument derivative.
  480.      * <p>
  481.      * If the orbit was created without derivatives, the value returned is null.
  482.      * </p>
  483.      * @return perigee argument derivative (rad/s)
  484.      */
  485.     public T getPerigeeArgumentDot() {
  486.         return paDot;
  487.     }

  488.     /** Get the right ascension of the ascending node.
  489.      * @return right ascension of the ascending node (rad)
  490.      */
  491.     public T getRightAscensionOfAscendingNode() {
  492.         return raan;
  493.     }

  494.     /** Get the right ascension of the ascending node derivative.
  495.      * <p>
  496.      * If the orbit was created without derivatives, the value returned is null.
  497.      * </p>
  498.      * @return right ascension of the ascending node derivative (rad/s)
  499.      */
  500.     public T getRightAscensionOfAscendingNodeDot() {
  501.         return raanDot;
  502.     }

  503.     /** Get the true anomaly.
  504.      * @return true anomaly (rad)
  505.      */
  506.     public T getTrueAnomaly() {
  507.         switch (cachedPositionAngleType) {
  508.             case MEAN: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(e, cachedAnomaly) :
  509.                     FieldKeplerianAnomalyUtility.ellipticMeanToTrue(e, cachedAnomaly);

  510.             case TRUE: return cachedAnomaly;

  511.             case ECCENTRIC: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, cachedAnomaly) :
  512.                     FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, cachedAnomaly);

  513.             default: throw new OrekitInternalError(null);
  514.         }
  515.     }

  516.     /** Get the true anomaly derivative.
  517.      * <p>
  518.      * If the orbit was created without derivatives, the value returned is null.
  519.      * </p>
  520.      * @return true anomaly derivative (rad/s)
  521.      */
  522.     public T getTrueAnomalyDot() {
  523.         if (hasDerivatives()) {
  524.             switch (cachedPositionAngleType) {
  525.                 case MEAN:
  526.                     final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  527.                     final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  528.                     final FieldUnivariateDerivative1<T> vUD = (a.getReal() < 0) ?
  529.                              FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, MUD) :
  530.                              FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, MUD);
  531.                     return vUD.getFirstDerivative();

  532.                 case TRUE:
  533.                     return cachedAnomalyDot;

  534.                 case ECCENTRIC:
  535.                     final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
  536.                     final FieldUnivariateDerivative1<T> EUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  537.                     final FieldUnivariateDerivative1<T> vUD2 = (a.getReal() < 0) ?
  538.                              FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(eUD2, EUD) :
  539.                              FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(eUD2, EUD);
  540.                     return vUD2.getFirstDerivative();

  541.                 default:
  542.                     throw new OrekitInternalError(null);
  543.             }
  544.         } else {
  545.             return null;
  546.         }
  547.     }

  548.     /** Get the eccentric anomaly.
  549.      * @return eccentric anomaly (rad)
  550.      */
  551.     public T getEccentricAnomaly() {
  552.         switch (cachedPositionAngleType) {
  553.             case MEAN:
  554.                 return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, cachedAnomaly) :
  555.                     FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, cachedAnomaly);

  556.             case ECCENTRIC:
  557.                 return cachedAnomaly;

  558.             case TRUE:
  559.                 return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, cachedAnomaly) :
  560.                     FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, cachedAnomaly);

  561.             default:
  562.                 throw new OrekitInternalError(null);
  563.         }
  564.     }

  565.     /** Get the eccentric anomaly derivative.
  566.      * <p>
  567.      * If the orbit was created without derivatives, the value returned is null.
  568.      * </p>
  569.      * @return eccentric anomaly derivative (rad/s)
  570.      */
  571.     public T getEccentricAnomalyDot() {
  572.         if (hasDerivatives()) {
  573.             switch (cachedPositionAngleType) {
  574.                 case ECCENTRIC:
  575.                     return cachedAnomalyDot;

  576.                 case TRUE:
  577.                     final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  578.                     final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  579.                     final FieldUnivariateDerivative1<T> EUD = (a.getReal() < 0) ?
  580.                              FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(eUD, vUD) :
  581.                              FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(eUD, vUD);
  582.                     return EUD.getFirstDerivative();

  583.                 case MEAN:
  584.                     final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
  585.                     final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  586.                     final FieldUnivariateDerivative1<T> EUD2 = (a.getReal() < 0) ?
  587.                              FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD2, MUD) :
  588.                              FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD2, MUD);
  589.                     return EUD2.getFirstDerivative();

  590.                 default:
  591.                     throw new OrekitInternalError(null);
  592.             }
  593.         } else {
  594.             return null;
  595.         }
  596.     }

  597.     /** Get the mean anomaly.
  598.      * @return mean anomaly (rad)
  599.      */
  600.     public T getMeanAnomaly() {
  601.         switch (cachedPositionAngleType) {
  602.             case ECCENTRIC: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(e, cachedAnomaly) :
  603.                     FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, cachedAnomaly);

  604.             case MEAN: return cachedAnomaly;

  605.             case TRUE: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(e, cachedAnomaly) :
  606.                     FieldKeplerianAnomalyUtility.ellipticTrueToMean(e, cachedAnomaly);

  607.             default: throw new OrekitInternalError(null);
  608.         }
  609.     }

  610.     /** Get the mean anomaly derivative.
  611.      * <p>
  612.      * If the orbit was created without derivatives, the value returned is null.
  613.      * </p>
  614.      * @return mean anomaly derivative (rad/s)
  615.      */
  616.     public T getMeanAnomalyDot() {
  617.         if (hasDerivatives()) {
  618.             switch (cachedPositionAngleType) {
  619.                 case MEAN:
  620.                     return cachedAnomalyDot;

  621.                 case ECCENTRIC:
  622.                     final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  623.                     final FieldUnivariateDerivative1<T> EUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  624.                     final FieldUnivariateDerivative1<T> MUD = (a.getReal() < 0) ?
  625.                              FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(eUD, EUD) :
  626.                              FieldKeplerianAnomalyUtility.ellipticEccentricToMean(eUD, EUD);
  627.                     return MUD.getFirstDerivative();

  628.                 case TRUE:
  629.                     final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
  630.                     final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  631.                     final FieldUnivariateDerivative1<T> MUD2 = (a.getReal() < 0) ?
  632.                              FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(eUD2, vUD) :
  633.                              FieldKeplerianAnomalyUtility.ellipticTrueToMean(eUD2, vUD);
  634.                     return MUD2.getFirstDerivative();

  635.                 default:
  636.                     throw new OrekitInternalError(null);
  637.             }
  638.         } else {
  639.             return null;
  640.         }
  641.     }

  642.     /** Get the anomaly.
  643.      * @param type type of the angle
  644.      * @return anomaly (rad)
  645.      */
  646.     public T getAnomaly(final PositionAngleType type) {
  647.         return (type == PositionAngleType.MEAN) ? getMeanAnomaly() :
  648.                                               ((type == PositionAngleType.ECCENTRIC) ? getEccentricAnomaly() :
  649.                                                                                    getTrueAnomaly());
  650.     }

  651.     /** Get the anomaly derivative.
  652.      * <p>
  653.      * If the orbit was created without derivatives, the value returned is null.
  654.      * </p>
  655.      * @param type type of the angle
  656.      * @return anomaly derivative (rad/s)
  657.      */
  658.     public T getAnomalyDot(final PositionAngleType type) {
  659.         return (type == PositionAngleType.MEAN) ? getMeanAnomalyDot() :
  660.                                               ((type == PositionAngleType.ECCENTRIC) ? getEccentricAnomalyDot() :
  661.                                                                                    getTrueAnomalyDot());
  662.     }

  663.     /** {@inheritDoc} */
  664.     @Override
  665.     public boolean hasDerivatives() {
  666.         return aDot != null;
  667.     }

  668.     /** {@inheritDoc} */
  669.     @Override
  670.     public T getEquinoctialEx() {
  671.         return e.multiply(pa.add(raan).cos());
  672.     }

  673.     /** {@inheritDoc} */
  674.     @Override
  675.     public T getEquinoctialExDot() {

  676.         if (!hasDerivatives()) {
  677.             return null;
  678.         }

  679.         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
  680.         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
  681.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  682.         return eUD.multiply(paUD.add(raanUD).cos()).getFirstDerivative();

  683.     }

  684.     /** {@inheritDoc} */
  685.     @Override
  686.     public T getEquinoctialEy() {
  687.         return e.multiply((pa.add(raan)).sin());
  688.     }

  689.     /** {@inheritDoc} */
  690.     @Override
  691.     public T getEquinoctialEyDot() {

  692.         if (!hasDerivatives()) {
  693.             return null;
  694.         }

  695.         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
  696.         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
  697.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  698.         return eUD.multiply(paUD.add(raanUD).sin()).getFirstDerivative();

  699.     }

  700.     /** {@inheritDoc} */
  701.     @Override
  702.     public T getHx() {
  703.         // Check for equatorial retrograde orbit
  704.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  705.             return getZero().add(Double.NaN);
  706.         }
  707.         return raan.cos().multiply(i.divide(2).tan());
  708.     }

  709.     /** {@inheritDoc} */
  710.     @Override
  711.     public T getHxDot() {

  712.         if (!hasDerivatives()) {
  713.             return null;
  714.         }

  715.         // Check for equatorial retrograde orbit
  716.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  717.             return getZero().add(Double.NaN);
  718.         }

  719.         final FieldUnivariateDerivative1<T> iUD    = new FieldUnivariateDerivative1<>(i,    iDot);
  720.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  721.         return raanUD.cos().multiply(iUD.multiply(0.5).tan()).getFirstDerivative();

  722.     }

  723.     /** {@inheritDoc} */
  724.     @Override
  725.     public T getHy() {
  726.         // Check for equatorial retrograde orbit
  727.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  728.             return getZero().add(Double.NaN);
  729.         }
  730.         return raan.sin().multiply(i.divide(2).tan());
  731.     }

  732.     /** {@inheritDoc} */
  733.     @Override
  734.     public T getHyDot() {

  735.         if (!hasDerivatives()) {
  736.             return null;
  737.         }

  738.         // Check for equatorial retrograde orbit
  739.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  740.             return getZero().add(Double.NaN);
  741.         }

  742.         final FieldUnivariateDerivative1<T> iUD    = new FieldUnivariateDerivative1<>(i,    iDot);
  743.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  744.         return raanUD.sin().multiply(iUD.multiply(0.5).tan()).getFirstDerivative();

  745.     }

  746.     /** {@inheritDoc} */
  747.     @Override
  748.     public T getLv() {
  749.         return pa.add(raan).add(getTrueAnomaly());
  750.     }

  751.     /** {@inheritDoc} */
  752.     @Override
  753.     public T getLvDot() {
  754.         return hasDerivatives() ?
  755.                paDot.add(raanDot).add(getTrueAnomalyDot()) :
  756.                null;
  757.     }

  758.     /** {@inheritDoc} */
  759.     @Override
  760.     public T getLE() {
  761.         return pa.add(raan).add(getEccentricAnomaly());
  762.     }

  763.     /** {@inheritDoc} */
  764.     @Override
  765.     public T getLEDot() {
  766.         return hasDerivatives() ?
  767.                paDot.add(raanDot).add(getEccentricAnomalyDot()) :
  768.                null;
  769.     }

  770.     /** {@inheritDoc} */
  771.     @Override
  772.     public T getLM() {
  773.         return pa.add(raan).add(getMeanAnomaly());
  774.     }

  775.     /** {@inheritDoc} */
  776.     @Override
  777.     public T getLMDot() {
  778.         return hasDerivatives() ?
  779.                paDot.add(raanDot).add(getMeanAnomalyDot()) :
  780.                null;
  781.     }

  782.     /** Initialize cached anomaly with rate.
  783.      * @param anomaly input anomaly
  784.      * @param anomalyDot rate of input anomaly
  785.      * @param inputType position angle type passed as input
  786.      * @return anomaly to cache with rate
  787.      * @since 12.1
  788.      */
  789.     private FieldUnivariateDerivative1<T> initializeCachedAnomaly(final T anomaly, final T anomalyDot,
  790.                                                                   final PositionAngleType inputType) {
  791.         if (cachedPositionAngleType == inputType) {
  792.             return new FieldUnivariateDerivative1<>(anomaly, anomalyDot);

  793.         } else {
  794.             final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  795.             final FieldUnivariateDerivative1<T> anomalyUD = new FieldUnivariateDerivative1<>(anomaly, anomalyDot);

  796.             if (a.getReal() < 0) {
  797.                 switch (cachedPositionAngleType) {
  798.                     case MEAN:
  799.                         if (inputType == PositionAngleType.ECCENTRIC) {
  800.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(eUD, anomalyUD);
  801.                         } else {
  802.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(eUD, anomalyUD);
  803.                         }

  804.                     case ECCENTRIC:
  805.                         if (inputType == PositionAngleType.MEAN) {
  806.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD, anomalyUD);
  807.                         } else {
  808.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(eUD, anomalyUD);
  809.                         }

  810.                     case TRUE:
  811.                         if (inputType == PositionAngleType.MEAN) {
  812.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, anomalyUD);
  813.                         } else {
  814.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(eUD, anomalyUD);
  815.                         }

  816.                     default:
  817.                         break;
  818.                 }

  819.             } else {
  820.                 switch (cachedPositionAngleType) {
  821.                     case MEAN:
  822.                         if (inputType == PositionAngleType.ECCENTRIC) {
  823.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(eUD, anomalyUD);
  824.                         } else {
  825.                             return FieldKeplerianAnomalyUtility.ellipticTrueToMean(eUD, anomalyUD);
  826.                         }

  827.                     case ECCENTRIC:
  828.                         if (inputType == PositionAngleType.MEAN) {
  829.                             return FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD, anomalyUD);
  830.                         } else {
  831.                             return FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(eUD, anomalyUD);
  832.                         }

  833.                     case TRUE:
  834.                         if (inputType == PositionAngleType.MEAN) {
  835.                             return FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, anomalyUD);
  836.                         } else {
  837.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(eUD, anomalyUD);
  838.                         }

  839.                     default:
  840.                         break;
  841.                 }

  842.             }
  843.             throw new OrekitInternalError(null);
  844.         }

  845.     }

  846.     /** Initialize cached anomaly.
  847.      * @param anomaly input anomaly
  848.      * @param inputType position angle type passed as input
  849.      * @return anomaly to cache
  850.      * @since 12.1
  851.      */
  852.     private T initializeCachedAnomaly(final T anomaly, final PositionAngleType inputType) {
  853.         if (inputType == cachedPositionAngleType) {
  854.             return anomaly;

  855.         } else {
  856.             if (a.getReal() < 0) {
  857.                 switch (cachedPositionAngleType) {
  858.                     case MEAN:
  859.                         if (inputType == PositionAngleType.ECCENTRIC) {
  860.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(e, anomaly);
  861.                         } else {
  862.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(e, anomaly);
  863.                         }

  864.                     case ECCENTRIC:
  865.                         if (inputType == PositionAngleType.MEAN) {
  866.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, anomaly);
  867.                         } else {
  868.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, anomaly);
  869.                         }

  870.                     case TRUE:
  871.                         if (inputType == PositionAngleType.ECCENTRIC) {
  872.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, anomaly);
  873.                         } else {
  874.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(e, anomaly);
  875.                         }

  876.                     default:
  877.                         break;
  878.                 }

  879.             } else {
  880.                 switch (cachedPositionAngleType) {
  881.                     case MEAN:
  882.                         if (inputType == PositionAngleType.ECCENTRIC) {
  883.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, anomaly);
  884.                         } else {
  885.                             return FieldKeplerianAnomalyUtility.ellipticTrueToMean(e, anomaly);
  886.                         }

  887.                     case ECCENTRIC:
  888.                         if (inputType == PositionAngleType.MEAN) {
  889.                             return FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, anomaly);
  890.                         } else {
  891.                             return FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, anomaly);
  892.                         }

  893.                     case TRUE:
  894.                         if (inputType == PositionAngleType.ECCENTRIC) {
  895.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, anomaly);
  896.                         } else {
  897.                             return FieldKeplerianAnomalyUtility.ellipticMeanToTrue(e, anomaly);
  898.                         }

  899.                     default:
  900.                         break;
  901.                 }
  902.             }
  903.             throw new OrekitInternalError(null);
  904.         }
  905.     }

  906.     /** Compute reference axes.
  907.      * @return referecne axes
  908.      * @since 12.0
  909.      */
  910.     private FieldVector3D<T>[] referenceAxes() {

  911.         // preliminary variables
  912.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  913.         final FieldSinCos<T> scPa   = FastMath.sinCos(pa);
  914.         final FieldSinCos<T> scI    = FastMath.sinCos(i);
  915.         final T cosRaan = scRaan.cos();
  916.         final T sinRaan = scRaan.sin();
  917.         final T cosPa   = scPa.cos();
  918.         final T sinPa   = scPa.sin();
  919.         final T cosI    = scI.cos();
  920.         final T sinI    = scI.sin();
  921.         final T crcp    = cosRaan.multiply(cosPa);
  922.         final T crsp    = cosRaan.multiply(sinPa);
  923.         final T srcp    = sinRaan.multiply(cosPa);
  924.         final T srsp    = sinRaan.multiply(sinPa);

  925.         // reference axes defining the orbital plane
  926.         @SuppressWarnings("unchecked")
  927.         final FieldVector3D<T>[] axes = (FieldVector3D<T>[]) Array.newInstance(FieldVector3D.class, 2);
  928.         axes[0] = new FieldVector3D<>(crcp.subtract(cosI.multiply(srsp)),  srcp.add(cosI.multiply(crsp)), sinI.multiply(sinPa));
  929.         axes[1] = new FieldVector3D<>(crsp.add(cosI.multiply(srcp)).negate(), cosI.multiply(crcp).subtract(srsp), sinI.multiply(cosPa));

  930.         return axes;

  931.     }

  932.     /** Compute position and velocity but not acceleration.
  933.      */
  934.     private void computePVWithoutA() {

  935.         if (partialPV != null) {
  936.             // already computed
  937.             return;
  938.         }

  939.         final FieldVector3D<T>[] axes = referenceAxes();

  940.         if (isElliptical()) {

  941.             // elliptical case

  942.             // elliptic eccentric anomaly
  943.             final T uME2             = e.negate().add(1).multiply(e.add(1));
  944.             final T s1Me2            = uME2.sqrt();
  945.             final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  946.             final T cosE             = scE.cos();
  947.             final T sinE             = scE.sin();

  948.             // coordinates of position and velocity in the orbital plane
  949.             final T x      = a.multiply(cosE.subtract(e));
  950.             final T y      = a.multiply(sinE).multiply(s1Me2);
  951.             final T factor = FastMath.sqrt(getMu().divide(a)).divide(e.negate().multiply(cosE).add(1));
  952.             final T xDot   = sinE.negate().multiply(factor);
  953.             final T yDot   = cosE.multiply(s1Me2).multiply(factor);

  954.             final FieldVector3D<T> position = new FieldVector3D<>(x, axes[0], y, axes[1]);
  955.             final FieldVector3D<T> velocity = new FieldVector3D<>(xDot, axes[0], yDot, axes[1]);
  956.             partialPV = new FieldPVCoordinates<>(position, velocity);

  957.         } else {

  958.             // hyperbolic case

  959.             // compute position and velocity factors
  960.             final FieldSinCos<T> scV = FastMath.sinCos(getTrueAnomaly());
  961.             final T sinV             = scV.sin();
  962.             final T cosV             = scV.cos();
  963.             final T f                = a.multiply(e.square().negate().add(1));
  964.             final T posFactor        = f.divide(e.multiply(cosV).add(1));
  965.             final T velFactor        = FastMath.sqrt(getMu().divide(f));

  966.             final FieldVector3D<T> position     = new FieldVector3D<>(posFactor.multiply(cosV), axes[0], posFactor.multiply(sinV), axes[1]);
  967.             final FieldVector3D<T> velocity     = new FieldVector3D<>(velFactor.multiply(sinV).negate(), axes[0], velFactor.multiply(e.add(cosV)), axes[1]);
  968.             partialPV = new FieldPVCoordinates<>(position, velocity);

  969.         }

  970.     }

  971.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  972.      * <p>
  973.      * This method should be called only when {@link #hasDerivatives()} returns true.
  974.      * </p>
  975.      * @return non-Keplerian part of the acceleration
  976.      */
  977.     private FieldVector3D<T> nonKeplerianAcceleration() {

  978.         final T[][] dCdP = MathArrays.buildArray(a.getField(), 6, 6);
  979.         getJacobianWrtParameters(PositionAngleType.MEAN, dCdP);

  980.         final T nonKeplerianMeanMotion = getMeanAnomalyDot().subtract(getKeplerianMeanMotion());
  981.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  982.                                  add(dCdP[3][1].multiply(eDot)).
  983.                                  add(dCdP[3][2].multiply(iDot)).
  984.                                  add(dCdP[3][3].multiply(paDot)).
  985.                                  add(dCdP[3][4].multiply(raanDot)).
  986.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  987.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  988.                                  add(dCdP[4][1].multiply(eDot)).
  989.                                  add(dCdP[4][2].multiply(iDot)).
  990.                                  add(dCdP[4][3].multiply(paDot)).
  991.                                  add(dCdP[4][4].multiply(raanDot)).
  992.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  993.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  994.                                  add(dCdP[5][1].multiply(eDot)).
  995.                                  add(dCdP[5][2].multiply(iDot)).
  996.                                  add(dCdP[5][3].multiply(paDot)).
  997.                                  add(dCdP[5][4].multiply(raanDot)).
  998.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  1000.     }

  1001.     /** {@inheritDoc} */
  1002.     @Override
  1003.     protected FieldVector3D<T> initPosition() {
  1004.         final FieldVector3D<T>[] axes = referenceAxes();

  1005.         if (isElliptical()) {

  1006.             // elliptical case

  1007.             // elliptic eccentric anomaly
  1008.             final T uME2             = e.negate().add(1).multiply(e.add(1));
  1009.             final T s1Me2            = uME2.sqrt();
  1010.             final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1011.             final T cosE             = scE.cos();
  1012.             final T sinE             = scE.sin();

  1013.             return new FieldVector3D<>(a.multiply(cosE.subtract(e)), axes[0], a.multiply(sinE).multiply(s1Me2), axes[1]);

  1014.         } else {

  1015.             // hyperbolic case

  1016.             // compute position and velocity factors
  1017.             final FieldSinCos<T> scV = FastMath.sinCos(getTrueAnomaly());
  1018.             final T sinV             = scV.sin();
  1019.             final T cosV             = scV.cos();
  1020.             final T f                = a.multiply(e.square().negate().add(1));
  1021.             final T posFactor        = f.divide(e.multiply(cosV).add(1));

  1022.             return new FieldVector3D<>(posFactor.multiply(cosV), axes[0], posFactor.multiply(sinV), axes[1]);

  1023.         }

  1024.     }

  1025.     /** {@inheritDoc} */
  1026.     @Override
  1027.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  1028.         // position and velocity
  1029.         computePVWithoutA();

  1030.         // acceleration
  1031.         final T r2 = partialPV.getPosition().getNormSq();
  1032.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(FastMath.sqrt(r2)).reciprocal().multiply(getMu().negate()),
  1033.                                                                            partialPV.getPosition());
  1034.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  1035.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  1036.                                               keplerianAcceleration;

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

  1038.     }

  1039.     /** {@inheritDoc} */
  1040.     @Override
  1041.     public FieldKeplerianOrbit<T> shiftedBy(final double dt) {
  1042.         return shiftedBy(getZero().newInstance(dt));
  1043.     }

  1044.     /** {@inheritDoc} */
  1045.     @Override
  1046.     public FieldKeplerianOrbit<T> shiftedBy(final T dt) {

  1047.         // use Keplerian-only motion
  1048.         final FieldKeplerianOrbit<T> keplerianShifted = new FieldKeplerianOrbit<>(a, e, i, pa, raan,
  1049.                                                                                   getKeplerianMeanMotion().multiply(dt).add(getMeanAnomaly()),
  1050.                                                                                   PositionAngleType.MEAN, cachedPositionAngleType, getFrame(), getDate().shiftedBy(dt), getMu());

  1051.         if (hasDerivatives()) {

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

  1054.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  1055.             keplerianShifted.computePVWithoutA();
  1056.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getPosition(),
  1057.                                                                   dt.square().multiply(0.5), nonKeplerianAcceleration);
  1058.             final T   fixedR2 = fixedP.getNormSq();
  1059.             final T   fixedR  = fixedR2.sqrt();
  1060.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getVelocity(),
  1061.                                                                  dt, nonKeplerianAcceleration);
  1062.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  1063.                                                                  keplerianShifted.partialPV.getPosition(),
  1064.                                                                  getOne(), nonKeplerianAcceleration);

  1065.             // build a new orbit, taking non-Keplerian acceleration into account
  1066.             return new FieldKeplerianOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  1067.                                                                                  fixedP, fixedV, fixedA),
  1068.                                              keplerianShifted.getFrame(), keplerianShifted.getMu());

  1069.         } else {
  1070.             // Keplerian-only motion is all we can do
  1071.             return keplerianShifted;
  1072.         }

  1073.     }

  1074.     /** {@inheritDoc} */
  1075.     @Override
  1076.     protected T[][] computeJacobianMeanWrtCartesian() {
  1077.         if (isElliptical()) {
  1078.             return computeJacobianMeanWrtCartesianElliptical();
  1079.         } else {
  1080.             return computeJacobianMeanWrtCartesianHyperbolic();
  1081.         }
  1082.     }

  1083.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1084.      * <p>
  1085.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1086.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1087.      * yDot for j=4, zDot for j=5).
  1088.      * </p>
  1089.      * @return 6x6 Jacobian matrix
  1090.      */
  1091.     private T[][] computeJacobianMeanWrtCartesianElliptical() {

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

  1093.         // compute various intermediate parameters
  1094.         computePVWithoutA();
  1095.         final FieldVector3D<T> position = partialPV.getPosition();
  1096.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1097.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  1098.         final T v2         = velocity.getNormSq();
  1099.         final T r2         = position.getNormSq();
  1100.         final T r          = r2.sqrt();
  1101.         final T r3         = r.multiply(r2);

  1102.         final T px         = position.getX();
  1103.         final T py         = position.getY();
  1104.         final T pz         = position.getZ();
  1105.         final T vx         = velocity.getX();
  1106.         final T vy         = velocity.getY();
  1107.         final T vz         = velocity.getZ();
  1108.         final T mx         = momentum.getX();
  1109.         final T my         = momentum.getY();
  1110.         final T mz         = momentum.getZ();

  1111.         final T mu         = getMu();
  1112.         final T sqrtMuA    = FastMath.sqrt(a.multiply(mu));
  1113.         final T sqrtAoMu   = FastMath.sqrt(a.divide(mu));
  1114.         final T a2         = a.square();
  1115.         final T twoA       = a.multiply(2);
  1116.         final T rOnA       = r.divide(a);

  1117.         final T oMe2       = e.square().negate().add(1);
  1118.         final T epsilon    = oMe2.sqrt();
  1119.         final T sqrtRec    = epsilon.reciprocal();

  1120.         final FieldSinCos<T> scI  = FastMath.sinCos(i);
  1121.         final FieldSinCos<T> scPA = FastMath.sinCos(pa);
  1122.         final T cosI       = scI.cos();
  1123.         final T sinI       = scI.sin();
  1124.         final T cosPA      = scPA.cos();
  1125.         final T sinPA      = scPA.sin();

  1126.         final T pv         = FieldVector3D.dotProduct(position, velocity);
  1127.         final T cosE       = a.subtract(r).divide(a.multiply(e));
  1128.         final T sinE       = pv.divide(e.multiply(sqrtMuA));

  1129.         // da
  1130.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  1131.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(mu.divide(2.).reciprocal()));
  1132.         fillHalfRow(getOne(), vectorAR,    jacobian[0], 0);
  1133.         fillHalfRow(getOne(), vectorARDot, jacobian[0], 3);

  1134.         // de
  1135.         final T factorER3 = pv.divide(twoA);
  1136.         final FieldVector3D<T> vectorER   = new FieldVector3D<>(cosE.multiply(v2).divide(r.multiply(mu)), position,
  1137.                                                                 sinE.divide(sqrtMuA), velocity,
  1138.                                                                 factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorAR);
  1139.         final FieldVector3D<T> vectorERDot = new FieldVector3D<>(sinE.divide(sqrtMuA), position,
  1140.                                                                  cosE.multiply(mu.divide(2.).reciprocal()).multiply(r), velocity,
  1141.                                                                  factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorARDot);
  1142.         fillHalfRow(getOne(), vectorER,    jacobian[1], 0);
  1143.         fillHalfRow(getOne(), vectorERDot, jacobian[1], 3);

  1144.         // dE / dr (Eccentric anomaly)
  1145.         final T coefE = cosE.divide(e.multiply(sqrtMuA));
  1146.         final FieldVector3D<T>  vectorEAnR =
  1147.             new FieldVector3D<>(sinE.negate().multiply(v2).divide(e.multiply(r).multiply(mu)), position, coefE, velocity,
  1148.                                 factorER3.negate().multiply(coefE), vectorAR);

  1149.         // dE / drDot
  1150.         final FieldVector3D<T>  vectorEAnRDot =
  1151.             new FieldVector3D<>(sinE.multiply(-2).multiply(r).divide(e.multiply(mu)), velocity, coefE, position,
  1152.                                 factorER3.negate().multiply(coefE), vectorARDot);

  1153.         // precomputing some more factors
  1154.         final T s1 = sinE.negate().multiply(pz).divide(r).subtract(cosE.multiply(vz).multiply(sqrtAoMu));
  1155.         final T s2 = cosE.negate().multiply(pz).divide(r3);
  1156.         final T s3 = sinE.multiply(vz).divide(sqrtMuA.multiply(-2));
  1157.         final T t1 = sqrtRec.multiply(cosE.multiply(pz).divide(r).subtract(sinE.multiply(vz).multiply(sqrtAoMu)));
  1158.         final T t2 = sqrtRec.multiply(sinE.negate().multiply(pz).divide(r3));
  1159.         final T t3 = sqrtRec.multiply(cosE.subtract(e)).multiply(vz).divide(sqrtMuA.multiply(2));
  1160.         final T t4 = sqrtRec.multiply(e.multiply(sinI).multiply(cosPA).multiply(sqrtRec).subtract(vz.multiply(sqrtAoMu)));
  1161.         final FieldVector3D<T> s = new FieldVector3D<>(cosE.divide(r), this.PLUS_K,
  1162.                                                        s1,       vectorEAnR,
  1163.                                                        s2,       position,
  1164.                                                        s3,       vectorAR);
  1165.         final FieldVector3D<T> sDot = new FieldVector3D<>(sinE.negate().multiply(sqrtAoMu), this.PLUS_K,
  1166.                                                           s1,               vectorEAnRDot,
  1167.                                                           s3,               vectorARDot);
  1168.         final FieldVector3D<T> t =
  1169.             new FieldVector3D<>(sqrtRec.multiply(sinE).divide(r), this.PLUS_K).add(new FieldVector3D<>(t1, vectorEAnR,
  1170.                                                                                                        t2, position,
  1171.                                                                                                        t3, vectorAR,
  1172.                                                                                                        t4, vectorER));
  1173.         final FieldVector3D<T> tDot = new FieldVector3D<>(sqrtRec.multiply(cosE.subtract(e)).multiply(sqrtAoMu), this.PLUS_K,
  1174.                                                           t1,                                                    vectorEAnRDot,
  1175.                                                           t3,                                                    vectorARDot,
  1176.                                                           t4,                                                    vectorERDot);

  1177.         // di
  1178.         final T factorI1 = sinI.negate().multiply(sqrtRec).divide(sqrtMuA);
  1179.         final T i1 =  factorI1;
  1180.         final T i2 =  factorI1.negate().multiply(mz).divide(twoA);
  1181.         final T i3 =  factorI1.multiply(mz).multiply(e).divide(oMe2);
  1182.         final T i4 = cosI.multiply(sinPA);
  1183.         final T i5 = cosI.multiply(cosPA);
  1184.         fillHalfRow(i1, new FieldVector3D<>(vy, vx.negate(), getZero()), i2, vectorAR, i3, vectorER, i4, s, i5, t,
  1185.                     jacobian[2], 0);
  1186.         fillHalfRow(i1, new FieldVector3D<>(py.negate(), px, getZero()), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
  1187.                     jacobian[2], 3);

  1188.         // dpa
  1189.         fillHalfRow(cosPA.divide(sinI), s,    sinPA.negate().divide(sinI), t,    jacobian[3], 0);
  1190.         fillHalfRow(cosPA.divide(sinI), sDot, sinPA.negate().divide(sinI), tDot, jacobian[3], 3);

  1191.         // dRaan
  1192.         final T factorRaanR = (a.multiply(mu).multiply(oMe2).multiply(sinI).multiply(sinI)).reciprocal();
  1193.         fillHalfRow( factorRaanR.negate().multiply(my), new FieldVector3D<>(getZero(), vz, vy.negate()),
  1194.                      factorRaanR.multiply(mx), new FieldVector3D<>(vz.negate(), getZero(),  vx),
  1195.                      jacobian[4], 0);
  1196.         fillHalfRow(factorRaanR.negate().multiply(my), new FieldVector3D<>(getZero(), pz.negate(),  py),
  1197.                      factorRaanR.multiply(mx), new FieldVector3D<>(pz, getZero(), px.negate()),
  1198.                      jacobian[4], 3);

  1199.         // dM
  1200.         fillHalfRow(rOnA, vectorEAnR,    sinE.negate(), vectorER,    jacobian[5], 0);
  1201.         fillHalfRow(rOnA, vectorEAnRDot, sinE.negate(), vectorERDot, jacobian[5], 3);

  1202.         return jacobian;

  1203.     }

  1204.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1205.      * <p>
  1206.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1207.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1208.      * yDot for j=4, zDot for j=5).
  1209.      * </p>
  1210.      * @return 6x6 Jacobian matrix
  1211.      */
  1212.     private T[][] computeJacobianMeanWrtCartesianHyperbolic() {

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

  1214.         // compute various intermediate parameters
  1215.         computePVWithoutA();
  1216.         final FieldVector3D<T> position = partialPV.getPosition();
  1217.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1218.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  1219.         final T r2         = position.getNormSq();
  1220.         final T r          = r2.sqrt();
  1221.         final T r3         = r.multiply(r2);

  1222.         final T x          = position.getX();
  1223.         final T y          = position.getY();
  1224.         final T z          = position.getZ();
  1225.         final T vx         = velocity.getX();
  1226.         final T vy         = velocity.getY();
  1227.         final T vz         = velocity.getZ();
  1228.         final T mx         = momentum.getX();
  1229.         final T my         = momentum.getY();
  1230.         final T mz         = momentum.getZ();

  1231.         final T mu         = getMu();
  1232.         final T absA       = a.negate();
  1233.         final T sqrtMuA    = absA.multiply(mu).sqrt();
  1234.         final T a2         = a.square();
  1235.         final T rOa        = r.divide(absA);

  1236.         final FieldSinCos<T> scI = FastMath.sinCos(i);
  1237.         final T cosI       = scI.cos();
  1238.         final T sinI       = scI.sin();

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

  1240.         // da
  1241.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(-2).divide(r3), position);
  1242.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(-2).divide(mu));
  1243.         fillHalfRow(getOne().negate(), vectorAR,    jacobian[0], 0);
  1244.         fillHalfRow(getOne().negate(), vectorARDot, jacobian[0], 3);

  1245.         // differentials of the momentum
  1246.         final T m      = momentum.getNorm();
  1247.         final T oOm    = m.reciprocal();
  1248.         final FieldVector3D<T> dcXP = new FieldVector3D<>(getZero(), vz, vy.negate());
  1249.         final FieldVector3D<T> dcYP = new FieldVector3D<>(vz.negate(),   getZero(),  vx);
  1250.         final FieldVector3D<T> dcZP = new FieldVector3D<>( vy, vx.negate(),   getZero());
  1251.         final FieldVector3D<T> dcXV = new FieldVector3D<>(  getZero(),  z.negate(),   y);
  1252.         final FieldVector3D<T> dcYV = new FieldVector3D<>(  z,   getZero(),  x.negate());
  1253.         final FieldVector3D<T> dcZV = new FieldVector3D<>( y.negate(),   x,   getZero());
  1254.         final FieldVector3D<T> dCP  = new FieldVector3D<>(mx.multiply(oOm), dcXP, my.multiply(oOm), dcYP, mz.multiply(oOm), dcZP);
  1255.         final FieldVector3D<T> dCV  = new FieldVector3D<>(mx.multiply(oOm), dcXV, my.multiply(oOm), dcYV, mz.multiply(oOm), dcZV);

  1256.         // dp
  1257.         final T mOMu   = m.divide(mu);
  1258.         final FieldVector3D<T> dpP  = new FieldVector3D<>(mOMu.multiply(2), dCP);
  1259.         final FieldVector3D<T> dpV  = new FieldVector3D<>(mOMu.multiply(2), dCV);

  1260.         // de
  1261.         final T p      = m.multiply(mOMu);
  1262.         final T moO2ae = absA.multiply(2).multiply(e).reciprocal();
  1263.         final T m2OaMu = p.negate().divide(absA);
  1264.         fillHalfRow(moO2ae, dpP, m2OaMu.multiply(moO2ae), vectorAR,    jacobian[1], 0);
  1265.         fillHalfRow(moO2ae, dpV, m2OaMu.multiply(moO2ae), vectorARDot, jacobian[1], 3);

  1266.         // di
  1267.         final T cI1 = m.multiply(sinI).reciprocal();
  1268.         final T cI2 = cosI.multiply(cI1);
  1269.         fillHalfRow(cI2, dCP, cI1.negate(), dcZP, jacobian[2], 0);
  1270.         fillHalfRow(cI2, dCV, cI1.negate(), dcZV, jacobian[2], 3);


  1271.         // dPA
  1272.         final T cP1     =  y.multiply(oOm);
  1273.         final T cP2     =  x.negate().multiply(oOm);
  1274.         final T cP3     =  mx.multiply(cP1).add(my.multiply(cP2)).negate();
  1275.         final T cP4     =  cP3.multiply(oOm);
  1276.         final T cP5     =  r2.multiply(sinI).multiply(sinI).negate().reciprocal();
  1277.         final T cP6     = z.multiply(cP5);
  1278.         final T cP7     = cP3.multiply(cP5);
  1279.         final FieldVector3D<T> dacP  = new FieldVector3D<>(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new FieldVector3D<>(my.negate(), mx, getZero()));
  1280.         final FieldVector3D<T> dacV  = new FieldVector3D<>(cP1, dcXV, cP2, dcYV, cP4, dCV);
  1281.         final FieldVector3D<T> dpoP  = new FieldVector3D<>(cP6, dacP, cP7, this.PLUS_K);
  1282.         final FieldVector3D<T> dpoV  = new FieldVector3D<>(cP6, dacV);

  1283.         final T re2     = r2.multiply(e.square());
  1284.         final T recOre2 = p.subtract(r).divide(re2);
  1285.         final T resOre2 = pv.multiply(mOMu).divide(re2);
  1286.         final FieldVector3D<T> dreP  = new FieldVector3D<>(mOMu, velocity, pv.divide(mu), dCP);
  1287.         final FieldVector3D<T> dreV  = new FieldVector3D<>(mOMu, position, pv.divide(mu), dCV);
  1288.         final FieldVector3D<T> davP  = new FieldVector3D<>(resOre2.negate(), dpP, recOre2, dreP, resOre2.divide(r), position);
  1289.         final FieldVector3D<T> davV  = new FieldVector3D<>(resOre2.negate(), dpV, recOre2, dreV);
  1290.         fillHalfRow(getOne(), dpoP, getOne().negate(), davP, jacobian[3], 0);
  1291.         fillHalfRow(getOne(), dpoV, getOne().negate(), davV, jacobian[3], 3);

  1292.         // dRAAN
  1293.         final T cO0 = cI1.square();
  1294.         final T cO1 =  mx.multiply(cO0);
  1295.         final T cO2 =  my.negate().multiply(cO0);
  1296.         fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
  1297.         fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);

  1298.         // dM
  1299.         final T s2a    = pv.divide(absA.multiply(2));
  1300.         final T oObux  = m.square().add(absA.multiply(mu)).sqrt().reciprocal();
  1301.         final T scasbu = pv.multiply(oObux);
  1302.         final FieldVector3D<T> dauP = new FieldVector3D<>(sqrtMuA.reciprocal(), velocity, s2a.negate().divide(sqrtMuA), vectorAR);
  1303.         final FieldVector3D<T> dauV = new FieldVector3D<>(sqrtMuA.reciprocal(), position, s2a.negate().divide(sqrtMuA), vectorARDot);
  1304.         final FieldVector3D<T> dbuP = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorAR,    m.multiply(oObux), dCP);
  1305.         final FieldVector3D<T> dbuV = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorARDot, m.multiply(oObux), dCV);
  1306.         final FieldVector3D<T> dcuP = new FieldVector3D<>(oObux, velocity, scasbu.negate().multiply(oObux), dbuP);
  1307.         final FieldVector3D<T> dcuV = new FieldVector3D<>(oObux, position, scasbu.negate().multiply(oObux), dbuV);
  1308.         fillHalfRow(getOne(), dauP, e.negate().divide(rOa.add(1)), dcuP, jacobian[5], 0);
  1309.         fillHalfRow(getOne(), dauV, e.negate().divide(rOa.add(1)), dcuV, jacobian[5], 3);

  1310.         return jacobian;

  1311.     }

  1312.     /** {@inheritDoc} */
  1313.     @Override
  1314.     protected T[][] computeJacobianEccentricWrtCartesian() {
  1315.         if (isElliptical()) {
  1316.             return computeJacobianEccentricWrtCartesianElliptical();
  1317.         } else {
  1318.             return computeJacobianEccentricWrtCartesianHyperbolic();
  1319.         }
  1320.     }

  1321.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1322.      * <p>
  1323.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1324.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1325.      * yDot for j=4, zDot for j=5).
  1326.      * </p>
  1327.      * @return 6x6 Jacobian matrix
  1328.      */
  1329.     private T[][] computeJacobianEccentricWrtCartesianElliptical() {

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

  1332.         // Differentiating the Kepler equation M = E - e sin E leads to:
  1333.         // dM = (1 - e cos E) dE - sin E de
  1334.         // which is inverted and rewritten as:
  1335.         // dE = a/r dM + sin E a/r de
  1336.         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1337.         final T aOr              = e.negate().multiply(scE.cos()).add(1).reciprocal();

  1338.         // update anomaly row
  1339.         final T[] eRow           = jacobian[1];
  1340.         final T[] anomalyRow     = jacobian[5];
  1341.         for (int j = 0; j < anomalyRow.length; ++j) {
  1342.             anomalyRow[j] = aOr.multiply(anomalyRow[j].add(scE.sin().multiply(eRow[j])));
  1343.         }

  1344.         return jacobian;

  1345.     }

  1346.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1347.      * <p>
  1348.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1349.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1350.      * yDot for j=4, zDot for j=5).
  1351.      * </p>
  1352.      * @return 6x6 Jacobian matrix
  1353.      */
  1354.     private T[][] computeJacobianEccentricWrtCartesianHyperbolic() {

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

  1357.         // Differentiating the Kepler equation M = e sinh H - H leads to:
  1358.         // dM = (e cosh H - 1) dH + sinh H de
  1359.         // which is inverted and rewritten as:
  1360.         // dH = 1 / (e cosh H - 1) dM - sinh H / (e cosh H - 1) de
  1361.         final T H      = getEccentricAnomaly();
  1362.         final T coshH  = H.cosh();
  1363.         final T sinhH  = H.sinh();
  1364.         final T absaOr = e.multiply(coshH).subtract(1).reciprocal();
  1365.         // update anomaly row
  1366.         final T[] eRow       = jacobian[1];
  1367.         final T[] anomalyRow = jacobian[5];

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

  1370.         }

  1371.         return jacobian;

  1372.     }

  1373.     /** {@inheritDoc} */
  1374.     @Override
  1375.     protected T[][] computeJacobianTrueWrtCartesian() {
  1376.         if (isElliptical()) {
  1377.             return computeJacobianTrueWrtCartesianElliptical();
  1378.         } else {
  1379.             return computeJacobianTrueWrtCartesianHyperbolic();
  1380.         }
  1381.     }

  1382.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1383.      * <p>
  1384.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1385.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1386.      * yDot for j=4, zDot for j=5).
  1387.      * </p>
  1388.      * @return 6x6 Jacobian matrix
  1389.      */
  1390.     private T[][] computeJacobianTrueWrtCartesianElliptical() {

  1391.         // start by computing the Jacobian with eccentric angle
  1392.         final T[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();
  1393.         // Differentiating the eccentric anomaly equation sin E = sqrt(1-e^2) sin v / (1 + e cos v)
  1394.         // and using cos E = (e + cos v) / (1 + e cos v) to get rid of cos E leads to:
  1395.         // dE = [sqrt (1 - e^2) / (1 + e cos v)] dv - [sin E / (1 - e^2)] de
  1396.         // which is inverted and rewritten as:
  1397.         // dv = sqrt (1 - e^2) a/r dE + [sin E / sqrt (1 - e^2)] a/r de
  1398.         final T e2               = e.square();
  1399.         final T oMe2             = e2.negate().add(1);
  1400.         final T epsilon          = oMe2.sqrt();
  1401.         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1402.         final T aOr              = e.multiply(scE.cos()).negate().add(1).reciprocal();
  1403.         final T aFactor          = epsilon.multiply(aOr);
  1404.         final T eFactor          = scE.sin().multiply(aOr).divide(epsilon);

  1405.         // update anomaly row
  1406.         final T[] eRow           = jacobian[1];
  1407.         final T[] anomalyRow     = jacobian[5];
  1408.         for (int j = 0; j < anomalyRow.length; ++j) {
  1409.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).add(eFactor.multiply(eRow[j]));
  1410.         }
  1411.         return jacobian;

  1412.     }

  1413.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1414.      * <p>
  1415.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1416.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1417.      * yDot for j=4, zDot for j=5).
  1418.      * </p>
  1419.      * @return 6x6 Jacobian matrix
  1420.      */
  1421.     private T[][] computeJacobianTrueWrtCartesianHyperbolic() {

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

  1424.         // Differentiating the eccentric anomaly equation sinh H = sqrt(e^2-1) sin v / (1 + e cos v)
  1425.         // and using cosh H = (e + cos v) / (1 + e cos v) to get rid of cosh H leads to:
  1426.         // dH = [sqrt (e^2 - 1) / (1 + e cos v)] dv + [sinh H / (e^2 - 1)] de
  1427.         // which is inverted and rewritten as:
  1428.         // dv = sqrt (1 - e^2) a/r dH - [sinh H / sqrt (e^2 - 1)] a/r de
  1429.         final T e2       = e.square();
  1430.         final T e2Mo     = e2.subtract(1);
  1431.         final T epsilon  = e2Mo.sqrt();
  1432.         final T H        = getEccentricAnomaly();
  1433.         final T coshH    = H.cosh();
  1434.         final T sinhH    = H.sinh();
  1435.         final T aOr      = e.multiply(coshH).subtract(1).reciprocal();
  1436.         final T aFactor  = epsilon.multiply(aOr);
  1437.         final T eFactor  = sinhH.multiply(aOr).divide(epsilon);

  1438.         // update anomaly row
  1439.         final T[] eRow           = jacobian[1];
  1440.         final T[] anomalyRow     = jacobian[5];
  1441.         for (int j = 0; j < anomalyRow.length; ++j) {
  1442.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).subtract(eFactor.multiply(eRow[j]));
  1443.         }

  1444.         return jacobian;

  1445.     }

  1446.     /** {@inheritDoc} */
  1447.     @Override
  1448.     public void addKeplerContribution(final PositionAngleType type, final T gm,
  1449.                                       final T[] pDot) {
  1450.         final T oMe2;
  1451.         final T ksi;
  1452.         final T absA = a.abs();
  1453.         final T n    = absA.reciprocal().multiply(gm).sqrt().divide(absA);
  1454.         switch (type) {
  1455.             case MEAN :
  1456.                 pDot[5] = pDot[5].add(n);
  1457.                 break;
  1458.             case ECCENTRIC :
  1459.                 oMe2 = e.square().negate().add(1).abs();
  1460.                 ksi  = e.multiply(getTrueAnomaly().cos()).add(1);
  1461.                 pDot[5] = pDot[5].add( n.multiply(ksi).divide(oMe2));
  1462.                 break;
  1463.             case TRUE :
  1464.                 oMe2 = e.square().negate().add(1).abs();
  1465.                 ksi  = e.multiply(getTrueAnomaly().cos()).add(1);
  1466.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1467.                 break;
  1468.             default :
  1469.                 throw new OrekitInternalError(null);
  1470.         }
  1471.     }

  1472.     /**  Returns a string representation of this Keplerian parameters object.
  1473.      * @return a string representation of this object
  1474.      */
  1475.     public String toString() {
  1476.         return new StringBuilder().append("Keplerian parameters: ").append('{').
  1477.                                   append("a: ").append(a.getReal()).
  1478.                                   append("; e: ").append(e.getReal()).
  1479.                                   append("; i: ").append(FastMath.toDegrees(i.getReal())).
  1480.                                   append("; pa: ").append(FastMath.toDegrees(pa.getReal())).
  1481.                                   append("; raan: ").append(FastMath.toDegrees(raan.getReal())).
  1482.                                   append("; v: ").append(FastMath.toDegrees(getTrueAnomaly().getReal())).
  1483.                                   append(";}").toString();
  1484.     }

  1485.     /** {@inheritDoc} */
  1486.     @Override
  1487.     public PositionAngleType getCachedPositionAngleType() {
  1488.         return cachedPositionAngleType;
  1489.     }

  1490.     /** {@inheritDoc} */
  1491.     @Override
  1492.     public boolean hasRates() {
  1493.         return hasDerivatives();
  1494.     }

  1495.     /** {@inheritDoc} */
  1496.     @Override
  1497.     public FieldKeplerianOrbit<T> removeRates() {
  1498.         return new FieldKeplerianOrbit<>(getA(), getE(), getI(), getPerigeeArgument(), getRightAscensionOfAscendingNode(),
  1499.                 cachedAnomaly, cachedPositionAngleType, getFrame(), getDate(), getMu());
  1500.     }

  1501.     /** Check if the given parameter is within an acceptable range.
  1502.      * The bounds are inclusive: an exception is raised when either of those conditions are met:
  1503.      * <ul>
  1504.      *     <li>The parameter is strictly greater than upperBound</li>
  1505.      *     <li>The parameter is strictly lower than lowerBound</li>
  1506.      * </ul>
  1507.      * <p>
  1508.      * In either of these cases, an OrekitException is raised.
  1509.      * </p>
  1510.      * @param parameterName name of the parameter
  1511.      * @param parameter value of the parameter
  1512.      * @param lowerBound lower bound of the acceptable range (inclusive)
  1513.      * @param upperBound upper bound of the acceptable range (inclusive)
  1514.      */
  1515.     private void checkParameterRangeInclusive(final String parameterName, final double parameter,
  1516.                                               final double lowerBound, final double upperBound) {
  1517.         if (parameter < lowerBound || parameter > upperBound) {
  1518.             throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, parameterName,
  1519.                                       parameter, lowerBound, upperBound);
  1520.         }
  1521.     }

  1522.     /** {@inheritDoc} */
  1523.     @Override
  1524.     public KeplerianOrbit toOrbit() {
  1525.         final double cachedPositionAngle = cachedAnomaly.getReal();
  1526.         if (hasDerivatives()) {
  1527.             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
  1528.                                       pa.getReal(), raan.getReal(), cachedPositionAngle,
  1529.                                       aDot.getReal(), eDot.getReal(), iDot.getReal(),
  1530.                                       paDot.getReal(), raanDot.getReal(),
  1531.                                       cachedAnomalyDot.getReal(),
  1532.                                       cachedPositionAngleType, cachedPositionAngleType,
  1533.                                       getFrame(), getDate().toAbsoluteDate(), getMu().getReal());
  1534.         } else {
  1535.             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
  1536.                                       pa.getReal(), raan.getReal(), cachedPositionAngle,
  1537.                                       cachedPositionAngleType, cachedPositionAngleType,
  1538.                                       getFrame(), getDate().toAbsoluteDate(), getMu().getReal());
  1539.         }
  1540.     }


  1541. }