FieldEquinoctialOrbit.java

  1. /* Copyright 2002-2023 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.     /** True longitude argument (rad). */
  94.     private final T lv;

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

  99.     /** First component of the eccentricity vector derivative. */
  100.     private final T exDot;

  102.     /** Second component of the eccentricity vector derivative. */
  103.     private final T eyDot;

  105.     /** First component of the inclination vector derivative. */
  106.     private final T hxDot;

  108.     /** Second component of the inclination vector derivative. */
  109.     private final T hyDot;

  111.     /** True longitude argument derivative (rad/s). */
  112.     private final T lvDot;

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

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

  142.     /** Creates a new instance.
  143.      * @param a  semi-major axis (m)
  144.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  145.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  146.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  147.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  148.      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
  149.      * @param aDot  semi-major axis derivative (m/s)
  150.      * @param exDot d(e cos(ω + Ω))/dt, first component of eccentricity vector derivative
  151.      * @param eyDot d(e sin(ω + Ω))/dt, second component of eccentricity vector derivative
  152.      * @param hxDot d(tan(i/2) cos(Ω))/dt, first component of inclination vector derivative
  153.      * @param hyDot d(tan(i/2) sin(Ω))/dt, second component of inclination vector derivative
  154.      * @param lDot  d(M or E or v) + ω + Ω)/dr, mean, eccentric or true longitude argument  derivative (rad/s)
  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 T aDot, final T exDot, final T eyDot,
  166.                                  final T hxDot, final T hyDot, final T lDot,
  167.                                  final PositionAngleType type,
  168.                                  final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  169.         throws IllegalArgumentException {
  170.         super(frame, date, mu);

  172.         if (ex.getReal() * ex.getReal() + ey.getReal() * ey.getReal() >= 1.0) {
  173.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  174.                                                      getClass().getName());
  175.         }

  177.         this.a     = a;
  178.         this.aDot  = aDot;
  179.         this.ex    = ex;
  180.         this.exDot = exDot;
  181.         this.ey    = ey;
  182.         this.eyDot = eyDot;
  183.         this.hx    = hx;
  184.         this.hxDot = hxDot;
  185.         this.hy    = hy;
  186.         this.hyDot = hyDot;

  188.         if (hasDerivatives()) {
  189.             final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  190.             final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  191.             final FieldUnivariateDerivative1<T> lUD  = new FieldUnivariateDerivative1<>(l,  lDot);
  192.             final FieldUnivariateDerivative1<T> lvUD;
  193.             switch (type) {
  194.                 case MEAN :
  195.                     lvUD = eccentricToTrue(meanToEccentric(lUD, exUD, eyUD), exUD, eyUD);
  196.                     break;
  197.                 case ECCENTRIC :
  198.                     lvUD = eccentricToTrue(lUD, exUD, eyUD);
  199.                     break;
  200.                 case TRUE :
  201.                     lvUD = lUD;
  202.                     break;
  203.                 default : // this should never happen
  204.                     throw new OrekitInternalError(null);
  205.             }
  206.             this.lv    = lvUD.getValue();
  207.             this.lvDot = lvUD.getDerivative(1);
  208.         } else {
  209.             switch (type) {
  210.                 case MEAN :
  211.                     this.lv = eccentricToTrue(meanToEccentric(l, ex, ey), ex, ey);
  212.                     break;
  213.                 case ECCENTRIC :
  214.                     this.lv = eccentricToTrue(l, ex, ey);
  215.                     break;
  216.                 case TRUE :
  217.                     this.lv = l;
  218.                     break;
  219.                 default :
  220.                     throw new OrekitInternalError(null);
  221.             }
  222.             this.lvDot = null;
  223.         }

  225.         this.partialPV = null;

  227.     }

  229.     /** Constructor from Cartesian parameters.
  230.      *
  231.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  232.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  233.      * use {@code mu} and the position to compute the acceleration, including
  234.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  235.      *
  236.      * @param pvCoordinates the position, velocity and acceleration
  237.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  238.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  239.      * @param mu central attraction coefficient (m³/s²)
  240.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  241.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  242.      */
  243.     public FieldEquinoctialOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  244.                                  final Frame frame, final T mu)
  245.         throws IllegalArgumentException {
  246.         super(pvCoordinates, frame, mu);

  248.         //  compute semi-major axis
  249.         final FieldVector3D<T> pvP = pvCoordinates.getPosition();
  250.         final FieldVector3D<T> pvV = pvCoordinates.getVelocity();
  251.         final FieldVector3D<T> pvA = pvCoordinates.getAcceleration();
  252.         final T r2 = pvP.getNormSq();
  253.         final T r  = r2.sqrt();
  254.         final T V2 = pvV.getNormSq();
  255.         final T rV2OnMu = r.multiply(V2).divide(mu);

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

  260.         if (!isElliptical()) {
  261.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  262.                                                      getClass().getName());
  263.         }

  265.         // compute inclination vector
  266.         final FieldVector3D<T> w = pvCoordinates.getMomentum().normalize();
  267.         final T d = getOne().divide(getOne().add(w.getZ()));
  268.         hx =  d.negate().multiply(w.getY());
  269.         hy =  d.multiply(w.getX());

  271.         // compute true longitude argument
  272.         final T cLv = (pvP.getX().subtract(d.multiply(pvP.getZ()).multiply(w.getX()))).divide(r);
  273.         final T sLv = (pvP.getY().subtract(d.multiply(pvP.getZ()).multiply(w.getY()))).divide(r);
  274.         lv = sLv.atan2(cLv);

  276.         // compute eccentricity vector
  277.         final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(mu).sqrt());
  278.         final T eCE = rV2OnMu.subtract(1);
  279.         final T e2  = eCE.multiply(eCE).add(eSE.multiply(eSE));
  280.         final T f   = eCE.subtract(e2);
  281.         final T g   = e2.negate().add(1).sqrt().multiply(eSE);
  282.         ex = a.multiply(f.multiply(cLv).add( g.multiply(sLv))).divide(r);
  283.         ey = a.multiply(f.multiply(sLv).subtract(g.multiply(cLv))).divide(r);

  285.         partialPV = pvCoordinates;

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

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

  293.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  294.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  295.             final T   aX                       = nonKeplerianAcceleration.getX();
  296.             final T   aY                       = nonKeplerianAcceleration.getY();
  297.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  298.             aDot  = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  299.             exDot = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  300.             eyDot = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  301.             hxDot = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  302.             hyDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  304.             // in order to compute true anomaly derivative, we must compute
  305.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  306.             final T lMDot = getKeplerianMeanMotion().
  307.                             add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  308.             final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  309.             final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  310.             final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(getLM(), lMDot);
  311.             final FieldUnivariateDerivative1<T> lvUD = eccentricToTrue(meanToEccentric(lMUD, exUD, eyUD), exUD, eyUD);
  312.             lvDot = lvUD.getDerivative(1);

  314.         } else {
  315.             // acceleration is either almost zero or NaN,
  316.             // we assume acceleration was not known
  317.             // we don't set up derivatives
  318.             aDot  = null;
  319.             exDot = null;
  320.             eyDot = null;
  321.             hxDot = null;
  322.             hyDot = null;
  323.             lvDot = null;
  324.         }

  326.     }

  328.     /** Constructor from Cartesian parameters.
  329.      *
  330.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  331.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  332.      * use {@code mu} and the position to compute the acceleration, including
  333.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  334.      *
  335.      * @param pvCoordinates the position end velocity
  336.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  337.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  338.      * @param date date of the orbital parameters
  339.      * @param mu central attraction coefficient (m³/s²)
  340.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  341.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  342.      */
  343.     public FieldEquinoctialOrbit(final FieldPVCoordinates<T> pvCoordinates, final Frame frame,
  344.                             final FieldAbsoluteDate<T> date, final T mu)
  345.         throws IllegalArgumentException {
  346.         this(new TimeStampedFieldPVCoordinates<>(date, pvCoordinates), frame, mu);
  347.     }

  349.     /** Constructor from any kind of orbital parameters.
  350.      * @param op orbital parameters to copy
  351.      */
  352.     public FieldEquinoctialOrbit(final FieldOrbit<T> op) {
  353.         super(op.getFrame(), op.getDate(), op.getMu());

  355.         a     = op.getA();
  356.         ex    = op.getEquinoctialEx();
  357.         ey    = op.getEquinoctialEy();
  358.         hx    = op.getHx();
  359.         hy    = op.getHy();
  360.         lv    = op.getLv();

  362.         aDot  = op.getADot();
  363.         exDot = op.getEquinoctialExDot();
  364.         eyDot = op.getEquinoctialEyDot();
  365.         hxDot = op.getHxDot();
  366.         hyDot = op.getHyDot();
  367.         lvDot = op.getLvDot();
  368.     }

  370.     /** Constructor from Field and EquinoctialOrbit.
  371.      * <p>Build a FieldEquinoctialOrbit from non-Field EquinoctialOrbit.</p>
  372.      * @param field CalculusField to base object on
  373.      * @param op non-field orbit with only "constant" terms
  374.      * @since 12.0
  375.      */
  376.     public FieldEquinoctialOrbit(final Field<T> field, final EquinoctialOrbit op) {
  377.         super(op.getFrame(), new FieldAbsoluteDate<>(field, op.getDate()), field.getZero().add(op.getMu()));

  379.         a     = getZero().add(op.getA());
  380.         ex    = getZero().add(op.getEquinoctialEx());
  381.         ey    = getZero().add(op.getEquinoctialEy());
  382.         hx    = getZero().add(op.getHx());
  383.         hy    = getZero().add(op.getHy());
  384.         lv    = getZero().add(op.getLv());

  386.         if (op.hasDerivatives()) {
  387.             aDot  = getZero().add(op.getADot());
  388.             exDot = getZero().add(op.getEquinoctialExDot());
  389.             eyDot = getZero().add(op.getEquinoctialEyDot());
  390.             hxDot = getZero().add(op.getHxDot());
  391.             hyDot = getZero().add(op.getHyDot());
  392.             lvDot = getZero().add(op.getLvDot());
  393.         } else {
  394.             aDot  = null;
  395.             exDot = null;
  396.             eyDot = null;
  397.             hxDot = null;
  398.             hyDot = null;
  399.             lvDot = null;
  400.         }

  402.     }

  404.     /** Constructor from Field and Orbit.
  405.      * <p>Build a FieldEquinoctialOrbit from any non-Field Orbit.</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 Orbit op) {
  411.         this(field, new EquinoctialOrbit(op));
  412.     }

  414.     /** {@inheritDoc} */
  415.     public OrbitType getType() {
  416.         return OrbitType.EQUINOCTIAL;
  417.     }

  419.     /** {@inheritDoc} */
  420.     public T getA() {
  421.         return a;
  422.     }

  424.     /** {@inheritDoc} */
  425.     public T getADot() {
  426.         return aDot;
  427.     }

  429.     /** {@inheritDoc} */
  430.     public T getEquinoctialEx() {
  431.         return ex;
  432.     }

  434.     /** {@inheritDoc} */
  435.     public T getEquinoctialExDot() {
  436.         return exDot;
  437.     }

  439.     /** {@inheritDoc} */
  440.     public T getEquinoctialEy() {
  441.         return ey;
  442.     }

  444.     /** {@inheritDoc} */
  445.     public T getEquinoctialEyDot() {
  446.         return eyDot;
  447.     }

  449.     /** {@inheritDoc} */
  450.     public T getHx() {
  451.         return hx;
  452.     }

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

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

  464.     /** {@inheritDoc} */
  465.     public T getHyDot() {
  466.         return hyDot;
  467.     }

  469.     /** {@inheritDoc} */
  470.     public T getLv() {
  471.         return lv;
  472.     }

  474.     /** {@inheritDoc} */
  475.     public T getLvDot() {
  476.         return lvDot;
  477.     }

  479.     /** {@inheritDoc} */
  480.     public T getLE() {
  481.         return trueToEccentric(lv, ex, ey);
  482.     }

  484.     /** {@inheritDoc} */
  485.     public T getLEDot() {

  487.         if (!hasDerivatives()) {
  488.             return null;
  489.         }

  491.         final FieldUnivariateDerivative1<T> lVUD = new FieldUnivariateDerivative1<>(lv, lvDot);
  492.         final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  493.         final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  494.         final FieldUnivariateDerivative1<T> lEUD = trueToEccentric(lVUD, exUD, eyUD);
  495.         return lEUD.getDerivative(1);

  497.     }

  499.     /** {@inheritDoc} */
  500.     public T getLM() {
  501.         return eccentricToMean(trueToEccentric(lv, ex, ey), ex, ey);
  502.     }

  504.     /** {@inheritDoc} */
  505.     public T getLMDot() {

  507.         if (!hasDerivatives()) {
  508.             return null;
  509.         }

  511.         final FieldUnivariateDerivative1<T> lVUD = new FieldUnivariateDerivative1<>(lv, lvDot);
  512.         final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  513.         final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  514.         final FieldUnivariateDerivative1<T> lMUD = eccentricToMean(trueToEccentric(lVUD, exUD, eyUD), exUD, eyUD);
  515.         return lMUD.getDerivative(1);

  517.     }

  519.     /** Get the longitude argument.
  520.      * @param type type of the angle
  521.      * @return longitude argument (rad)
  522.      */
  523.     public T getL(final PositionAngleType type) {
  524.         return (type == PositionAngleType.MEAN) ? getLM() :
  525.                                               ((type == PositionAngleType.ECCENTRIC) ? getLE() :
  526.                                                                                    getLv());
  527.     }

  529.     /** Get the longitude argument derivative.
  530.      * @param type type of the angle
  531.      * @return longitude argument derivative (rad/s)
  532.      */
  533.     public T getLDot(final PositionAngleType type) {
  534.         return (type == PositionAngleType.MEAN) ? getLMDot() :
  535.                                               ((type == PositionAngleType.ECCENTRIC) ? getLEDot() :
  536.                                                                                    getLvDot());
  537.     }

  539.     /** {@inheritDoc} */
  540.     @Override
  541.     public boolean hasDerivatives() {
  542.         return aDot != null;
  543.     }

  545.     /** Computes the true longitude argument from the eccentric longitude argument.
  546.      * @param lE = E + ω + Ω eccentric longitude argument (rad)
  547.      * @param ex first component of the eccentricity vector
  548.      * @param ey second component of the eccentricity vector
  549.      * @param <T> Type of the field elements
  550.      * @return the true longitude argument
  551.      */
  552.     public static <T extends CalculusFieldElement<T>> T eccentricToTrue(final T lE, final T ex, final T ey) {
  553.         final T epsilon           = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  554.         final FieldSinCos<T> scLE = FastMath.sinCos(lE);
  555.         final T cosLE             = scLE.cos();
  556.         final T sinLE             = scLE.sin();
  557.         final T num               = ex.multiply(sinLE).subtract(ey.multiply(cosLE));
  558.         final T den               = epsilon.add(1).subtract(ex.multiply(cosLE)).subtract(ey.multiply(sinLE));
  559.         return lE.add(num.divide(den).atan().multiply(2));
  560.     }

  562.     /** Computes the eccentric longitude argument from the true longitude argument.
  563.      * @param lv = v + ω + Ω true longitude argument (rad)
  564.      * @param ex first component of the eccentricity vector
  565.      * @param ey second component of the eccentricity vector
  566.      * @param <T> Type of the field elements
  567.      * @return the eccentric longitude argument
  568.      */
  569.     public static <T extends CalculusFieldElement<T>> T trueToEccentric(final T lv, final T ex, final T ey) {
  570.         final T epsilon           = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  571.         final FieldSinCos<T> scLv = FastMath.sinCos(lv);
  572.         final T cosLv             = scLv.cos();
  573.         final T sinLv             = scLv.sin();
  574.         final T num               = ey.multiply(cosLv).subtract(ex.multiply(sinLv));
  575.         final T den               = epsilon.add(1).add(ex.multiply(cosLv)).add(ey.multiply(sinLv));
  576.         return lv.add(num.divide(den).atan().multiply(2));
  577.     }

  579.     /** Computes the eccentric longitude argument from the mean longitude argument.
  580.      * @param lM = M + ω + Ω mean longitude argument (rad)
  581.      * @param ex first component of the eccentricity vector
  582.      * @param ey second component of the eccentricity vector
  583.      * @param <T> Type of the field elements
  584.      * @return the eccentric longitude argument
  585.      */
  586.     public static <T extends CalculusFieldElement<T>> T meanToEccentric(final T lM, final T ex, final T ey) {
  587.         // Generalization of Kepler equation to equinoctial parameters
  588.         // with lE = PA + RAAN + E and
  589.         //      lM = PA + RAAN + M = lE - ex.sin(lE) + ey.cos(lE)
  590.         T lE = lM;
  591.         T shift = lM.getField().getZero();
  592.         T lEmlM = lM.getField().getZero();
  593.         FieldSinCos<T> scLE = FastMath.sinCos(lE);
  594.         int iter = 0;
  595.         do {
  596.             final T f2 = ex.multiply(scLE.sin()).subtract(ey.multiply(scLE.cos()));
  597.             final T f1 = ex.multiply(scLE.cos()).add(ey.multiply(scLE.sin())).negate().add(1);
  598.             final T f0 = lEmlM.subtract(f2);

  600.             final T f12 = f1.multiply(2.0);
  601.             shift = f0.multiply(f12).divide(f1.multiply(f12).subtract(f0.multiply(f2)));

  603.             lEmlM = lEmlM.subtract(shift);
  604.             lE     = lM.add(lEmlM);
  605.             scLE = FastMath.sinCos(lE);

  607.         } while (++iter < 50 && FastMath.abs(shift.getReal()) > 1.0e-12);

  609.         return lE;

  611.     }

  613.     /** Computes the mean longitude argument from the eccentric longitude argument.
  614.      * @param lE = E + ω + Ω mean longitude argument (rad)
  615.      * @param ex first component of the eccentricity vector
  616.      * @param ey second component of the eccentricity vector
  617.      * @param <T> Type of the field elements
  618.      * @return the mean longitude argument
  619.      */
  620.     public static <T extends CalculusFieldElement<T>> T eccentricToMean(final T lE, final T ex, final T ey) {
  621.         final FieldSinCos<T> scLE = FastMath.sinCos(lE);
  622.         return lE.subtract(ex.multiply(scLE.sin())).add(ey.multiply(scLE.cos()));
  623.     }

  625.     /** {@inheritDoc} */
  626.     public T getE() {
  627.         return ex.multiply(ex).add(ey.multiply(ey)).sqrt();
  628.     }

  630.     /** {@inheritDoc} */
  631.     public T getEDot() {

  633.         if (!hasDerivatives()) {
  634.             return null;
  635.         }

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

  639.     }

  641.     /** {@inheritDoc} */
  642.     public T getI() {
  643.         return hx.multiply(hx).add(hy.multiply(hy)).sqrt().atan().multiply(2);
  644.     }

  646.     /** {@inheritDoc} */
  647.     public T getIDot() {

  649.         if (!hasDerivatives()) {
  650.             return null;
  651.         }

  653.         final T h2 = hx.multiply(hx).add(hy.multiply(hy));
  654.         final T h  = h2.sqrt();
  655.         return hx.multiply(hxDot).add(hy.multiply(hyDot)).multiply(2).divide(h.multiply(h2.add(1)));

  657.     }

  659.     /** Compute position and velocity but not acceleration.
  660.      */
  661.     private void computePVWithoutA() {

  663.         if (partialPV != null) {
  664.             // already computed
  665.             return;
  666.         }

  668.         // get equinoctial parameters
  669.         final T lE = getLE();

  671.         // inclination-related intermediate parameters
  672.         final T hx2   = hx.multiply(hx);
  673.         final T hy2   = hy.multiply(hy);
  674.         final T factH = getOne().divide(hx2.add(1.0).add(hy2));

  676.         // reference axes defining the orbital plane
  677.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  678.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  679.         final T uz = hy.multiply(-2).multiply(factH);

  681.         final T vx = uy;
  682.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  683.         final T vz =  hx.multiply(factH).multiply(2);

  685.         // eccentricity-related intermediate parameters
  686.         final T ex2  = ex.multiply(ex);
  687.         final T exey = ex.multiply(ey);
  688.         final T ey2  = ey.multiply(ey);
  689.         final T e2   = ex2.add(ey2);
  690.         final T eta  = getOne().subtract(e2).sqrt().add(1);
  691.         final T beta = getOne().divide(eta);

  693.         // eccentric longitude argument
  694.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  695.         final T cLe    = scLe.cos();
  696.         final T sLe    = scLe.sin();
  697.         final T exCeyS = ex.multiply(cLe).add(ey.multiply(sLe));

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

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

  707.         final FieldVector3D<T> position =
  708.                         new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  709.                                             x.multiply(uy).add(y.multiply(vy)),
  710.                                             x.multiply(uz).add(y.multiply(vz)));
  711.         final FieldVector3D<T> velocity =
  712.                         new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)), xdot.multiply(uy).add(ydot.multiply(vy)), xdot.multiply(uz).add(ydot.multiply(vz)));

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

  716.     }

  718.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  719.      * <p>
  720.      * This method should be called only when {@link #hasDerivatives()} returns true.
  721.      * </p>
  722.      * @return non-Keplerian part of the acceleration
  723.      */
  724.     private FieldVector3D<T> nonKeplerianAcceleration() {

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

  729.         final T nonKeplerianMeanMotion = getLMDot().subtract(getKeplerianMeanMotion());
  730.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  731.                                  add(dCdP[3][1].multiply(exDot)).
  732.                                  add(dCdP[3][2].multiply(eyDot)).
  733.                                  add(dCdP[3][3].multiply(hxDot)).
  734.                                  add(dCdP[3][4].multiply(hyDot)).
  735.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  736.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  737.                                  add(dCdP[4][1].multiply(exDot)).
  738.                                  add(dCdP[4][2].multiply(eyDot)).
  739.                                  add(dCdP[4][3].multiply(hxDot)).
  740.                                  add(dCdP[4][4].multiply(hyDot)).
  741.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  742.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  743.                                  add(dCdP[5][1].multiply(exDot)).
  744.                                  add(dCdP[5][2].multiply(eyDot)).
  745.                                  add(dCdP[5][3].multiply(hxDot)).
  746.                                  add(dCdP[5][4].multiply(hyDot)).
  747.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  751.     }

  753.     /** {@inheritDoc} */
  754.     protected FieldVector3D<T> initPosition() {

  756.         // get equinoctial parameters
  757.         final T lE = getLE();

  759.         // inclination-related intermediate parameters
  760.         final T hx2   = hx.multiply(hx);
  761.         final T hy2   = hy.multiply(hy);
  762.         final T factH = getOne().divide(hx2.add(1.0).add(hy2));

  764.         // reference axes defining the orbital plane
  765.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  766.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  767.         final T uz = hy.multiply(-2).multiply(factH);

  769.         final T vx = uy;
  770.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  771.         final T vz =  hx.multiply(factH).multiply(2);

  773.         // eccentricity-related intermediate parameters
  774.         final T ex2  = ex.multiply(ex);
  775.         final T exey = ex.multiply(ey);
  776.         final T ey2  = ey.multiply(ey);
  777.         final T e2   = ex2.add(ey2);
  778.         final T eta  = getOne().subtract(e2).sqrt().add(1);
  779.         final T beta = getOne().divide(eta);

  781.         // eccentric longitude argument
  782.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  783.         final T cLe    = scLe.cos();
  784.         final T sLe    = scLe.sin();

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

  790.         return new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  791.                                    x.multiply(uy).add(y.multiply(vy)),
  792.                                    x.multiply(uz).add(y.multiply(vz)));

  794.     }

  796.     /** {@inheritDoc} */
  797.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  799.         // position and velocity
  800.         computePVWithoutA();

  802.         // acceleration
  803.         final T r2 = partialPV.getPosition().getNormSq();
  804.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(getMu().negate()),
  805.                                                                            partialPV.getPosition());
  806.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  807.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  808.                                               keplerianAcceleration;

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

  812.     }

  814.     /** {@inheritDoc} */
  815.     public FieldEquinoctialOrbit<T> shiftedBy(final double dt) {
  816.         return shiftedBy(getZero().add(dt));
  817.     }

  819.     /** {@inheritDoc} */
  820.     public FieldEquinoctialOrbit<T> shiftedBy(final T dt) {

  822.         // use Keplerian-only motion
  823.         final FieldEquinoctialOrbit<T> keplerianShifted = new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy,
  824.                                                                                       getLM().add(getKeplerianMeanMotion().multiply(dt)),
  825.                                                                                       PositionAngleType.MEAN, getFrame(),
  826.                                                                                       getDate().shiftedBy(dt), getMu());

  828.         if (hasDerivatives()) {

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

  833.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  834.             keplerianShifted.computePVWithoutA();
  835.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getPosition(),
  836.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  837.             final T   fixedR2 = fixedP.getNormSq();
  838.             final T   fixedR  = fixedR2.sqrt();
  839.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getVelocity(),
  840.                                                                  dt, nonKeplerianAcceleration);
  841.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  842.                                                                  keplerianShifted.partialPV.getPosition(),
  843.                                                                  getOne(), nonKeplerianAcceleration);

  845.             // build a new orbit, taking non-Keplerian acceleration into account
  846.             return new FieldEquinoctialOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  847.                                                                                    fixedP, fixedV, fixedA),
  848.                                                keplerianShifted.getFrame(), keplerianShifted.getMu());

  850.         } else {
  851.             // Keplerian-only motion is all we can do
  852.             return keplerianShifted;
  853.         }

  855.     }

  857.     /** {@inheritDoc} */
  858.     protected T[][] computeJacobianMeanWrtCartesian() {

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

  862.         // compute various intermediate parameters
  863.         computePVWithoutA();
  864.         final FieldVector3D<T> position = partialPV.getPosition();
  865.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  866.         final T r2         = position.getNormSq();
  867.         final T r          = r2.sqrt();
  868.         final T r3         = r.multiply(r2);

  870.         final T mu         = getMu();
  871.         final T sqrtMuA    = a.multiply(mu).sqrt();
  872.         final T a2         = a.multiply(a);

  874.         final T e2         = ex.multiply(ex).add(ey.multiply(ey));
  875.         final T oMe2       = getOne().subtract(e2);
  876.         final T epsilon    = oMe2.sqrt();
  877.         final T beta       = getOne().divide(epsilon.add(1));
  878.         final T ratio      = epsilon.multiply(beta);

  880.         final T hx2        = hx.multiply(hx);
  881.         final T hy2        = hy.multiply(hy);
  882.         final T hxhy       = hx.multiply(hy);

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

  889.         // coordinates of the spacecraft in the equinoctial frame
  890.         final T x    = FieldVector3D.dotProduct(position, f);
  891.         final T y    = FieldVector3D.dotProduct(position, g);
  892.         final T xDot = FieldVector3D.dotProduct(velocity, f);
  893.         final T yDot = FieldVector3D.dotProduct(velocity, g);

  895.         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
  896.         final T c1  = a.divide(sqrtMuA.multiply(epsilon));
  897.         final T c1N = c1.negate();
  898.         final T c2  = a.multiply(sqrtMuA).multiply(beta).divide(r3);
  899.         final T c3  = sqrtMuA.divide(r3.multiply(epsilon));
  900.         final FieldVector3D<T> drDotSdEx = new FieldVector3D<>(c1.multiply(xDot).multiply(yDot).subtract(c2.multiply(ey).multiply(x)).subtract(c3.multiply(x).multiply(y)), f,
  901.                                                                c1N.multiply(xDot).multiply(xDot).subtract(c2.multiply(ey).multiply(y)).add(c3.multiply(x).multiply(x)), g);

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

  907.         // da
  908.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  909.         final FieldVector3D<T> vectorARDot = new FieldVector3D<>(a2.multiply(2).divide(mu), velocity);
  910.         fillHalfRow(getOne(), vectorAR,    jacobian[0], 0);
  911.         fillHalfRow(getOne(), vectorARDot, jacobian[0], 3);

  913.         // dEx
  914.         final T d1 = a.negate().multiply(ratio).divide(r3);
  915.         final T d2 = (hy.multiply(xDot).subtract(hx.multiply(yDot))).divide(sqrtMuA.multiply(epsilon));
  916.         final T d3 = hx.multiply(y).subtract(hy.multiply(x)).divide(sqrtMuA);
  917.         final FieldVector3D<T> vectorExRDot =
  918.             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);
  919.         fillHalfRow(ex.multiply(d1), position, ey.negate().multiply(d2), w, epsilon.divide(sqrtMuA), drDotSdEy, jacobian[1], 0);
  920.         fillHalfRow(getOne(), vectorExRDot, jacobian[1], 3);

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

  928.         // dHx
  929.         final T h = (hx2.add(1).add(hy2)).divide(sqrtMuA.multiply(2).multiply(epsilon));
  930.         fillHalfRow( h.negate().multiply(xDot), w, jacobian[3], 0);
  931.         fillHalfRow( h.multiply(x),    w, jacobian[3], 3);

  933.        // dHy
  934.         fillHalfRow( h.negate().multiply(yDot), w, jacobian[4], 0);
  935.         fillHalfRow( h.multiply(y),    w, jacobian[4], 3);

  937.         // dLambdaM
  938.         final T l = ratio.negate().divide(sqrtMuA);
  939.         fillHalfRow(getOne().negate().divide(sqrtMuA), velocity, d2, w, l.multiply(ex), drDotSdEx, l.multiply(ey), drDotSdEy, jacobian[5], 0);
  940.         fillHalfRow(getZero().add(-2).divide(sqrtMuA), position, ex.multiply(beta), vectorEyRDot, ey.negate().multiply(beta), vectorExRDot, d3, w, jacobian[5], 3);

  942.         return jacobian;

  944.     }

  946.     /** {@inheritDoc} */
  947.     protected T[][] computeJacobianEccentricWrtCartesian() {

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

  952.         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
  953.         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
  954.         // which is inverted and rewritten as:
  955.         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
  956.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  957.         final T cosLe = scLe.cos();
  958.         final T sinLe = scLe.sin();
  959.         final T aOr   = getOne().divide(getOne().subtract(ex.multiply(cosLe)).subtract(ey.multiply(sinLe)));

  961.         // update longitude row
  962.         final T[] rowEx = jacobian[1];
  963.         final T[] rowEy = jacobian[2];
  964.         final T[] rowL  = jacobian[5];

  966.         for (int j = 0; j < 6; ++j) {
  967.             rowL[j] = aOr.multiply(rowL[j].add(sinLe.multiply(rowEx[j])).subtract(cosLe.multiply(rowEy[j])));
  968.         }
  969.         return jacobian;

  971.     }

  973.     /** {@inheritDoc} */
  974.     protected T[][] computeJacobianTrueWrtCartesian() {

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

  979.         // Differentiating the eccentric longitude equation
  980.         // tan((lV - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
  981.         // leads to
  982.         // cT (dlV - dlE) = cE dlE + cX dex + cY dey
  983.         // with
  984.         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
  985.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
  986.         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  987.         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  988.         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  989.         // which can be solved to find the differential of the true longitude
  990.         // dlV = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
  991.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  992.         final T cosLe     = scLe.cos();
  993.         final T sinLe     = scLe.sin();
  994.         final T eSinE     = ex.multiply(sinLe).subtract(ey.multiply(cosLe));
  995.         final T ecosE     = ex.multiply(cosLe).add(ey.multiply(sinLe));
  996.         final T e2        = ex.multiply(ex).add(ey.multiply(ey));
  997.         final T epsilon   = getOne().subtract(e2).sqrt();
  998.         final T onePeps   = epsilon.add(1);
  999.         final T d         = onePeps.subtract(ecosE);
  1000.         final T cT        = d.multiply(d).add(eSinE.multiply(eSinE)).divide(2);
  1001.         final T cE        = ecosE.multiply(onePeps).subtract(e2);
  1002.         final T cX        = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinLe.multiply(onePeps));
  1003.         final T cY        = ey.multiply(eSinE).divide(epsilon).add( ex).subtract(cosLe.multiply(onePeps));
  1004.         final T factorLe  = cT.add(cE).divide(cT);
  1005.         final T factorEx  = cX.divide(cT);
  1006.         final T factorEy  = cY.divide(cT);

  1008.         // update longitude row
  1009.         final T[] rowEx = jacobian[1];
  1010.         final T[] rowEy = jacobian[2];
  1011.         final T[] rowL = jacobian[5];
  1012.         for (int j = 0; j < 6; ++j) {
  1013.             rowL[j] = factorLe.multiply(rowL[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
  1014.         }

  1016.         return jacobian;

  1018.     }

  1020.     /** {@inheritDoc} */
  1021.     public void addKeplerContribution(final PositionAngleType type, final T gm,
  1022.                                       final T[] pDot) {
  1023.         final T oMe2;
  1024.         final T ksi;
  1025.         final T n               = gm.divide(a).sqrt().divide(a);
  1026.         final FieldSinCos<T> sc = FastMath.sinCos(lv);
  1027.         switch (type) {
  1028.             case MEAN :
  1029.                 pDot[5] = pDot[5].add(n);
  1030.                 break;
  1031.             case ECCENTRIC :
  1032.                 oMe2 = getOne().subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1033.                 ksi  = ex.multiply(sc.cos()).add(1).add(ey.multiply(sc.sin()));
  1034.                 pDot[5] = pDot[5].add(n.multiply(ksi).divide(oMe2));
  1035.                 break;
  1036.             case TRUE :
  1037.                 oMe2 = getOne().subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1038.                 ksi  =  ex.multiply(sc.cos()).add(1).add(ey.multiply(sc.sin()));
  1039.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1040.                 break;
  1041.             default :
  1042.                 throw new OrekitInternalError(null);
  1043.         }
  1044.     }

  1046.     /**  Returns a string representation of this equinoctial parameters object.
  1047.      * @return a string representation of this object
  1048.      */
  1049.     public String toString() {
  1050.         return new StringBuilder().append("equinoctial parameters: ").append('{').
  1051.                                   append("a: ").append(a.getReal()).
  1052.                                   append("; ex: ").append(ex.getReal()).append("; ey: ").append(ey.getReal()).
  1053.                                   append("; hx: ").append(hx.getReal()).append("; hy: ").append(hy.getReal()).
  1054.                                   append("; lv: ").append(FastMath.toDegrees(lv.getReal())).
  1055.                                   append(";}").toString();
  1056.     }

  1058.     /** {@inheritDoc} */
  1059.     @Override
  1060.     public PositionAngleType getCachedPositionAngleType() {
  1061.         return PositionAngleType.TRUE;
  1062.     }

  1064.     /** {@inheritDoc} */
  1065.     @Override
  1066.     public boolean hasRates() {
  1067.         return hasDerivatives();
  1068.     }

  1070.     /** {@inheritDoc} */
  1071.     @Override
  1072.     public FieldEquinoctialOrbit<T> removeRates() {
  1073.         final PositionAngleType positionAngleType = getCachedPositionAngleType();
  1074.         return new FieldEquinoctialOrbit<>(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
  1075.                 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
  1076.     }

  1078.     /** {@inheritDoc} */
  1079.     @Override
  1080.     public EquinoctialOrbit toOrbit() {
  1081.         if (hasDerivatives()) {
  1082.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1083.                                         hx.getReal(), hy.getReal(), lv.getReal(),
  1084.                                         aDot.getReal(), exDot.getReal(), eyDot.getReal(),
  1085.                                         hxDot.getReal(), hyDot.getReal(), lvDot.getReal(),
  1086.                                         PositionAngleType.TRUE, getFrame(),
  1087.                                         getDate().toAbsoluteDate(), getMu().getReal());
  1088.         } else {
  1089.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1090.                                         hx.getReal(), hy.getReal(), lv.getReal(),
  1091.                                         PositionAngleType.TRUE, getFrame(),
  1092.                                         getDate().toAbsoluteDate(), getMu().getReal());
  1093.         }
  1094.     }

  1096. }
Created with JaCoCo 0.8.11.202310140853