KeplerianOrbit.java

  1. /* Copyright 2002-2022 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;

  19. import java.io.Serializable;
  20. import java.util.List;
  21. import java.util.stream.Collectors;
  22. import java.util.stream.Stream;

  24. import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
  25. import org.hipparchus.analysis.interpolation.HermiteInterpolator;
  26. import org.hipparchus.exception.MathIllegalStateException;
  27. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  28. import org.hipparchus.util.FastMath;
  29. import org.hipparchus.util.MathUtils;
  30. import org.hipparchus.util.SinCos;
  31. import org.orekit.annotation.DefaultDataContext;
  32. import org.orekit.data.DataContext;
  33. import org.orekit.errors.OrekitException;
  34. import org.orekit.errors.OrekitIllegalArgumentException;
  35. import org.orekit.errors.OrekitInternalError;
  36. import org.orekit.errors.OrekitMessages;
  37. import org.orekit.frames.Frame;
  38. import org.orekit.time.AbsoluteDate;
  39. import org.orekit.utils.PVCoordinates;
  40. import org.orekit.utils.TimeStampedPVCoordinates;


  43. /**
  44.  * This class handles traditional Keplerian orbital parameters.

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

  70.  * <p>
  71.  * The instance <code>KeplerianOrbit</code> is guaranteed to be immutable.
  72.  * </p>
  73.  * @see     Orbit
  74.  * @see    CircularOrbit
  75.  * @see    CartesianOrbit
  76.  * @see    EquinoctialOrbit
  77.  * @author Luc Maisonobe
  78.  * @author Guylaine Prat
  79.  * @author Fabien Maussion
  80.  * @author V&eacute;ronique Pommier-Maurussane
  81.  */
  82. public class KeplerianOrbit extends Orbit {

  84.     /** Serializable UID. */
  85.     private static final long serialVersionUID = 20170414L;

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

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

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

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

  104.     /** Semi-major axis (m). */
  105.     private final double a;

  107.     /** Eccentricity. */
  108.     private final double e;

  110.     /** Inclination (rad). */
  111.     private final double i;

  113.     /** Perigee Argument (rad). */
  114.     private final double pa;

  116.     /** Right Ascension of Ascending Node (rad). */
  117.     private final double raan;

  119.     /** True anomaly (rad). */
  120.     private final double v;

  122.     /** Semi-major axis derivative (m/s). */
  123.     private final double aDot;

  125.     /** Eccentricity derivative. */
  126.     private final double eDot;

  128.     /** Inclination derivative (rad/s). */
  129.     private final double iDot;

  131.     /** Perigee Argument derivative (rad/s). */
  132.     private final double paDot;

  134.     /** Right Ascension of Ascending Node derivative (rad/s). */
  135.     private final double raanDot;

  137.     /** True anomaly derivative (rad/s). */
  138.     private final double vDot;

  140.     /** Partial Cartesian coordinates (position and velocity are valid, acceleration may be missing). */
  141.     private transient PVCoordinates partialPV;

  143.     /** Creates a new instance.
  144.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  145.      * @param e eccentricity (positive or equal to 0)
  146.      * @param i inclination (rad)
  147.      * @param pa perigee argument (ω, rad)
  148.      * @param raan right ascension of ascending node (Ω, rad)
  149.      * @param anomaly mean, eccentric or true anomaly (rad)
  150.      * @param type type of anomaly
  151.      * @param frame the frame in which the parameters are defined
  152.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  153.      * @param date date of the orbital parameters
  154.      * @param mu central attraction coefficient (m³/s²)
  155.      * @exception IllegalArgumentException if frame is not a {@link
  156.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  157.      * or v is out of range for hyperbolic orbits
  158.      */
  159.     public KeplerianOrbit(final double a, final double e, final double i,
  160.                           final double pa, final double raan, final double anomaly,
  161.                           final PositionAngle type,
  162.                           final Frame frame, final AbsoluteDate date, final double mu)
  163.         throws IllegalArgumentException {
  164.         this(a, e, i, pa, raan, anomaly,
  165.              Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN,
  166.              type, frame, date, mu);
  167.     }

  169.     /** Creates a new instance.
  170.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  171.      * @param e eccentricity (positive or equal to 0)
  172.      * @param i inclination (rad)
  173.      * @param pa perigee argument (ω, rad)
  174.      * @param raan right ascension of ascending node (Ω, rad)
  175.      * @param anomaly mean, eccentric or true anomaly (rad)
  176.      * @param aDot  semi-major axis derivative (m/s)
  177.      * @param eDot eccentricity derivative
  178.      * @param iDot inclination derivative (rad/s)
  179.      * @param paDot perigee argument derivative (rad/s)
  180.      * @param raanDot right ascension of ascending node derivative (rad/s)
  181.      * @param anomalyDot mean, eccentric or true anomaly derivative (rad/s)
  182.      * @param type type of anomaly
  183.      * @param frame the frame in which the parameters are defined
  184.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  185.      * @param date date of the orbital parameters
  186.      * @param mu central attraction coefficient (m³/s²)
  187.      * @exception IllegalArgumentException if frame is not a {@link
  188.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  189.      * or v is out of range for hyperbolic orbits
  190.      * @since 9.0
  191.      */
  192.     public KeplerianOrbit(final double a, final double e, final double i,
  193.                           final double pa, final double raan, final double anomaly,
  194.                           final double aDot, final double eDot, final double iDot,
  195.                           final double paDot, final double raanDot, final double anomalyDot,
  196.                           final PositionAngle type,
  197.                           final Frame frame, final AbsoluteDate date, final double mu)
  198.         throws IllegalArgumentException {
  199.         super(frame, date, mu);

  201.         if (a * (1 - e) < 0) {
  202.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a, e);
  203.         }

  205.         // Checking eccentricity range
  206.         checkParameterRangeInclusive(ECCENTRICITY, e, 0.0, Double.POSITIVE_INFINITY);

  208.         this.a       = a;
  209.         this.aDot    = aDot;
  210.         this.e       = e;
  211.         this.eDot    = eDot;
  212.         this.i       = i;
  213.         this.iDot    = iDot;
  214.         this.pa      = pa;
  215.         this.paDot   = paDot;
  216.         this.raan    = raan;
  217.         this.raanDot = raanDot;

  219.         if (hasDerivatives()) {
  220.             final UnivariateDerivative1 eUD        = new UnivariateDerivative1(e, eDot);
  221.             final UnivariateDerivative1 anomalyUD  = new UnivariateDerivative1(anomaly,  anomalyDot);
  222.             final UnivariateDerivative1 vUD;
  223.             switch (type) {
  224.                 case MEAN :
  225.                     vUD = (a < 0) ?
  226.                           FieldKeplerianOrbit.hyperbolicEccentricToTrue(FieldKeplerianOrbit.meanToHyperbolicEccentric(anomalyUD, eUD), eUD) :
  227.                           FieldKeplerianOrbit.ellipticEccentricToTrue(FieldKeplerianOrbit.meanToEllipticEccentric(anomalyUD, eUD), eUD);
  228.                     break;
  229.                 case ECCENTRIC :
  230.                     vUD = (a < 0) ?
  231.                           FieldKeplerianOrbit.hyperbolicEccentricToTrue(anomalyUD, eUD) :
  232.                           FieldKeplerianOrbit.ellipticEccentricToTrue(anomalyUD, eUD);
  233.                     break;
  234.                 case TRUE :
  235.                     vUD = anomalyUD;
  236.                     break;
  237.                 default : // this should never happen
  238.                     throw new OrekitInternalError(null);
  239.             }
  240.             this.v    = vUD.getValue();
  241.             this.vDot = vUD.getDerivative(1);
  242.         } else {
  243.             switch (type) {
  244.                 case MEAN :
  245.                     this.v = (a < 0) ?
  246.                              hyperbolicEccentricToTrue(meanToHyperbolicEccentric(anomaly, e), e) :
  247.                              ellipticEccentricToTrue(meanToEllipticEccentric(anomaly, e), e);
  248.                     break;
  249.                 case ECCENTRIC :
  250.                     this.v = (a < 0) ?
  251.                              hyperbolicEccentricToTrue(anomaly, e) :
  252.                              ellipticEccentricToTrue(anomaly, e);
  253.                     break;
  254.                 case TRUE :
  255.                     this.v = anomaly;
  256.                     break;
  257.                 default : // this should never happen
  258.                     throw new OrekitInternalError(null);
  259.             }
  260.             this.vDot = Double.NaN;
  261.         }

  263.         // check true anomaly range
  264.         if (1 + e * FastMath.cos(v) <= 0) {
  265.             final double vMax = FastMath.acos(-1 / e);
  266.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
  267.                                                      v, e, -vMax, vMax);
  268.         }

  270.         this.partialPV = null;

  272.     }

  274.     /** Constructor from Cartesian parameters.
  275.      *
  276.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  277.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  278.      * use {@code mu} and the position to compute the acceleration, including
  279.      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
  280.      *
  281.      * @param pvCoordinates the PVCoordinates of the satellite
  282.      * @param frame the frame in which are defined the {@link PVCoordinates}
  283.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  284.      * @param mu central attraction coefficient (m³/s²)
  285.      * @exception IllegalArgumentException if frame is not a {@link
  286.      * Frame#isPseudoInertial pseudo-inertial frame}
  287.      */
  288.     public KeplerianOrbit(final TimeStampedPVCoordinates pvCoordinates,
  289.                           final Frame frame, final double mu)
  290.         throws IllegalArgumentException {
  291.         this(pvCoordinates, frame, mu, hasNonKeplerianAcceleration(pvCoordinates, mu));
  292.     }

  294.     /** Constructor from Cartesian parameters.
  295.      *
  296.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  297.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  298.      * use {@code mu} and the position to compute the acceleration, including
  299.      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
  300.      *
  301.      * @param pvCoordinates the PVCoordinates of the satellite
  302.      * @param frame the frame in which are defined the {@link PVCoordinates}
  303.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  304.      * @param mu central attraction coefficient (m³/s²)
  305.      * @param reliableAcceleration if true, the acceleration is considered to be reliable
  306.      * @exception IllegalArgumentException if frame is not a {@link
  307.      * Frame#isPseudoInertial pseudo-inertial frame}
  308.      */
  309.     private KeplerianOrbit(final TimeStampedPVCoordinates pvCoordinates,
  310.                            final Frame frame, final double mu,
  311.                            final boolean reliableAcceleration)
  312.         throws IllegalArgumentException {
  313.         super(pvCoordinates, frame, mu);

  315.         // compute inclination
  316.         final Vector3D momentum = pvCoordinates.getMomentum();
  317.         final double m2 = momentum.getNormSq();
  318.         i = Vector3D.angle(momentum, Vector3D.PLUS_K);

  320.         // compute right ascension of ascending node
  321.         raan = Vector3D.crossProduct(Vector3D.PLUS_K, momentum).getAlpha();

  323.         // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic)
  324.         final Vector3D pvP     = pvCoordinates.getPosition();
  325.         final Vector3D pvV     = pvCoordinates.getVelocity();
  326.         final Vector3D pvA     = pvCoordinates.getAcceleration();
  327.         final double   r2      = pvP.getNormSq();
  328.         final double   r       = FastMath.sqrt(r2);
  329.         final double   V2      = pvV.getNormSq();
  330.         final double   rV2OnMu = r * V2 / mu;

  332.         // compute semi-major axis (will be negative for hyperbolic orbits)
  333.         a = r / (2 - rV2OnMu);
  334.         final double muA = mu * a;

  336.         // compute true anomaly
  337.         if (a > 0) {
  338.             // elliptic or circular orbit
  339.             final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
  340.             final double eCE = rV2OnMu - 1;
  341.             e = FastMath.sqrt(eSE * eSE + eCE * eCE);
  342.             v = ellipticEccentricToTrue(FastMath.atan2(eSE, eCE), e);
  343.         } else {
  344.             // hyperbolic orbit
  345.             final double eSH = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA);
  346.             final double eCH = rV2OnMu - 1;
  347.             e = FastMath.sqrt(1 - m2 / muA);
  348.             v = hyperbolicEccentricToTrue(FastMath.log((eCH + eSH) / (eCH - eSH)) / 2, e);
  349.         }

  351.         // Checking eccentricity range
  352.         checkParameterRangeInclusive(ECCENTRICITY, e, 0.0, Double.POSITIVE_INFINITY);

  354.         // compute perigee argument
  355.         final Vector3D node = new Vector3D(raan, 0.0);
  356.         final double px = Vector3D.dotProduct(pvP, node);
  357.         final double py = Vector3D.dotProduct(pvP, Vector3D.crossProduct(momentum, node)) / FastMath.sqrt(m2);
  358.         pa = FastMath.atan2(py, px) - v;

  360.         partialPV = pvCoordinates;

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

  365.             final double[][] jacobian = new double[6][6];
  366.             getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);

  368.             final Vector3D keplerianAcceleration    = new Vector3D(-mu / (r * r2), pvP);
  369.             final Vector3D nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  370.             final double   aX                       = nonKeplerianAcceleration.getX();
  371.             final double   aY                       = nonKeplerianAcceleration.getY();
  372.             final double   aZ                       = nonKeplerianAcceleration.getZ();
  373.             aDot    = jacobian[0][3] * aX + jacobian[0][4] * aY + jacobian[0][5] * aZ;
  374.             eDot    = jacobian[1][3] * aX + jacobian[1][4] * aY + jacobian[1][5] * aZ;
  375.             iDot    = jacobian[2][3] * aX + jacobian[2][4] * aY + jacobian[2][5] * aZ;
  376.             paDot   = jacobian[3][3] * aX + jacobian[3][4] * aY + jacobian[3][5] * aZ;
  377.             raanDot = jacobian[4][3] * aX + jacobian[4][4] * aY + jacobian[4][5] * aZ;

  379.             // in order to compute true anomaly derivative, we must compute
  380.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  381.             final double MDot = getKeplerianMeanMotion() +
  382.                                 jacobian[5][3] * aX + jacobian[5][4] * aY + jacobian[5][5] * aZ;
  383.             final UnivariateDerivative1 eUD = new UnivariateDerivative1(e, eDot);
  384.             final UnivariateDerivative1 MUD = new UnivariateDerivative1(getMeanAnomaly(), MDot);
  385.             final UnivariateDerivative1 vUD = (a < 0) ?
  386.                                             FieldKeplerianOrbit.hyperbolicEccentricToTrue(FieldKeplerianOrbit.meanToHyperbolicEccentric(MUD, eUD), eUD) :
  387.                                             FieldKeplerianOrbit.ellipticEccentricToTrue(FieldKeplerianOrbit.meanToEllipticEccentric(MUD, eUD), eUD);
  388.             vDot = vUD.getDerivative(1);

  390.         } else {
  391.             // acceleration is either almost zero or NaN,
  392.             // we assume acceleration was not known
  393.             // we don't set up derivatives
  394.             aDot    = Double.NaN;
  395.             eDot    = Double.NaN;
  396.             iDot    = Double.NaN;
  397.             paDot   = Double.NaN;
  398.             raanDot = Double.NaN;
  399.             vDot    = Double.NaN;
  400.         }

  402.     }

  404.     /** Constructor from Cartesian parameters.
  405.      *
  406.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  407.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  408.      * use {@code mu} and the position to compute the acceleration, including
  409.      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
  410.      *
  411.      * @param pvCoordinates the PVCoordinates of the satellite
  412.      * @param frame the frame in which are defined the {@link PVCoordinates}
  413.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  414.      * @param date date of the orbital parameters
  415.      * @param mu central attraction coefficient (m³/s²)
  416.      * @exception IllegalArgumentException if frame is not a {@link
  417.      * Frame#isPseudoInertial pseudo-inertial frame}
  418.      */
  419.     public KeplerianOrbit(final PVCoordinates pvCoordinates,
  420.                           final Frame frame, final AbsoluteDate date, final double mu)
  421.         throws IllegalArgumentException {
  422.         this(new TimeStampedPVCoordinates(date, pvCoordinates), frame, mu);
  423.     }

  425.     /** Constructor from any kind of orbital parameters.
  426.      * @param op orbital parameters to copy
  427.      */
  428.     public KeplerianOrbit(final Orbit op) {
  429.         this(op.getPVCoordinates(), op.getFrame(), op.getMu(), op.hasDerivatives());
  430.     }

  432.     /** {@inheritDoc} */
  433.     public OrbitType getType() {
  434.         return OrbitType.KEPLERIAN;
  435.     }

  437.     /** {@inheritDoc} */
  438.     public double getA() {
  439.         return a;
  440.     }

  442.     /** {@inheritDoc} */
  443.     public double getADot() {
  444.         return aDot;
  445.     }

  447.     /** {@inheritDoc} */
  448.     public double getE() {
  449.         return e;
  450.     }

  452.     /** {@inheritDoc} */
  453.     public double getEDot() {
  454.         return eDot;
  455.     }

  457.     /** {@inheritDoc} */
  458.     public double getI() {
  459.         return i;
  460.     }

  462.     /** {@inheritDoc} */
  463.     public double getIDot() {
  464.         return iDot;
  465.     }

  467.     /** Get the perigee argument.
  468.      * @return perigee argument (rad)
  469.      */
  470.     public double getPerigeeArgument() {
  471.         return pa;
  472.     }

  474.     /** Get the perigee argument derivative.
  475.      * <p>
  476.      * If the orbit was created without derivatives, the value returned is {@link Double#NaN}.
  477.      * </p>
  478.      * @return perigee argument derivative (rad/s)
  479.      * @since 9.0
  480.      */
  481.     public double getPerigeeArgumentDot() {
  482.         return paDot;
  483.     }

  485.     /** Get the right ascension of the ascending node.
  486.      * @return right ascension of the ascending node (rad)
  487.      */
  488.     public double getRightAscensionOfAscendingNode() {
  489.         return raan;
  490.     }

  492.     /** Get the right ascension of the ascending node derivative.
  493.      * <p>
  494.      * If the orbit was created without derivatives, the value returned is {@link Double#NaN}.
  495.      * </p>
  496.      * @return right ascension of the ascending node derivative (rad/s)
  497.      * @since 9.0
  498.      */
  499.     public double getRightAscensionOfAscendingNodeDot() {
  500.         return raanDot;
  501.     }

  503.     /** Get the true anomaly.
  504.      * @return true anomaly (rad)
  505.      */
  506.     public double getTrueAnomaly() {
  507.         return v;
  508.     }

  510.     /** Get the true anomaly derivative.
  511.      * @return true anomaly derivative (rad/s)
  512.      */
  513.     public double getTrueAnomalyDot() {
  514.         return vDot;
  515.     }

  517.     /** Get the eccentric anomaly.
  518.      * @return eccentric anomaly (rad)
  519.      */
  520.     public double getEccentricAnomaly() {
  521.         return (a < 0) ? trueToHyperbolicEccentric(v, e) : trueToEllipticEccentric(v, e);
  522.     }

  524.     /** Get the eccentric anomaly derivative.
  525.      * @return eccentric anomaly derivative (rad/s)
  526.      * @since 9.0
  527.      */
  528.     public double getEccentricAnomalyDot() {
  529.         final UnivariateDerivative1 eUD = new UnivariateDerivative1(e, eDot);
  530.         final UnivariateDerivative1 vUD = new UnivariateDerivative1(v, vDot);
  531.         final UnivariateDerivative1 EUD = (a < 0) ?
  532.                                         FieldKeplerianOrbit.trueToHyperbolicEccentric(vUD, eUD) :
  533.                                         FieldKeplerianOrbit.trueToEllipticEccentric(vUD, eUD);
  534.         return EUD.getDerivative(1);
  535.     }

  537.     /** Get the mean anomaly.
  538.      * @return mean anomaly (rad)
  539.      */
  540.     public double getMeanAnomaly() {
  541.         return (a < 0) ?
  542.                hyperbolicEccentricToMean(trueToHyperbolicEccentric(v, e), e) :
  543.                ellipticEccentricToMean(trueToEllipticEccentric(v, e), e);
  544.     }

  546.     /** Get the mean anomaly derivative.
  547.      * @return mean anomaly derivative (rad/s)
  548.      * @since 9.0
  549.      */
  550.     public double getMeanAnomalyDot() {
  551.         final UnivariateDerivative1 eUD = new UnivariateDerivative1(e, eDot);
  552.         final UnivariateDerivative1 vUD = new UnivariateDerivative1(v, vDot);
  553.         final UnivariateDerivative1 MUD = (a < 0) ?
  554.                                         FieldKeplerianOrbit.hyperbolicEccentricToMean(FieldKeplerianOrbit.trueToHyperbolicEccentric(vUD, eUD), eUD) :
  555.                                         FieldKeplerianOrbit.ellipticEccentricToMean(FieldKeplerianOrbit.trueToEllipticEccentric(vUD, eUD), eUD);
  556.         return MUD.getDerivative(1);
  557.     }

  559.     /** Get the anomaly.
  560.      * @param type type of the angle
  561.      * @return anomaly (rad)
  562.      */
  563.     public double getAnomaly(final PositionAngle type) {
  564.         return (type == PositionAngle.MEAN) ? getMeanAnomaly() :
  565.                                               ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomaly() :
  566.                                                                                    getTrueAnomaly());
  567.     }

  569.     /** Get the anomaly derivative.
  570.      * @param type type of the angle
  571.      * @return anomaly derivative (rad/s)
  572.      * @since 9.0
  573.      */
  574.     public double getAnomalyDot(final PositionAngle type) {
  575.         return (type == PositionAngle.MEAN) ? getMeanAnomalyDot() :
  576.                                               ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomalyDot() :
  577.                                                                                    getTrueAnomalyDot());
  578.     }

  580.     /** Computes the true anomaly from the elliptic eccentric anomaly.
  581.      * @param E eccentric anomaly (rad)
  582.      * @param e eccentricity
  583.      * @return v the true anomaly
  584.      * @since 9.0
  585.      */
  586.     public static double ellipticEccentricToTrue(final double E, final double e) {
  587.         final double beta = e / (1 + FastMath.sqrt((1 - e) * (1 + e)));
  588.         final SinCos scE = FastMath.sinCos(E);
  589.         return E + 2 * FastMath.atan(beta * scE.sin() / (1 - beta * scE.cos()));
  590.     }

  592.     /** Computes the elliptic eccentric anomaly from the true anomaly.
  593.      * @param v true anomaly (rad)
  594.      * @param e eccentricity
  595.      * @return E the elliptic eccentric anomaly
  596.      * @since 9.0
  597.      */
  598.     public static double trueToEllipticEccentric(final double v, final double e) {
  599.         final double beta = e / (1 + FastMath.sqrt(1 - e * e));
  600.         final SinCos scv = FastMath.sinCos(v);
  601.         return v - 2 * FastMath.atan(beta * scv.sin() / (1 + beta * scv.cos()));
  602.     }

  604.     /** Computes the true anomaly from the hyperbolic eccentric anomaly.
  605.      * @param H hyperbolic eccentric anomaly (rad)
  606.      * @param e eccentricity
  607.      * @return v the true anomaly
  608.      */
  609.     public static double hyperbolicEccentricToTrue(final double H, final double e) {
  610.         return 2 * FastMath.atan(FastMath.sqrt((e + 1) / (e - 1)) * FastMath.tanh(H / 2));
  611.     }

  613.     /** Computes the hyperbolic eccentric anomaly from the true anomaly.
  614.      * @param v true anomaly (rad)
  615.      * @param e eccentricity
  616.      * @return H the hyperbolic eccentric anomaly
  617.      * @since 9.0
  618.      */
  619.     public static double trueToHyperbolicEccentric(final double v, final double e) {
  620.         final SinCos scv = FastMath.sinCos(v);
  621.         final double sinhH = FastMath.sqrt(e * e - 1) * scv.sin() / (1 + e * scv.cos());
  622.         return FastMath.asinh(sinhH);
  623.     }

  625.     /** Computes the elliptic eccentric anomaly from the mean anomaly.
  626.      * <p>
  627.      * The algorithm used here for solving Kepler equation has been published
  628.      * in: "Procedures for  solving Kepler's Equation", A. W. Odell and
  629.      * R. H. Gooding, Celestial Mechanics 38 (1986) 307-334
  630.      * </p>
  631.      * @param M mean anomaly (rad)
  632.      * @param e eccentricity
  633.      * @return E the eccentric anomaly
  634.      */
  635.     public static double meanToEllipticEccentric(final double M, final double e) {

  637.         // reduce M to [-PI PI) interval
  638.         final double reducedM = MathUtils.normalizeAngle(M, 0.0);

  640.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  641.         double E;
  642.         if (FastMath.abs(reducedM) < 1.0 / 6.0) {
  643.             E = reducedM + e * (FastMath.cbrt(6 * reducedM) - reducedM);
  644.         } else {
  645.             if (reducedM < 0) {
  646.                 final double w = FastMath.PI + reducedM;
  647.                 E = reducedM + e * (A * w / (B - w) - FastMath.PI - reducedM);
  648.             } else {
  649.                 final double w = FastMath.PI - reducedM;
  650.                 E = reducedM + e * (FastMath.PI - A * w / (B - w) - reducedM);
  651.             }
  652.         }

  654.         final double e1 = 1 - e;
  655.         final boolean noCancellationRisk = (e1 + E * E / 6) >= 0.1;

  657.         // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
  658.         for (int j = 0; j < 2; ++j) {
  659.             final double f;
  660.             double fd;
  661.             final SinCos sc   = FastMath.sinCos(E);
  662.             final double fdd  = e * sc.sin();
  663.             final double fddd = e * sc.cos();
  664.             if (noCancellationRisk) {
  665.                 f  = (E - fdd) - reducedM;
  666.                 fd = 1 - fddd;
  667.             } else {
  668.                 f  = eMeSinE(E, e) - reducedM;
  669.                 final double s = FastMath.sin(0.5 * E);
  670.                 fd = e1 + 2 * e * s * s;
  671.             }
  672.             final double dee = f * fd / (0.5 * f * fdd - fd * fd);

  674.             // update eccentric anomaly, using expressions that limit underflow problems
  675.             final double w = fd + 0.5 * dee * (fdd + dee * fddd / 3);
  676.             fd += dee * (fdd + 0.5 * dee * fddd);
  677.             E  -= (f - dee * (fd - w)) / fd;

  679.         }

  681.         // expand the result back to original range
  682.         E += M - reducedM;

  684.         return E;

  686.     }

  688.     /** Accurate computation of E - e sin(E).
  689.      * <p>
  690.      * This method is used when E is close to 0 and e close to 1,
  691.      * i.e. near the perigee of almost parabolic orbits
  692.      * </p>
  693.      * @param E eccentric anomaly
  694.      * @param e eccentricity
  695.      * @return E - e sin(E)
  696.      */
  697.     private static double eMeSinE(final double E, final double e) {
  698.         double x = (1 - e) * FastMath.sin(E);
  699.         final double mE2 = -E * E;
  700.         double term = E;
  701.         double d    = 0;
  702.         // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
  703.         for (double x0 = Double.NaN; !Double.valueOf(x).equals(Double.valueOf(x0));) {
  704.             d += 2;
  705.             term *= mE2 / (d * (d + 1));
  706.             x0 = x;
  707.             x = x - term;
  708.         }
  709.         return x;
  710.     }

  712.     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
  713.      * <p>
  714.      * The algorithm used here for solving hyperbolic Kepler equation is
  715.      * Danby's iterative method (3rd order) with Vallado's initial guess.
  716.      * </p>
  717.      * @param M mean anomaly (rad)
  718.      * @param ecc eccentricity
  719.      * @return H the hyperbolic eccentric anomaly
  720.      */
  721.     public static double meanToHyperbolicEccentric(final double M, final double ecc) {

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

  725.         // Initial guess
  726.         double H;
  727.         if (ecc < 1.6) {
  728.             if (-FastMath.PI < M && M < 0. || M > FastMath.PI) {
  729.                 H = M - ecc;
  730.             } else {
  731.                 H = M + ecc;
  732.             }
  733.         } else {
  734.             if (ecc < 3.6 && FastMath.abs(M) > FastMath.PI) {
  735.                 H = M - FastMath.copySign(ecc, M);
  736.             } else {
  737.                 H = M / (ecc - 1.);
  738.             }
  739.         }

  741.         // Iterative computation
  742.         int iter = 0;
  743.         do {
  744.             final double f3  = ecc * FastMath.cosh(H);
  745.             final double f2  = ecc * FastMath.sinh(H);
  746.             final double f1  = f3 - 1.;
  747.             final double f0  = f2 - H - M;
  748.             final double f12 = 2. * f1;
  749.             final double d   = f0 / f12;
  750.             final double fdf = f1 - d * f2;
  751.             final double ds  = f0 / fdf;

  753.             final double shift = f0 / (fdf + ds * ds * f3 / 6.);

  755.             H -= shift;

  757.             if (FastMath.abs(shift) <= 1.0e-12) {
  758.                 return H;
  759.             }

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

  763.         throw new MathIllegalStateException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
  764.                                             iter);
  765.     }

  767.     /** Computes the mean anomaly from the elliptic eccentric anomaly.
  768.      * @param E eccentric anomaly (rad)
  769.      * @param e eccentricity
  770.      * @return M the mean anomaly
  771.      * @since 9.0
  772.      */
  773.     public static double ellipticEccentricToMean(final double E, final double e) {
  774.         return E - e * FastMath.sin(E);
  775.     }

  777.     /** Computes the mean anomaly from the hyperbolic eccentric anomaly.
  778.      * @param H hyperbolic eccentric anomaly (rad)
  779.      * @param e eccentricity
  780.      * @return M the mean anomaly
  781.      * @since 9.0
  782.      */
  783.     public static double hyperbolicEccentricToMean(final double H, final double e) {
  784.         return e * FastMath.sinh(H) - H;
  785.     }

  787.     /** {@inheritDoc} */
  788.     public double getEquinoctialEx() {
  789.         return e * FastMath.cos(pa + raan);
  790.     }

  792.     /** {@inheritDoc} */
  793.     public double getEquinoctialExDot() {
  794.         final double paPraan = pa + raan;
  795.         final SinCos sc      = FastMath.sinCos(paPraan);
  796.         return eDot * sc.cos() - e * sc.sin() * (paDot + raanDot);
  797.     }

  799.     /** {@inheritDoc} */
  800.     public double getEquinoctialEy() {
  801.         return e * FastMath.sin(pa + raan);
  802.     }

  804.     /** {@inheritDoc} */
  805.     public double getEquinoctialEyDot() {
  806.         final double paPraan = pa + raan;
  807.         final SinCos sc      = FastMath.sinCos(paPraan);
  808.         return eDot * sc.sin() + e * sc.cos() * (paDot + raanDot);
  809.     }

  811.     /** {@inheritDoc} */
  812.     public double getHx() {
  813.         // Check for equatorial retrograde orbit
  814.         if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
  815.             return Double.NaN;
  816.         }
  817.         return FastMath.cos(raan) * FastMath.tan(0.5 * i);
  818.     }

  820.     /** {@inheritDoc} */
  821.     public double getHxDot() {
  822.         // Check for equatorial retrograde orbit
  823.         if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
  824.             return Double.NaN;
  825.         }
  826.         final SinCos sc      = FastMath.sinCos(raan);
  827.         final double tan     = FastMath.tan(0.5 * i);
  828.         return 0.5 * (1 + tan * tan) * sc.cos() * iDot - tan * sc.sin() * raanDot;
  829.     }

  831.     /** {@inheritDoc} */
  832.     public double getHy() {
  833.         // Check for equatorial retrograde orbit
  834.         if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
  835.             return Double.NaN;
  836.         }
  837.         return  FastMath.sin(raan) * FastMath.tan(0.5 * i);
  838.     }

  840.     /** {@inheritDoc} */
  841.     public double getHyDot() {
  842.         // Check for equatorial retrograde orbit
  843.         if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
  844.             return Double.NaN;
  845.         }
  846.         final SinCos sc      = FastMath.sinCos(raan);
  847.         final double tan     = FastMath.tan(0.5 * i);
  848.         return 0.5 * (1 + tan * tan) * sc.sin() * iDot + tan * sc.cos() * raanDot;
  849.     }

  851.     /** {@inheritDoc} */
  852.     public double getLv() {
  853.         return pa + raan + v;
  854.     }

  856.     /** {@inheritDoc} */
  857.     public double getLvDot() {
  858.         return paDot + raanDot + vDot;
  859.     }

  861.     /** {@inheritDoc} */
  862.     public double getLE() {
  863.         return pa + raan + getEccentricAnomaly();
  864.     }

  866.     /** {@inheritDoc} */
  867.     public double getLEDot() {
  868.         return paDot + raanDot + getEccentricAnomalyDot();
  869.     }

  871.     /** {@inheritDoc} */
  872.     public double getLM() {
  873.         return pa + raan + getMeanAnomaly();
  874.     }

  876.     /** {@inheritDoc} */
  877.     public double getLMDot() {
  878.         return paDot + raanDot + getMeanAnomalyDot();
  879.     }

  881.     /** Compute position and velocity but not acceleration.
  882.      */
  883.     private void computePVWithoutA() {

  885.         if (partialPV != null) {
  886.             // already computed
  887.             return;
  888.         }

  890.         // preliminary variables
  891.         final SinCos scRaan  = FastMath.sinCos(raan);
  892.         final SinCos scPa    = FastMath.sinCos(pa);
  893.         final SinCos scI     = FastMath.sinCos(i);
  894.         final double cosRaan = scRaan.cos();
  895.         final double sinRaan = scRaan.sin();
  896.         final double cosPa   = scPa.cos();
  897.         final double sinPa   = scPa.sin();
  898.         final double cosI    = scI.cos();
  899.         final double sinI    = scI.sin();

  901.         final double crcp    = cosRaan * cosPa;
  902.         final double crsp    = cosRaan * sinPa;
  903.         final double srcp    = sinRaan * cosPa;
  904.         final double srsp    = sinRaan * sinPa;

  906.         // reference axes defining the orbital plane
  907.         final Vector3D p = new Vector3D( crcp - cosI * srsp,  srcp + cosI * crsp, sinI * sinPa);
  908.         final Vector3D q = new Vector3D(-crsp - cosI * srcp, -srsp + cosI * crcp, sinI * cosPa);

  910.         if (a > 0) {

  912.             // elliptical case

  914.             // elliptic eccentric anomaly
  915.             final double uME2   = (1 - e) * (1 + e);
  916.             final double s1Me2  = FastMath.sqrt(uME2);
  917.             final SinCos scE    = FastMath.sinCos(getEccentricAnomaly());
  918.             final double cosE   = scE.cos();
  919.             final double sinE   = scE.sin();

  921.             // coordinates of position and velocity in the orbital plane
  922.             final double x      = a * (cosE - e);
  923.             final double y      = a * sinE * s1Me2;
  924.             final double factor = FastMath.sqrt(getMu() / a) / (1 - e * cosE);
  925.             final double xDot   = -sinE * factor;
  926.             final double yDot   =  cosE * s1Me2 * factor;

  928.             final Vector3D position = new Vector3D(x, p, y, q);
  929.             final Vector3D velocity = new Vector3D(xDot, p, yDot, q);
  930.             partialPV = new PVCoordinates(position, velocity);

  932.         } else {

  934.             // hyperbolic case

  936.             // compute position and velocity factors
  937.             final SinCos scV       = FastMath.sinCos(v);
  938.             final double sinV      = scV.sin();
  939.             final double cosV      = scV.cos();
  940.             final double f         = a * (1 - e * e);
  941.             final double posFactor = f / (1 + e * cosV);
  942.             final double velFactor = FastMath.sqrt(getMu() / f);

  944.             final double   x            =  posFactor * cosV;
  945.             final double   y            =  posFactor * sinV;
  946.             final double   xDot         = -velFactor * sinV;
  947.             final double   yDot         =  velFactor * (e + cosV);

  949.             final Vector3D position     = new Vector3D(x, p, y, q);
  950.             final Vector3D velocity     = new Vector3D(xDot, p, yDot, q);
  951.             partialPV = new PVCoordinates(position, velocity);

  953.         }

  955.     }

  957.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  958.      * <p>
  959.      * This method should be called only when {@link #hasDerivatives()} returns true.
  960.      * </p>
  961.      * @return non-Keplerian part of the acceleration
  962.      */
  963.     private Vector3D nonKeplerianAcceleration() {

  965.         final double[][] dCdP = new double[6][6];
  966.         getJacobianWrtParameters(PositionAngle.MEAN, dCdP);

  968.         final double nonKeplerianMeanMotion = getMeanAnomalyDot() - getKeplerianMeanMotion();
  969.         final double nonKeplerianAx = dCdP[3][0] * aDot    + dCdP[3][1] * eDot    + dCdP[3][2] * iDot    +
  970.                                       dCdP[3][3] * paDot   + dCdP[3][4] * raanDot + dCdP[3][5] * nonKeplerianMeanMotion;
  971.         final double nonKeplerianAy = dCdP[4][0] * aDot    + dCdP[4][1] * eDot    + dCdP[4][2] * iDot    +
  972.                                       dCdP[4][3] * paDot   + dCdP[4][4] * raanDot + dCdP[4][5] * nonKeplerianMeanMotion;
  973.         final double nonKeplerianAz = dCdP[5][0] * aDot    + dCdP[5][1] * eDot    + dCdP[5][2] * iDot    +
  974.                                       dCdP[5][3] * paDot   + dCdP[5][4] * raanDot + dCdP[5][5] * nonKeplerianMeanMotion;

  976.         return new Vector3D(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);

  978.     }

  980.     /** {@inheritDoc} */
  981.     protected TimeStampedPVCoordinates initPVCoordinates() {

  983.         // position and velocity
  984.         computePVWithoutA();

  986.         // acceleration
  987.         final double r2 = partialPV.getPosition().getNormSq();
  988.         final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
  989.         final Vector3D acceleration = hasDerivatives() ?
  990.                                       keplerianAcceleration.add(nonKeplerianAcceleration()) :
  991.                                       keplerianAcceleration;

  993.         return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);

  995.     }

  997.     /** {@inheritDoc} */
  998.     public KeplerianOrbit shiftedBy(final double dt) {

  1000.         // use Keplerian-only motion
  1001.         final KeplerianOrbit keplerianShifted = new KeplerianOrbit(a, e, i, pa, raan,
  1002.                                                                    getMeanAnomaly() + getKeplerianMeanMotion() * dt,
  1003.                                                                    PositionAngle.MEAN, getFrame(),
  1004.                                                                    getDate().shiftedBy(dt), getMu());

  1006.         if (hasDerivatives()) {

  1008.             // extract non-Keplerian acceleration from first time derivatives
  1009.             final Vector3D nonKeplerianAcceleration = nonKeplerianAcceleration();

  1011.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  1012.             keplerianShifted.computePVWithoutA();
  1013.             final Vector3D fixedP   = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
  1014.                                                    0.5 * dt * dt, nonKeplerianAcceleration);
  1015.             final double   fixedR2 = fixedP.getNormSq();
  1016.             final double   fixedR  = FastMath.sqrt(fixedR2);
  1017.             final Vector3D fixedV  = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
  1018.                                                   dt, nonKeplerianAcceleration);
  1019.             final Vector3D fixedA  = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
  1020.                                                   1, nonKeplerianAcceleration);

  1022.             // build a new orbit, taking non-Keplerian acceleration into account
  1023.             return new KeplerianOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
  1024.                                                                    fixedP, fixedV, fixedA),
  1025.                                       keplerianShifted.getFrame(), keplerianShifted.getMu());

  1027.         } else {
  1028.             // Keplerian-only motion is all we can do
  1029.             return keplerianShifted;
  1030.         }

  1032.     }

  1034.     /** {@inheritDoc}
  1035.      * <p>
  1036.      * The interpolated instance is created by polynomial Hermite interpolation
  1037.      * on Keplerian elements, without derivatives (which means the interpolation
  1038.      * falls back to Lagrange interpolation only).
  1039.      * </p>
  1040.      * <p>
  1041.      * As this implementation of interpolation is polynomial, it should be used only
  1042.      * with small samples (about 10-20 points) in order to avoid <a
  1043.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  1044.      * and numerical problems (including NaN appearing).
  1045.      * </p>
  1046.      * <p>
  1047.      * If orbit interpolation on large samples is needed, using the {@link
  1048.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  1049.      * low-level interpolation. The Ephemeris class automatically handles selection of
  1050.      * a neighboring sub-sample with a predefined number of point from a large global sample
  1051.      * in a thread-safe way.
  1052.      * </p>
  1053.      */
  1054.     public KeplerianOrbit interpolate(final AbsoluteDate date, final Stream<Orbit> sample) {

  1056.         // first pass to check if derivatives are available throughout the sample
  1057.         final List<Orbit> list = sample.collect(Collectors.toList());
  1058.         boolean useDerivatives = true;
  1059.         for (final Orbit orbit : list) {
  1060.             useDerivatives = useDerivatives && orbit.hasDerivatives();
  1061.         }

  1063.         // set up an interpolator
  1064.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  1066.         // second pass to feed interpolator
  1067.         AbsoluteDate previousDate = null;
  1068.         double       previousPA   = Double.NaN;
  1069.         double       previousRAAN = Double.NaN;
  1070.         double       previousM    = Double.NaN;
  1071.         for (final Orbit orbit : list) {
  1072.             final KeplerianOrbit kep = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(orbit);
  1073.             final double continuousPA;
  1074.             final double continuousRAAN;
  1075.             final double continuousM;
  1076.             if (previousDate == null) {
  1077.                 continuousPA   = kep.getPerigeeArgument();
  1078.                 continuousRAAN = kep.getRightAscensionOfAscendingNode();
  1079.                 continuousM    = kep.getMeanAnomaly();
  1080.             } else {
  1081.                 final double dt      = kep.getDate().durationFrom(previousDate);
  1082.                 final double keplerM = previousM + kep.getKeplerianMeanMotion() * dt;
  1083.                 continuousPA   = MathUtils.normalizeAngle(kep.getPerigeeArgument(), previousPA);
  1084.                 continuousRAAN = MathUtils.normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
  1085.                 continuousM    = MathUtils.normalizeAngle(kep.getMeanAnomaly(), keplerM);
  1086.             }
  1087.             previousDate = kep.getDate();
  1088.             previousPA   = continuousPA;
  1089.             previousRAAN = continuousRAAN;
  1090.             previousM    = continuousM;
  1091.             if (useDerivatives) {
  1092.                 interpolator.addSamplePoint(kep.getDate().durationFrom(date),
  1093.                                             new double[] {
  1094.                                                 kep.getA(),
  1095.                                                 kep.getE(),
  1096.                                                 kep.getI(),
  1097.                                                 continuousPA,
  1098.                                                 continuousRAAN,
  1099.                                                 continuousM
  1100.                                             }, new double[] {
  1101.                                                 kep.getADot(),
  1102.                                                 kep.getEDot(),
  1103.                                                 kep.getIDot(),
  1104.                                                 kep.getPerigeeArgumentDot(),
  1105.                                                 kep.getRightAscensionOfAscendingNodeDot(),
  1106.                                                 kep.getMeanAnomalyDot()
  1107.                                             });
  1108.             } else {
  1109.                 interpolator.addSamplePoint(kep.getDate().durationFrom(date),
  1110.                                             new double[] {
  1111.                                                 kep.getA(),
  1112.                                                 kep.getE(),
  1113.                                                 kep.getI(),
  1114.                                                 continuousPA,
  1115.                                                 continuousRAAN,
  1116.                                                 continuousM
  1117.                                             });
  1118.             }
  1119.         }

  1121.         // interpolate
  1122.         final double[][] interpolated = interpolator.derivatives(0.0, 1);

  1124.         // build a new interpolated instance
  1125.         return new KeplerianOrbit(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  1126.                                   interpolated[0][3], interpolated[0][4], interpolated[0][5],
  1127.                                   interpolated[1][0], interpolated[1][1], interpolated[1][2],
  1128.                                   interpolated[1][3], interpolated[1][4], interpolated[1][5],
  1129.                                   PositionAngle.MEAN, getFrame(), date, getMu());

  1131.     }

  1133.     /** {@inheritDoc} */
  1134.     protected double[][] computeJacobianMeanWrtCartesian() {
  1135.         if (a > 0) {
  1136.             return computeJacobianMeanWrtCartesianElliptical();
  1137.         } else {
  1138.             return computeJacobianMeanWrtCartesianHyperbolic();
  1139.         }
  1140.     }

  1142.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1143.      * <p>
  1144.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1145.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1146.      * yDot for j=4, zDot for j=5).
  1147.      * </p>
  1148.      * @return 6x6 Jacobian matrix
  1149.      */
  1150.     private double[][] computeJacobianMeanWrtCartesianElliptical() {

  1152.         final double[][] jacobian = new double[6][6];

  1154.         // compute various intermediate parameters
  1155.         computePVWithoutA();
  1156.         final Vector3D position = partialPV.getPosition();
  1157.         final Vector3D velocity = partialPV.getVelocity();
  1158.         final Vector3D momentum = partialPV.getMomentum();
  1159.         final double v2         = velocity.getNormSq();
  1160.         final double r2         = position.getNormSq();
  1161.         final double r          = FastMath.sqrt(r2);
  1162.         final double r3         = r * r2;

  1164.         final double px         = position.getX();
  1165.         final double py         = position.getY();
  1166.         final double pz         = position.getZ();
  1167.         final double vx         = velocity.getX();
  1168.         final double vy         = velocity.getY();
  1169.         final double vz         = velocity.getZ();
  1170.         final double mx         = momentum.getX();
  1171.         final double my         = momentum.getY();
  1172.         final double mz         = momentum.getZ();

  1174.         final double mu         = getMu();
  1175.         final double sqrtMuA    = FastMath.sqrt(a * mu);
  1176.         final double sqrtAoMu   = FastMath.sqrt(a / mu);
  1177.         final double a2         = a * a;
  1178.         final double twoA       = 2 * a;
  1179.         final double rOnA       = r / a;

  1181.         final double oMe2       = 1 - e * e;
  1182.         final double epsilon    = FastMath.sqrt(oMe2);
  1183.         final double sqrtRec    = 1 / epsilon;

  1185.         final SinCos scI        = FastMath.sinCos(i);
  1186.         final SinCos scPA       = FastMath.sinCos(pa);
  1187.         final double cosI       = scI.cos();
  1188.         final double sinI       = scI.sin();
  1189.         final double cosPA      = scPA.cos();
  1190.         final double sinPA      = scPA.sin();

  1192.         final double pv         = Vector3D.dotProduct(position, velocity);
  1193.         final double cosE       = (a - r) / (a * e);
  1194.         final double sinE       = pv / (e * sqrtMuA);

  1196.         // da
  1197.         final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
  1198.         final Vector3D vectorARDot = velocity.scalarMultiply(2 * a2 / mu);
  1199.         fillHalfRow(1, vectorAR,    jacobian[0], 0);
  1200.         fillHalfRow(1, vectorARDot, jacobian[0], 3);

  1202.         // de
  1203.         final double factorER3 = pv / twoA;
  1204.         final Vector3D vectorER   = new Vector3D(cosE * v2 / (r * mu), position,
  1205.                                                  sinE / sqrtMuA, velocity,
  1206.                                                  -factorER3 * sinE / sqrtMuA, vectorAR);
  1207.         final Vector3D vectorERDot = new Vector3D(sinE / sqrtMuA, position,
  1208.                                                   cosE * 2 * r / mu, velocity,
  1209.                                                   -factorER3 * sinE / sqrtMuA, vectorARDot);
  1210.         fillHalfRow(1, vectorER,    jacobian[1], 0);
  1211.         fillHalfRow(1, vectorERDot, jacobian[1], 3);

  1213.         // dE / dr (Eccentric anomaly)
  1214.         final double coefE = cosE / (e * sqrtMuA);
  1215.         final Vector3D  vectorEAnR =
  1216.             new Vector3D(-sinE * v2 / (e * r * mu), position, coefE, velocity,
  1217.                          -factorER3 * coefE, vectorAR);

  1219.         // dE / drDot
  1220.         final Vector3D  vectorEAnRDot =
  1221.             new Vector3D(-sinE * 2 * r / (e * mu), velocity, coefE, position,
  1222.                          -factorER3 * coefE, vectorARDot);

  1224.         // precomputing some more factors
  1225.         final double s1 = -sinE * pz / r - cosE * vz * sqrtAoMu;
  1226.         final double s2 = -cosE * pz / r3;
  1227.         final double s3 = -sinE * vz / (2 * sqrtMuA);
  1228.         final double t1 = sqrtRec * (cosE * pz / r - sinE * vz * sqrtAoMu);
  1229.         final double t2 = sqrtRec * (-sinE * pz / r3);
  1230.         final double t3 = sqrtRec * (cosE - e) * vz / (2 * sqrtMuA);
  1231.         final double t4 = sqrtRec * (e * sinI * cosPA * sqrtRec - vz * sqrtAoMu);
  1232.         final Vector3D s = new Vector3D(cosE / r, Vector3D.PLUS_K,
  1233.                                         s1,       vectorEAnR,
  1234.                                         s2,       position,
  1235.                                         s3,       vectorAR);
  1236.         final Vector3D sDot = new Vector3D(-sinE * sqrtAoMu, Vector3D.PLUS_K,
  1237.                                            s1,               vectorEAnRDot,
  1238.                                            s3,               vectorARDot);
  1239.         final Vector3D t =
  1240.             new Vector3D(sqrtRec * sinE / r, Vector3D.PLUS_K).add(new Vector3D(t1, vectorEAnR,
  1241.                                                                                t2, position,
  1242.                                                                                t3, vectorAR,
  1243.                                                                                t4, vectorER));
  1244.         final Vector3D tDot = new Vector3D(sqrtRec * (cosE - e) * sqrtAoMu, Vector3D.PLUS_K,
  1245.                                            t1,                              vectorEAnRDot,
  1246.                                            t3,                              vectorARDot,
  1247.                                            t4,                              vectorERDot);

  1249.         // di
  1250.         final double factorI1 = -sinI * sqrtRec / sqrtMuA;
  1251.         final double i1 =  factorI1;
  1252.         final double i2 = -factorI1 * mz / twoA;
  1253.         final double i3 =  factorI1 * mz * e / oMe2;
  1254.         final double i4 = cosI * sinPA;
  1255.         final double i5 = cosI * cosPA;
  1256.         fillHalfRow(i1, new Vector3D(vy, -vx, 0), i2, vectorAR, i3, vectorER, i4, s, i5, t,
  1257.                     jacobian[2], 0);
  1258.         fillHalfRow(i1, new Vector3D(-py, px, 0), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
  1259.                     jacobian[2], 3);

  1261.         // dpa
  1262.         fillHalfRow(cosPA / sinI, s,    -sinPA / sinI, t,    jacobian[3], 0);
  1263.         fillHalfRow(cosPA / sinI, sDot, -sinPA / sinI, tDot, jacobian[3], 3);

  1265.         // dRaan
  1266.         final double factorRaanR = 1 / (mu * a * oMe2 * sinI * sinI);
  1267.         fillHalfRow(-factorRaanR * my, new Vector3D(  0, vz, -vy),
  1268.                      factorRaanR * mx, new Vector3D(-vz,  0,  vx),
  1269.                      jacobian[4], 0);
  1270.         fillHalfRow(-factorRaanR * my, new Vector3D( 0, -pz,  py),
  1271.                      factorRaanR * mx, new Vector3D(pz,   0, -px),
  1272.                      jacobian[4], 3);

  1274.         // dM
  1275.         fillHalfRow(rOnA, vectorEAnR,    -sinE, vectorER,    jacobian[5], 0);
  1276.         fillHalfRow(rOnA, vectorEAnRDot, -sinE, vectorERDot, jacobian[5], 3);

  1278.         return jacobian;

  1280.     }

  1282.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1283.      * <p>
  1284.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1285.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1286.      * yDot for j=4, zDot for j=5).
  1287.      * </p>
  1288.      * @return 6x6 Jacobian matrix
  1289.      */
  1290.     private double[][] computeJacobianMeanWrtCartesianHyperbolic() {

  1292.         final double[][] jacobian = new double[6][6];

  1294.         // compute various intermediate parameters
  1295.         computePVWithoutA();
  1296.         final Vector3D position = partialPV.getPosition();
  1297.         final Vector3D velocity = partialPV.getVelocity();
  1298.         final Vector3D momentum = partialPV.getMomentum();
  1299.         final double r2         = position.getNormSq();
  1300.         final double r          = FastMath.sqrt(r2);
  1301.         final double r3         = r * r2;

  1303.         final double x          = position.getX();
  1304.         final double y          = position.getY();
  1305.         final double z          = position.getZ();
  1306.         final double vx         = velocity.getX();
  1307.         final double vy         = velocity.getY();
  1308.         final double vz         = velocity.getZ();
  1309.         final double mx         = momentum.getX();
  1310.         final double my         = momentum.getY();
  1311.         final double mz         = momentum.getZ();

  1313.         final double mu         = getMu();
  1314.         final double absA       = -a;
  1315.         final double sqrtMuA    = FastMath.sqrt(absA * mu);
  1316.         final double a2         = a * a;
  1317.         final double rOa        = r / absA;

  1319.         final SinCos scI        = FastMath.sinCos(i);
  1320.         final double cosI       = scI.cos();
  1321.         final double sinI       = scI.sin();

  1323.         final double pv         = Vector3D.dotProduct(position, velocity);

  1325.         // da
  1326.         final Vector3D vectorAR = new Vector3D(-2 * a2 / r3, position);
  1327.         final Vector3D vectorARDot = velocity.scalarMultiply(-2 * a2 / mu);
  1328.         fillHalfRow(-1, vectorAR,    jacobian[0], 0);
  1329.         fillHalfRow(-1, vectorARDot, jacobian[0], 3);

  1331.         // differentials of the momentum
  1332.         final double m      = momentum.getNorm();
  1333.         final double oOm    = 1 / m;
  1334.         final Vector3D dcXP = new Vector3D(  0,  vz, -vy);
  1335.         final Vector3D dcYP = new Vector3D(-vz,   0,  vx);
  1336.         final Vector3D dcZP = new Vector3D( vy, -vx,   0);
  1337.         final Vector3D dcXV = new Vector3D(  0,  -z,   y);
  1338.         final Vector3D dcYV = new Vector3D(  z,   0,  -x);
  1339.         final Vector3D dcZV = new Vector3D( -y,   x,   0);
  1340.         final Vector3D dCP  = new Vector3D(mx * oOm, dcXP, my * oOm, dcYP, mz * oOm, dcZP);
  1341.         final Vector3D dCV  = new Vector3D(mx * oOm, dcXV, my * oOm, dcYV, mz * oOm, dcZV);

  1343.         // dp
  1344.         final double mOMu   = m / mu;
  1345.         final Vector3D dpP  = new Vector3D(2 * mOMu, dCP);
  1346.         final Vector3D dpV  = new Vector3D(2 * mOMu, dCV);

  1348.         // de
  1349.         final double p      = m * mOMu;
  1350.         final double moO2ae = 1 / (2 * absA * e);
  1351.         final double m2OaMu = -p / absA;
  1352.         fillHalfRow(moO2ae, dpP, m2OaMu * moO2ae, vectorAR,    jacobian[1], 0);
  1353.         fillHalfRow(moO2ae, dpV, m2OaMu * moO2ae, vectorARDot, jacobian[1], 3);

  1355.         // di
  1356.         final double cI1 = 1 / (m * sinI);
  1357.         final double cI2 = cosI * cI1;
  1358.         fillHalfRow(cI2, dCP, -cI1, dcZP, jacobian[2], 0);
  1359.         fillHalfRow(cI2, dCV, -cI1, dcZV, jacobian[2], 3);

  1361.         // dPA
  1362.         final double cP1     =  y * oOm;
  1363.         final double cP2     = -x * oOm;
  1364.         final double cP3     = -(mx * cP1 + my * cP2);
  1365.         final double cP4     = cP3 * oOm;
  1366.         final double cP5     = -1 / (r2 * sinI * sinI);
  1367.         final double cP6     = z  * cP5;
  1368.         final double cP7     = cP3 * cP5;
  1369.         final Vector3D dacP  = new Vector3D(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new Vector3D(-my, mx, 0));
  1370.         final Vector3D dacV  = new Vector3D(cP1, dcXV, cP2, dcYV, cP4, dCV);
  1371.         final Vector3D dpoP  = new Vector3D(cP6, dacP, cP7, Vector3D.PLUS_K);
  1372.         final Vector3D dpoV  = new Vector3D(cP6, dacV);

  1374.         final double re2     = r2 * e * e;
  1375.         final double recOre2 = (p - r) / re2;
  1376.         final double resOre2 = (pv * mOMu) / re2;
  1377.         final Vector3D dreP  = new Vector3D(mOMu, velocity, pv / mu, dCP);
  1378.         final Vector3D dreV  = new Vector3D(mOMu, position, pv / mu, dCV);
  1379.         final Vector3D davP  = new Vector3D(-resOre2, dpP, recOre2, dreP, resOre2 / r, position);
  1380.         final Vector3D davV  = new Vector3D(-resOre2, dpV, recOre2, dreV);
  1381.         fillHalfRow(1, dpoP, -1, davP, jacobian[3], 0);
  1382.         fillHalfRow(1, dpoV, -1, davV, jacobian[3], 3);

  1384.         // dRAAN
  1385.         final double cO0 = cI1 * cI1;
  1386.         final double cO1 =  mx * cO0;
  1387.         final double cO2 = -my * cO0;
  1388.         fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
  1389.         fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);

  1391.         // dM
  1392.         final double s2a    = pv / (2 * absA);
  1393.         final double oObux  = 1 / FastMath.sqrt(m * m + mu * absA);
  1394.         final double scasbu = pv * oObux;
  1395.         final Vector3D dauP = new Vector3D(1 / sqrtMuA, velocity, -s2a / sqrtMuA, vectorAR);
  1396.         final Vector3D dauV = new Vector3D(1 / sqrtMuA, position, -s2a / sqrtMuA, vectorARDot);
  1397.         final Vector3D dbuP = new Vector3D(oObux * mu / 2, vectorAR,    m * oObux, dCP);
  1398.         final Vector3D dbuV = new Vector3D(oObux * mu / 2, vectorARDot, m * oObux, dCV);
  1399.         final Vector3D dcuP = new Vector3D(oObux, velocity, -scasbu * oObux, dbuP);
  1400.         final Vector3D dcuV = new Vector3D(oObux, position, -scasbu * oObux, dbuV);
  1401.         fillHalfRow(1, dauP, -e / (1 + rOa), dcuP, jacobian[5], 0);
  1402.         fillHalfRow(1, dauV, -e / (1 + rOa), dcuV, jacobian[5], 3);

  1404.         return jacobian;

  1406.     }

  1408.     /** {@inheritDoc} */
  1409.     protected double[][] computeJacobianEccentricWrtCartesian() {
  1410.         if (a > 0) {
  1411.             return computeJacobianEccentricWrtCartesianElliptical();
  1412.         } else {
  1413.             return computeJacobianEccentricWrtCartesianHyperbolic();
  1414.         }
  1415.     }

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

  1427.         // start by computing the Jacobian with mean angle
  1428.         final double[][] jacobian = computeJacobianMeanWrtCartesianElliptical();

  1430.         // Differentiating the Kepler equation M = E - e sin E leads to:
  1431.         // dM = (1 - e cos E) dE - sin E de
  1432.         // which is inverted and rewritten as:
  1433.         // dE = a/r dM + sin E a/r de
  1434.         final SinCos scE              = FastMath.sinCos(getEccentricAnomaly());
  1435.         final double aOr              = 1 / (1 - e * scE.cos());

  1437.         // update anomaly row
  1438.         final double[] eRow           = jacobian[1];
  1439.         final double[] anomalyRow     = jacobian[5];
  1440.         for (int j = 0; j < anomalyRow.length; ++j) {
  1441.             anomalyRow[j] = aOr * (anomalyRow[j] + scE.sin() * eRow[j]);
  1442.         }

  1444.         return jacobian;

  1446.     }

  1448.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1449.      * <p>
  1450.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1451.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1452.      * yDot for j=4, zDot for j=5).
  1453.      * </p>
  1454.      * @return 6x6 Jacobian matrix
  1455.      */
  1456.     private double[][] computeJacobianEccentricWrtCartesianHyperbolic() {

  1458.         // start by computing the Jacobian with mean angle
  1459.         final double[][] jacobian = computeJacobianMeanWrtCartesianHyperbolic();

  1461.         // Differentiating the Kepler equation M = e sinh H - H leads to:
  1462.         // dM = (e cosh H - 1) dH + sinh H de
  1463.         // which is inverted and rewritten as:
  1464.         // dH = 1 / (e cosh H - 1) dM - sinh H / (e cosh H - 1) de
  1465.         final double H      = getEccentricAnomaly();
  1466.         final double coshH  = FastMath.cosh(H);
  1467.         final double sinhH  = FastMath.sinh(H);
  1468.         final double absaOr = 1 / (e * coshH - 1);

  1470.         // update anomaly row
  1471.         final double[] eRow       = jacobian[1];
  1472.         final double[] anomalyRow = jacobian[5];
  1473.         for (int j = 0; j < anomalyRow.length; ++j) {
  1474.             anomalyRow[j] = absaOr * (anomalyRow[j] - sinhH * eRow[j]);
  1475.         }

  1477.         return jacobian;

  1479.     }

  1481.     /** {@inheritDoc} */
  1482.     protected double[][] computeJacobianTrueWrtCartesian() {
  1483.         if (a > 0) {
  1484.             return computeJacobianTrueWrtCartesianElliptical();
  1485.         } else {
  1486.             return computeJacobianTrueWrtCartesianHyperbolic();
  1487.         }
  1488.     }

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

  1500.         // start by computing the Jacobian with eccentric angle
  1501.         final double[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();

  1503.         // Differentiating the eccentric anomaly equation sin E = sqrt(1-e^2) sin v / (1 + e cos v)
  1504.         // and using cos E = (e + cos v) / (1 + e cos v) to get rid of cos E leads to:
  1505.         // dE = [sqrt (1 - e^2) / (1 + e cos v)] dv - [sin E / (1 - e^2)] de
  1506.         // which is inverted and rewritten as:
  1507.         // dv = sqrt (1 - e^2) a/r dE + [sin E / sqrt (1 - e^2)] a/r de
  1508.         final double e2           = e * e;
  1509.         final double oMe2         = 1 - e2;
  1510.         final double epsilon      = FastMath.sqrt(oMe2);
  1511.         final SinCos scE          = FastMath.sinCos(getEccentricAnomaly());
  1512.         final double aOr          = 1 / (1 - e * scE.cos());
  1513.         final double aFactor      = epsilon * aOr;
  1514.         final double eFactor      = scE.sin() * aOr / epsilon;

  1516.         // update anomaly row
  1517.         final double[] eRow       = jacobian[1];
  1518.         final double[] anomalyRow = jacobian[5];
  1519.         for (int j = 0; j < anomalyRow.length; ++j) {
  1520.             anomalyRow[j] = aFactor * anomalyRow[j] + eFactor * eRow[j];
  1521.         }

  1523.         return jacobian;

  1525.     }

  1527.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1528.      * <p>
  1529.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1530.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1531.      * yDot for j=4, zDot for j=5).
  1532.      * </p>
  1533.      * @return 6x6 Jacobian matrix
  1534.      */
  1535.     private double[][] computeJacobianTrueWrtCartesianHyperbolic() {

  1537.         // start by computing the Jacobian with eccentric angle
  1538.         final double[][] jacobian = computeJacobianEccentricWrtCartesianHyperbolic();

  1540.         // Differentiating the eccentric anomaly equation sinh H = sqrt(e^2-1) sin v / (1 + e cos v)
  1541.         // and using cosh H = (e + cos v) / (1 + e cos v) to get rid of cosh H leads to:
  1542.         // dH = [sqrt (e^2 - 1) / (1 + e cos v)] dv + [sinh H / (e^2 - 1)] de
  1543.         // which is inverted and rewritten as:
  1544.         // dv = sqrt (1 - e^2) a/r dH - [sinh H / sqrt (e^2 - 1)] a/r de
  1545.         final double e2       = e * e;
  1546.         final double e2Mo     = e2 - 1;
  1547.         final double epsilon  = FastMath.sqrt(e2Mo);
  1548.         final double H        = getEccentricAnomaly();
  1549.         final double coshH    = FastMath.cosh(H);
  1550.         final double sinhH    = FastMath.sinh(H);
  1551.         final double aOr      = 1 / (e * coshH - 1);
  1552.         final double aFactor  = epsilon * aOr;
  1553.         final double eFactor  = sinhH * aOr / epsilon;

  1555.         // update anomaly row
  1556.         final double[] eRow           = jacobian[1];
  1557.         final double[] anomalyRow     = jacobian[5];
  1558.         for (int j = 0; j < anomalyRow.length; ++j) {
  1559.             anomalyRow[j] = aFactor * anomalyRow[j] - eFactor * eRow[j];
  1560.         }

  1562.         return jacobian;

  1564.     }

  1566.     /** {@inheritDoc} */
  1567.     public void addKeplerContribution(final PositionAngle type, final double gm,
  1568.                                       final double[] pDot) {
  1569.         final double oMe2;
  1570.         final double ksi;
  1571.         final double absA = FastMath.abs(a);
  1572.         final double n    = FastMath.sqrt(gm / absA) / absA;
  1573.         switch (type) {
  1574.             case MEAN :
  1575.                 pDot[5] += n;
  1576.                 break;
  1577.             case ECCENTRIC :
  1578.                 oMe2 = FastMath.abs(1 - e * e);
  1579.                 ksi  = 1 + e * FastMath.cos(v);
  1580.                 pDot[5] += n * ksi / oMe2;
  1581.                 break;
  1582.             case TRUE :
  1583.                 oMe2 = FastMath.abs(1 - e * e);
  1584.                 ksi  = 1 + e * FastMath.cos(v);
  1585.                 pDot[5] += n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
  1586.                 break;
  1587.             default :
  1588.                 throw new OrekitInternalError(null);
  1589.         }
  1590.     }

  1592.     /**  Returns a string representation of this Keplerian parameters object.
  1593.      * @return a string representation of this object
  1594.      */
  1595.     public String toString() {
  1596.         return new StringBuilder().append("Keplerian parameters: ").append('{').
  1597.                                   append("a: ").append(a).
  1598.                                   append("; e: ").append(e).
  1599.                                   append("; i: ").append(FastMath.toDegrees(i)).
  1600.                                   append("; pa: ").append(FastMath.toDegrees(pa)).
  1601.                                   append("; raan: ").append(FastMath.toDegrees(raan)).
  1602.                                   append("; v: ").append(FastMath.toDegrees(v)).
  1603.                                   append(";}").toString();
  1604.     }

  1606.     /** Check if the given parameter is within an acceptable range.
  1607.      * The bounds are inclusive: an exception is raised when either of those conditions are met:
  1608.      * <ul>
  1609.      *     <li>The parameter is strictly greater than upperBound</li>
  1610.      *     <li>The parameter is strictly lower than lowerBound</li>
  1611.      * </ul>
  1612.      * <p>
  1613.      * In either of these cases, an OrekitException is raised.
  1614.      * </p>
  1615.      * @param parameterName name of the parameter
  1616.      * @param parameter value of the parameter
  1617.      * @param lowerBound lower bound of the acceptable range (inclusive)
  1618.      * @param upperBound upper bound of the acceptable range (inclusive)
  1619.      */
  1620.     private void checkParameterRangeInclusive(final String parameterName, final double parameter,
  1621.                                               final double lowerBound, final double upperBound) {
  1622.         if (parameter < lowerBound || parameter > upperBound) {
  1623.             throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, parameterName,
  1624.                                       parameter, lowerBound, upperBound);
  1625.         }
  1626.     }

  1628.     /** Replace the instance with a data transfer object for serialization.
  1629.      * @return data transfer object that will be serialized
  1630.      */
  1631.     @DefaultDataContext
  1632.     private Object writeReplace() {
  1633.         return new DTO(this);
  1634.     }

  1636.     /** Internal class used only for serialization. */
  1637.     @DefaultDataContext
  1638.     private static class DTO implements Serializable {

  1640.         /** Serializable UID. */
  1641.         private static final long serialVersionUID = 20170414L;

  1643.         /** Double values. */
  1644.         private double[] d;

  1646.         /** Frame in which are defined the orbital parameters. */
  1647.         private final Frame frame;

  1649.         /** Simple constructor.
  1650.          * @param orbit instance to serialize
  1651.          */
  1652.         private DTO(final KeplerianOrbit orbit) {

  1654.             final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();

  1656.             // decompose date
  1657.             final AbsoluteDate j2000Epoch =
  1658.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  1659.             final double epoch  = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
  1660.             final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));

  1662.             if (orbit.hasDerivatives()) {
  1663.                 // we have derivatives
  1664.                 this.d = new double[] {
  1665.                     epoch, offset, orbit.getMu(),
  1666.                     orbit.a, orbit.e, orbit.i,
  1667.                     orbit.pa, orbit.raan, orbit.v,
  1668.                     orbit.aDot, orbit.eDot, orbit.iDot,
  1669.                     orbit.paDot, orbit.raanDot, orbit.vDot
  1670.                 };
  1671.             } else {
  1672.                 // we don't have derivatives
  1673.                 this.d = new double[] {
  1674.                     epoch, offset, orbit.getMu(),
  1675.                     orbit.a, orbit.e, orbit.i,
  1676.                     orbit.pa, orbit.raan, orbit.v
  1677.                 };
  1678.             }

  1680.             this.frame = orbit.getFrame();

  1682.         }

  1684.         /** Replace the deserialized data transfer object with a {@link KeplerianOrbit}.
  1685.          * @return replacement {@link KeplerianOrbit}
  1686.          */
  1687.         private Object readResolve() {
  1688.             final AbsoluteDate j2000Epoch =
  1689.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  1690.             if (d.length >= 15) {
  1691.                 // we have derivatives
  1692.                 return new KeplerianOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8],
  1693.                                           d[ 9], d[10], d[11], d[12], d[13], d[14],
  1694.                                           PositionAngle.TRUE,
  1695.                                           frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  1696.                                           d[2]);
  1697.             } else {
  1698.                 // we don't have derivatives
  1699.                 return new KeplerianOrbit(d[3], d[4], d[5], d[6], d[7], d[8], PositionAngle.TRUE,
  1700.                                           frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  1701.                                           d[2]);
  1702.             }
  1703.         }

  1705.     }

  1707. }
Created with JaCoCo 0.8.7.202105040129