FieldEquinoctialOrbit.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 org.hipparchus.Field;
  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.util.FastMath;
  24. import org.hipparchus.util.FieldSinCos;
  25. import org.hipparchus.util.MathArrays;
  26. import org.orekit.errors.OrekitIllegalArgumentException;
  27. import org.orekit.errors.OrekitInternalError;
  28. import org.orekit.errors.OrekitMessages;
  29. import org.orekit.frames.Frame;
  30. import org.orekit.time.FieldAbsoluteDate;
  31. import org.orekit.utils.FieldPVCoordinates;
  32. import org.orekit.utils.TimeStampedFieldPVCoordinates;


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

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

  81.     /** First component of the eccentricity vector. */
  82.     private final T ex;

  84.     /** Second component of the eccentricity vector. */
  85.     private final T ey;

  87.     /** First component of the inclination vector. */
  88.     private final T hx;

  90.     /** Second component of the inclination vector. */
  91.     private final T hy;

  93.     /** Cached longitude argument (rad). */
  94.     private final T cachedL;

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

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

  102.     /** First component of the eccentricity vector derivative. */
  103.     private final T exDot;

  105.     /** Second component of the eccentricity vector derivative. */
  106.     private final T eyDot;

  108.     /** First component of the inclination vector derivative. */
  109.     private final T hxDot;

  111.     /** Second component of the inclination vector derivative. */
  112.     private final T hyDot;

  114.     /** Derivative of cached longitude argument (rad/s). */
  115.     private final T cachedLDot;

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

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

  147.     /** Creates a new instance.
  148.      * @param a  semi-major axis (m)
  149.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  150.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  151.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  152.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  153.      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
  154.      * @param type type of longitude argument
  155.      * @param frame the frame in which the parameters are defined
  156.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  157.      * @param date date of the orbital parameters
  158.      * @param mu central attraction coefficient (m³/s²)
  159.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  160.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  161.      */
  162.     public FieldEquinoctialOrbit(final T a, final T ex, final T ey,
  163.                                  final T hx, final T hy, final T l,
  164.                                  final PositionAngleType type,
  165.                                  final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  166.             throws IllegalArgumentException {
  167.         this(a, ex, ey, hx, hy, l,
  168.                 null, null, null, null, null, null,
  169.                 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 FieldEquinoctialOrbit(final T a, final T ex, final T ey,
  196.                                  final T hx, final T hy, final T l,
  197.                                  final T aDot, final T exDot, final T eyDot,
  198.                                  final T hxDot, final T hyDot, final T lDot,
  199.                                  final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
  200.                                  final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  201.         throws IllegalArgumentException {
  202.         super(frame, date, mu);

  204.         if (ex.getReal() * ex.getReal() + ey.getReal() * ey.getReal() >= 1.0) {
  205.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  206.                                                      getClass().getName());
  207.         }
  208.         this.cachedPositionAngleType = cachedPositionAngleType;
  209.         this.a     = a;
  210.         this.aDot  = aDot;
  211.         this.ex    = ex;
  212.         this.exDot = exDot;
  213.         this.ey    = ey;
  214.         this.eyDot = eyDot;
  215.         this.hx    = hx;
  216.         this.hxDot = hxDot;
  217.         this.hy    = hy;
  218.         this.hyDot = hyDot;

  220.         if (hasDerivatives()) {
  221.             final FieldUnivariateDerivative1<T> lUD = initializeCachedL(l, lDot, type);
  222.             this.cachedL = lUD.getValue();
  223.             this.cachedLDot = lUD.getFirstDerivative();
  224.         } else {
  225.             this.cachedL = initializeCachedL(l, type);
  226.             this.cachedLDot = null;
  227.         }

  229.         this.partialPV = null;

  231.     }

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

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

  284.         //  compute semi-major axis
  285.         final FieldVector3D<T> pvP = pvCoordinates.getPosition();
  286.         final FieldVector3D<T> pvV = pvCoordinates.getVelocity();
  287.         final FieldVector3D<T> pvA = pvCoordinates.getAcceleration();
  288.         final T r2 = pvP.getNormSq();
  289.         final T r  = r2.sqrt();
  290.         final T V2 = pvV.getNormSq();
  291.         final T rV2OnMu = r.multiply(V2).divide(mu);

  293.         // compute semi-major axis
  294.         a = r.divide(rV2OnMu.negate().add(2));

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

  301.         // compute inclination vector
  302.         final FieldVector3D<T> w = pvCoordinates.getMomentum().normalize();
  303.         final T d = getOne().divide(getOne().add(w.getZ()));
  304.         hx =  d.negate().multiply(w.getY());
  305.         hy =  d.multiply(w.getX());

  307.         // compute true longitude argument
  308.         cachedPositionAngleType = PositionAngleType.TRUE;
  309.         final T cLv = (pvP.getX().subtract(d.multiply(pvP.getZ()).multiply(w.getX()))).divide(r);
  310.         final T sLv = (pvP.getY().subtract(d.multiply(pvP.getZ()).multiply(w.getY()))).divide(r);
  311.         cachedL = sLv.atan2(cLv);

  313.         // compute eccentricity vector
  314.         final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(mu).sqrt());
  315.         final T eCE = rV2OnMu.subtract(1);
  316.         final T e2  = eCE.square().add(eSE.square());
  317.         final T f   = eCE.subtract(e2);
  318.         final T g   = e2.negate().add(1).sqrt().multiply(eSE);
  319.         ex = a.multiply(f.multiply(cLv).add( g.multiply(sLv))).divide(r);
  320.         ey = a.multiply(f.multiply(sLv).subtract(g.multiply(cLv))).divide(r);

  322.         partialPV = pvCoordinates;

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

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

  330.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  331.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  332.             final T   aX                       = nonKeplerianAcceleration.getX();
  333.             final T   aY                       = nonKeplerianAcceleration.getY();
  334.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  335.             aDot  = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  336.             exDot = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  337.             eyDot = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  338.             hxDot = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  339.             hyDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  341.             // in order to compute true longitude argument derivative, we must compute
  342.             // mean longitude argument derivative including Keplerian motion and convert to true anomaly
  343.             final T lMDot = getKeplerianMeanMotion().
  344.                             add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  345.             final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  346.             final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  347.             final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(getLM(), lMDot);
  348.             final FieldUnivariateDerivative1<T> lvUD = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lMUD);
  349.             cachedLDot = lvUD.getFirstDerivative();

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

  363.     }

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

  386.     /** Constructor from any kind of orbital parameters.
  387.      * @param op orbital parameters to copy
  388.      */
  389.     public FieldEquinoctialOrbit(final FieldOrbit<T> op) {
  390.         super(op.getFrame(), op.getDate(), op.getMu());

  392.         a     = op.getA();
  393.         ex    = op.getEquinoctialEx();
  394.         ey    = op.getEquinoctialEy();
  395.         hx    = op.getHx();
  396.         hy    = op.getHy();
  397.         cachedPositionAngleType = PositionAngleType.TRUE;
  398.         cachedL    = op.getLv();

  400.         aDot  = op.getADot();
  401.         exDot = op.getEquinoctialExDot();
  402.         eyDot = op.getEquinoctialEyDot();
  403.         hxDot = op.getHxDot();
  404.         hyDot = op.getHyDot();
  405.         cachedLDot = op.getLvDot();
  406.     }

  408.     /** Constructor from Field and EquinoctialOrbit.
  409.      * <p>Build a FieldEquinoctialOrbit from non-Field EquinoctialOrbit.</p>
  410.      * @param field CalculusField to base object on
  411.      * @param op non-field orbit with only "constant" terms
  412.      * @since 12.0
  413.      */
  414.     public FieldEquinoctialOrbit(final Field<T> field, final EquinoctialOrbit op) {
  415.         super(op.getFrame(), new FieldAbsoluteDate<>(field, op.getDate()), field.getZero().newInstance(op.getMu()));

  417.         a     = getZero().newInstance(op.getA());
  418.         ex    = getZero().newInstance(op.getEquinoctialEx());
  419.         ey    = getZero().newInstance(op.getEquinoctialEy());
  420.         hx    = getZero().newInstance(op.getHx());
  421.         hy    = getZero().newInstance(op.getHy());
  422.         cachedPositionAngleType = op.getCachedPositionAngleType();
  423.         cachedL    = getZero().newInstance(op.getL(cachedPositionAngleType));

  425.         if (op.hasDerivatives()) {
  426.             aDot  = getZero().newInstance(op.getADot());
  427.             exDot = getZero().newInstance(op.getEquinoctialExDot());
  428.             eyDot = getZero().newInstance(op.getEquinoctialEyDot());
  429.             hxDot = getZero().newInstance(op.getHxDot());
  430.             hyDot = getZero().newInstance(op.getHyDot());
  431.             cachedLDot = getZero().newInstance(op.getLDot(cachedPositionAngleType));
  432.         } else {
  433.             aDot  = null;
  434.             exDot = null;
  435.             eyDot = null;
  436.             hxDot = null;
  437.             hyDot = null;
  438.             cachedLDot = null;
  439.         }

  441.     }

  443.     /** Constructor from Field and Orbit.
  444.      * <p>Build a FieldEquinoctialOrbit from any non-Field Orbit.</p>
  445.      * @param field CalculusField to base object on
  446.      * @param op non-field orbit with only "constant" terms
  447.      * @since 12.0
  448.      */
  449.     public FieldEquinoctialOrbit(final Field<T> field, final Orbit op) {
  450.         this(field, (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(op));
  451.     }

  453.     /** {@inheritDoc} */
  454.     @Override
  455.     public OrbitType getType() {
  456.         return OrbitType.EQUINOCTIAL;
  457.     }

  459.     /** {@inheritDoc} */
  460.     @Override
  461.     public T getA() {
  462.         return a;
  463.     }

  465.     /** {@inheritDoc} */
  466.     @Override
  467.     public T getADot() {
  468.         return aDot;
  469.     }

  471.     /** {@inheritDoc} */
  472.     @Override
  473.     public T getEquinoctialEx() {
  474.         return ex;
  475.     }

  477.     /** {@inheritDoc} */
  478.     @Override
  479.     public T getEquinoctialExDot() {
  480.         return exDot;
  481.     }

  483.     /** {@inheritDoc} */
  484.     @Override
  485.     public T getEquinoctialEy() {
  486.         return ey;
  487.     }

  489.     /** {@inheritDoc} */
  490.     @Override
  491.     public T getEquinoctialEyDot() {
  492.         return eyDot;
  493.     }

  495.     /** {@inheritDoc} */
  496.     @Override
  497.     public T getHx() {
  498.         return hx;
  499.     }

  501.     /** {@inheritDoc} */
  502.     @Override
  503.     public T getHxDot() {
  504.         return hxDot;
  505.     }

  507.     /** {@inheritDoc} */
  508.     @Override
  509.     public T getHy() {
  510.         return hy;
  511.     }

  513.     /** {@inheritDoc} */
  514.     @Override
  515.     public T getHyDot() {
  516.         return hyDot;
  517.     }

  519.     /** {@inheritDoc} */
  520.     @Override
  521.     public T getLv() {
  522.         switch (cachedPositionAngleType) {
  523.             case TRUE:
  524.                 return cachedL;

  526.             case ECCENTRIC:
  527.                 return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(ex, ey, cachedL);

  529.             case MEAN:
  530.                 return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(ex, ey, cachedL);

  532.             default:
  533.                 throw new OrekitInternalError(null);
  534.         }
  535.     }

  537.     /** {@inheritDoc} */
  538.     @Override
  539.     public T getLvDot() {

  541.         if (!hasDerivatives()) {
  542.             return null;
  543.         }
  544.         switch (cachedPositionAngleType) {
  545.             case ECCENTRIC:
  546.                 final FieldUnivariateDerivative1<T> lEUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  547.                 final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex,     exDot);
  548.                 final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey,     eyDot);
  549.                 final FieldUnivariateDerivative1<T> lvUD = FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
  550.                         lEUD);
  551.                 return lvUD.getFirstDerivative();

  553.             case TRUE:
  554.                 return cachedLDot;

  556.             case MEAN:
  557.                 final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  558.                 final FieldUnivariateDerivative1<T> exUD2    = new FieldUnivariateDerivative1<>(ex,     exDot);
  559.                 final FieldUnivariateDerivative1<T> eyUD2    = new FieldUnivariateDerivative1<>(ey,     eyDot);
  560.                 final FieldUnivariateDerivative1<T> lvUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD2,
  561.                         eyUD2, lMUD);
  562.                 return lvUD2.getFirstDerivative();

  564.             default:
  565.                 throw new OrekitInternalError(null);
  566.         }
  567.     }

  569.     /** {@inheritDoc} */
  570.     @Override
  571.     public T getLE() {
  572.         switch (cachedPositionAngleType) {
  573.             case TRUE:
  574.                 return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(ex, ey, cachedL);

  576.             case ECCENTRIC:
  577.                 return cachedL;

  579.             case MEAN:
  580.                 return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(ex, ey, cachedL);

  582.             default:
  583.                 throw new OrekitInternalError(null);
  584.         }
  585.     }

  587.     /** {@inheritDoc} */
  588.     @Override
  589.     public T getLEDot() {

  591.         if (!hasDerivatives()) {
  592.             return null;
  593.         }
  594.         switch (cachedPositionAngleType) {
  595.             case TRUE:
  596.                 final FieldUnivariateDerivative1<T> lvUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  597.                 final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex,     exDot);
  598.                 final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey,     eyDot);
  599.                 final FieldUnivariateDerivative1<T> lEUD = FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD,
  600.                         lvUD);
  601.                 return lEUD.getFirstDerivative();

  603.             case ECCENTRIC:
  604.                 return cachedLDot;

  606.             case MEAN:
  607.                 final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  608.                 final FieldUnivariateDerivative1<T> exUD2    = new FieldUnivariateDerivative1<>(ex,     exDot);
  609.                 final FieldUnivariateDerivative1<T> eyUD2    = new FieldUnivariateDerivative1<>(ey,     eyDot);
  610.                 final FieldUnivariateDerivative1<T> lEUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD2,
  611.                         eyUD2, lMUD);
  612.                 return lEUD2.getFirstDerivative();

  614.             default:
  615.                 throw new OrekitInternalError(null);
  616.         }
  617.     }

  619.     /** {@inheritDoc} */
  620.     @Override
  621.     public T getLM() {
  622.         switch (cachedPositionAngleType) {
  623.             case TRUE:
  624.                 return FieldEquinoctialLongitudeArgumentUtility.trueToMean(ex, ey, cachedL);

  626.             case MEAN:
  627.                 return cachedL;

  629.             case ECCENTRIC:
  630.                 return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(ex, ey, cachedL);

  632.             default:
  633.                 throw new OrekitInternalError(null);
  634.         }
  635.     }

  637.     /** {@inheritDoc} */
  638.     @Override
  639.     public T getLMDot() {

  641.         if (!hasDerivatives()) {
  642.             return null;
  643.         }
  644.         switch (cachedPositionAngleType) {
  645.             case TRUE:
  646.                 final FieldUnivariateDerivative1<T> lvUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  647.                 final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex,     exDot);
  648.                 final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey,     eyDot);
  649.                 final FieldUnivariateDerivative1<T> lMUD = FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lvUD);
  650.                 return lMUD.getFirstDerivative();

  652.             case MEAN:
  653.                 return cachedLDot;

  655.             case ECCENTRIC:
  656.                 final FieldUnivariateDerivative1<T> lEUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  657.                 final FieldUnivariateDerivative1<T> exUD2    = new FieldUnivariateDerivative1<>(ex,     exDot);
  658.                 final FieldUnivariateDerivative1<T> eyUD2    = new FieldUnivariateDerivative1<>(ey,     eyDot);
  659.                 final FieldUnivariateDerivative1<T> lMUD2 = FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD2,
  660.                         eyUD2, lEUD);
  661.                 return lMUD2.getFirstDerivative();

  663.             default:
  664.                 throw new OrekitInternalError(null);
  665.         }
  666.     }

  668.     /** Get the longitude argument.
  669.      * @param type type of the angle
  670.      * @return longitude argument (rad)
  671.      */
  672.     public T getL(final PositionAngleType type) {
  673.         return (type == PositionAngleType.MEAN) ? getLM() :
  674.                                               ((type == PositionAngleType.ECCENTRIC) ? getLE() :
  675.                                                                                    getLv());
  676.     }

  678.     /** Get the longitude argument derivative.
  679.      * @param type type of the angle
  680.      * @return longitude argument derivative (rad/s)
  681.      */
  682.     public T getLDot(final PositionAngleType type) {
  683.         return (type == PositionAngleType.MEAN) ? getLMDot() :
  684.                                               ((type == PositionAngleType.ECCENTRIC) ? getLEDot() :
  685.                                                                                    getLvDot());
  686.     }

  688.     /** {@inheritDoc} */
  689.     @Override
  690.     public boolean hasDerivatives() {
  691.         return aDot != null;
  692.     }

  694.     /** Computes the true longitude argument from the eccentric longitude argument.
  695.      * @param lE = E + ω + Ω eccentric longitude argument (rad)
  696.      * @param ex first component of the eccentricity vector
  697.      * @param ey second component of the eccentricity vector
  698.      * @param <T> Type of the field elements
  699.      * @return the true longitude argument
  700.      */
  701.     @Deprecated
  702.     public static <T extends CalculusFieldElement<T>> T eccentricToTrue(final T lE, final T ex, final T ey) {
  703.         return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(ex, ey, lE);
  704.     }

  706.     /** Computes the eccentric longitude argument from the true longitude argument.
  707.      * @param lv = v + ω + Ω true longitude argument (rad)
  708.      * @param ex first component of the eccentricity vector
  709.      * @param ey second component of the eccentricity vector
  710.      * @param <T> Type of the field elements
  711.      * @return the eccentric longitude argument
  712.      */
  713.     @Deprecated
  714.     public static <T extends CalculusFieldElement<T>> T trueToEccentric(final T lv, final T ex, final T ey) {
  715.         return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(ex, ey, lv);
  716.     }

  718.     /** Computes the eccentric longitude argument from the mean longitude argument.
  719.      * @param lM = M + ω + Ω mean longitude argument (rad)
  720.      * @param ex first component of the eccentricity vector
  721.      * @param ey second component of the eccentricity vector
  722.      * @param <T> Type of the field elements
  723.      * @return the eccentric longitude argument
  724.      */
  725.     @Deprecated
  726.     public static <T extends CalculusFieldElement<T>> T meanToEccentric(final T lM, final T ex, final T ey) {
  727.         return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(ex, ey, lM);
  728.     }

  730.     /** Computes the mean longitude argument from the eccentric longitude argument.
  731.      * @param lE = E + ω + Ω mean longitude argument (rad)
  732.      * @param ex first component of the eccentricity vector
  733.      * @param ey second component of the eccentricity vector
  734.      * @param <T> Type of the field elements
  735.      * @return the mean longitude argument
  736.      */
  737.     @Deprecated
  738.     public static <T extends CalculusFieldElement<T>> T eccentricToMean(final T lE, final T ex, final T ey) {
  739.         return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(ex, ey, lE);
  740.     }

  742.     /** {@inheritDoc} */
  743.     @Override
  744.     public T getE() {
  745.         return ex.square().add(ey.square()).sqrt();
  746.     }

  748.     /** {@inheritDoc} */
  749.     @Override
  750.     public T getEDot() {

  752.         if (!hasDerivatives()) {
  753.             return null;
  754.         }

  756.         return ex.multiply(exDot).add(ey.multiply(eyDot)).divide(ex.square().add(ey.square()).sqrt());

  758.     }

  760.     /** {@inheritDoc} */
  761.     @Override
  762.     public T getI() {
  763.         return hx.square().add(hy.square()).sqrt().atan().multiply(2);
  764.     }

  766.     /** {@inheritDoc} */
  767.     @Override
  768.     public T getIDot() {

  770.         if (!hasDerivatives()) {
  771.             return null;
  772.         }

  774.         final T h2 = hx.square().add(hy.square());
  775.         final T h  = h2.sqrt();
  776.         return hx.multiply(hxDot).add(hy.multiply(hyDot)).multiply(2).divide(h.multiply(h2.add(1)));

  778.     }

  780.     /** Compute position and velocity but not acceleration.
  781.      */
  782.     private void computePVWithoutA() {

  784.         if (partialPV != null) {
  785.             // already computed
  786.             return;
  787.         }

  789.         // get equinoctial parameters
  790.         final T lE = getLE();

  792.         // inclination-related intermediate parameters
  793.         final T hx2   = hx.square();
  794.         final T hy2   = hy.square();
  795.         final T factH = getOne().divide(hx2.add(1.0).add(hy2));

  797.         // reference axes defining the orbital plane
  798.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  799.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  800.         final T uz = hy.multiply(-2).multiply(factH);

  802.         final T vx = uy;
  803.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  804.         final T vz =  hx.multiply(factH).multiply(2);

  806.         // eccentricity-related intermediate parameters
  807.         final T ex2  = ex.square();
  808.         final T exey = ex.multiply(ey);
  809.         final T ey2  = ey.square();
  810.         final T e2   = ex2.add(ey2);
  811.         final T eta  = getOne().subtract(e2).sqrt().add(1);
  812.         final T beta = getOne().divide(eta);

  814.         // eccentric longitude argument
  815.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  816.         final T cLe    = scLe.cos();
  817.         final T sLe    = scLe.sin();
  818.         final T exCeyS = ex.multiply(cLe).add(ey.multiply(sLe));

  820.         // coordinates of position and velocity in the orbital plane
  821.         final T x      = a.multiply(getOne().subtract(beta.multiply(ey2)).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(ex));
  822.         final T y      = a.multiply(getOne().subtract(beta.multiply(ex2)).multiply(sLe).add(beta .multiply(exey).multiply(cLe)).subtract(ey));

  824.         final T factor = getMu().divide(a).sqrt().divide(getOne().subtract(exCeyS));
  825.         final T xdot   = factor.multiply(sLe.negate().add(beta.multiply(ey).multiply(exCeyS)));
  826.         final T ydot   = factor.multiply(cLe.subtract(beta.multiply(ex).multiply(exCeyS)));

  828.         final FieldVector3D<T> position =
  829.                         new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  830.                                             x.multiply(uy).add(y.multiply(vy)),
  831.                                             x.multiply(uz).add(y.multiply(vz)));
  832.         final FieldVector3D<T> velocity =
  833.                         new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)), xdot.multiply(uy).add(ydot.multiply(vy)), xdot.multiply(uz).add(ydot.multiply(vz)));

  835.         partialPV = new FieldPVCoordinates<>(position, velocity);

  837.     }

  839.     /** Initialize cached argument of longitude with rate.
  840.      * @param l input argument of longitude
  841.      * @param lDot rate of input argument of longitude
  842.      * @param inputType position angle type passed as input
  843.      * @return argument of longitude to cache with rate
  844.      * @since 12.1
  845.      */
  846.     private FieldUnivariateDerivative1<T> initializeCachedL(final T l, final T lDot,
  847.                                                             final PositionAngleType inputType) {
  848.         if (cachedPositionAngleType == inputType) {
  849.             return new FieldUnivariateDerivative1<>(l, lDot);

  851.         } else {
  852.             final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  853.             final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  854.             final FieldUnivariateDerivative1<T> lUD = new FieldUnivariateDerivative1<>(l, lDot);

  856.             switch (cachedPositionAngleType) {

  858.                 case ECCENTRIC:
  859.                     if (inputType == PositionAngleType.MEAN) {
  860.                         return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD, eyUD, lUD);
  861.                     } else {
  862.                         return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD, lUD);
  863.                     }

  865.                 case TRUE:
  866.                     if (inputType == PositionAngleType.MEAN) {
  867.                         return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lUD);
  868.                     } else {
  869.                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD, lUD);
  870.                     }

  872.                 case MEAN:
  873.                     if (inputType == PositionAngleType.TRUE) {
  874.                         return FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lUD);
  875.                     } else {
  876.                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD, eyUD, lUD);
  877.                     }

  879.                 default:
  880.                     throw new OrekitInternalError(null);

  882.             }

  884.         }

  886.     }

  888.     /** Initialize cached argument of longitude.
  889.      * @param l input argument of longitude
  890.      * @param positionAngleType position angle type passed as input
  891.      * @return argument of longitude to cache
  892.      * @since 12.1
  893.      */
  894.     private T initializeCachedL(final T l, final PositionAngleType positionAngleType) {
  895.         return FieldEquinoctialLongitudeArgumentUtility.convertL(positionAngleType, l, ex, ey, cachedPositionAngleType);
  896.     }

  898.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  899.      * <p>
  900.      * This method should be called only when {@link #hasDerivatives()} returns true.
  901.      * </p>
  902.      * @return non-Keplerian part of the acceleration
  903.      */
  904.     private FieldVector3D<T> nonKeplerianAcceleration() {

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

  909.         final T nonKeplerianMeanMotion = getLMDot().subtract(getKeplerianMeanMotion());
  910.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  911.                                  add(dCdP[3][1].multiply(exDot)).
  912.                                  add(dCdP[3][2].multiply(eyDot)).
  913.                                  add(dCdP[3][3].multiply(hxDot)).
  914.                                  add(dCdP[3][4].multiply(hyDot)).
  915.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  916.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  917.                                  add(dCdP[4][1].multiply(exDot)).
  918.                                  add(dCdP[4][2].multiply(eyDot)).
  919.                                  add(dCdP[4][3].multiply(hxDot)).
  920.                                  add(dCdP[4][4].multiply(hyDot)).
  921.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  922.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  923.                                  add(dCdP[5][1].multiply(exDot)).
  924.                                  add(dCdP[5][2].multiply(eyDot)).
  925.                                  add(dCdP[5][3].multiply(hxDot)).
  926.                                  add(dCdP[5][4].multiply(hyDot)).
  927.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  931.     }

  933.     /** {@inheritDoc} */
  934.     @Override
  935.     protected FieldVector3D<T> initPosition() {

  937.         // get equinoctial parameters
  938.         final T lE = getLE();

  940.         // inclination-related intermediate parameters
  941.         final T hx2   = hx.square();
  942.         final T hy2   = hy.square();
  943.         final T factH = getOne().divide(hx2.add(1.0).add(hy2));

  945.         // reference axes defining the orbital plane
  946.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  947.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  948.         final T uz = hy.multiply(-2).multiply(factH);

  950.         final T vx = uy;
  951.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  952.         final T vz =  hx.multiply(factH).multiply(2);

  954.         // eccentricity-related intermediate parameters
  955.         final T ex2  = ex.square();
  956.         final T exey = ex.multiply(ey);
  957.         final T ey2  = ey.square();
  958.         final T e2   = ex2.add(ey2);
  959.         final T eta  = getOne().subtract(e2).sqrt().add(1);
  960.         final T beta = getOne().divide(eta);

  962.         // eccentric longitude argument
  963.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  964.         final T cLe    = scLe.cos();
  965.         final T sLe    = scLe.sin();

  967.         // coordinates of position and velocity in the orbital plane
  968.         final T x      = a.multiply(getOne().subtract(beta.multiply(ey2)).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(ex));
  969.         final T y      = a.multiply(getOne().subtract(beta.multiply(ex2)).multiply(sLe).add(beta .multiply(exey).multiply(cLe)).subtract(ey));

  971.         return new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  972.                                    x.multiply(uy).add(y.multiply(vy)),
  973.                                    x.multiply(uz).add(y.multiply(vz)));

  975.     }

  977.     /** {@inheritDoc} */
  978.     @Override
  979.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  981.         // position and velocity
  982.         computePVWithoutA();

  984.         // acceleration
  985.         final T r2 = partialPV.getPosition().getNormSq();
  986.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(getMu().negate()),
  987.                                                                            partialPV.getPosition());
  988.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  989.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  990.                                               keplerianAcceleration;

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

  994.     }

  996.     /** {@inheritDoc} */
  997.     @Override
  998.     public FieldEquinoctialOrbit<T> shiftedBy(final double dt) {
  999.         return shiftedBy(getZero().newInstance(dt));
  1000.     }

  1002.     /** {@inheritDoc} */
  1003.     @Override
  1004.     public FieldEquinoctialOrbit<T> shiftedBy(final T dt) {

  1006.         // use Keplerian-only motion
  1007.         final FieldEquinoctialOrbit<T> keplerianShifted = new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy,
  1008.                                                                                       getLM().add(getKeplerianMeanMotion().multiply(dt)),
  1009.                                                                                       PositionAngleType.MEAN, cachedPositionAngleType, getFrame(),
  1010.                                                                                       getDate().shiftedBy(dt), getMu());

  1012.         if (hasDerivatives()) {

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

  1017.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  1018.             keplerianShifted.computePVWithoutA();
  1019.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getPosition(),
  1020.                                                                   dt.square().multiply(0.5), nonKeplerianAcceleration);
  1021.             final T   fixedR2 = fixedP.getNormSq();
  1022.             final T   fixedR  = fixedR2.sqrt();
  1023.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getVelocity(),
  1024.                                                                  dt, nonKeplerianAcceleration);
  1025.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  1026.                                                                  keplerianShifted.partialPV.getPosition(),
  1027.                                                                  getOne(), nonKeplerianAcceleration);

  1029.             // build a new orbit, taking non-Keplerian acceleration into account
  1030.             return new FieldEquinoctialOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  1031.                                                                                    fixedP, fixedV, fixedA),
  1032.                                                keplerianShifted.getFrame(), keplerianShifted.getMu());

  1034.         } else {
  1035.             // Keplerian-only motion is all we can do
  1036.             return keplerianShifted;
  1037.         }

  1039.     }

  1041.     /** {@inheritDoc} */
  1042.     @Override
  1043.     protected T[][] computeJacobianMeanWrtCartesian() {

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

  1047.         // compute various intermediate parameters
  1048.         computePVWithoutA();
  1049.         final FieldVector3D<T> position = partialPV.getPosition();
  1050.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1051.         final T r2         = position.getNormSq();
  1052.         final T r          = r2.sqrt();
  1053.         final T r3         = r.multiply(r2);

  1055.         final T mu         = getMu();
  1056.         final T sqrtMuA    = a.multiply(mu).sqrt();
  1057.         final T a2         = a.square();

  1059.         final T e2         = ex.square().add(ey.square());
  1060.         final T oMe2       = getOne().subtract(e2);
  1061.         final T epsilon    = oMe2.sqrt();
  1062.         final T beta       = getOne().divide(epsilon.add(1));
  1063.         final T ratio      = epsilon.multiply(beta);

  1065.         final T hx2        = hx.square();
  1066.         final T hy2        = hy.square();
  1067.         final T hxhy       = hx.multiply(hy);

  1069.         // precomputing equinoctial frame unit vectors (f, g, w)
  1070.         final FieldVector3D<T> f  = new FieldVector3D<>(hx2.subtract(hy2).add(1), hxhy.multiply(2), hy.multiply(-2)).normalize();
  1071.         final FieldVector3D<T> g  = new FieldVector3D<>(hxhy.multiply(2), hy2.add(1).subtract(hx2), hx.multiply(2)).normalize();
  1072.         final FieldVector3D<T> w  = FieldVector3D.crossProduct(position, velocity).normalize();

  1074.         // coordinates of the spacecraft in the equinoctial frame
  1075.         final T x    = FieldVector3D.dotProduct(position, f);
  1076.         final T y    = FieldVector3D.dotProduct(position, g);
  1077.         final T xDot = FieldVector3D.dotProduct(velocity, f);
  1078.         final T yDot = FieldVector3D.dotProduct(velocity, g);

  1080.         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
  1081.         final T c1  = a.divide(sqrtMuA.multiply(epsilon));
  1082.         final T c1N = c1.negate();
  1083.         final T c2  = a.multiply(sqrtMuA).multiply(beta).divide(r3);
  1084.         final T c3  = sqrtMuA.divide(r3.multiply(epsilon));
  1085.         final FieldVector3D<T> drDotSdEx = new FieldVector3D<>(c1.multiply(xDot).multiply(yDot).subtract(c2.multiply(ey).multiply(x)).subtract(c3.multiply(x).multiply(y)), f,
  1086.                                                                c1N.multiply(xDot).multiply(xDot).subtract(c2.multiply(ey).multiply(y)).add(c3.multiply(x).multiply(x)), g);

  1088.         // drDot / dEy = dXDot / dEy * f + dYDot / dEy * g
  1089.         final FieldVector3D<T> drDotSdEy = new FieldVector3D<>(c1.multiply(yDot).multiply(yDot).add(c2.multiply(ex).multiply(x)).subtract(c3.multiply(y).multiply(y)), f,
  1090.                                                                c1N.multiply(xDot).multiply(yDot).add(c2.multiply(ex).multiply(y)).add(c3.multiply(x).multiply(y)), g);

  1092.         // da
  1093.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  1094.         final FieldVector3D<T> vectorARDot = new FieldVector3D<>(a2.multiply(2).divide(mu), velocity);
  1095.         fillHalfRow(getOne(), vectorAR,    jacobian[0], 0);
  1096.         fillHalfRow(getOne(), vectorARDot, jacobian[0], 3);

  1098.         // dEx
  1099.         final T d1 = a.negate().multiply(ratio).divide(r3);
  1100.         final T d2 = (hy.multiply(xDot).subtract(hx.multiply(yDot))).divide(sqrtMuA.multiply(epsilon));
  1101.         final T d3 = hx.multiply(y).subtract(hy.multiply(x)).divide(sqrtMuA);
  1102.         final FieldVector3D<T> vectorExRDot =
  1103.             new FieldVector3D<>(x.multiply(2).multiply(yDot).subtract(xDot.multiply(y)).divide(mu), g, y.negate().multiply(yDot).divide(mu), f, ey.negate().multiply(d3).divide(epsilon), w);
  1104.         fillHalfRow(ex.multiply(d1), position, ey.negate().multiply(d2), w, epsilon.divide(sqrtMuA), drDotSdEy, jacobian[1], 0);
  1105.         fillHalfRow(getOne(), vectorExRDot, jacobian[1], 3);

  1107.         // dEy
  1108.         final FieldVector3D<T> vectorEyRDot =
  1109.             new FieldVector3D<>(xDot.multiply(2).multiply(y).subtract(x.multiply(yDot)).divide(mu), f, x.negate().multiply(xDot).divide(mu), g, ex.multiply(d3).divide(epsilon), w);
  1110.         fillHalfRow(ey.multiply(d1), position, ex.multiply(d2), w, epsilon.negate().divide(sqrtMuA), drDotSdEx, jacobian[2], 0);
  1111.         fillHalfRow(getOne(), vectorEyRDot, jacobian[2], 3);

  1113.         // dHx
  1114.         final T h = (hx2.add(1).add(hy2)).divide(sqrtMuA.multiply(2).multiply(epsilon));
  1115.         fillHalfRow( h.negate().multiply(xDot), w, jacobian[3], 0);
  1116.         fillHalfRow( h.multiply(x),    w, jacobian[3], 3);

  1118.        // dHy
  1119.         fillHalfRow( h.negate().multiply(yDot), w, jacobian[4], 0);
  1120.         fillHalfRow( h.multiply(y),    w, jacobian[4], 3);

  1122.         // dLambdaM
  1123.         final T l = ratio.negate().divide(sqrtMuA);
  1124.         fillHalfRow(getOne().negate().divide(sqrtMuA), velocity, d2, w, l.multiply(ex), drDotSdEx, l.multiply(ey), drDotSdEy, jacobian[5], 0);
  1125.         fillHalfRow(getZero().newInstance(-2).divide(sqrtMuA), position, ex.multiply(beta), vectorEyRDot, ey.negate().multiply(beta), vectorExRDot, d3, w, jacobian[5], 3);

  1127.         return jacobian;

  1129.     }

  1131.     /** {@inheritDoc} */
  1132.     @Override
  1133.     protected T[][] computeJacobianEccentricWrtCartesian() {

  1135.         // start by computing the Jacobian with mean angle
  1136.         final T[][] jacobian = computeJacobianMeanWrtCartesian();

  1138.         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
  1139.         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
  1140.         // which is inverted and rewritten as:
  1141.         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
  1142.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  1143.         final T cosLe = scLe.cos();
  1144.         final T sinLe = scLe.sin();
  1145.         final T aOr   = getOne().divide(getOne().subtract(ex.multiply(cosLe)).subtract(ey.multiply(sinLe)));

  1147.         // update longitude row
  1148.         final T[] rowEx = jacobian[1];
  1149.         final T[] rowEy = jacobian[2];
  1150.         final T[] rowL  = jacobian[5];

  1152.         for (int j = 0; j < 6; ++j) {
  1153.             rowL[j] = aOr.multiply(rowL[j].add(sinLe.multiply(rowEx[j])).subtract(cosLe.multiply(rowEy[j])));
  1154.         }
  1155.         return jacobian;

  1157.     }

  1159.     /** {@inheritDoc} */
  1160.     @Override
  1161.     protected T[][] computeJacobianTrueWrtCartesian() {

  1163.         // start by computing the Jacobian with eccentric angle
  1164.         final T[][] jacobian = computeJacobianEccentricWrtCartesian();

  1166.         // Differentiating the eccentric longitude equation
  1167.         // tan((lV - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
  1168.         // leads to
  1169.         // cT (dlV - dlE) = cE dlE + cX dex + cY dey
  1170.         // with
  1171.         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
  1172.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
  1173.         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  1174.         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1175.         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1176.         // which can be solved to find the differential of the true longitude
  1177.         // dlV = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
  1178.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  1179.         final T cosLe     = scLe.cos();
  1180.         final T sinLe     = scLe.sin();
  1181.         final T eSinE     = ex.multiply(sinLe).subtract(ey.multiply(cosLe));
  1182.         final T ecosE     = ex.multiply(cosLe).add(ey.multiply(sinLe));
  1183.         final T e2        = ex.square().add(ey.square());
  1184.         final T epsilon   = getOne().subtract(e2).sqrt();
  1185.         final T onePeps   = epsilon.add(1);
  1186.         final T d         = onePeps.subtract(ecosE);
  1187.         final T cT        = d.multiply(d).add(eSinE.multiply(eSinE)).divide(2);
  1188.         final T cE        = ecosE.multiply(onePeps).subtract(e2);
  1189.         final T cX        = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinLe.multiply(onePeps));
  1190.         final T cY        = ey.multiply(eSinE).divide(epsilon).add( ex).subtract(cosLe.multiply(onePeps));
  1191.         final T factorLe  = cT.add(cE).divide(cT);
  1192.         final T factorEx  = cX.divide(cT);
  1193.         final T factorEy  = cY.divide(cT);

  1195.         // update longitude row
  1196.         final T[] rowEx = jacobian[1];
  1197.         final T[] rowEy = jacobian[2];
  1198.         final T[] rowL = jacobian[5];
  1199.         for (int j = 0; j < 6; ++j) {
  1200.             rowL[j] = factorLe.multiply(rowL[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
  1201.         }

  1203.         return jacobian;

  1205.     }

  1207.     /** {@inheritDoc} */
  1208.     @Override
  1209.     public void addKeplerContribution(final PositionAngleType type, final T gm,
  1210.                                       final T[] pDot) {
  1211.         pDot[5] = pDot[5].add(computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType));
  1212.     }

  1214.     /**
  1215.      * Compute rate of argument of longitude.
  1216.      * @param type position angle type of rate
  1217.      * @param a semi major axis
  1218.      * @param ex ex
  1219.      * @param ey ey
  1220.      * @param mu mu
  1221.      * @param l argument of longitude
  1222.      * @param cachedType position angle type of passed l
  1223.      * @param <T> field type
  1224.      * @return first-order time derivative for l
  1225.      * @since 12.2
  1226.      */
  1227.     private static <T extends CalculusFieldElement<T>> T computeKeplerianLDot(final PositionAngleType type, final T a, final T ex,
  1228.                                                                               final T ey, final T mu, final T l, final PositionAngleType cachedType) {
  1229.         final T n               = mu.divide(a).sqrt().divide(a);
  1230.         if (type == PositionAngleType.MEAN) {
  1231.             return n;
  1232.         }
  1233.         final FieldSinCos<T> sc;
  1234.         final T ksi;
  1235.         if (type == PositionAngleType.ECCENTRIC) {
  1236.             sc = FastMath.sinCos(FieldEquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
  1237.             ksi = ((ex.multiply(sc.cos())).add(ey.multiply(sc.sin()))).negate().add(1).reciprocal();
  1238.             return n.multiply(ksi);
  1239.         } else {
  1240.             sc = FastMath.sinCos(FieldEquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
  1241.             final T oMe2 = a.getField().getOne().subtract(ex.square()).subtract(ey.square());
  1242.             ksi  =  ex.multiply(sc.cos()).add(1).add(ey.multiply(sc.sin()));
  1243.             return n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt()));
  1244.         }
  1245.     }

  1247.     /**  Returns a string representation of this equinoctial parameters object.
  1248.      * @return a string representation of this object
  1249.      */
  1250.     public String toString() {
  1251.         return new StringBuilder().append("equinoctial parameters: ").append('{').
  1252.                                   append("a: ").append(a.getReal()).
  1253.                                   append("; ex: ").append(ex.getReal()).append("; ey: ").append(ey.getReal()).
  1254.                                   append("; hx: ").append(hx.getReal()).append("; hy: ").append(hy.getReal()).
  1255.                                   append("; lv: ").append(FastMath.toDegrees(getLv().getReal())).
  1256.                                   append(";}").toString();
  1257.     }

  1259.     /** {@inheritDoc} */
  1260.     @Override
  1261.     public PositionAngleType getCachedPositionAngleType() {
  1262.         return cachedPositionAngleType;
  1263.     }

  1265.     /** {@inheritDoc} */
  1266.     @Override
  1267.     public boolean hasRates() {
  1268.         return hasDerivatives();
  1269.     }

  1271.     /** {@inheritDoc} */
  1272.     @Override
  1273.     public FieldEquinoctialOrbit<T> removeRates() {
  1274.         return new FieldEquinoctialOrbit<>(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
  1275.                 cachedL, cachedPositionAngleType, getFrame(), getDate(), getMu());
  1276.     }

  1278.     /** {@inheritDoc} */
  1279.     @Override
  1280.     public EquinoctialOrbit toOrbit() {
  1281.         final double cachedPositionAngle = cachedL.getReal();
  1282.         if (hasDerivatives()) {
  1283.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1284.                                         hx.getReal(), hy.getReal(), cachedPositionAngle,
  1285.                                         aDot.getReal(), exDot.getReal(), eyDot.getReal(),
  1286.                                         hxDot.getReal(), hyDot.getReal(), cachedLDot.getReal(),
  1287.                                         cachedPositionAngleType, getFrame(),
  1288.                                         getDate().toAbsoluteDate(), getMu().getReal());
  1289.         } else {
  1290.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1291.                                         hx.getReal(), hy.getReal(), cachedPositionAngle,
  1292.                                         cachedPositionAngleType, getFrame(),
  1293.                                         getDate().toAbsoluteDate(), getMu().getReal());
  1294.         }
  1295.     }

  1297. }
Created with JaCoCo 0.8.12.202403310830