FieldEquinoctialOrbit.java

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

  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.FieldKinematicTransform;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.time.FieldAbsoluteDate;
  32. import org.orekit.utils.FieldPVCoordinates;
  33. import org.orekit.utils.TimeStampedFieldPVCoordinates;


  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.  * <p>
  53.  * The conversion equations from and to Keplerian elements given above hold only
  54.  * when both sides are unambiguously defined, i.e. when orbit is neither equatorial
  55.  * nor circular. When orbit is either equatorial or circular, the equinoctial
  56.  * parameters are still unambiguously defined whereas some Keplerian elements
  57.  * (more precisely ω and Ω) become ambiguous. For this reason, equinoctial
  58.  * parameters are the recommended way to represent orbits. Note however than
  59.  * the present implementation does not handle non-elliptical cases.
  60.  * </p>
  61.  * <p>
  62.  * The instance <code>EquinoctialOrbit</code> is guaranteed to be immutable.
  63.  * </p>
  64.  * @see    Orbit
  65.  * @see    KeplerianOrbit
  66.  * @see    CircularOrbit
  67.  * @see    CartesianOrbit
  68.  * @author Mathieu Rom&eacute;ro
  69.  * @author Luc Maisonobe
  70.  * @author Guylaine Prat
  71.  * @author Fabien Maussion
  72.  * @author V&eacute;ronique Pommier-Maurussane
  73.  * @since 9.0
  74.  * @param <T> type of the field elements
  75.  */
  76. public class FieldEquinoctialOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T>
  77.         implements PositionAngleBased<FieldEquinoctialOrbit<T>> {

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  221.         final FieldUnivariateDerivative1<T> lUD = initializeCachedL(l, lDot, type);
  222.         this.cachedL = lUD.getValue();
  223.         this.cachedLDot = lUD.getFirstDerivative();

  225.         this.partialPV = null;

  227.     }

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

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

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

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

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

  297.         // compute inclination vector
  298.         final FieldVector3D<T> w = pvCoordinates.getMomentum().normalize();
  299.         final T d = getOne().divide(getOne().add(w.getZ()));
  300.         hx =  d.negate().multiply(w.getY());
  301.         hy =  d.multiply(w.getX());

  303.         // compute true longitude argument
  304.         cachedPositionAngleType = PositionAngleType.TRUE;
  305.         final T cLv = (pvP.getX().subtract(d.multiply(pvP.getZ()).multiply(w.getX()))).divide(r);
  306.         final T sLv = (pvP.getY().subtract(d.multiply(pvP.getZ()).multiply(w.getY()))).divide(r);
  307.         cachedL = sLv.atan2(cLv);

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

  318.         partialPV = pvCoordinates;

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

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

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

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

  347.         } else {
  348.             // acceleration is either almost zero or NaN,
  349.             // we assume acceleration was not known
  350.             // we don't set up derivatives
  351.             aDot  = getZero();
  352.             exDot = getZero();
  353.             eyDot = getZero();
  354.             hxDot = getZero();
  355.             hyDot = getZero();
  356.             cachedLDot = computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, mu, cachedL, cachedPositionAngleType);
  357.         }

  359.     }

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

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

  388.         a     = op.getA();
  389.         ex    = op.getEquinoctialEx();
  390.         ey    = op.getEquinoctialEy();
  391.         hx    = op.getHx();
  392.         hy    = op.getHy();
  393.         cachedPositionAngleType = PositionAngleType.TRUE;
  394.         cachedL    = op.getLv();

  396.         aDot  = op.getADot();
  397.         exDot = op.getEquinoctialExDot();
  398.         eyDot = op.getEquinoctialEyDot();
  399.         hxDot = op.getHxDot();
  400.         hyDot = op.getHyDot();
  401.         cachedLDot = op.getLvDot();
  402.     }

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

  413.         a     = getZero().newInstance(op.getA());
  414.         ex    = getZero().newInstance(op.getEquinoctialEx());
  415.         ey    = getZero().newInstance(op.getEquinoctialEy());
  416.         hx    = getZero().newInstance(op.getHx());
  417.         hy    = getZero().newInstance(op.getHy());
  418.         cachedPositionAngleType = op.getCachedPositionAngleType();
  419.         cachedL    = getZero().newInstance(op.getL(cachedPositionAngleType));

  421.         aDot  = getZero().newInstance(op.getADot());
  422.         exDot = getZero().newInstance(op.getEquinoctialExDot());
  423.         eyDot = getZero().newInstance(op.getEquinoctialEyDot());
  424.         hxDot = getZero().newInstance(op.getHxDot());
  425.         hyDot = getZero().newInstance(op.getHyDot());
  426.         cachedLDot = getZero().newInstance(op.getLDot(cachedPositionAngleType));
  427.     }

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

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

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

  451.     /** {@inheritDoc} */
  452.     @Override
  453.     public T getADot() {
  454.         return aDot;
  455.     }

  457.     /** {@inheritDoc} */
  458.     @Override
  459.     public T getEquinoctialEx() {
  460.         return ex;
  461.     }

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

  469.     /** {@inheritDoc} */
  470.     @Override
  471.     public T getEquinoctialEy() {
  472.         return ey;
  473.     }

  475.     /** {@inheritDoc} */
  476.     @Override
  477.     public T getEquinoctialEyDot() {
  478.         return eyDot;
  479.     }

  481.     /** {@inheritDoc} */
  482.     @Override
  483.     public T getHx() {
  484.         return hx;
  485.     }

  487.     /** {@inheritDoc} */
  488.     @Override
  489.     public T getHxDot() {
  490.         return hxDot;
  491.     }

  493.     /** {@inheritDoc} */
  494.     @Override
  495.     public T getHy() {
  496.         return hy;
  497.     }

  499.     /** {@inheritDoc} */
  500.     @Override
  501.     public T getHyDot() {
  502.         return hyDot;
  503.     }

  505.     /** {@inheritDoc} */
  506.     @Override
  507.     public T getLv() {
  508.         switch (cachedPositionAngleType) {
  509.             case TRUE:
  510.                 return cachedL;

  512.             case ECCENTRIC:
  513.                 return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(ex, ey, cachedL);

  515.             case MEAN:
  516.                 return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(ex, ey, cachedL);

  518.             default:
  519.                 throw new OrekitInternalError(null);
  520.         }
  521.     }

  523.     /** {@inheritDoc} */
  524.     @Override
  525.     public T getLvDot() {
  526.         switch (cachedPositionAngleType) {
  527.             case ECCENTRIC:
  528.                 final FieldUnivariateDerivative1<T> lEUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  529.                 final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex,     exDot);
  530.                 final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey,     eyDot);
  531.                 final FieldUnivariateDerivative1<T> lvUD = FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
  532.                         lEUD);
  533.                 return lvUD.getFirstDerivative();

  535.             case TRUE:
  536.                 return cachedLDot;

  538.             case MEAN:
  539.                 final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  540.                 final FieldUnivariateDerivative1<T> exUD2    = new FieldUnivariateDerivative1<>(ex,     exDot);
  541.                 final FieldUnivariateDerivative1<T> eyUD2    = new FieldUnivariateDerivative1<>(ey,     eyDot);
  542.                 final FieldUnivariateDerivative1<T> lvUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD2,
  543.                         eyUD2, lMUD);
  544.                 return lvUD2.getFirstDerivative();

  546.             default:
  547.                 throw new OrekitInternalError(null);
  548.         }
  549.     }

  551.     /** {@inheritDoc} */
  552.     @Override
  553.     public T getLE() {
  554.         switch (cachedPositionAngleType) {
  555.             case TRUE:
  556.                 return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(ex, ey, cachedL);

  558.             case ECCENTRIC:
  559.                 return cachedL;

  561.             case MEAN:
  562.                 return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(ex, ey, cachedL);

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

  569.     /** {@inheritDoc} */
  570.     @Override
  571.     public T getLEDot() {

  573.         switch (cachedPositionAngleType) {
  574.             case TRUE:
  575.                 final FieldUnivariateDerivative1<T> lvUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  576.                 final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex,     exDot);
  577.                 final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey,     eyDot);
  578.                 final FieldUnivariateDerivative1<T> lEUD = FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD,
  579.                         lvUD);
  580.                 return lEUD.getFirstDerivative();

  582.             case ECCENTRIC:
  583.                 return cachedLDot;

  585.             case MEAN:
  586.                 final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  587.                 final FieldUnivariateDerivative1<T> exUD2    = new FieldUnivariateDerivative1<>(ex,     exDot);
  588.                 final FieldUnivariateDerivative1<T> eyUD2    = new FieldUnivariateDerivative1<>(ey,     eyDot);
  589.                 final FieldUnivariateDerivative1<T> lEUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD2,
  590.                         eyUD2, lMUD);
  591.                 return lEUD2.getFirstDerivative();

  593.             default:
  594.                 throw new OrekitInternalError(null);
  595.         }
  596.     }

  598.     /** {@inheritDoc} */
  599.     @Override
  600.     public T getLM() {
  601.         switch (cachedPositionAngleType) {
  602.             case TRUE:
  603.                 return FieldEquinoctialLongitudeArgumentUtility.trueToMean(ex, ey, cachedL);

  605.             case MEAN:
  606.                 return cachedL;

  608.             case ECCENTRIC:
  609.                 return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(ex, ey, cachedL);

  611.             default:
  612.                 throw new OrekitInternalError(null);
  613.         }
  614.     }

  616.     /** {@inheritDoc} */
  617.     @Override
  618.     public T getLMDot() {

  620.         switch (cachedPositionAngleType) {
  621.             case TRUE:
  622.                 final FieldUnivariateDerivative1<T> lvUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  623.                 final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex,     exDot);
  624.                 final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey,     eyDot);
  625.                 final FieldUnivariateDerivative1<T> lMUD = FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lvUD);
  626.                 return lMUD.getFirstDerivative();

  628.             case MEAN:
  629.                 return cachedLDot;

  631.             case ECCENTRIC:
  632.                 final FieldUnivariateDerivative1<T> lEUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
  633.                 final FieldUnivariateDerivative1<T> exUD2    = new FieldUnivariateDerivative1<>(ex,     exDot);
  634.                 final FieldUnivariateDerivative1<T> eyUD2    = new FieldUnivariateDerivative1<>(ey,     eyDot);
  635.                 final FieldUnivariateDerivative1<T> lMUD2 = FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD2,
  636.                         eyUD2, lEUD);
  637.                 return lMUD2.getFirstDerivative();

  639.             default:
  640.                 throw new OrekitInternalError(null);
  641.         }
  642.     }

  644.     /** Get the longitude argument.
  645.      * @param type type of the angle
  646.      * @return longitude argument (rad)
  647.      */
  648.     public T getL(final PositionAngleType type) {
  649.         return (type == PositionAngleType.MEAN) ? getLM() :
  650.                                               ((type == PositionAngleType.ECCENTRIC) ? getLE() :
  651.                                                                                    getLv());
  652.     }

  654.     /** Get the longitude argument derivative.
  655.      * @param type type of the angle
  656.      * @return longitude argument derivative (rad/s)
  657.      */
  658.     public T getLDot(final PositionAngleType type) {
  659.         return (type == PositionAngleType.MEAN) ? getLMDot() :
  660.                                               ((type == PositionAngleType.ECCENTRIC) ? getLEDot() :
  661.                                                                                    getLvDot());
  662.     }

  664.     /** {@inheritDoc} */
  665.     @Override
  666.     public boolean hasNonKeplerianAcceleration() {
  667.         return aDot.getReal() != 0. || exDot.getReal() != 0 || hxDot.getReal() != 0. || eyDot.getReal() != 0. || hyDot.getReal() != 0. ||
  668.                 FastMath.abs(cachedLDot.subtract(computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, getMu(), cachedL, cachedPositionAngleType)).getReal()) > TOLERANCE_POSITION_ANGLE_RATE;
  669.     }

  671.     /** {@inheritDoc} */
  672.     @Override
  673.     public T getE() {
  674.         return ex.square().add(ey.square()).sqrt();
  675.     }

  677.     /** {@inheritDoc} */
  678.     @Override
  679.     public T getEDot() {
  680.         if (!hasNonKeplerianRates()) {
  681.             return getZero();
  682.         }
  683.         return ex.multiply(exDot).add(ey.multiply(eyDot)).divide(ex.square().add(ey.square()).sqrt());

  685.     }

  687.     /** {@inheritDoc} */
  688.     @Override
  689.     public T getI() {
  690.         return hx.square().add(hy.square()).sqrt().atan().multiply(2);
  691.     }

  693.     /** {@inheritDoc} */
  694.     @Override
  695.     public T getIDot() {
  696.         if (!hasNonKeplerianRates()) {
  697.             return getZero();
  698.         }
  699.         final T h2 = hx.square().add(hy.square());
  700.         final T h  = h2.sqrt();
  701.         return hx.multiply(hxDot).add(hy.multiply(hyDot)).multiply(2).divide(h.multiply(h2.add(1)));

  703.     }

  705.     /** Compute position and velocity but not acceleration.
  706.      */
  707.     private void computePVWithoutA() {

  709.         if (partialPV != null) {
  710.             // already computed
  711.             return;
  712.         }

  714.         // get equinoctial parameters
  715.         final T lE = getLE();

  717.         // inclination-related intermediate parameters
  718.         final T hx2   = hx.square();
  719.         final T hy2   = hy.square();
  720.         final T factH = getOne().divide(hx2.add(1.0).add(hy2));

  722.         // reference axes defining the orbital plane
  723.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  724.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  725.         final T uz = hy.multiply(-2).multiply(factH);

  727.         final T vx = uy;
  728.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  729.         final T vz =  hx.multiply(factH).multiply(2);

  731.         // eccentricity-related intermediate parameters
  732.         final T ex2  = ex.square();
  733.         final T exey = ex.multiply(ey);
  734.         final T ey2  = ey.square();
  735.         final T e2   = ex2.add(ey2);
  736.         final T eta  = getOne().subtract(e2).sqrt().add(1);
  737.         final T beta = getOne().divide(eta);

  739.         // eccentric longitude argument
  740.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  741.         final T cLe    = scLe.cos();
  742.         final T sLe    = scLe.sin();
  743.         final T exCeyS = ex.multiply(cLe).add(ey.multiply(sLe));

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

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

  753.         final FieldVector3D<T> position =
  754.                         new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  755.                                             x.multiply(uy).add(y.multiply(vy)),
  756.                                             x.multiply(uz).add(y.multiply(vz)));
  757.         final FieldVector3D<T> velocity =
  758.                         new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)), xdot.multiply(uy).add(ydot.multiply(vy)), xdot.multiply(uz).add(ydot.multiply(vz)));

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

  762.     }

  764.     /** Initialize cached argument of longitude with rate.
  765.      * @param l input argument of longitude
  766.      * @param lDot rate of input argument of longitude
  767.      * @param inputType position angle type passed as input
  768.      * @return argument of longitude to cache with rate
  769.      * @since 12.1
  770.      */
  771.     private FieldUnivariateDerivative1<T> initializeCachedL(final T l, final T lDot,
  772.                                                             final PositionAngleType inputType) {
  773.         if (cachedPositionAngleType == inputType) {
  774.             return new FieldUnivariateDerivative1<>(l, lDot);

  776.         } else {
  777.             final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  778.             final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  779.             final FieldUnivariateDerivative1<T> lUD = new FieldUnivariateDerivative1<>(l, lDot);

  781.             switch (cachedPositionAngleType) {

  783.                 case ECCENTRIC:
  784.                     if (inputType == PositionAngleType.MEAN) {
  785.                         return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD, eyUD, lUD);
  786.                     } else {
  787.                         return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD, lUD);
  788.                     }

  790.                 case TRUE:
  791.                     if (inputType == PositionAngleType.MEAN) {
  792.                         return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lUD);
  793.                     } else {
  794.                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD, lUD);
  795.                     }

  797.                 case MEAN:
  798.                     if (inputType == PositionAngleType.TRUE) {
  799.                         return FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lUD);
  800.                     } else {
  801.                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD, eyUD, lUD);
  802.                     }

  804.                 default:
  805.                     throw new OrekitInternalError(null);

  807.             }

  809.         }

  811.     }

  813.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  814.      * @return non-Keplerian part of the acceleration
  815.      */
  816.     private FieldVector3D<T> nonKeplerianAcceleration() {

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

  821.         final T nonKeplerianMeanMotion = getLMDot().subtract(getKeplerianMeanMotion());
  822.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  823.                                  add(dCdP[3][1].multiply(exDot)).
  824.                                  add(dCdP[3][2].multiply(eyDot)).
  825.                                  add(dCdP[3][3].multiply(hxDot)).
  826.                                  add(dCdP[3][4].multiply(hyDot)).
  827.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  828.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  829.                                  add(dCdP[4][1].multiply(exDot)).
  830.                                  add(dCdP[4][2].multiply(eyDot)).
  831.                                  add(dCdP[4][3].multiply(hxDot)).
  832.                                  add(dCdP[4][4].multiply(hyDot)).
  833.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  834.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  835.                                  add(dCdP[5][1].multiply(exDot)).
  836.                                  add(dCdP[5][2].multiply(eyDot)).
  837.                                  add(dCdP[5][3].multiply(hxDot)).
  838.                                  add(dCdP[5][4].multiply(hyDot)).
  839.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  843.     }

  845.     /** {@inheritDoc} */
  846.     @Override
  847.     protected FieldVector3D<T> initPosition() {

  849.         // get equinoctial parameters
  850.         final T lE = getLE();

  852.         // inclination-related intermediate parameters
  853.         final T hx2   = hx.square();
  854.         final T hy2   = hy.square();
  855.         final T factH = getOne().divide(hx2.add(1.0).add(hy2));

  857.         // reference axes defining the orbital plane
  858.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  859.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  860.         final T uz = hy.multiply(-2).multiply(factH);

  862.         final T vx = uy;
  863.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  864.         final T vz =  hx.multiply(factH).multiply(2);

  866.         // eccentricity-related intermediate parameters
  867.         final T ex2  = ex.square();
  868.         final T exey = ex.multiply(ey);
  869.         final T ey2  = ey.square();
  870.         final T e2   = ex2.add(ey2);
  871.         final T eta  = getOne().subtract(e2).sqrt().add(1);
  872.         final T beta = getOne().divide(eta);

  874.         // eccentric longitude argument
  875.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  876.         final T cLe    = scLe.cos();
  877.         final T sLe    = scLe.sin();

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

  883.         return new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  884.                                    x.multiply(uy).add(y.multiply(vy)),
  885.                                    x.multiply(uz).add(y.multiply(vz)));

  887.     }

  889.     /** {@inheritDoc} */
  890.     @Override
  891.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  893.         // position and velocity
  894.         computePVWithoutA();

  896.         // acceleration
  897.         final T r2 = partialPV.getPosition().getNormSq();
  898.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(getMu().negate()),
  899.                                                                            partialPV.getPosition());
  900.         final FieldVector3D<T> acceleration = hasNonKeplerianRates() ?
  901.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  902.                                               keplerianAcceleration;

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

  906.     }

  908.     /** {@inheritDoc} */
  909.     @Override
  910.     public FieldEquinoctialOrbit<T> inFrame(final Frame inertialFrame) {
  911.         final FieldPVCoordinates<T> fieldPVCoordinates;
  912.         if (hasNonKeplerianAcceleration()) {
  913.             fieldPVCoordinates = getPVCoordinates(inertialFrame);
  914.         } else {
  915.             final FieldKinematicTransform<T> transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
  916.             fieldPVCoordinates = transform.transformOnlyPV(getPVCoordinates());
  917.         }
  918.         final FieldEquinoctialOrbit<T> fieldOrbit = new FieldEquinoctialOrbit<>(fieldPVCoordinates, inertialFrame, getDate(), getMu());
  919.         if (fieldOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
  920.             return fieldOrbit;
  921.         } else {
  922.             return fieldOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
  923.         }
  924.     }

  926.     /** {@inheritDoc} */
  927.     @Override
  928.     public FieldEquinoctialOrbit<T> withCachedPositionAngleType(final PositionAngleType positionAngleType) {
  929.         return new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy, getL(positionAngleType), aDot, exDot, eyDot, hxDot, hyDot,
  930.                 getLDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
  931.     }

  933.     /** {@inheritDoc} */
  934.     @Override
  935.     public FieldEquinoctialOrbit<T> shiftedBy(final double dt) {
  936.         return shiftedBy(getZero().newInstance(dt));
  937.     }

  939.     /** {@inheritDoc} */
  940.     @Override
  941.     public FieldEquinoctialOrbit<T> shiftedBy(final T dt) {

  943.         // use Keplerian-only motion
  944.         final FieldEquinoctialOrbit<T> keplerianShifted = new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy,
  945.                                                                                       getLM().add(getKeplerianMeanMotion().multiply(dt)),
  946.                                                                                       PositionAngleType.MEAN, cachedPositionAngleType, getFrame(),
  947.                                                                                       getDate().shiftedBy(dt), getMu());

  949.         if (hasNonKeplerianRates()) {

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

  954.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  955.             keplerianShifted.computePVWithoutA();
  956.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getPosition(),
  957.                                                                   dt.square().multiply(0.5), nonKeplerianAcceleration);
  958.             final T   fixedR2 = fixedP.getNormSq();
  959.             final T   fixedR  = fixedR2.sqrt();
  960.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getVelocity(),
  961.                                                                  dt, nonKeplerianAcceleration);
  962.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  963.                                                                  keplerianShifted.partialPV.getPosition(),
  964.                                                                  getOne(), nonKeplerianAcceleration);

  966.             // build a new orbit, taking non-Keplerian acceleration into account
  967.             return new FieldEquinoctialOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  968.                                                                                    fixedP, fixedV, fixedA),
  969.                                                keplerianShifted.getFrame(), keplerianShifted.getMu());

  971.         } else {
  972.             // Keplerian-only motion is all we can do
  973.             return keplerianShifted;
  974.         }

  976.     }

  978.     /** {@inheritDoc} */
  979.     @Override
  980.     protected T[][] computeJacobianMeanWrtCartesian() {

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

  984.         // compute various intermediate parameters
  985.         computePVWithoutA();
  986.         final FieldVector3D<T> position = partialPV.getPosition();
  987.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  988.         final T r2         = position.getNormSq();
  989.         final T r          = r2.sqrt();
  990.         final T r3         = r.multiply(r2);

  992.         final T mu         = getMu();
  993.         final T sqrtMuA    = a.multiply(mu).sqrt();
  994.         final T a2         = a.square();

  996.         final T e2         = ex.square().add(ey.square());
  997.         final T oMe2       = getOne().subtract(e2);
  998.         final T epsilon    = oMe2.sqrt();
  999.         final T beta       = getOne().divide(epsilon.add(1));
  1000.         final T ratio      = epsilon.multiply(beta);

  1002.         final T hx2        = hx.square();
  1003.         final T hy2        = hy.square();
  1004.         final T hxhy       = hx.multiply(hy);

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

  1011.         // coordinates of the spacecraft in the equinoctial frame
  1012.         final T x    = FieldVector3D.dotProduct(position, f);
  1013.         final T y    = FieldVector3D.dotProduct(position, g);
  1014.         final T xDot = FieldVector3D.dotProduct(velocity, f);
  1015.         final T yDot = FieldVector3D.dotProduct(velocity, g);

  1017.         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
  1018.         final T c1  = a.divide(sqrtMuA.multiply(epsilon));
  1019.         final T c1N = c1.negate();
  1020.         final T c2  = a.multiply(sqrtMuA).multiply(beta).divide(r3);
  1021.         final T c3  = sqrtMuA.divide(r3.multiply(epsilon));
  1022.         final FieldVector3D<T> drDotSdEx = new FieldVector3D<>(c1.multiply(xDot).multiply(yDot).subtract(c2.multiply(ey).multiply(x)).subtract(c3.multiply(x).multiply(y)), f,
  1023.                                                                c1N.multiply(xDot).multiply(xDot).subtract(c2.multiply(ey).multiply(y)).add(c3.multiply(x).multiply(x)), g);

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

  1029.         // da
  1030.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  1031.         final FieldVector3D<T> vectorARDot = new FieldVector3D<>(a2.multiply(2).divide(mu), velocity);
  1032.         fillHalfRow(getOne(), vectorAR,    jacobian[0], 0);
  1033.         fillHalfRow(getOne(), vectorARDot, jacobian[0], 3);

  1035.         // dEx
  1036.         final T d1 = a.negate().multiply(ratio).divide(r3);
  1037.         final T d2 = (hy.multiply(xDot).subtract(hx.multiply(yDot))).divide(sqrtMuA.multiply(epsilon));
  1038.         final T d3 = hx.multiply(y).subtract(hy.multiply(x)).divide(sqrtMuA);
  1039.         final FieldVector3D<T> vectorExRDot =
  1040.             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);
  1041.         fillHalfRow(ex.multiply(d1), position, ey.negate().multiply(d2), w, epsilon.divide(sqrtMuA), drDotSdEy, jacobian[1], 0);
  1042.         fillHalfRow(getOne(), vectorExRDot, jacobian[1], 3);

  1044.         // dEy
  1045.         final FieldVector3D<T> vectorEyRDot =
  1046.             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);
  1047.         fillHalfRow(ey.multiply(d1), position, ex.multiply(d2), w, epsilon.negate().divide(sqrtMuA), drDotSdEx, jacobian[2], 0);
  1048.         fillHalfRow(getOne(), vectorEyRDot, jacobian[2], 3);

  1050.         // dHx
  1051.         final T h = (hx2.add(1).add(hy2)).divide(sqrtMuA.multiply(2).multiply(epsilon));
  1052.         fillHalfRow( h.negate().multiply(xDot), w, jacobian[3], 0);
  1053.         fillHalfRow( h.multiply(x),    w, jacobian[3], 3);

  1055.        // dHy
  1056.         fillHalfRow( h.negate().multiply(yDot), w, jacobian[4], 0);
  1057.         fillHalfRow( h.multiply(y),    w, jacobian[4], 3);

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

  1064.         return jacobian;

  1066.     }

  1068.     /** {@inheritDoc} */
  1069.     @Override
  1070.     protected T[][] computeJacobianEccentricWrtCartesian() {

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

  1075.         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
  1076.         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
  1077.         // which is inverted and rewritten as:
  1078.         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
  1079.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  1080.         final T cosLe = scLe.cos();
  1081.         final T sinLe = scLe.sin();
  1082.         final T aOr   = getOne().divide(getOne().subtract(ex.multiply(cosLe)).subtract(ey.multiply(sinLe)));

  1084.         // update longitude row
  1085.         final T[] rowEx = jacobian[1];
  1086.         final T[] rowEy = jacobian[2];
  1087.         final T[] rowL  = jacobian[5];

  1089.         for (int j = 0; j < 6; ++j) {
  1090.             rowL[j] = aOr.multiply(rowL[j].add(sinLe.multiply(rowEx[j])).subtract(cosLe.multiply(rowEy[j])));
  1091.         }
  1092.         return jacobian;

  1094.     }

  1096.     /** {@inheritDoc} */
  1097.     @Override
  1098.     protected T[][] computeJacobianTrueWrtCartesian() {

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

  1103.         // Differentiating the eccentric longitude equation
  1104.         // tan((lV - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
  1105.         // leads to
  1106.         // cT (dlV - dlE) = cE dlE + cX dex + cY dey
  1107.         // with
  1108.         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
  1109.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
  1110.         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  1111.         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1112.         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1113.         // which can be solved to find the differential of the true longitude
  1114.         // dlV = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
  1115.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  1116.         final T cosLe     = scLe.cos();
  1117.         final T sinLe     = scLe.sin();
  1118.         final T eSinE     = ex.multiply(sinLe).subtract(ey.multiply(cosLe));
  1119.         final T ecosE     = ex.multiply(cosLe).add(ey.multiply(sinLe));
  1120.         final T e2        = ex.square().add(ey.square());
  1121.         final T epsilon   = getOne().subtract(e2).sqrt();
  1122.         final T onePeps   = epsilon.add(1);
  1123.         final T d         = onePeps.subtract(ecosE);
  1124.         final T cT        = d.multiply(d).add(eSinE.multiply(eSinE)).divide(2);
  1125.         final T cE        = ecosE.multiply(onePeps).subtract(e2);
  1126.         final T cX        = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinLe.multiply(onePeps));
  1127.         final T cY        = ey.multiply(eSinE).divide(epsilon).add( ex).subtract(cosLe.multiply(onePeps));
  1128.         final T factorLe  = cT.add(cE).divide(cT);
  1129.         final T factorEx  = cX.divide(cT);
  1130.         final T factorEy  = cY.divide(cT);

  1132.         // update longitude row
  1133.         final T[] rowEx = jacobian[1];
  1134.         final T[] rowEy = jacobian[2];
  1135.         final T[] rowL = jacobian[5];
  1136.         for (int j = 0; j < 6; ++j) {
  1137.             rowL[j] = factorLe.multiply(rowL[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
  1138.         }

  1140.         return jacobian;

  1142.     }

  1144.     /** {@inheritDoc} */
  1145.     @Override
  1146.     public void addKeplerContribution(final PositionAngleType type, final T gm,
  1147.                                       final T[] pDot) {
  1148.         pDot[5] = pDot[5].add(computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType));
  1149.     }

  1151.     /**
  1152.      * Compute rate of argument of longitude.
  1153.      * @param type position angle type of rate
  1154.      * @param a semi major axis
  1155.      * @param ex ex
  1156.      * @param ey ey
  1157.      * @param mu mu
  1158.      * @param l argument of longitude
  1159.      * @param cachedType position angle type of passed l
  1160.      * @param <T> field type
  1161.      * @return first-order time derivative for l
  1162.      * @since 12.2
  1163.      */
  1164.     private static <T extends CalculusFieldElement<T>> T computeKeplerianLDot(final PositionAngleType type, final T a, final T ex,
  1165.                                                                               final T ey, final T mu, final T l, final PositionAngleType cachedType) {
  1166.         final T n               = mu.divide(a).sqrt().divide(a);
  1167.         if (type == PositionAngleType.MEAN) {
  1168.             return n;
  1169.         }
  1170.         final FieldSinCos<T> sc;
  1171.         final T ksi;
  1172.         if (type == PositionAngleType.ECCENTRIC) {
  1173.             sc = FastMath.sinCos(FieldEquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
  1174.             ksi = ((ex.multiply(sc.cos())).add(ey.multiply(sc.sin()))).negate().add(1).reciprocal();
  1175.             return n.multiply(ksi);
  1176.         } else {
  1177.             sc = FastMath.sinCos(FieldEquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
  1178.             final T oMe2 = a.getField().getOne().subtract(ex.square()).subtract(ey.square());
  1179.             ksi  =  ex.multiply(sc.cos()).add(1).add(ey.multiply(sc.sin()));
  1180.             return n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt()));
  1181.         }
  1182.     }

  1184.     /**  Returns a string representation of this equinoctial parameters object.
  1185.      * @return a string representation of this object
  1186.      */
  1187.     public String toString() {
  1188.         return new StringBuilder().append("equinoctial parameters: ").append('{').
  1189.                                   append("a: ").append(a.getReal()).
  1190.                                   append("; ex: ").append(ex.getReal()).append("; ey: ").append(ey.getReal()).
  1191.                                   append("; hx: ").append(hx.getReal()).append("; hy: ").append(hy.getReal()).
  1192.                                   append("; lv: ").append(FastMath.toDegrees(getLv().getReal())).
  1193.                                   append(";}").toString();
  1194.     }

  1196.     /** {@inheritDoc} */
  1197.     @Override
  1198.     public PositionAngleType getCachedPositionAngleType() {
  1199.         return cachedPositionAngleType;
  1200.     }

  1202.     /** {@inheritDoc} */
  1203.     @Override
  1204.     public boolean hasNonKeplerianRates() {
  1205.         return hasNonKeplerianAcceleration();
  1206.     }

  1208.     /** {@inheritDoc} */
  1209.     @Override
  1210.     public FieldEquinoctialOrbit<T> withKeplerianRates() {
  1211.         return new FieldEquinoctialOrbit<>(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
  1212.                 cachedL, cachedPositionAngleType, getFrame(), getDate(), getMu());
  1213.     }

  1215.     /** {@inheritDoc} */
  1216.     @Override
  1217.     public EquinoctialOrbit toOrbit() {
  1218.         final double cachedPositionAngle = cachedL.getReal();
  1219.         if (hasNonKeplerianRates()) {
  1220.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1221.                                         hx.getReal(), hy.getReal(), cachedPositionAngle,
  1222.                                         aDot.getReal(), exDot.getReal(), eyDot.getReal(),
  1223.                                         hxDot.getReal(), hyDot.getReal(), cachedLDot.getReal(),
  1224.                                         cachedPositionAngleType, getFrame(),
  1225.                                         getDate().toAbsoluteDate(), getMu().getReal());
  1226.         } else {
  1227.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1228.                                         hx.getReal(), hy.getReal(), cachedPositionAngle,
  1229.                                         cachedPositionAngleType, getFrame(),
  1230.                                         getDate().toAbsoluteDate(), getMu().getReal());
  1231.         }
  1232.     }

  1234. }
Created with JaCoCo 0.8.12.202403310830