EquinoctialOrbit.java

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

  19. import java.io.Serializable;

  21. import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
  22. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  23. import org.hipparchus.util.FastMath;
  24. import org.hipparchus.util.SinCos;
  25. import org.orekit.annotation.DefaultDataContext;
  26. import org.orekit.data.DataContext;
  27. import org.orekit.errors.OrekitIllegalArgumentException;
  28. import org.orekit.errors.OrekitInternalError;
  29. import org.orekit.errors.OrekitMessages;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.utils.PVCoordinates;
  33. import org.orekit.utils.TimeStampedPVCoordinates;


  36. /**
  37.  * This class handles equinoctial orbital parameters, which can support both
  38.  * circular and equatorial orbits.
  39.  * <p>
  40.  * The parameters used internally are the equinoctial elements which can be
  41.  * related to Keplerian elements as follows:
  42.  *   <pre>
  43.  *     a
  44.  *     ex = e cos(ω + Ω)
  45.  *     ey = e sin(ω + Ω)
  46.  *     hx = tan(i/2) cos(Ω)
  47.  *     hy = tan(i/2) sin(Ω)
  48.  *     lv = v + ω + Ω
  49.  *   </pre>
  50.  * where ω stands for the Perigee Argument and Ω stands for the
  51.  * Right Ascension of the Ascending Node.
  52.  *
  53.  * <p>
  54.  * The conversion equations from and to Keplerian elements given above hold only
  55.  * when both sides are unambiguously defined, i.e. when orbit is neither equatorial
  56.  * nor circular. When orbit is either equatorial or circular, the equinoctial
  57.  * parameters are still unambiguously defined whereas some Keplerian elements
  58.  * (more precisely ω and Ω) become ambiguous. For this reason, equinoctial
  59.  * parameters are the recommended way to represent orbits. Note however than
  60.  * the present implementation does not handle non-elliptical cases.
  61.  * </p>
  62.  * <p>
  63.  * The instance <code>EquinoctialOrbit</code> is guaranteed to be immutable.
  64.  * </p>
  65.  * @see    Orbit
  66.  * @see    KeplerianOrbit
  67.  * @see    CircularOrbit
  68.  * @see    CartesianOrbit
  69.  * @author Mathieu Rom&eacute;ro
  70.  * @author Luc Maisonobe
  71.  * @author Guylaine Prat
  72.  * @author Fabien Maussion
  73.  * @author V&eacute;ronique Pommier-Maurussane
  74.  */
  75. public class EquinoctialOrbit extends Orbit implements PositionAngleBased {

  77.     /** Serializable UID. */
  78.     private static final long serialVersionUID = 20170414L;

  80.     /** Semi-major axis (m). */
  81.     private final double a;

  83.     /** First component of the eccentricity vector. */
  84.     private final double ex;

  86.     /** Second component of the eccentricity vector. */
  87.     private final double ey;

  89.     /** First component of the inclination vector. */
  90.     private final double hx;

  92.     /** Second component of the inclination vector. */
  93.     private final double hy;

  95.     /** Cached longitude argument (rad). */
  96.     private final double cachedL;

  98.     /** Cache type of position angle (longitude argument). */
  99.     private final PositionAngleType cachedPositionAngleType;

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

  104.     /** First component of the eccentricity vector derivative. */
  105.     private final double exDot;

  107.     /** Second component of the eccentricity vector derivative. */
  108.     private final double eyDot;

  110.     /** First component of the inclination vector derivative. */
  111.     private final double hxDot;

  113.     /** Second component of the inclination vector derivative. */
  114.     private final double hyDot;

  116.     /** Derivative of cached longitude argument (rad/s). */
  117.     private final double cachedLDot;

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

  122.     /** Creates a new instance.
  123.      * @param a  semi-major axis (m)
  124.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  125.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  126.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  127.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  128.      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
  129.      * @param type type of longitude argument
  130.      * @param cachedPositionAngleType type of cached longitude argument
  131.      * @param frame the frame in which the parameters are defined
  132.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  133.      * @param date date of the orbital parameters
  134.      * @param mu central attraction coefficient (m³/s²)
  135.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  136.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  137.      * @since 12.1
  138.      */
  139.     public EquinoctialOrbit(final double a, final double ex, final double ey,
  140.                             final double hx, final double hy, final double l,
  141.                             final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
  142.                             final Frame frame, final AbsoluteDate date, final double mu)
  143.         throws IllegalArgumentException {
  144.         this(a, ex, ey, hx, hy, l,
  145.              Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN,
  146.              type, cachedPositionAngleType, frame, date, mu);
  147.     }

  149.     /** Creates a new instance without derivatives and with cached position angle same as value inputted.
  150.      * @param a  semi-major axis (m)
  151.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  152.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  153.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  154.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  155.      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
  156.      * @param type type of longitude argument
  157.      * @param frame the frame in which the parameters are defined
  158.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  159.      * @param date date of the orbital parameters
  160.      * @param mu central attraction coefficient (m³/s²)
  161.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  162.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  163.      */
  164.     public EquinoctialOrbit(final double a, final double ex, final double ey,
  165.                             final double hx, final double hy, final double l,
  166.                             final PositionAngleType type,
  167.                             final Frame frame, final AbsoluteDate date, final double mu)
  168.             throws IllegalArgumentException {
  169.         this(a, ex, ey, hx, hy, l, type, type, frame, date, mu);
  170.     }

  172.     /** Creates a new instance.
  173.      * @param a  semi-major axis (m)
  174.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  175.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  176.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  177.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  178.      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
  179.      * @param aDot  semi-major axis derivative (m/s)
  180.      * @param exDot d(e cos(ω + Ω))/dt, first component of eccentricity vector derivative
  181.      * @param eyDot d(e sin(ω + Ω))/dt, second component of eccentricity vector derivative
  182.      * @param hxDot d(tan(i/2) cos(Ω))/dt, first component of inclination vector derivative
  183.      * @param hyDot d(tan(i/2) sin(Ω))/dt, second component of inclination vector derivative
  184.      * @param lDot  d(M or E or v) + ω + Ω)/dr, mean, eccentric or true longitude argument  derivative (rad/s)
  185.      * @param type type of longitude argument
  186.      * @param cachedPositionAngleType of cached longitude argument
  187.      * @param frame the frame in which the parameters are defined
  188.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  189.      * @param date date of the orbital parameters
  190.      * @param mu central attraction coefficient (m³/s²)
  191.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  192.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  193.      * @since 12.1
  194.      */
  195.     public EquinoctialOrbit(final double a, final double ex, final double ey,
  196.                             final double hx, final double hy, final double l,
  197.                             final double aDot, final double exDot, final double eyDot,
  198.                             final double hxDot, final double hyDot, final double lDot,
  199.                             final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
  200.                             final Frame frame, final AbsoluteDate date, final double mu)
  201.         throws IllegalArgumentException {
  202.         super(frame, date, mu);
  203.         if (ex * ex + ey * ey >= 1.0) {
  204.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  205.                                                      getClass().getName());
  206.         }
  207.         this.cachedPositionAngleType = cachedPositionAngleType;
  208.         this.a     = a;
  209.         this.aDot  = aDot;
  210.         this.ex    = ex;
  211.         this.exDot = exDot;
  212.         this.ey    = ey;
  213.         this.eyDot = eyDot;
  214.         this.hx    = hx;
  215.         this.hxDot = hxDot;
  216.         this.hy    = hy;
  217.         this.hyDot = hyDot;

  219.         if (hasDerivatives()) {
  220.             final UnivariateDerivative1 lUD = initializeCachedL(l, lDot, type);
  221.             this.cachedL = lUD.getValue();
  222.             this.cachedLDot = lUD.getFirstDerivative();
  223.         } else {
  224.             this.cachedL = initializeCachedL(l, type);
  225.             this.cachedLDot = Double.NaN;
  226.         }

  228.         this.partialPV = null;

  230.     }

  232.     /** Creates a new instance with derivatives and with cached position angle same as value inputted.
  233.      * @param a  semi-major axis (m)
  234.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  235.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  236.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  237.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  238.      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
  239.      * @param aDot  semi-major axis derivative (m/s)
  240.      * @param exDot d(e cos(ω + Ω))/dt, first component of eccentricity vector derivative
  241.      * @param eyDot d(e sin(ω + Ω))/dt, second component of eccentricity vector derivative
  242.      * @param hxDot d(tan(i/2) cos(Ω))/dt, first component of inclination vector derivative
  243.      * @param hyDot d(tan(i/2) sin(Ω))/dt, second component of inclination vector derivative
  244.      * @param lDot  d(M or E or v) + ω + Ω)/dr, mean, eccentric or true longitude argument  derivative (rad/s)
  245.      * @param type type of longitude argument
  246.      * @param frame the frame in which the parameters are defined
  247.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  248.      * @param date date of the orbital parameters
  249.      * @param mu central attraction coefficient (m³/s²)
  250.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  251.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  252.      */
  253.     public EquinoctialOrbit(final double a, final double ex, final double ey,
  254.                             final double hx, final double hy, final double l,
  255.                             final double aDot, final double exDot, final double eyDot,
  256.                             final double hxDot, final double hyDot, final double lDot,
  257.                             final PositionAngleType type,
  258.                             final Frame frame, final AbsoluteDate date, final double mu)
  259.             throws IllegalArgumentException {
  260.         this(a, ex, ey, hx, hy, l, aDot, exDot, eyDot, hxDot, hyDot, lDot, type, type, frame, date, mu);
  261.     }

  263.     /** Constructor from Cartesian parameters.
  264.      *
  265.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  266.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  267.      * use {@code mu} and the position to compute the acceleration, including
  268.      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
  269.      *
  270.      * @param pvCoordinates the position, velocity and acceleration
  271.      * @param frame the frame in which are defined the {@link PVCoordinates}
  272.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  273.      * @param mu central attraction coefficient (m³/s²)
  274.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  275.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  276.      */
  277.     public EquinoctialOrbit(final TimeStampedPVCoordinates pvCoordinates,
  278.                             final Frame frame, final double mu)
  279.         throws IllegalArgumentException {
  280.         super(pvCoordinates, frame, mu);

  282.         //  compute semi-major axis
  283.         final Vector3D pvP   = pvCoordinates.getPosition();
  284.         final Vector3D pvV   = pvCoordinates.getVelocity();
  285.         final Vector3D pvA   = pvCoordinates.getAcceleration();
  286.         final double r2      = pvP.getNormSq();
  287.         final double r       = FastMath.sqrt(r2);
  288.         final double V2      = pvV.getNormSq();
  289.         final double rV2OnMu = r * V2 / mu;

  291.         // compute semi-major axis
  292.         a = r / (2 - rV2OnMu);

  294.         if (!isElliptical()) {
  295.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  296.                                                      getClass().getName());
  297.         }

  299.         // compute inclination vector
  300.         final Vector3D w = pvCoordinates.getMomentum().normalize();
  301.         final double d = 1.0 / (1 + w.getZ());
  302.         hx = -d * w.getY();
  303.         hy =  d * w.getX();

  305.         // compute true longitude argument
  306.         cachedPositionAngleType = PositionAngleType.TRUE;
  307.         final double cLv = (pvP.getX() - d * pvP.getZ() * w.getX()) / r;
  308.         final double sLv = (pvP.getY() - d * pvP.getZ() * w.getY()) / r;
  309.         cachedL = FastMath.atan2(sLv, cLv);

  311.         // compute eccentricity vector
  312.         final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a);
  313.         final double eCE = rV2OnMu - 1;
  314.         final double e2  = eCE * eCE + eSE * eSE;
  315.         final double f   = eCE - e2;
  316.         final double g   = FastMath.sqrt(1 - e2) * eSE;
  317.         ex = a * (f * cLv + g * sLv) / r;
  318.         ey = a * (f * sLv - g * cLv) / r;

  320.         partialPV = pvCoordinates;

  322.         if (hasNonKeplerianAcceleration(pvCoordinates, mu)) {
  323.             // we have a relevant acceleration, we can compute derivatives

  325.             final double[][] jacobian = new double[6][6];
  326.             getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);

  328.             final Vector3D keplerianAcceleration    = new Vector3D(-mu / (r * r2), pvP);
  329.             final Vector3D nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  330.             final double   aX                       = nonKeplerianAcceleration.getX();
  331.             final double   aY                       = nonKeplerianAcceleration.getY();
  332.             final double   aZ                       = nonKeplerianAcceleration.getZ();
  333.             aDot  = jacobian[0][3] * aX + jacobian[0][4] * aY + jacobian[0][5] * aZ;
  334.             exDot = jacobian[1][3] * aX + jacobian[1][4] * aY + jacobian[1][5] * aZ;
  335.             eyDot = jacobian[2][3] * aX + jacobian[2][4] * aY + jacobian[2][5] * aZ;
  336.             hxDot = jacobian[3][3] * aX + jacobian[3][4] * aY + jacobian[3][5] * aZ;
  337.             hyDot = jacobian[4][3] * aX + jacobian[4][4] * aY + jacobian[4][5] * aZ;

  339.             // in order to compute true longitude argument derivative, we must compute
  340.             // mean longitude argument derivative including Keplerian motion and convert to true longitude argument
  341.             final double lMDot = getKeplerianMeanMotion() +
  342.                                  jacobian[5][3] * aX + jacobian[5][4] * aY + jacobian[5][5] * aZ;
  343.             final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
  344.             final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
  345.             final UnivariateDerivative1 lMUD = new UnivariateDerivative1(getLM(), lMDot);
  346.             final UnivariateDerivative1 lvUD = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lMUD);
  347.             cachedLDot = lvUD.getFirstDerivative();

  349.         } else {
  350.             // acceleration is either almost zero or NaN,
  351.             // we assume acceleration was not known
  352.             // we don't set up derivatives
  353.             aDot  = Double.NaN;
  354.             exDot = Double.NaN;
  355.             eyDot = Double.NaN;
  356.             hxDot = Double.NaN;
  357.             hyDot = Double.NaN;
  358.             cachedLDot = Double.NaN;
  359.         }

  361.     }

  363.     /** Constructor from Cartesian parameters.
  364.      *
  365.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  366.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  367.      * use {@code mu} and the position to compute the acceleration, including
  368.      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
  369.      *
  370.      * @param pvCoordinates the position end velocity
  371.      * @param frame the frame in which are defined the {@link PVCoordinates}
  372.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  373.      * @param date date of the orbital parameters
  374.      * @param mu central attraction coefficient (m³/s²)
  375.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  376.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  377.      */
  378.     public EquinoctialOrbit(final PVCoordinates pvCoordinates, final Frame frame,
  379.                             final AbsoluteDate date, final double mu)
  380.         throws IllegalArgumentException {
  381.         this(new TimeStampedPVCoordinates(date, pvCoordinates), frame, mu);
  382.     }

  384.     /** Constructor from any kind of orbital parameters.
  385.      * @param op orbital parameters to copy
  386.      */
  387.     public EquinoctialOrbit(final Orbit op) {
  388.         super(op.getFrame(), op.getDate(), op.getMu());
  389.         a         = op.getA();
  390.         aDot      = op.getADot();
  391.         ex        = op.getEquinoctialEx();
  392.         exDot     = op.getEquinoctialExDot();
  393.         ey        = op.getEquinoctialEy();
  394.         eyDot     = op.getEquinoctialEyDot();
  395.         hx        = op.getHx();
  396.         hxDot     = op.getHxDot();
  397.         hy        = op.getHy();
  398.         hyDot     = op.getHyDot();
  399.         cachedPositionAngleType = PositionAngleType.TRUE;
  400.         cachedL   = op.getLv();
  401.         cachedLDot = op.getLvDot();
  402.         partialPV = null;
  403.     }

  405.     /** {@inheritDoc} */
  406.     @Override
  407.     public OrbitType getType() {
  408.         return OrbitType.EQUINOCTIAL;
  409.     }

  411.     /** {@inheritDoc} */
  412.     @Override
  413.     public double getA() {
  414.         return a;
  415.     }

  417.     /** {@inheritDoc} */
  418.     @Override
  419.     public double getADot() {
  420.         return aDot;
  421.     }

  423.     /** {@inheritDoc} */
  424.     @Override
  425.     public double getEquinoctialEx() {
  426.         return ex;
  427.     }

  429.     /** {@inheritDoc} */
  430.     @Override
  431.     public double getEquinoctialExDot() {
  432.         return exDot;
  433.     }

  435.     /** {@inheritDoc} */
  436.     @Override
  437.     public double getEquinoctialEy() {
  438.         return ey;
  439.     }

  441.     /** {@inheritDoc} */
  442.     @Override
  443.     public double getEquinoctialEyDot() {
  444.         return eyDot;
  445.     }

  447.     /** {@inheritDoc} */
  448.     @Override
  449.     public double getHx() {
  450.         return hx;
  451.     }

  453.     /** {@inheritDoc} */
  454.     @Override
  455.     public double getHxDot() {
  456.         return hxDot;
  457.     }

  459.     /** {@inheritDoc} */
  460.     @Override
  461.     public double getHy() {
  462.         return hy;
  463.     }

  465.     /** {@inheritDoc} */
  466.     @Override
  467.     public double getHyDot() {
  468.         return hyDot;
  469.     }

  471.     /** {@inheritDoc} */
  472.     @Override
  473.     public double getLv() {
  474.         switch (cachedPositionAngleType) {
  475.             case TRUE:
  476.                 return cachedL;

  478.             case ECCENTRIC:
  479.                 return EquinoctialLongitudeArgumentUtility.eccentricToTrue(ex, ey, cachedL);

  481.             case MEAN:
  482.                 return EquinoctialLongitudeArgumentUtility.meanToTrue(ex, ey, cachedL);

  484.             default:
  485.                 throw new OrekitInternalError(null);
  486.         }
  487.     }

  489.     /** {@inheritDoc} */
  490.     @Override
  491.     public double getLvDot() {
  492.         switch (cachedPositionAngleType) {
  493.             case ECCENTRIC:
  494.                 final UnivariateDerivative1 lEUD = new UnivariateDerivative1(cachedL, cachedLDot);
  495.                 final UnivariateDerivative1 exUD     = new UnivariateDerivative1(ex,     exDot);
  496.                 final UnivariateDerivative1 eyUD     = new UnivariateDerivative1(ey,     eyDot);
  497.                 final UnivariateDerivative1 lvUD = FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
  498.                         lEUD);
  499.                 return lvUD.getFirstDerivative();

  501.             case TRUE:
  502.                 return cachedLDot;

  504.             case MEAN:
  505.                 final UnivariateDerivative1 lMUD = new UnivariateDerivative1(cachedL, cachedLDot);
  506.                 final UnivariateDerivative1 exUD2    = new UnivariateDerivative1(ex,     exDot);
  507.                 final UnivariateDerivative1 eyUD2    = new UnivariateDerivative1(ey,     eyDot);
  508.                 final UnivariateDerivative1 lvUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD2,
  509.                         eyUD2, lMUD);
  510.                 return lvUD2.getFirstDerivative();

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

  517.     /** {@inheritDoc} */
  518.     @Override
  519.     public double getLE() {
  520.         switch (cachedPositionAngleType) {
  521.             case TRUE:
  522.                 return EquinoctialLongitudeArgumentUtility.trueToEccentric(ex, ey, cachedL);

  524.             case ECCENTRIC:
  525.                 return cachedL;

  527.             case MEAN:
  528.                 return EquinoctialLongitudeArgumentUtility.meanToEccentric(ex, ey, cachedL);

  530.             default:
  531.                 throw new OrekitInternalError(null);
  532.         }
  533.     }

  535.     /** {@inheritDoc} */
  536.     @Override
  537.     public double getLEDot() {
  538.         switch (cachedPositionAngleType) {
  539.             case TRUE:
  540.                 final UnivariateDerivative1 lvUD = new UnivariateDerivative1(cachedL, cachedLDot);
  541.                 final UnivariateDerivative1 exUD     = new UnivariateDerivative1(ex,     exDot);
  542.                 final UnivariateDerivative1 eyUD     = new UnivariateDerivative1(ey,     eyDot);
  543.                 final UnivariateDerivative1 lEUD = FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD,
  544.                         lvUD);
  545.                 return lEUD.getFirstDerivative();

  547.             case ECCENTRIC:
  548.                 return cachedLDot;

  550.             case MEAN:
  551.                 final UnivariateDerivative1 lMUD = new UnivariateDerivative1(cachedL, cachedLDot);
  552.                 final UnivariateDerivative1 exUD2    = new UnivariateDerivative1(ex,     exDot);
  553.                 final UnivariateDerivative1 eyUD2    = new UnivariateDerivative1(ey,     eyDot);
  554.                 final UnivariateDerivative1 lEUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD2,
  555.                         eyUD2, lMUD);
  556.                 return lEUD2.getFirstDerivative();

  558.             default:
  559.                 throw new OrekitInternalError(null);
  560.         }
  561.     }

  563.     /** {@inheritDoc} */
  564.     @Override
  565.     public double getLM() {
  566.         switch (cachedPositionAngleType) {
  567.             case TRUE:
  568.                 return EquinoctialLongitudeArgumentUtility.trueToMean(ex, ey, cachedL);

  570.             case MEAN:
  571.                 return cachedL;

  573.             case ECCENTRIC:
  574.                 return EquinoctialLongitudeArgumentUtility.eccentricToMean(ex, ey, cachedL);

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

  581.     /** {@inheritDoc} */
  582.     @Override
  583.     public double getLMDot() {
  584.         switch (cachedPositionAngleType) {
  585.             case TRUE:
  586.                 final UnivariateDerivative1 lvUD = new UnivariateDerivative1(cachedL, cachedLDot);
  587.                 final UnivariateDerivative1 exUD     = new UnivariateDerivative1(ex,     exDot);
  588.                 final UnivariateDerivative1 eyUD     = new UnivariateDerivative1(ey,     eyDot);
  589.                 final UnivariateDerivative1 lMUD = FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lvUD);
  590.                 return lMUD.getFirstDerivative();

  592.             case MEAN:
  593.                 return cachedLDot;

  595.             case ECCENTRIC:
  596.                 final UnivariateDerivative1 lEUD = new UnivariateDerivative1(cachedL, cachedLDot);
  597.                 final UnivariateDerivative1 exUD2    = new UnivariateDerivative1(ex,     exDot);
  598.                 final UnivariateDerivative1 eyUD2    = new UnivariateDerivative1(ey,     eyDot);
  599.                 final UnivariateDerivative1 lMUD2 = FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD2,
  600.                         eyUD2, lEUD);
  601.                 return lMUD2.getFirstDerivative();

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

  608.     /** Get the longitude argument.
  609.      * @param type type of the angle
  610.      * @return longitude argument (rad)
  611.      */
  612.     public double getL(final PositionAngleType type) {
  613.         return (type == PositionAngleType.MEAN) ? getLM() :
  614.                                               ((type == PositionAngleType.ECCENTRIC) ? getLE() :
  615.                                                                                    getLv());
  616.     }

  618.     /** Get the longitude argument derivative.
  619.      * @param type type of the angle
  620.      * @return longitude argument derivative (rad/s)
  621.      */
  622.     public double getLDot(final PositionAngleType type) {
  623.         return (type == PositionAngleType.MEAN) ? getLMDot() :
  624.                                               ((type == PositionAngleType.ECCENTRIC) ? getLEDot() :
  625.                                                                                    getLvDot());
  626.     }

  628.     /** Computes the true longitude argument from the eccentric longitude argument.
  629.      * @param lE = E + ω + Ω eccentric longitude argument (rad)
  630.      * @param ex first component of the eccentricity vector
  631.      * @param ey second component of the eccentricity vector
  632.      * @return the true longitude argument
  633.      */
  634.     @Deprecated
  635.     public static double eccentricToTrue(final double lE, final double ex, final double ey) {
  636.         return EquinoctialLongitudeArgumentUtility.eccentricToTrue(ex, ey, lE);
  637.     }

  639.     /** Computes the eccentric longitude argument from the true longitude argument.
  640.      * @param lv = v + ω + Ω true longitude argument (rad)
  641.      * @param ex first component of the eccentricity vector
  642.      * @param ey second component of the eccentricity vector
  643.      * @return the eccentric longitude argument
  644.      */
  645.     @Deprecated
  646.     public static double trueToEccentric(final double lv, final double ex, final double ey) {
  647.         return EquinoctialLongitudeArgumentUtility.trueToEccentric(ex, ey, lv);
  648.     }

  650.     /** Computes the eccentric longitude argument from the mean longitude argument.
  651.      * @param lM = M + ω + Ω mean longitude argument (rad)
  652.      * @param ex first component of the eccentricity vector
  653.      * @param ey second component of the eccentricity vector
  654.      * @return the eccentric longitude argument
  655.      */
  656.     @Deprecated
  657.     public static double meanToEccentric(final double lM, final double ex, final double ey) {
  658.         return EquinoctialLongitudeArgumentUtility.meanToEccentric(ex, ey, lM);
  659.     }

  661.     /** Computes the mean longitude argument from the eccentric longitude argument.
  662.      * @param lE = E + ω + Ω mean longitude argument (rad)
  663.      * @param ex first component of the eccentricity vector
  664.      * @param ey second component of the eccentricity vector
  665.      * @return the mean longitude argument
  666.      */
  667.     @Deprecated
  668.     public static double eccentricToMean(final double lE, final double ex, final double ey) {
  669.         return EquinoctialLongitudeArgumentUtility.eccentricToMean(ex, ey, lE);
  670.     }

  672.     /** {@inheritDoc} */
  673.     @Override
  674.     public double getE() {
  675.         return FastMath.sqrt(ex * ex + ey * ey);
  676.     }

  678.     /** {@inheritDoc} */
  679.     @Override
  680.     public double getEDot() {
  681.         return (ex * exDot + ey * eyDot) / FastMath.sqrt(ex * ex + ey * ey);
  682.     }

  684.     /** {@inheritDoc} */
  685.     @Override
  686.     public double getI() {
  687.         return 2 * FastMath.atan(FastMath.sqrt(hx * hx + hy * hy));
  688.     }

  690.     /** {@inheritDoc} */
  691.     @Override
  692.     public double getIDot() {
  693.         final double h2 = hx * hx + hy * hy;
  694.         final double h  = FastMath.sqrt(h2);
  695.         return 2 * (hx * hxDot + hy * hyDot) / (h * (1 + h2));
  696.     }

  698.     /** Compute position and velocity but not acceleration.
  699.      */
  700.     private void computePVWithoutA() {

  702.         if (partialPV != null) {
  703.             // already computed
  704.             return;
  705.         }

  707.         // get equinoctial parameters
  708.         final double lE = getLE();

  710.         // inclination-related intermediate parameters
  711.         final double hx2   = hx * hx;
  712.         final double hy2   = hy * hy;
  713.         final double factH = 1. / (1 + hx2 + hy2);

  715.         // reference axes defining the orbital plane
  716.         final double ux = (1 + hx2 - hy2) * factH;
  717.         final double uy =  2 * hx * hy * factH;
  718.         final double uz = -2 * hy * factH;

  720.         final double vx = uy;
  721.         final double vy = (1 - hx2 + hy2) * factH;
  722.         final double vz =  2 * hx * factH;

  724.         // eccentricity-related intermediate parameters
  725.         final double exey = ex * ey;
  726.         final double ex2  = ex * ex;
  727.         final double ey2  = ey * ey;
  728.         final double e2   = ex2 + ey2;
  729.         final double eta  = 1 + FastMath.sqrt(1 - e2);
  730.         final double beta = 1. / eta;

  732.         // eccentric longitude argument
  733.         final SinCos scLe   = FastMath.sinCos(lE);
  734.         final double cLe    = scLe.cos();
  735.         final double sLe    = scLe.sin();
  736.         final double exCeyS = ex * cLe + ey * sLe;

  738.         // coordinates of position and velocity in the orbital plane
  739.         final double x      = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
  740.         final double y      = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);

  742.         final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
  743.         final double xdot   = factor * (-sLe + beta * ey * exCeyS);
  744.         final double ydot   = factor * ( cLe - beta * ex * exCeyS);

  746.         final Vector3D position =
  747.                         new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
  748.         final Vector3D velocity =
  749.                         new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz);
  750.         partialPV = new PVCoordinates(position, velocity);

  752.     }

  754.     /** Initialize cached argument of longitude with rate.
  755.      * @param l input argument of longitude
  756.      * @param lDot rate of input argument of longitude
  757.      * @param inputType position angle type passed as input
  758.      * @return argument of longitude to cache with rate
  759.      * @since 12.1
  760.      */
  761.     private UnivariateDerivative1 initializeCachedL(final double l, final double lDot,
  762.                                                     final PositionAngleType inputType) {
  763.         if (cachedPositionAngleType == inputType) {
  764.             return new UnivariateDerivative1(l, lDot);

  766.         } else {
  767.             final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
  768.             final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
  769.             final UnivariateDerivative1 lUD = new UnivariateDerivative1(l, lDot);

  771.             switch (cachedPositionAngleType) {

  773.                 case ECCENTRIC:
  774.                     if (inputType == PositionAngleType.MEAN) {
  775.                         return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD, eyUD, lUD);
  776.                     } else {
  777.                         return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD, lUD);
  778.                     }

  780.                 case TRUE:
  781.                     if (inputType == PositionAngleType.MEAN) {
  782.                         return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lUD);
  783.                     } else {
  784.                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD, lUD);
  785.                     }

  787.                 case MEAN:
  788.                     if (inputType == PositionAngleType.TRUE) {
  789.                         return FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lUD);
  790.                     } else {
  791.                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD, eyUD, lUD);
  792.                     }

  794.                 default:
  795.                     throw new OrekitInternalError(null);

  797.             }

  799.         }

  801.     }

  803.     /** Initialize cached argument of longitude.
  804.      * @param l input argument of longitude
  805.      * @param positionAngleType position angle type passed as input
  806.      * @return argument of longitude to cache
  807.      * @since 12.1
  808.      */
  809.     private double initializeCachedL(final double l, final PositionAngleType positionAngleType) {
  810.         return EquinoctialLongitudeArgumentUtility.convertL(positionAngleType, l, ex, ey, cachedPositionAngleType);
  811.     }

  813.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  814.      * <p>
  815.      * This method should be called only when {@link #hasDerivatives()} returns true.
  816.      * </p>
  817.      * @return non-Keplerian part of the acceleration
  818.      */
  819.     private Vector3D nonKeplerianAcceleration() {

  821.         final double[][] dCdP = new double[6][6];
  822.         getJacobianWrtParameters(PositionAngleType.MEAN, dCdP);

  824.         final double nonKeplerianMeanMotion = getLMDot() - getKeplerianMeanMotion();
  825.         final double nonKeplerianAx = dCdP[3][0] * aDot  + dCdP[3][1] * exDot + dCdP[3][2] * eyDot +
  826.                                       dCdP[3][3] * hxDot + dCdP[3][4] * hyDot + dCdP[3][5] * nonKeplerianMeanMotion;
  827.         final double nonKeplerianAy = dCdP[4][0] * aDot  + dCdP[4][1] * exDot + dCdP[4][2] * eyDot +
  828.                                       dCdP[4][3] * hxDot + dCdP[4][4] * hyDot + dCdP[4][5] * nonKeplerianMeanMotion;
  829.         final double nonKeplerianAz = dCdP[5][0] * aDot  + dCdP[5][1] * exDot + dCdP[5][2] * eyDot +
  830.                                       dCdP[5][3] * hxDot + dCdP[5][4] * hyDot + dCdP[5][5] * nonKeplerianMeanMotion;

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

  834.     }

  836.     /** {@inheritDoc} */
  837.     @Override
  838.     protected Vector3D initPosition() {

  840.         // get equinoctial parameters
  841.         final double lE = getLE();

  843.         // inclination-related intermediate parameters
  844.         final double hx2   = hx * hx;
  845.         final double hy2   = hy * hy;
  846.         final double factH = 1. / (1 + hx2 + hy2);

  848.         // reference axes defining the orbital plane
  849.         final double ux = (1 + hx2 - hy2) * factH;
  850.         final double uy =  2 * hx * hy * factH;
  851.         final double uz = -2 * hy * factH;

  853.         final double vx = uy;
  854.         final double vy = (1 - hx2 + hy2) * factH;
  855.         final double vz =  2 * hx * factH;

  857.         // eccentricity-related intermediate parameters
  858.         final double exey = ex * ey;
  859.         final double ex2  = ex * ex;
  860.         final double ey2  = ey * ey;
  861.         final double e2   = ex2 + ey2;
  862.         final double eta  = 1 + FastMath.sqrt(1 - e2);
  863.         final double beta = 1. / eta;

  865.         // eccentric longitude argument
  866.         final SinCos scLe   = FastMath.sinCos(lE);
  867.         final double cLe    = scLe.cos();
  868.         final double sLe    = scLe.sin();

  870.         // coordinates of position and velocity in the orbital plane
  871.         final double x      = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
  872.         final double y      = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);

  874.         return new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);

  876.     }

  878.     /** {@inheritDoc} */
  879.     @Override
  880.     protected TimeStampedPVCoordinates initPVCoordinates() {

  882.         // position and velocity
  883.         computePVWithoutA();

  885.         // acceleration
  886.         final double r2 = partialPV.getPosition().getNormSq();
  887.         final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
  888.         final Vector3D acceleration = hasDerivatives() ?
  889.                                       keplerianAcceleration.add(nonKeplerianAcceleration()) :
  890.                                       keplerianAcceleration;

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

  894.     }

  896.     /** {@inheritDoc} */
  897.     @Override
  898.     public EquinoctialOrbit shiftedBy(final double dt) {

  900.         // use Keplerian-only motion
  901.         final EquinoctialOrbit keplerianShifted = new EquinoctialOrbit(a, ex, ey, hx, hy,
  902.                                                                        getLM() + getKeplerianMeanMotion() * dt,
  903.                                                                        PositionAngleType.MEAN, cachedPositionAngleType,
  904.                                                                        getFrame(),
  905.                                                                        getDate().shiftedBy(dt), getMu());

  907.         if (hasDerivatives()) {

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

  912.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  913.             keplerianShifted.computePVWithoutA();
  914.             final Vector3D fixedP   = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
  915.                                                    0.5 * dt * dt, nonKeplerianAcceleration);
  916.             final double   fixedR2 = fixedP.getNormSq();
  917.             final double   fixedR  = FastMath.sqrt(fixedR2);
  918.             final Vector3D fixedV  = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
  919.                                                   dt, nonKeplerianAcceleration);
  920.             final Vector3D fixedA  = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
  921.                                                   1, nonKeplerianAcceleration);

  923.             // build a new orbit, taking non-Keplerian acceleration into account
  924.             return new EquinoctialOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
  925.                                                                      fixedP, fixedV, fixedA),
  926.                                         keplerianShifted.getFrame(), keplerianShifted.getMu());

  928.         } else {
  929.             // Keplerian-only motion is all we can do
  930.             return keplerianShifted;
  931.         }

  933.     }

  935.     /** {@inheritDoc} */
  936.     @Override
  937.     protected double[][] computeJacobianMeanWrtCartesian() {

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

  941.         // compute various intermediate parameters
  942.         computePVWithoutA();
  943.         final Vector3D position = partialPV.getPosition();
  944.         final Vector3D velocity = partialPV.getVelocity();
  945.         final double r2         = position.getNormSq();
  946.         final double r          = FastMath.sqrt(r2);
  947.         final double r3         = r * r2;

  949.         final double mu         = getMu();
  950.         final double sqrtMuA    = FastMath.sqrt(a * mu);
  951.         final double a2         = a * a;

  953.         final double e2         = ex * ex + ey * ey;
  954.         final double oMe2       = 1 - e2;
  955.         final double epsilon    = FastMath.sqrt(oMe2);
  956.         final double beta       = 1 / (1 + epsilon);
  957.         final double ratio      = epsilon * beta;

  959.         final double hx2        = hx * hx;
  960.         final double hy2        = hy * hy;
  961.         final double hxhy       = hx * hy;

  963.         // precomputing equinoctial frame unit vectors (f, g, w)
  964.         final Vector3D f  = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
  965.         final Vector3D g  = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
  966.         final Vector3D w  = Vector3D.crossProduct(position, velocity).normalize();

  968.         // coordinates of the spacecraft in the equinoctial frame
  969.         final double x    = Vector3D.dotProduct(position, f);
  970.         final double y    = Vector3D.dotProduct(position, g);
  971.         final double xDot = Vector3D.dotProduct(velocity, f);
  972.         final double yDot = Vector3D.dotProduct(velocity, g);

  974.         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
  975.         final double c1 = a / (sqrtMuA * epsilon);
  976.         final double c2 = a * sqrtMuA * beta / r3;
  977.         final double c3 = sqrtMuA / (r3 * epsilon);
  978.         final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
  979.                                                 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);

  981.         // drDot / dEy = dXDot / dEy * f + dYDot / dEy * g
  982.         final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
  983.                                                 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);

  985.         // da
  986.         final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
  987.         final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
  988.         fillHalfRow(1, vectorAR,    jacobian[0], 0);
  989.         fillHalfRow(1, vectorARDot, jacobian[0], 3);

  991.         // dEx
  992.         final double d1 = -a * ratio / r3;
  993.         final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
  994.         final double d3 = (hx * y - hy * x) / sqrtMuA;
  995.         final Vector3D vectorExRDot =
  996.             new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
  997.         fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
  998.         fillHalfRow(1, vectorExRDot, jacobian[1], 3);

  1000.         // dEy
  1001.         final Vector3D vectorEyRDot =
  1002.             new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
  1003.         fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
  1004.         fillHalfRow(1, vectorEyRDot, jacobian[2], 3);

  1006.         // dHx
  1007.         final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
  1008.         fillHalfRow(-h * xDot, w, jacobian[3], 0);
  1009.         fillHalfRow( h * x,    w, jacobian[3], 3);

  1011.         // dHy
  1012.         fillHalfRow(-h * yDot, w, jacobian[4], 0);
  1013.         fillHalfRow( h * y,    w, jacobian[4], 3);

  1015.         // dLambdaM
  1016.         final double l = -ratio / sqrtMuA;
  1017.         fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
  1018.         fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);

  1020.         return jacobian;

  1022.     }

  1024.     /** {@inheritDoc} */
  1025.     @Override
  1026.     protected double[][] computeJacobianEccentricWrtCartesian() {

  1028.         // start by computing the Jacobian with mean angle
  1029.         final double[][] jacobian = computeJacobianMeanWrtCartesian();

  1031.         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
  1032.         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
  1033.         // which is inverted and rewritten as:
  1034.         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
  1035.         final SinCos scLe  = FastMath.sinCos(getLE());
  1036.         final double cosLe = scLe.cos();
  1037.         final double sinLe = scLe.sin();
  1038.         final double aOr   = 1 / (1 - ex * cosLe - ey * sinLe);

  1040.         // update longitude row
  1041.         final double[] rowEx = jacobian[1];
  1042.         final double[] rowEy = jacobian[2];
  1043.         final double[] rowL  = jacobian[5];
  1044.         for (int j = 0; j < 6; ++j) {
  1045.             rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
  1046.         }

  1048.         return jacobian;

  1050.     }

  1052.     /** {@inheritDoc} */
  1053.     @Override
  1054.     protected double[][] computeJacobianTrueWrtCartesian() {

  1056.         // start by computing the Jacobian with eccentric angle
  1057.         final double[][] jacobian = computeJacobianEccentricWrtCartesian();

  1059.         // Differentiating the eccentric longitude equation
  1060.         // tan((lv - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
  1061.         // leads to
  1062.         // cT (dlv - dlE) = cE dlE + cX dex + cY dey
  1063.         // with
  1064.         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
  1065.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
  1066.         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  1067.         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1068.         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1069.         // which can be solved to find the differential of the true longitude
  1070.         // dlv = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
  1071.         final SinCos scLe      = FastMath.sinCos(getLE());
  1072.         final double cosLe     = scLe.cos();
  1073.         final double sinLe     = scLe.sin();
  1074.         final double eSinE     = ex * sinLe - ey * cosLe;
  1075.         final double ecosE     = ex * cosLe + ey * sinLe;
  1076.         final double e2        = ex * ex + ey * ey;
  1077.         final double epsilon   = FastMath.sqrt(1 - e2);
  1078.         final double onePeps   = 1 + epsilon;
  1079.         final double d         = onePeps - ecosE;
  1080.         final double cT        = (d * d + eSinE * eSinE) / 2;
  1081.         final double cE        = ecosE * onePeps - e2;
  1082.         final double cX        = ex * eSinE / epsilon - ey + sinLe * onePeps;
  1083.         final double cY        = ey * eSinE / epsilon + ex - cosLe * onePeps;
  1084.         final double factorLe  = (cT + cE) / cT;
  1085.         final double factorEx  = cX / cT;
  1086.         final double factorEy  = cY / cT;

  1088.         // update longitude row
  1089.         final double[] rowEx = jacobian[1];
  1090.         final double[] rowEy = jacobian[2];
  1091.         final double[] rowL = jacobian[5];
  1092.         for (int j = 0; j < 6; ++j) {
  1093.             rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
  1094.         }

  1096.         return jacobian;

  1098.     }

  1100.     /** {@inheritDoc} */
  1101.     @Override
  1102.     public void addKeplerContribution(final PositionAngleType type, final double gm,
  1103.                                       final double[] pDot) {
  1104.         pDot[5] += computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType);
  1105.     }

  1107.     /**
  1108.      * Compute rate of argument of longitude.
  1109.      * @param type position angle type of rate
  1110.      * @param a semi major axis
  1111.      * @param ex ex
  1112.      * @param ey ey
  1113.      * @param mu mu
  1114.      * @param l argument of longitude
  1115.      * @param cachedType position angle type of passed l
  1116.      * @return first-order time derivative for l
  1117.      * @since 12.2
  1118.      */
  1119.     private static double computeKeplerianLDot(final PositionAngleType type, final double a, final double ex,
  1120.                                                final double ey, final double mu,
  1121.                                                final double l, final PositionAngleType cachedType) {
  1122.         final double n  = FastMath.sqrt(mu / a) / a;
  1123.         if (type == PositionAngleType.MEAN) {
  1124.             return n;
  1125.         }
  1126.         final double oMe2;
  1127.         final double ksi;
  1128.         final SinCos sc;
  1129.         if (type == PositionAngleType.ECCENTRIC) {
  1130.             sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
  1131.             ksi  = 1. / (1 - ex * sc.cos() - ey * sc.sin());
  1132.             return n * ksi;
  1133.         } else { // TRUE
  1134.             sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
  1135.             oMe2 = 1 - ex * ex - ey * ey;
  1136.             ksi  = 1 + ex * sc.cos() + ey * sc.sin();
  1137.             return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
  1138.         }
  1139.     }

  1141.     /**  Returns a string representation of this equinoctial parameters object.
  1142.      * @return a string representation of this object
  1143.      */
  1144.     public String toString() {
  1145.         return new StringBuilder().append("equinoctial parameters: ").append('{').
  1146.                                   append("a: ").append(a).
  1147.                                   append("; ex: ").append(ex).append("; ey: ").append(ey).
  1148.                                   append("; hx: ").append(hx).append("; hy: ").append(hy).
  1149.                                   append("; lv: ").append(FastMath.toDegrees(getLv())).
  1150.                                   append(";}").toString();
  1151.     }

  1153.     /** {@inheritDoc} */
  1154.     @Override
  1155.     public PositionAngleType getCachedPositionAngleType() {
  1156.         return cachedPositionAngleType;
  1157.     }

  1159.     /** {@inheritDoc} */
  1160.     @Override
  1161.     public boolean hasRates() {
  1162.         return hasDerivatives();
  1163.     }

  1165.     /** {@inheritDoc} */
  1166.     @Override
  1167.     public EquinoctialOrbit removeRates() {
  1168.         final PositionAngleType positionAngleType = getCachedPositionAngleType();
  1169.         return new EquinoctialOrbit(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
  1170.                 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
  1171.     }

  1173.     /** Replace the instance with a data transfer object for serialization.
  1174.      * @return data transfer object that will be serialized
  1175.      */
  1176.     @DefaultDataContext
  1177.     private Object writeReplace() {
  1178.         return new DTO(this);
  1179.     }

  1181.     /** Internal class used only for serialization. */
  1182.     @DefaultDataContext
  1183.     private static class DTO implements Serializable {

  1185.         /** Serializable UID. */
  1186.         private static final long serialVersionUID = 20231217L;

  1188.         /** Double values. */
  1189.         private final double[] d;

  1191.         /** Frame in which are defined the orbital parameters. */
  1192.         private final Frame frame;

  1194.         /** Cached type of position angle. */
  1195.         private final PositionAngleType positionAngleType;

  1197.         /** Simple constructor.
  1198.          * @param orbit instance to serialize
  1199.          */
  1200.         private DTO(final EquinoctialOrbit orbit) {

  1202.             final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();
  1203.             positionAngleType = orbit.cachedPositionAngleType;

  1205.             // decompose date
  1206.             final AbsoluteDate j2000Epoch =
  1207.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  1208.             final double epoch  = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
  1209.             final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));

  1211.             if (orbit.hasDerivatives()) {
  1212.                 // we have derivatives
  1213.                 this.d = new double[] {
  1214.                     epoch, offset, orbit.getMu(),
  1215.                     orbit.a, orbit.ex, orbit.ey,
  1216.                     orbit.hx, orbit.hy, orbit.cachedL,
  1217.                     orbit.aDot, orbit.exDot, orbit.eyDot,
  1218.                     orbit.hxDot, orbit.hyDot, orbit.cachedLDot
  1219.                 };
  1220.             } else {
  1221.                 // we don't have derivatives
  1222.                 this.d = new double[] {
  1223.                     epoch, offset, orbit.getMu(),
  1224.                     orbit.a, orbit.ex, orbit.ey,
  1225.                     orbit.hx, orbit.hy, orbit.cachedL
  1226.                 };
  1227.             }

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

  1231.         }

  1233.         /** Replace the deserialized data transfer object with a {@link EquinoctialOrbit}.
  1234.          * @return replacement {@link EquinoctialOrbit}
  1235.          */
  1236.         private Object readResolve() {
  1237.             final AbsoluteDate j2000Epoch =
  1238.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  1239.             if (d.length >= 15) {
  1240.                 // we have derivatives
  1241.                 return new EquinoctialOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8],
  1242.                                             d[ 9], d[10], d[11], d[12], d[13], d[14],
  1243.                                             positionAngleType,
  1244.                                             frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  1245.                                             d[2]);
  1246.             } else {
  1247.                 // we don't have derivatives
  1248.                 return new EquinoctialOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8], positionAngleType,
  1249.                                             frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  1250.                                             d[2]);
  1251.             }
  1252.         }

  1254.     }

  1256. }
Created with JaCoCo 0.8.12.202403310830