FieldKeplerianOrbit.java

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


  20. import java.lang.reflect.Array;

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


  40. /**
  41.  * This class handles traditional Keplerian orbital parameters.

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

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

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

  87.     /** Semi-major axis (m). */
  88.     private final T a;

  90.     /** Eccentricity. */
  91.     private final T e;

  93.     /** Inclination (rad). */
  94.     private final T i;

  96.     /** Perigee Argument (rad). */
  97.     private final T pa;

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

  102.     /** Cached anomaly (rad). */
  103.     private final T cachedAnomaly;

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

  108.     /** Eccentricity derivative. */
  109.     private final T eDot;

  111.     /** Inclination derivative (rad/s). */
  112.     private final T iDot;

  114.     /** Perigee Argument derivative (rad/s). */
  115.     private final T paDot;

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

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

  123.     /** Cached type of position angle. */
  124.     private final PositionAngleType cachedPositionAngleType;

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

  129.     /** PThird Canonical Vector. */
  130.     private final FieldVector3D<T> PLUS_K;

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

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

  186.     /** Creates a new instance.
  187.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  188.      * @param e eccentricity (positive or equal to 0)
  189.      * @param i inclination (rad)
  190.      * @param pa perigee argument (ω, rad)
  191.      * @param raan right ascension of ascending node (Ω, rad)
  192.      * @param anomaly mean, eccentric or true anomaly (rad)
  193.      * @param aDot  semi-major axis derivative, null if unknown (m/s)
  194.      * @param eDot eccentricity derivative, null if unknown
  195.      * @param iDot inclination derivative, null if unknown (rad/s)
  196.      * @param paDot perigee argument derivative, null if unknown (rad/s)
  197.      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
  198.      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
  199.      * @param type type of anomaly
  200.      * @param cachedPositionAngleType type of cached anomaly
  201.      * @param frame the frame in which the parameters are defined
  202.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  203.      * @param date date of the orbital parameters
  204.      * @param mu central attraction coefficient (m³/s²)
  205.      * @exception IllegalArgumentException if frame is not a {@link
  206.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  207.      * or v is out of range for hyperbolic orbits
  208.      * @since 12.1
  209.      */
  210.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  211.                                final T pa, final T raan, final T anomaly,
  212.                                final T aDot, final T eDot, final T iDot,
  213.                                final T paDot, final T raanDot, final T anomalyDot,
  214.                                final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
  215.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  216.         throws IllegalArgumentException {
  217.         super(frame, date, mu);
  218.         this.cachedPositionAngleType = cachedPositionAngleType;

  220.         if (a.multiply(e.negate().add(1)).getReal() < 0) {
  221.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a.getReal(), e.getReal());
  222.         }

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

  227.         this.a       =    a;
  228.         this.aDot    =    aDot;
  229.         this.e       =    e;
  230.         this.eDot    =    eDot;
  231.         this.i       =    i;
  232.         this.iDot    =    iDot;
  233.         this.pa      =    pa;
  234.         this.paDot   =    paDot;
  235.         this.raan    =    raan;
  236.         this.raanDot =    raanDot;

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

  240.         final FieldUnivariateDerivative1<T> cachedAnomalyUD = initializeCachedAnomaly(anomaly, anomalyDot, type);
  241.         this.cachedAnomaly = cachedAnomalyUD.getValue();
  242.         this.cachedAnomalyDot = cachedAnomalyUD.getFirstDerivative();

  244.         // check true anomaly range
  245.         if (!isElliptical()) {
  246.             final T trueAnomaly = getTrueAnomaly();
  247.             if (e.multiply(trueAnomaly.cos()).add(1).getReal() <= 0) {
  248.                 final double vMax = e.reciprocal().negate().acos().getReal();
  249.                 throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
  250.                         trueAnomaly.getReal(), e.getReal(), -vMax, vMax);
  251.             }
  252.         }

  254.         this.partialPV = null;

  256.     }

  258.     /** Creates a new instance.
  259.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  260.      * @param e eccentricity (positive or equal to 0)
  261.      * @param i inclination (rad)
  262.      * @param pa perigee argument (ω, rad)
  263.      * @param raan right ascension of ascending node (Ω, rad)
  264.      * @param anomaly mean, eccentric or true anomaly (rad)
  265.      * @param aDot  semi-major axis derivative, null if unknown (m/s)
  266.      * @param eDot eccentricity derivative, null if unknown
  267.      * @param iDot inclination derivative, null if unknown (rad/s)
  268.      * @param paDot perigee argument derivative, null if unknown (rad/s)
  269.      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
  270.      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
  271.      * @param type type of anomaly
  272.      * @param frame the frame in which the parameters are defined
  273.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  274.      * @param date date of the orbital parameters
  275.      * @param mu central attraction coefficient (m³/s²)
  276.      * @exception IllegalArgumentException if frame is not a {@link
  277.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  278.      * or v is out of range for hyperbolic orbits
  279.      */
  280.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  281.                                final T pa, final T raan, final T anomaly,
  282.                                final T aDot, final T eDot, final T iDot,
  283.                                final T paDot, final T raanDot, final T anomalyDot,
  284.                                final PositionAngleType type,
  285.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  286.             throws IllegalArgumentException {
  287.         this(a, e, i, pa, raan, anomaly, aDot, eDot, iDot, paDot, raanDot, anomalyDot,
  288.                 type, type, frame, date, mu);
  289.     }

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

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

  331.         super(pvCoordinates, frame, mu);

  333.         // third canonical vector
  334.         this.PLUS_K = FieldVector3D.getPlusK(getOne().getField());

  336.         // compute inclination
  337.         final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
  338.         final T m2 = momentum.getNormSq();

  340.         i = FieldVector3D.angle(momentum, PLUS_K);
  341.         // compute right ascension of ascending node
  342.         raan = FieldVector3D.crossProduct(PLUS_K, momentum).getAlpha();
  343.         // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic)
  344.         final FieldVector3D<T> pvP     = pvCoordinates.getPosition();
  345.         final FieldVector3D<T> pvV     = pvCoordinates.getVelocity();
  346.         final FieldVector3D<T> pvA     = pvCoordinates.getAcceleration();

  348.         final T   r2      = pvP.getNormSq();
  349.         final T   r       = r2.sqrt();
  350.         final T   V2      = pvV.getNormSq();
  351.         final T   rV2OnMu = r.multiply(V2).divide(mu);

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

  357.         // compute true anomaly
  358.         if (isElliptical()) {
  359.             // elliptic or circular orbit
  360.             final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  361.             final T eCE = rV2OnMu.subtract(1);
  362.             e = (eSE.multiply(eSE).add(eCE.multiply(eCE))).sqrt();
  363.             this.cachedPositionAngleType = PositionAngleType.ECCENTRIC;
  364.             cachedAnomaly = eSE.atan2(eCE);
  365.         } else {
  366.             // hyperbolic orbit
  367.             final T eSH = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  368.             final T eCH = rV2OnMu.subtract(1);
  369.             e = (m2.negate().divide(muA).add(1)).sqrt();
  370.             this.cachedPositionAngleType = PositionAngleType.TRUE;
  371.             cachedAnomaly = FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, (eCH.add(eSH)).divide(eCH.subtract(eSH)).log().divide(2));
  372.         }

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

  377.         // compute perigee argument
  378.         final FieldVector3D<T> node = new FieldVector3D<>(raan, getZero());
  379.         final T px = FieldVector3D.dotProduct(pvP, node);
  380.         final T py = FieldVector3D.dotProduct(pvP, FieldVector3D.crossProduct(momentum, node)).divide(m2.sqrt());
  381.         pa = py.atan2(px).subtract(getTrueAnomaly());

  383.         partialPV = pvCoordinates;

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

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

  391.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  392.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  393.             final T   aX                       = nonKeplerianAcceleration.getX();
  394.             final T   aY                       = nonKeplerianAcceleration.getY();
  395.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  396.             aDot    = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  397.             eDot    = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  398.             iDot    = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  399.             paDot   = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  400.             raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  402.             // in order to compute true anomaly derivative, we must compute
  403.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  404.             final T MDot = getKeplerianMeanMotion().
  405.                            add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  406.             final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  407.             final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(getMeanAnomaly(), MDot);
  408.             if (cachedPositionAngleType == PositionAngleType.ECCENTRIC) {
  409.                 final FieldUnivariateDerivative1<T> EUD = (a.getReal() < 0) ?
  410.                          FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD, MUD) :
  411.                          FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD, MUD);
  412.                 cachedAnomalyDot = EUD.getFirstDerivative();
  413.             } else { // TRUE
  414.                 final FieldUnivariateDerivative1<T> vUD = (a.getReal() < 0) ?
  415.                          FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, MUD) :
  416.                          FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, MUD);
  417.                 cachedAnomalyDot = vUD.getFirstDerivative();
  418.             }

  420.         } else {
  421.             // acceleration is either almost zero or NaN,
  422.             // we assume acceleration was not known
  423.             // we don't set up derivatives
  424.             aDot    = getZero();
  425.             eDot    = getZero();
  426.             iDot    = getZero();
  427.             paDot   = getZero();
  428.             raanDot = getZero();
  429.             cachedAnomalyDot    = computeKeplerianAnomalyDot(cachedPositionAngleType, a, e, mu, cachedAnomaly, cachedPositionAngleType);
  430.         }

  432.     }

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

  455.     /** Constructor from any kind of orbital parameters.
  456.      * @param op orbital parameters to copy
  457.      */
  458.     public FieldKeplerianOrbit(final FieldOrbit<T> op) {
  459.         this(op.getPVCoordinates(), op.getFrame(), op.getMu());
  460.     }

  462.     /** Constructor from Field and KeplerianOrbit.
  463.      * <p>Build a FieldKeplerianOrbit from non-Field KeplerianOrbit.</p>
  464.      * @param field CalculusField to base object on
  465.      * @param op non-field orbit with only "constant" terms
  466.      * @since 12.0
  467.      */
  468.     public FieldKeplerianOrbit(final Field<T> field, final KeplerianOrbit op) {
  469.         this(field.getZero().newInstance(op.getA()), field.getZero().newInstance(op.getE()), field.getZero().newInstance(op.getI()),
  470.                 field.getZero().newInstance(op.getPerigeeArgument()), field.getZero().newInstance(op.getRightAscensionOfAscendingNode()),
  471.                 field.getZero().newInstance(op.getAnomaly(op.getCachedPositionAngleType())),
  472.                 field.getZero().newInstance(op.getADot()),
  473.                 field.getZero().newInstance(op.getEDot()),
  474.                 field.getZero().newInstance(op.getIDot()),
  475.                 field.getZero().newInstance(op.getPerigeeArgumentDot()),
  476.                 field.getZero().newInstance(op.getRightAscensionOfAscendingNodeDot()),
  477.                 field.getZero().newInstance(op.getAnomalyDot(op.getCachedPositionAngleType())),
  478.                 op.getCachedPositionAngleType(), op.getFrame(),
  479.                 new FieldAbsoluteDate<>(field, op.getDate()), field.getZero().newInstance(op.getMu()));
  480.     }

  482.     /** Constructor from Field and Orbit.
  483.      * <p>Build a FieldKeplerianOrbit from any non-Field Orbit.</p>
  484.      * @param field CalculusField to base object on
  485.      * @param op non-field orbit with only "constant" terms
  486.      * @since 12.0
  487.      */
  488.     public FieldKeplerianOrbit(final Field<T> field, final Orbit op) {
  489.         this(field, (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(op));
  490.     }

  492.     /** {@inheritDoc} */
  493.     @Override
  494.     public OrbitType getType() {
  495.         return OrbitType.KEPLERIAN;
  496.     }

  498.     /** {@inheritDoc} */
  499.     @Override
  500.     public T getA() {
  501.         return a;
  502.     }

  504.     /** {@inheritDoc} */
  505.     @Override
  506.     public T getADot() {
  507.         return aDot;
  508.     }

  510.     /** {@inheritDoc} */
  511.     @Override
  512.     public T getE() {
  513.         return e;
  514.     }

  516.     /** {@inheritDoc} */
  517.     @Override
  518.     public T getEDot() {
  519.         return eDot;
  520.     }

  522.     /** {@inheritDoc} */
  523.     @Override
  524.     public T getI() {
  525.         return i;
  526.     }

  528.     /** {@inheritDoc} */
  529.     @Override
  530.     public T getIDot() {
  531.         return iDot;
  532.     }

  534.     /** Get the perigee argument.
  535.      * @return perigee argument (rad)
  536.      */
  537.     public T getPerigeeArgument() {
  538.         return pa;
  539.     }

  541.     /** Get the perigee argument derivative.
  542.      * @return perigee argument derivative (rad/s)
  543.      */
  544.     public T getPerigeeArgumentDot() {
  545.         return paDot;
  546.     }

  548.     /** Get the right ascension of the ascending node.
  549.      * @return right ascension of the ascending node (rad)
  550.      */
  551.     public T getRightAscensionOfAscendingNode() {
  552.         return raan;
  553.     }

  555.     /** Get the right ascension of the ascending node derivative.
  556.      * @return right ascension of the ascending node derivative (rad/s)
  557.      */
  558.     public T getRightAscensionOfAscendingNodeDot() {
  559.         return raanDot;
  560.     }

  562.     /** Get the true anomaly.
  563.      * @return true anomaly (rad)
  564.      */
  565.     public T getTrueAnomaly() {
  566.         switch (cachedPositionAngleType) {
  567.             case MEAN: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(e, cachedAnomaly) :
  568.                     FieldKeplerianAnomalyUtility.ellipticMeanToTrue(e, cachedAnomaly);

  570.             case TRUE: return cachedAnomaly;

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

  575.             default: throw new OrekitInternalError(null);
  576.         }
  577.     }

  579.     /** Get the true anomaly derivative.
  580.      * @return true anomaly derivative (rad/s)
  581.      */
  582.     public T getTrueAnomalyDot() {
  583.         switch (cachedPositionAngleType) {
  584.             case MEAN:
  585.                 final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  586.                 final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  587.                 final FieldUnivariateDerivative1<T> vUD = (a.getReal() < 0) ?
  588.                          FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, MUD) :
  589.                          FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, MUD);
  590.                 return vUD.getFirstDerivative();

  592.             case TRUE:
  593.                 return cachedAnomalyDot;

  595.             case ECCENTRIC:
  596.                 final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
  597.                 final FieldUnivariateDerivative1<T> EUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  598.                 final FieldUnivariateDerivative1<T> vUD2 = (a.getReal() < 0) ?
  599.                          FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(eUD2, EUD) :
  600.                          FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(eUD2, EUD);
  601.                 return vUD2.getFirstDerivative();

  603.             default:
  604.                 throw new OrekitInternalError(null);
  605.         }
  606.     }

  608.     /** Get the eccentric anomaly.
  609.      * @return eccentric anomaly (rad)
  610.      */
  611.     public T getEccentricAnomaly() {
  612.         switch (cachedPositionAngleType) {
  613.             case MEAN:
  614.                 return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, cachedAnomaly) :
  615.                     FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, cachedAnomaly);

  617.             case ECCENTRIC:
  618.                 return cachedAnomaly;

  620.             case TRUE:
  621.                 return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, cachedAnomaly) :
  622.                     FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, cachedAnomaly);

  624.             default:
  625.                 throw new OrekitInternalError(null);
  626.         }
  627.     }

  629.     /** Get the eccentric anomaly derivative.
  630.      * @return eccentric anomaly derivative (rad/s)
  631.      */
  632.     public T getEccentricAnomalyDot() {
  633.         switch (cachedPositionAngleType) {
  634.             case ECCENTRIC:
  635.                 return cachedAnomalyDot;

  637.             case TRUE:
  638.                 final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  639.                 final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  640.                 final FieldUnivariateDerivative1<T> EUD = (a.getReal() < 0) ?
  641.                          FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(eUD, vUD) :
  642.                          FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(eUD, vUD);
  643.                 return EUD.getFirstDerivative();

  645.             case MEAN:
  646.                 final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
  647.                 final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  648.                 final FieldUnivariateDerivative1<T> EUD2 = (a.getReal() < 0) ?
  649.                          FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD2, MUD) :
  650.                          FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD2, MUD);
  651.                 return EUD2.getFirstDerivative();

  653.             default:
  654.                 throw new OrekitInternalError(null);
  655.         }
  656.     }

  658.     /** Get the mean anomaly.
  659.      * @return mean anomaly (rad)
  660.      */
  661.     public T getMeanAnomaly() {
  662.         switch (cachedPositionAngleType) {
  663.             case ECCENTRIC: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(e, cachedAnomaly) :
  664.                     FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, cachedAnomaly);

  666.             case MEAN: return cachedAnomaly;

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

  671.             default: throw new OrekitInternalError(null);
  672.         }
  673.     }

  675.     /** Get the mean anomaly derivative.

  677.      * @return mean anomaly derivative (rad/s)
  678.      */
  679.     public T getMeanAnomalyDot() {
  680.         switch (cachedPositionAngleType) {
  681.             case MEAN:
  682.                 return cachedAnomalyDot;

  684.             case ECCENTRIC:
  685.                 final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  686.                 final FieldUnivariateDerivative1<T> EUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  687.                 final FieldUnivariateDerivative1<T> MUD = (a.getReal() < 0) ?
  688.                          FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(eUD, EUD) :
  689.                          FieldKeplerianAnomalyUtility.ellipticEccentricToMean(eUD, EUD);
  690.                 return MUD.getFirstDerivative();

  692.             case TRUE:
  693.                 final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
  694.                 final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
  695.                 final FieldUnivariateDerivative1<T> MUD2 = (a.getReal() < 0) ?
  696.                          FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(eUD2, vUD) :
  697.                          FieldKeplerianAnomalyUtility.ellipticTrueToMean(eUD2, vUD);
  698.                 return MUD2.getFirstDerivative();

  700.             default:
  701.                 throw new OrekitInternalError(null);
  702.         }
  703.     }

  705.     /** Get the anomaly.
  706.      * @param type type of the angle
  707.      * @return anomaly (rad)
  708.      */
  709.     public T getAnomaly(final PositionAngleType type) {
  710.         return (type == PositionAngleType.MEAN) ? getMeanAnomaly() :
  711.                                               ((type == PositionAngleType.ECCENTRIC) ? getEccentricAnomaly() :
  712.                                                                                    getTrueAnomaly());
  713.     }

  715.     /** Get the anomaly derivative.
  716.      * @param type type of the angle
  717.      * @return anomaly derivative (rad/s)
  718.      */
  719.     public T getAnomalyDot(final PositionAngleType type) {
  720.         return (type == PositionAngleType.MEAN) ? getMeanAnomalyDot() :
  721.                                               ((type == PositionAngleType.ECCENTRIC) ? getEccentricAnomalyDot() :
  722.                                                                                    getTrueAnomalyDot());
  723.     }

  725.     /** {@inheritDoc} */
  726.     @Override
  727.     public boolean hasNonKeplerianAcceleration() {
  728.         return aDot.getReal() != 0. || eDot.getReal() != 0 || iDot.getReal() != 0. || raanDot.getReal() != 0. || paDot.getReal() != 0. ||
  729.                 FastMath.abs(cachedAnomalyDot.subtract(computeKeplerianAnomalyDot(cachedPositionAngleType, a, e, getMu(), cachedAnomaly, cachedPositionAngleType)).getReal()) > TOLERANCE_POSITION_ANGLE_RATE;
  730.     }

  732.     /** {@inheritDoc} */
  733.     @Override
  734.     public T getEquinoctialEx() {
  735.         return e.multiply(pa.add(raan).cos());
  736.     }

  738.     /** {@inheritDoc} */
  739.     @Override
  740.     public T getEquinoctialExDot() {
  741.         if (!hasNonKeplerianRates()) {
  742.             return getZero();
  743.         }
  744.         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
  745.         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
  746.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  747.         return eUD.multiply(paUD.add(raanUD).cos()).getFirstDerivative();

  749.     }

  751.     /** {@inheritDoc} */
  752.     @Override
  753.     public T getEquinoctialEy() {
  754.         return e.multiply((pa.add(raan)).sin());
  755.     }

  757.     /** {@inheritDoc} */
  758.     @Override
  759.     public T getEquinoctialEyDot() {
  760.         if (!hasNonKeplerianRates()) {
  761.             return getZero();
  762.         }
  763.         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
  764.         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
  765.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  766.         return eUD.multiply(paUD.add(raanUD).sin()).getFirstDerivative();

  768.     }

  770.     /** {@inheritDoc} */
  771.     @Override
  772.     public T getHx() {
  773.         // Check for equatorial retrograde orbit
  774.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  775.             return getZero().add(Double.NaN);
  776.         }
  777.         return raan.cos().multiply(i.divide(2).tan());
  778.     }

  780.     /** {@inheritDoc} */
  781.     @Override
  782.     public T getHxDot() {

  784.         // Check for equatorial retrograde orbit
  785.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  786.             return getZero().add(Double.NaN);
  787.         }
  788.         if (!hasNonKeplerianRates()) {
  789.             return getZero();
  790.         }

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

  796.     }

  798.     /** {@inheritDoc} */
  799.     @Override
  800.     public T getHy() {
  801.         // Check for equatorial retrograde orbit
  802.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  803.             return getZero().add(Double.NaN);
  804.         }
  805.         return raan.sin().multiply(i.divide(2).tan());
  806.     }

  808.     /** {@inheritDoc} */
  809.     @Override
  810.     public T getHyDot() {

  812.         // Check for equatorial retrograde orbit
  813.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  814.             return getZero().add(Double.NaN);
  815.         }
  816.         if (!hasNonKeplerianRates()) {
  817.             return getZero();
  818.         }

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

  824.     }

  826.     /** {@inheritDoc} */
  827.     @Override
  828.     public T getLv() {
  829.         return pa.add(raan).add(getTrueAnomaly());
  830.     }

  832.     /** {@inheritDoc} */
  833.     @Override
  834.     public T getLvDot() {
  835.         return paDot.add(raanDot).add(getTrueAnomalyDot());
  836.     }

  838.     /** {@inheritDoc} */
  839.     @Override
  840.     public T getLE() {
  841.         return pa.add(raan).add(getEccentricAnomaly());
  842.     }

  844.     /** {@inheritDoc} */
  845.     @Override
  846.     public T getLEDot() {
  847.         return paDot.add(raanDot).add(getEccentricAnomalyDot());
  848.     }

  850.     /** {@inheritDoc} */
  851.     @Override
  852.     public T getLM() {
  853.         return pa.add(raan).add(getMeanAnomaly());
  854.     }

  856.     /** {@inheritDoc} */
  857.     @Override
  858.     public T getLMDot() {
  859.         return paDot.add(raanDot).add(getMeanAnomalyDot());
  860.     }

  862.     /** Initialize cached anomaly with rate.
  863.      * @param anomaly input anomaly
  864.      * @param anomalyDot rate of input anomaly
  865.      * @param inputType position angle type passed as input
  866.      * @return anomaly to cache with rate
  867.      * @since 12.1
  868.      */
  869.     private FieldUnivariateDerivative1<T> initializeCachedAnomaly(final T anomaly, final T anomalyDot,
  870.                                                                   final PositionAngleType inputType) {
  871.         if (cachedPositionAngleType == inputType) {
  872.             return new FieldUnivariateDerivative1<>(anomaly, anomalyDot);

  874.         } else {
  875.             final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  876.             final FieldUnivariateDerivative1<T> anomalyUD = new FieldUnivariateDerivative1<>(anomaly, anomalyDot);

  878.             if (a.getReal() < 0) {
  879.                 switch (cachedPositionAngleType) {
  880.                     case MEAN:
  881.                         if (inputType == PositionAngleType.ECCENTRIC) {
  882.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(eUD, anomalyUD);
  883.                         } else {
  884.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(eUD, anomalyUD);
  885.                         }

  887.                     case ECCENTRIC:
  888.                         if (inputType == PositionAngleType.MEAN) {
  889.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD, anomalyUD);
  890.                         } else {
  891.                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(eUD, anomalyUD);
  892.                         }

  894.                     case TRUE:
  895.                         if (inputType == PositionAngleType.MEAN) {
  896.                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, anomalyUD);
  897.                         } else {
  898.                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(eUD, anomalyUD);
  899.                         }

  901.                     default:
  902.                         break;
  903.                 }

  905.             } else {
  906.                 switch (cachedPositionAngleType) {
  907.                     case MEAN:
  908.                         if (inputType == PositionAngleType.ECCENTRIC) {
  909.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(eUD, anomalyUD);
  910.                         } else {
  911.                             return FieldKeplerianAnomalyUtility.ellipticTrueToMean(eUD, anomalyUD);
  912.                         }

  914.                     case ECCENTRIC:
  915.                         if (inputType == PositionAngleType.MEAN) {
  916.                             return FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD, anomalyUD);
  917.                         } else {
  918.                             return FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(eUD, anomalyUD);
  919.                         }

  921.                     case TRUE:
  922.                         if (inputType == PositionAngleType.MEAN) {
  923.                             return FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, anomalyUD);
  924.                         } else {
  925.                             return FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(eUD, anomalyUD);
  926.                         }

  928.                     default:
  929.                         break;
  930.                 }

  932.             }
  933.             throw new OrekitInternalError(null);
  934.         }

  936.     }

  938.     /** Compute reference axes.
  939.      * @return reference axes
  940.      * @since 12.0
  941.      */
  942.     private FieldVector3D<T>[] referenceAxes() {

  944.         // preliminary variables
  945.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  946.         final FieldSinCos<T> scPa   = FastMath.sinCos(pa);
  947.         final FieldSinCos<T> scI    = FastMath.sinCos(i);
  948.         final T cosRaan = scRaan.cos();
  949.         final T sinRaan = scRaan.sin();
  950.         final T cosPa   = scPa.cos();
  951.         final T sinPa   = scPa.sin();
  952.         final T cosI    = scI.cos();
  953.         final T sinI    = scI.sin();
  954.         final T crcp    = cosRaan.multiply(cosPa);
  955.         final T crsp    = cosRaan.multiply(sinPa);
  956.         final T srcp    = sinRaan.multiply(cosPa);
  957.         final T srsp    = sinRaan.multiply(sinPa);

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

  965.         return axes;

  967.     }

  969.     /** Compute position and velocity but not acceleration.
  970.      */
  971.     private void computePVWithoutA() {

  973.         if (partialPV != null) {
  974.             // already computed
  975.             return;
  976.         }

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

  980.         if (isElliptical()) {

  982.             // elliptical case

  984.             // elliptic eccentric anomaly
  985.             final T uME2             = e.negate().add(1).multiply(e.add(1));
  986.             final T s1Me2            = uME2.sqrt();
  987.             final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  988.             final T cosE             = scE.cos();
  989.             final T sinE             = scE.sin();

  991.             // coordinates of position and velocity in the orbital plane
  992.             final T x      = a.multiply(cosE.subtract(e));
  993.             final T y      = a.multiply(sinE).multiply(s1Me2);
  994.             final T factor = FastMath.sqrt(getMu().divide(a)).divide(e.negate().multiply(cosE).add(1));
  995.             final T xDot   = sinE.negate().multiply(factor);
  996.             final T yDot   = cosE.multiply(s1Me2).multiply(factor);

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

  1002.         } else {

  1004.             // hyperbolic case

  1006.             // compute position and velocity factors
  1007.             final FieldSinCos<T> scV = FastMath.sinCos(getTrueAnomaly());
  1008.             final T sinV             = scV.sin();
  1009.             final T cosV             = scV.cos();
  1010.             final T f                = a.multiply(e.square().negate().add(1));
  1011.             final T posFactor        = f.divide(e.multiply(cosV).add(1));
  1012.             final T velFactor        = FastMath.sqrt(getMu().divide(f));

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

  1018.         }

  1020.     }

  1022.     /** {@inheritDoc} */
  1023.     @Override
  1024.     protected FieldVector3D<T> initPosition() {
  1025.         final FieldVector3D<T>[] axes = referenceAxes();

  1027.         if (isElliptical()) {

  1029.             // elliptical case

  1031.             // elliptic eccentric anomaly
  1032.             final T uME2             = e.negate().add(1).multiply(e.add(1));
  1033.             final T s1Me2            = uME2.sqrt();
  1034.             final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1035.             final T cosE             = scE.cos();
  1036.             final T sinE             = scE.sin();

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

  1040.         } else {

  1042.             // hyperbolic case

  1044.             // compute position and velocity factors
  1045.             final FieldSinCos<T> scV = FastMath.sinCos(getTrueAnomaly());
  1046.             final T sinV             = scV.sin();
  1047.             final T cosV             = scV.cos();
  1048.             final T f                = a.multiply(e.square().negate().add(1));
  1049.             final T posFactor        = f.divide(e.multiply(cosV).add(1));

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

  1053.         }

  1055.     }

  1057.     /** {@inheritDoc} */
  1058.     @Override
  1059.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  1061.         // position and velocity
  1062.         computePVWithoutA();

  1064.         // acceleration
  1065.         final T r2 = partialPV.getPosition().getNormSq();
  1066.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(FastMath.sqrt(r2)).reciprocal().multiply(getMu().negate()),
  1067.                                                                            partialPV.getPosition());
  1068.         final FieldVector3D<T> acceleration = hasNonKeplerianRates() ?
  1069.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  1070.                                               keplerianAcceleration;

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

  1074.     }

  1076.     /** {@inheritDoc} */
  1077.     @Override
  1078.     public FieldKeplerianOrbit<T> inFrame(final Frame inertialFrame) {
  1079.         final FieldPVCoordinates<T> fieldPVCoordinates;
  1080.         if (hasNonKeplerianAcceleration()) {
  1081.             fieldPVCoordinates = getPVCoordinates(inertialFrame);
  1082.         } else {
  1083.             final FieldKinematicTransform<T> transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
  1084.             fieldPVCoordinates = transform.transformOnlyPV(getPVCoordinates());
  1085.         }
  1086.         final FieldKeplerianOrbit<T> fieldOrbit = new FieldKeplerianOrbit<>(fieldPVCoordinates, inertialFrame, getDate(), getMu());
  1087.         if (fieldOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
  1088.             return fieldOrbit;
  1089.         } else {
  1090.             return fieldOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
  1091.         }
  1092.     }

  1094.     /** {@inheritDoc} */
  1095.     @Override
  1096.     public FieldKeplerianOrbit<T> withCachedPositionAngleType(final PositionAngleType positionAngleType) {
  1097.         return new FieldKeplerianOrbit<>(a, e, i, pa, raan, getAnomaly(positionAngleType), aDot, eDot, iDot, paDot, raanDot,
  1098.                 getAnomalyDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
  1099.     }

  1101.     /** {@inheritDoc} */
  1102.     @Override
  1103.     public FieldKeplerianOrbit<T> shiftedBy(final double dt) {
  1104.         return shiftedBy(getZero().newInstance(dt));
  1105.     }

  1107.     /** {@inheritDoc} */
  1108.     @Override
  1109.     public FieldKeplerianOrbit<T> shiftedBy(final T dt) {

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

  1116.         if (!dt.isZero() && hasNonKeplerianRates()) {

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

  1121.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  1122.             keplerianShifted.computePVWithoutA();
  1123.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getPosition(),
  1124.                                                                   dt.square().multiply(0.5), nonKeplerianAcceleration);
  1125.             final T   fixedR2 = fixedP.getNormSq();
  1126.             final T   fixedR  = fixedR2.sqrt();
  1127.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getVelocity(),
  1128.                                                                  dt, nonKeplerianAcceleration);
  1129.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  1130.                                                                  keplerianShifted.partialPV.getPosition(),
  1131.                                                                  getOne(), nonKeplerianAcceleration);

  1133.             // build a new orbit, taking non-Keplerian acceleration into account
  1134.             return new FieldKeplerianOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  1135.                                                                                  fixedP, fixedV, fixedA),
  1136.                                              keplerianShifted.getFrame(), keplerianShifted.getMu());

  1138.         } else {
  1139.             // Keplerian-only motion is all we can do
  1140.             return keplerianShifted;
  1141.         }

  1143.     }

  1145.     /** {@inheritDoc} */
  1146.     @Override
  1147.     protected T[][] computeJacobianMeanWrtCartesian() {
  1148.         if (isElliptical()) {
  1149.             return computeJacobianMeanWrtCartesianElliptical();
  1150.         } else {
  1151.             return computeJacobianMeanWrtCartesianHyperbolic();
  1152.         }
  1153.     }

  1155.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1156.      * <p>
  1157.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1158.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1159.      * yDot for j=4, zDot for j=5).
  1160.      * </p>
  1161.      * @return 6x6 Jacobian matrix
  1162.      */
  1163.     private T[][] computeJacobianMeanWrtCartesianElliptical() {

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

  1167.         // compute various intermediate parameters
  1168.         computePVWithoutA();
  1169.         final FieldVector3D<T> position = partialPV.getPosition();
  1170.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1171.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  1172.         final T v2         = velocity.getNormSq();
  1173.         final T r2         = position.getNormSq();
  1174.         final T r          = r2.sqrt();
  1175.         final T r3         = r.multiply(r2);

  1177.         final T px         = position.getX();
  1178.         final T py         = position.getY();
  1179.         final T pz         = position.getZ();
  1180.         final T vx         = velocity.getX();
  1181.         final T vy         = velocity.getY();
  1182.         final T vz         = velocity.getZ();
  1183.         final T mx         = momentum.getX();
  1184.         final T my         = momentum.getY();
  1185.         final T mz         = momentum.getZ();

  1187.         final T mu         = getMu();
  1188.         final T sqrtMuA    = FastMath.sqrt(a.multiply(mu));
  1189.         final T sqrtAoMu   = FastMath.sqrt(a.divide(mu));
  1190.         final T a2         = a.square();
  1191.         final T twoA       = a.multiply(2);
  1192.         final T rOnA       = r.divide(a);

  1194.         final T oMe2       = e.square().negate().add(1);
  1195.         final T epsilon    = oMe2.sqrt();
  1196.         final T sqrtRec    = epsilon.reciprocal();

  1198.         final FieldSinCos<T> scI  = FastMath.sinCos(i);
  1199.         final FieldSinCos<T> scPA = FastMath.sinCos(pa);
  1200.         final T cosI       = scI.cos();
  1201.         final T sinI       = scI.sin();
  1202.         final T cosPA      = scPA.cos();
  1203.         final T sinPA      = scPA.sin();

  1205.         final T pv         = FieldVector3D.dotProduct(position, velocity);
  1206.         final T cosE       = a.subtract(r).divide(a.multiply(e));
  1207.         final T sinE       = pv.divide(e.multiply(sqrtMuA));

  1209.         // da
  1210.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  1211.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(mu.divide(2.).reciprocal()));
  1212.         fillHalfRow(getOne(), vectorAR,    jacobian[0], 0);
  1213.         fillHalfRow(getOne(), vectorARDot, jacobian[0], 3);

  1215.         // de
  1216.         final T factorER3 = pv.divide(twoA);
  1217.         final FieldVector3D<T> vectorER   = new FieldVector3D<>(cosE.multiply(v2).divide(r.multiply(mu)), position,
  1218.                                                                 sinE.divide(sqrtMuA), velocity,
  1219.                                                                 factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorAR);
  1220.         final FieldVector3D<T> vectorERDot = new FieldVector3D<>(sinE.divide(sqrtMuA), position,
  1221.                                                                  cosE.multiply(mu.divide(2.).reciprocal()).multiply(r), velocity,
  1222.                                                                  factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorARDot);
  1223.         fillHalfRow(getOne(), vectorER,    jacobian[1], 0);
  1224.         fillHalfRow(getOne(), vectorERDot, jacobian[1], 3);

  1226.         // dE / dr (Eccentric anomaly)
  1227.         final T coefE = cosE.divide(e.multiply(sqrtMuA));
  1228.         final FieldVector3D<T>  vectorEAnR =
  1229.             new FieldVector3D<>(sinE.negate().multiply(v2).divide(e.multiply(r).multiply(mu)), position, coefE, velocity,
  1230.                                 factorER3.negate().multiply(coefE), vectorAR);

  1232.         // dE / drDot
  1233.         final FieldVector3D<T>  vectorEAnRDot =
  1234.             new FieldVector3D<>(sinE.multiply(-2).multiply(r).divide(e.multiply(mu)), velocity, coefE, position,
  1235.                                 factorER3.negate().multiply(coefE), vectorARDot);

  1237.         // precomputing some more factors
  1238.         final T s1 = sinE.negate().multiply(pz).divide(r).subtract(cosE.multiply(vz).multiply(sqrtAoMu));
  1239.         final T s2 = cosE.negate().multiply(pz).divide(r3);
  1240.         final T s3 = sinE.multiply(vz).divide(sqrtMuA.multiply(-2));
  1241.         final T t1 = sqrtRec.multiply(cosE.multiply(pz).divide(r).subtract(sinE.multiply(vz).multiply(sqrtAoMu)));
  1242.         final T t2 = sqrtRec.multiply(sinE.negate().multiply(pz).divide(r3));
  1243.         final T t3 = sqrtRec.multiply(cosE.subtract(e)).multiply(vz).divide(sqrtMuA.multiply(2));
  1244.         final T t4 = sqrtRec.multiply(e.multiply(sinI).multiply(cosPA).multiply(sqrtRec).subtract(vz.multiply(sqrtAoMu)));
  1245.         final FieldVector3D<T> s = new FieldVector3D<>(cosE.divide(r), this.PLUS_K,
  1246.                                                        s1,       vectorEAnR,
  1247.                                                        s2,       position,
  1248.                                                        s3,       vectorAR);
  1249.         final FieldVector3D<T> sDot = new FieldVector3D<>(sinE.negate().multiply(sqrtAoMu), this.PLUS_K,
  1250.                                                           s1,               vectorEAnRDot,
  1251.                                                           s3,               vectorARDot);
  1252.         final FieldVector3D<T> t =
  1253.             new FieldVector3D<>(sqrtRec.multiply(sinE).divide(r), this.PLUS_K).add(new FieldVector3D<>(t1, vectorEAnR,
  1254.                                                                                                        t2, position,
  1255.                                                                                                        t3, vectorAR,
  1256.                                                                                                        t4, vectorER));
  1257.         final FieldVector3D<T> tDot = new FieldVector3D<>(sqrtRec.multiply(cosE.subtract(e)).multiply(sqrtAoMu), this.PLUS_K,
  1258.                                                           t1,                                                    vectorEAnRDot,
  1259.                                                           t3,                                                    vectorARDot,
  1260.                                                           t4,                                                    vectorERDot);

  1262.         // di
  1263.         final T factorI1 = sinI.negate().multiply(sqrtRec).divide(sqrtMuA);
  1264.         final T i1 =  factorI1;
  1265.         final T i2 =  factorI1.negate().multiply(mz).divide(twoA);
  1266.         final T i3 =  factorI1.multiply(mz).multiply(e).divide(oMe2);
  1267.         final T i4 = cosI.multiply(sinPA);
  1268.         final T i5 = cosI.multiply(cosPA);
  1269.         fillHalfRow(i1, new FieldVector3D<>(vy, vx.negate(), getZero()), i2, vectorAR, i3, vectorER, i4, s, i5, t,
  1270.                     jacobian[2], 0);
  1271.         fillHalfRow(i1, new FieldVector3D<>(py.negate(), px, getZero()), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
  1272.                     jacobian[2], 3);

  1274.         // dpa
  1275.         fillHalfRow(cosPA.divide(sinI), s,    sinPA.negate().divide(sinI), t,    jacobian[3], 0);
  1276.         fillHalfRow(cosPA.divide(sinI), sDot, sinPA.negate().divide(sinI), tDot, jacobian[3], 3);

  1278.         // dRaan
  1279.         final T factorRaanR = (a.multiply(mu).multiply(oMe2).multiply(sinI).multiply(sinI)).reciprocal();
  1280.         fillHalfRow( factorRaanR.negate().multiply(my), new FieldVector3D<>(getZero(), vz, vy.negate()),
  1281.                      factorRaanR.multiply(mx), new FieldVector3D<>(vz.negate(), getZero(),  vx),
  1282.                      jacobian[4], 0);
  1283.         fillHalfRow(factorRaanR.negate().multiply(my), new FieldVector3D<>(getZero(), pz.negate(),  py),
  1284.                      factorRaanR.multiply(mx), new FieldVector3D<>(pz, getZero(), px.negate()),
  1285.                      jacobian[4], 3);

  1287.         // dM
  1288.         fillHalfRow(rOnA, vectorEAnR,    sinE.negate(), vectorER,    jacobian[5], 0);
  1289.         fillHalfRow(rOnA, vectorEAnRDot, sinE.negate(), vectorERDot, jacobian[5], 3);

  1291.         return jacobian;

  1293.     }

  1295.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1296.      * <p>
  1297.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1298.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1299.      * yDot for j=4, zDot for j=5).
  1300.      * </p>
  1301.      * @return 6x6 Jacobian matrix
  1302.      */
  1303.     private T[][] computeJacobianMeanWrtCartesianHyperbolic() {

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

  1307.         // compute various intermediate parameters
  1308.         computePVWithoutA();
  1309.         final FieldVector3D<T> position = partialPV.getPosition();
  1310.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1311.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  1312.         final T r2         = position.getNormSq();
  1313.         final T r          = r2.sqrt();
  1314.         final T r3         = r.multiply(r2);

  1316.         final T x          = position.getX();
  1317.         final T y          = position.getY();
  1318.         final T z          = position.getZ();
  1319.         final T vx         = velocity.getX();
  1320.         final T vy         = velocity.getY();
  1321.         final T vz         = velocity.getZ();
  1322.         final T mx         = momentum.getX();
  1323.         final T my         = momentum.getY();
  1324.         final T mz         = momentum.getZ();

  1326.         final T mu         = getMu();
  1327.         final T absA       = a.negate();
  1328.         final T sqrtMuA    = absA.multiply(mu).sqrt();
  1329.         final T a2         = a.square();
  1330.         final T rOa        = r.divide(absA);

  1332.         final FieldSinCos<T> scI = FastMath.sinCos(i);
  1333.         final T cosI       = scI.cos();
  1334.         final T sinI       = scI.sin();

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

  1338.         // da
  1339.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(-2).divide(r3), position);
  1340.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(-2).divide(mu));
  1341.         fillHalfRow(getOne().negate(), vectorAR,    jacobian[0], 0);
  1342.         fillHalfRow(getOne().negate(), vectorARDot, jacobian[0], 3);

  1344.         // differentials of the momentum
  1345.         final T m      = momentum.getNorm();
  1346.         final T oOm    = m.reciprocal();
  1347.         final FieldVector3D<T> dcXP = new FieldVector3D<>(getZero(), vz, vy.negate());
  1348.         final FieldVector3D<T> dcYP = new FieldVector3D<>(vz.negate(),   getZero(),  vx);
  1349.         final FieldVector3D<T> dcZP = new FieldVector3D<>( vy, vx.negate(),   getZero());
  1350.         final FieldVector3D<T> dcXV = new FieldVector3D<>(  getZero(),  z.negate(),   y);
  1351.         final FieldVector3D<T> dcYV = new FieldVector3D<>(  z,   getZero(),  x.negate());
  1352.         final FieldVector3D<T> dcZV = new FieldVector3D<>( y.negate(),   x,   getZero());
  1353.         final FieldVector3D<T> dCP  = new FieldVector3D<>(mx.multiply(oOm), dcXP, my.multiply(oOm), dcYP, mz.multiply(oOm), dcZP);
  1354.         final FieldVector3D<T> dCV  = new FieldVector3D<>(mx.multiply(oOm), dcXV, my.multiply(oOm), dcYV, mz.multiply(oOm), dcZV);

  1356.         // dp
  1357.         final T mOMu   = m.divide(mu);
  1358.         final FieldVector3D<T> dpP  = new FieldVector3D<>(mOMu.multiply(2), dCP);
  1359.         final FieldVector3D<T> dpV  = new FieldVector3D<>(mOMu.multiply(2), dCV);

  1361.         // de
  1362.         final T p      = m.multiply(mOMu);
  1363.         final T moO2ae = absA.multiply(2).multiply(e).reciprocal();
  1364.         final T m2OaMu = p.negate().divide(absA);
  1365.         fillHalfRow(moO2ae, dpP, m2OaMu.multiply(moO2ae), vectorAR,    jacobian[1], 0);
  1366.         fillHalfRow(moO2ae, dpV, m2OaMu.multiply(moO2ae), vectorARDot, jacobian[1], 3);

  1368.         // di
  1369.         final T cI1 = m.multiply(sinI).reciprocal();
  1370.         final T cI2 = cosI.multiply(cI1);
  1371.         fillHalfRow(cI2, dCP, cI1.negate(), dcZP, jacobian[2], 0);
  1372.         fillHalfRow(cI2, dCV, cI1.negate(), dcZV, jacobian[2], 3);


  1375.         // dPA
  1376.         final T cP1     =  y.multiply(oOm);
  1377.         final T cP2     =  x.negate().multiply(oOm);
  1378.         final T cP3     =  mx.multiply(cP1).add(my.multiply(cP2)).negate();
  1379.         final T cP4     =  cP3.multiply(oOm);
  1380.         final T cP5     =  r2.multiply(sinI).multiply(sinI).negate().reciprocal();
  1381.         final T cP6     = z.multiply(cP5);
  1382.         final T cP7     = cP3.multiply(cP5);
  1383.         final FieldVector3D<T> dacP  = new FieldVector3D<>(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new FieldVector3D<>(my.negate(), mx, getZero()));
  1384.         final FieldVector3D<T> dacV  = new FieldVector3D<>(cP1, dcXV, cP2, dcYV, cP4, dCV);
  1385.         final FieldVector3D<T> dpoP  = new FieldVector3D<>(cP6, dacP, cP7, this.PLUS_K);
  1386.         final FieldVector3D<T> dpoV  = new FieldVector3D<>(cP6, dacV);

  1388.         final T re2     = r2.multiply(e.square());
  1389.         final T recOre2 = p.subtract(r).divide(re2);
  1390.         final T resOre2 = pv.multiply(mOMu).divide(re2);
  1391.         final FieldVector3D<T> dreP  = new FieldVector3D<>(mOMu, velocity, pv.divide(mu), dCP);
  1392.         final FieldVector3D<T> dreV  = new FieldVector3D<>(mOMu, position, pv.divide(mu), dCV);
  1393.         final FieldVector3D<T> davP  = new FieldVector3D<>(resOre2.negate(), dpP, recOre2, dreP, resOre2.divide(r), position);
  1394.         final FieldVector3D<T> davV  = new FieldVector3D<>(resOre2.negate(), dpV, recOre2, dreV);
  1395.         fillHalfRow(getOne(), dpoP, getOne().negate(), davP, jacobian[3], 0);
  1396.         fillHalfRow(getOne(), dpoV, getOne().negate(), davV, jacobian[3], 3);

  1398.         // dRAAN
  1399.         final T cO0 = cI1.square();
  1400.         final T cO1 =  mx.multiply(cO0);
  1401.         final T cO2 =  my.negate().multiply(cO0);
  1402.         fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
  1403.         fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);

  1405.         // dM
  1406.         final T s2a    = pv.divide(absA.multiply(2));
  1407.         final T oObux  = m.square().add(absA.multiply(mu)).sqrt().reciprocal();
  1408.         final T scasbu = pv.multiply(oObux);
  1409.         final FieldVector3D<T> dauP = new FieldVector3D<>(sqrtMuA.reciprocal(), velocity, s2a.negate().divide(sqrtMuA), vectorAR);
  1410.         final FieldVector3D<T> dauV = new FieldVector3D<>(sqrtMuA.reciprocal(), position, s2a.negate().divide(sqrtMuA), vectorARDot);
  1411.         final FieldVector3D<T> dbuP = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorAR,    m.multiply(oObux), dCP);
  1412.         final FieldVector3D<T> dbuV = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorARDot, m.multiply(oObux), dCV);
  1413.         final FieldVector3D<T> dcuP = new FieldVector3D<>(oObux, velocity, scasbu.negate().multiply(oObux), dbuP);
  1414.         final FieldVector3D<T> dcuV = new FieldVector3D<>(oObux, position, scasbu.negate().multiply(oObux), dbuV);
  1415.         fillHalfRow(getOne(), dauP, e.negate().divide(rOa.add(1)), dcuP, jacobian[5], 0);
  1416.         fillHalfRow(getOne(), dauV, e.negate().divide(rOa.add(1)), dcuV, jacobian[5], 3);

  1418.         return jacobian;

  1420.     }

  1422.     /** {@inheritDoc} */
  1423.     @Override
  1424.     protected T[][] computeJacobianEccentricWrtCartesian() {
  1425.         if (isElliptical()) {
  1426.             return computeJacobianEccentricWrtCartesianElliptical();
  1427.         } else {
  1428.             return computeJacobianEccentricWrtCartesianHyperbolic();
  1429.         }
  1430.     }

  1432.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1433.      * <p>
  1434.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1435.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1436.      * yDot for j=4, zDot for j=5).
  1437.      * </p>
  1438.      * @return 6x6 Jacobian matrix
  1439.      */
  1440.     private T[][] computeJacobianEccentricWrtCartesianElliptical() {

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

  1445.         // Differentiating the Kepler equation M = E - e sin E leads to:
  1446.         // dM = (1 - e cos E) dE - sin E de
  1447.         // which is inverted and rewritten as:
  1448.         // dE = a/r dM + sin E a/r de
  1449.         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1450.         final T aOr              = e.negate().multiply(scE.cos()).add(1).reciprocal();

  1452.         // update anomaly row
  1453.         final T[] eRow           = jacobian[1];
  1454.         final T[] anomalyRow     = jacobian[5];
  1455.         for (int j = 0; j < anomalyRow.length; ++j) {
  1456.             anomalyRow[j] = aOr.multiply(anomalyRow[j].add(scE.sin().multiply(eRow[j])));
  1457.         }

  1459.         return jacobian;

  1461.     }

  1463.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1464.      * <p>
  1465.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1466.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1467.      * yDot for j=4, zDot for j=5).
  1468.      * </p>
  1469.      * @return 6x6 Jacobian matrix
  1470.      */
  1471.     private T[][] computeJacobianEccentricWrtCartesianHyperbolic() {

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

  1476.         // Differentiating the Kepler equation M = e sinh H - H leads to:
  1477.         // dM = (e cosh H - 1) dH + sinh H de
  1478.         // which is inverted and rewritten as:
  1479.         // dH = 1 / (e cosh H - 1) dM - sinh H / (e cosh H - 1) de
  1480.         final T H      = getEccentricAnomaly();
  1481.         final T coshH  = H.cosh();
  1482.         final T sinhH  = H.sinh();
  1483.         final T absaOr = e.multiply(coshH).subtract(1).reciprocal();
  1484.         // update anomaly row
  1485.         final T[] eRow       = jacobian[1];
  1486.         final T[] anomalyRow = jacobian[5];

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

  1491.         }

  1493.         return jacobian;

  1495.     }

  1497.     /** {@inheritDoc} */
  1498.     @Override
  1499.     protected T[][] computeJacobianTrueWrtCartesian() {
  1500.         if (isElliptical()) {
  1501.             return computeJacobianTrueWrtCartesianElliptical();
  1502.         } else {
  1503.             return computeJacobianTrueWrtCartesianHyperbolic();
  1504.         }
  1505.     }

  1507.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1508.      * <p>
  1509.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1510.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1511.      * yDot for j=4, zDot for j=5).
  1512.      * </p>
  1513.      * @return 6x6 Jacobian matrix
  1514.      */
  1515.     private T[][] computeJacobianTrueWrtCartesianElliptical() {

  1517.         // start by computing the Jacobian with eccentric angle
  1518.         final T[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();
  1519.         // Differentiating the eccentric anomaly equation sin E = sqrt(1-e^2) sin v / (1 + e cos v)
  1520.         // and using cos E = (e + cos v) / (1 + e cos v) to get rid of cos E leads to:
  1521.         // dE = [sqrt (1 - e^2) / (1 + e cos v)] dv - [sin E / (1 - e^2)] de
  1522.         // which is inverted and rewritten as:
  1523.         // dv = sqrt (1 - e^2) a/r dE + [sin E / sqrt (1 - e^2)] a/r de
  1524.         final T e2               = e.square();
  1525.         final T oMe2             = e2.negate().add(1);
  1526.         final T epsilon          = oMe2.sqrt();
  1527.         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1528.         final T aOr              = e.multiply(scE.cos()).negate().add(1).reciprocal();
  1529.         final T aFactor          = epsilon.multiply(aOr);
  1530.         final T eFactor          = scE.sin().multiply(aOr).divide(epsilon);

  1532.         // update anomaly row
  1533.         final T[] eRow           = jacobian[1];
  1534.         final T[] anomalyRow     = jacobian[5];
  1535.         for (int j = 0; j < anomalyRow.length; ++j) {
  1536.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).add(eFactor.multiply(eRow[j]));
  1537.         }
  1538.         return jacobian;

  1540.     }

  1542.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1543.      * <p>
  1544.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1545.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1546.      * yDot for j=4, zDot for j=5).
  1547.      * </p>
  1548.      * @return 6x6 Jacobian matrix
  1549.      */
  1550.     private T[][] computeJacobianTrueWrtCartesianHyperbolic() {

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

  1555.         // Differentiating the eccentric anomaly equation sinh H = sqrt(e^2-1) sin v / (1 + e cos v)
  1556.         // and using cosh H = (e + cos v) / (1 + e cos v) to get rid of cosh H leads to:
  1557.         // dH = [sqrt (e^2 - 1) / (1 + e cos v)] dv + [sinh H / (e^2 - 1)] de
  1558.         // which is inverted and rewritten as:
  1559.         // dv = sqrt (1 - e^2) a/r dH - [sinh H / sqrt (e^2 - 1)] a/r de
  1560.         final T e2       = e.square();
  1561.         final T e2Mo     = e2.subtract(1);
  1562.         final T epsilon  = e2Mo.sqrt();
  1563.         final T H        = getEccentricAnomaly();
  1564.         final T coshH    = H.cosh();
  1565.         final T sinhH    = H.sinh();
  1566.         final T aOr      = e.multiply(coshH).subtract(1).reciprocal();
  1567.         final T aFactor  = epsilon.multiply(aOr);
  1568.         final T eFactor  = sinhH.multiply(aOr).divide(epsilon);

  1570.         // update anomaly row
  1571.         final T[] eRow           = jacobian[1];
  1572.         final T[] anomalyRow     = jacobian[5];
  1573.         for (int j = 0; j < anomalyRow.length; ++j) {
  1574.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).subtract(eFactor.multiply(eRow[j]));
  1575.         }

  1577.         return jacobian;

  1579.     }

  1581.     /** {@inheritDoc} */
  1582.     @Override
  1583.     public void addKeplerContribution(final PositionAngleType type, final T gm,
  1584.                                       final T[] pDot) {
  1585.         pDot[5] = pDot[5].add(computeKeplerianAnomalyDot(type, a, e, gm, cachedAnomaly, cachedPositionAngleType));
  1586.     }

  1588.     /**
  1589.      * Compute rate of argument of latitude.
  1590.      * @param type position angle type of rate
  1591.      * @param a semi major axis
  1592.      * @param e eccentricity
  1593.      * @param mu mu
  1594.      * @param anomaly anomaly
  1595.      * @param cachedType position angle type of passed anomaly
  1596.      * @param <T> field type
  1597.      * @return first-order time derivative for anomaly
  1598.      * @since 12.2
  1599.      */
  1600.     private static <T extends CalculusFieldElement<T>> T computeKeplerianAnomalyDot(final PositionAngleType type, final T a, final T e,
  1601.                                                                                     final T mu, final T anomaly, final PositionAngleType cachedType) {
  1602.         final T absA = a.abs();
  1603.         final T n    = absA.reciprocal().multiply(mu).sqrt().divide(absA);
  1604.         if (type == PositionAngleType.MEAN) {
  1605.             return n;
  1606.         }
  1607.         final T ksi;
  1608.         final T oMe2;
  1609.         final T trueAnomaly = FieldKeplerianAnomalyUtility.convertAnomaly(cachedType, anomaly, e, PositionAngleType.TRUE);
  1610.         if (type == PositionAngleType.ECCENTRIC) {
  1611.             oMe2 = e.square().negate().add(1).abs();
  1612.             ksi = e.multiply(trueAnomaly.cos()).add(1);
  1613.             return n.multiply(ksi).divide(oMe2);
  1614.         } else {
  1615.             oMe2 = e.square().negate().add(1).abs();
  1616.             ksi  = e.multiply(trueAnomaly.cos()).add(1);
  1617.             return n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt()));
  1618.         }
  1619.     }

  1621.     /**  Returns a string representation of this Keplerian parameters object.
  1622.      * @return a string representation of this object
  1623.      */
  1624.     public String toString() {
  1625.         return new StringBuilder().append("Keplerian parameters: ").append('{').
  1626.                                   append("a: ").append(a.getReal()).
  1627.                                   append("; e: ").append(e.getReal()).
  1628.                                   append("; i: ").append(FastMath.toDegrees(i.getReal())).
  1629.                                   append("; pa: ").append(FastMath.toDegrees(pa.getReal())).
  1630.                                   append("; raan: ").append(FastMath.toDegrees(raan.getReal())).
  1631.                                   append("; v: ").append(FastMath.toDegrees(getTrueAnomaly().getReal())).
  1632.                                   append(";}").toString();
  1633.     }

  1635.     /** {@inheritDoc} */
  1636.     @Override
  1637.     public PositionAngleType getCachedPositionAngleType() {
  1638.         return cachedPositionAngleType;
  1639.     }

  1641.     /** {@inheritDoc} */
  1642.     @Override
  1643.     public boolean hasNonKeplerianRates() {
  1644.         return hasNonKeplerianAcceleration();
  1645.     }

  1647.     /** {@inheritDoc} */
  1648.     @Override
  1649.     public FieldKeplerianOrbit<T> withKeplerianRates() {
  1650.         return new FieldKeplerianOrbit<>(getA(), getE(), getI(), getPerigeeArgument(), getRightAscensionOfAscendingNode(),
  1651.                 cachedAnomaly, cachedPositionAngleType, getFrame(), getDate(), getMu());
  1652.     }

  1654.     /** Check if the given parameter is within an acceptable range.
  1655.      * The bounds are inclusive: an exception is raised when either of those conditions are met:
  1656.      * <ul>
  1657.      *     <li>The parameter is strictly greater than upperBound</li>
  1658.      *     <li>The parameter is strictly lower than lowerBound</li>
  1659.      * </ul>
  1660.      * <p>
  1661.      * In either of these cases, an OrekitException is raised.
  1662.      * </p>
  1663.      * @param parameterName name of the parameter
  1664.      * @param parameter value of the parameter
  1665.      * @param lowerBound lower bound of the acceptable range (inclusive)
  1666.      * @param upperBound upper bound of the acceptable range (inclusive)
  1667.      */
  1668.     private void checkParameterRangeInclusive(final String parameterName, final double parameter,
  1669.                                               final double lowerBound, final double upperBound) {
  1670.         if (parameter < lowerBound || parameter > upperBound) {
  1671.             throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, parameterName,
  1672.                                       parameter, lowerBound, upperBound);
  1673.         }
  1674.     }

  1676.     /** {@inheritDoc} */
  1677.     @Override
  1678.     public KeplerianOrbit toOrbit() {
  1679.         final double cachedPositionAngle = cachedAnomaly.getReal();
  1680.         return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
  1681.                                   pa.getReal(), raan.getReal(), cachedPositionAngle,
  1682.                                   aDot.getReal(), eDot.getReal(), iDot.getReal(),
  1683.                                   paDot.getReal(), raanDot.getReal(),
  1684.                                   cachedAnomalyDot.getReal(),
  1685.                                   cachedPositionAngleType, cachedPositionAngleType,
  1686.                                   getFrame(), getDate().toAbsoluteDate(), getMu().getReal());
  1687.     }


  1690. }
Created with JaCoCo 0.8.12.202403310830