FieldEquinoctialOrbit.java

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

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

  23. import org.hipparchus.Field;
  24. import org.hipparchus.CalculusFieldElement;
  25. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
  26. import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
  27. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  28. import org.hipparchus.util.FastMath;
  29. import org.hipparchus.util.FieldSinCos;
  30. import org.hipparchus.util.MathArrays;
  31. import org.hipparchus.util.MathUtils;
  32. import org.orekit.errors.OrekitIllegalArgumentException;
  33. import org.orekit.errors.OrekitInternalError;
  34. import org.orekit.errors.OrekitMessages;
  35. import org.orekit.frames.Frame;
  36. import org.orekit.time.FieldAbsoluteDate;
  37. import org.orekit.utils.FieldPVCoordinates;
  38. import org.orekit.utils.TimeStampedFieldPVCoordinates;


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

  81.     /** Semi-major axis (m). */
  82.     private final T a;

  84.     /** First component of the eccentricity vector. */
  85.     private final T ex;

  87.     /** Second component of the eccentricity vector. */
  88.     private final T ey;

  90.     /** First component of the inclination vector. */
  91.     private final T hx;

  93.     /** Second component of the inclination vector. */
  94.     private final T hy;

  96.     /** True longitude argument (rad). */
  97.     private final T lv;

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

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

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

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

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

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

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

  120.     /** Field used by this class.*/
  121.     private Field<T> field;

  123.     /**Zero.*/
  124.     private T zero;

  126.     /**One.*/
  127.     private T one;

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

  154.     /** Creates a new instance.
  155.      * @param a  semi-major axis (m)
  156.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  157.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  158.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  159.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  160.      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
  161.      * @param aDot  semi-major axis derivative (m/s)
  162.      * @param exDot d(e cos(ω + Ω))/dt, first component of eccentricity vector derivative
  163.      * @param eyDot d(e sin(ω + Ω))/dt, second component of eccentricity vector derivative
  164.      * @param hxDot d(tan(i/2) cos(Ω))/dt, first component of inclination vector derivative
  165.      * @param hyDot d(tan(i/2) sin(Ω))/dt, second component of inclination vector derivative
  166.      * @param lDot  d(M or E or v) + ω + Ω)/dr, mean, eccentric or true longitude argument  derivative (rad/s)
  167.      * @param type type of longitude argument
  168.      * @param frame the frame in which the parameters are defined
  169.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  170.      * @param date date of the orbital parameters
  171.      * @param mu central attraction coefficient (m³/s²)
  172.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  173.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  174.      */
  175.     public FieldEquinoctialOrbit(final T a, final T ex, final T ey,
  176.                                  final T hx, final T hy, final T l,
  177.                                  final T aDot, final T exDot, final T eyDot,
  178.                                  final T hxDot, final T hyDot, final T lDot,
  179.                                  final PositionAngle type,
  180.                                  final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  181.         throws IllegalArgumentException {
  182.         super(frame, date, mu);
  183.         field = a.getField();
  184.         zero = field.getZero();
  185.         one = field.getOne();
  186.         if (ex.getReal() * ex.getReal() + ey.getReal() * ey.getReal() >= 1.0) {
  187.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  188.                                                      getClass().getName());
  189.         }

  191.         this.a     = a;
  192.         this.aDot  = aDot;
  193.         this.ex    = ex;
  194.         this.exDot = exDot;
  195.         this.ey    = ey;
  196.         this.eyDot = eyDot;
  197.         this.hx    = hx;
  198.         this.hxDot = hxDot;
  199.         this.hy    = hy;
  200.         this.hyDot = hyDot;

  202.         if (hasDerivatives()) {
  203.             final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  204.             final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  205.             final FieldUnivariateDerivative1<T> lUD  = new FieldUnivariateDerivative1<>(l,  lDot);
  206.             final FieldUnivariateDerivative1<T> lvUD;
  207.             switch (type) {
  208.                 case MEAN :
  209.                     lvUD = eccentricToTrue(meanToEccentric(lUD, exUD, eyUD), exUD, eyUD);
  210.                     break;
  211.                 case ECCENTRIC :
  212.                     lvUD = eccentricToTrue(lUD, exUD, eyUD);
  213.                     break;
  214.                 case TRUE :
  215.                     lvUD = lUD;
  216.                     break;
  217.                 default : // this should never happen
  218.                     throw new OrekitInternalError(null);
  219.             }
  220.             this.lv    = lvUD.getValue();
  221.             this.lvDot = lvUD.getDerivative(1);
  222.         } else {
  223.             switch (type) {
  224.                 case MEAN :
  225.                     this.lv = eccentricToTrue(meanToEccentric(l, ex, ey), ex, ey);
  226.                     break;
  227.                 case ECCENTRIC :
  228.                     this.lv = eccentricToTrue(l, ex, ey);
  229.                     break;
  230.                 case TRUE :
  231.                     this.lv = l;
  232.                     break;
  233.                 default :
  234.                     throw new OrekitInternalError(null);
  235.             }
  236.             this.lvDot = null;
  237.         }

  239.         this.partialPV = null;

  241.     }

  243.     /** Constructor from Cartesian parameters.
  244.      *
  245.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  246.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  247.      * use {@code mu} and the position to compute the acceleration, including
  248.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  249.      *
  250.      * @param pvCoordinates the position, velocity and acceleration
  251.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  252.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  253.      * @param mu central attraction coefficient (m³/s²)
  254.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  255.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  256.      */
  257.     public FieldEquinoctialOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  258.                                  final Frame frame, final T mu)
  259.         throws IllegalArgumentException {
  260.         super(pvCoordinates, frame, mu);

  262.         field = pvCoordinates.getPosition().getX().getField();
  263.         zero = field.getZero();
  264.         one = field.getOne();

  266.         //  compute semi-major axis
  267.         final FieldVector3D<T> pvP = pvCoordinates.getPosition();
  268.         final FieldVector3D<T> pvV = pvCoordinates.getVelocity();
  269.         final FieldVector3D<T> pvA = pvCoordinates.getAcceleration();
  270.         final T r2 = pvP.getNormSq();
  271.         final T r  = r2.sqrt();
  272.         final T V2 = pvV.getNormSq();
  273.         final T rV2OnMu = r.multiply(V2).divide(mu);

  275.         if (rV2OnMu.getReal() > 2) {
  276.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  277.                                                      getClass().getName());
  278.         }

  280.         // compute inclination vector
  281.         final FieldVector3D<T> w = pvCoordinates.getMomentum().normalize();
  282.         final T d = one.divide(one.add(w.getZ()));
  283.         hx =  d.negate().multiply(w.getY());
  284.         hy =  d.multiply(w.getX());

  286.         // compute true longitude argument
  287.         final T cLv = (pvP.getX().subtract(d.multiply(pvP.getZ()).multiply(w.getX()))).divide(r);
  288.         final T sLv = (pvP.getY().subtract(d.multiply(pvP.getZ()).multiply(w.getY()))).divide(r);
  289.         lv = sLv.atan2(cLv);


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

  295.         // compute eccentricity vector
  296.         final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(mu).sqrt());
  297.         final T eCE = rV2OnMu.subtract(1);
  298.         final T e2  = eCE.multiply(eCE).add(eSE.multiply(eSE));
  299.         final T f   = eCE.subtract(e2);
  300.         final T g   = e2.negate().add(1).sqrt().multiply(eSE);
  301.         ex = a.multiply(f.multiply(cLv).add( g.multiply(sLv))).divide(r);
  302.         ey = a.multiply(f.multiply(sLv).subtract(g.multiply(cLv))).divide(r);

  304.         partialPV = pvCoordinates;

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

  309.             final T[][] jacobian = MathArrays.buildArray(a.getField(), 6, 6);
  310.             getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);

  312.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  313.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  314.             final T   aX                       = nonKeplerianAcceleration.getX();
  315.             final T   aY                       = nonKeplerianAcceleration.getY();
  316.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  317.             aDot  = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  318.             exDot = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  319.             eyDot = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  320.             hxDot = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  321.             hyDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  323.             // in order to compute true anomaly derivative, we must compute
  324.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  325.             final T lMDot = getKeplerianMeanMotion().
  326.                             add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  327.             final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  328.             final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  329.             final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(getLM(), lMDot);
  330.             final FieldUnivariateDerivative1<T> lvUD = eccentricToTrue(meanToEccentric(lMUD, exUD, eyUD), exUD, eyUD);
  331.             lvDot = lvUD.getDerivative(1);

  333.         } else {
  334.             // acceleration is either almost zero or NaN,
  335.             // we assume acceleration was not known
  336.             // we don't set up derivatives
  337.             aDot  = null;
  338.             exDot = null;
  339.             eyDot = null;
  340.             hxDot = null;
  341.             hyDot = null;
  342.             lvDot = null;
  343.         }

  345.     }

  347.     /** Constructor from Cartesian parameters.
  348.      *
  349.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  350.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  351.      * use {@code mu} and the position to compute the acceleration, including
  352.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  353.      *
  354.      * @param pvCoordinates the position end velocity
  355.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  356.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  357.      * @param date date of the orbital parameters
  358.      * @param mu central attraction coefficient (m³/s²)
  359.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  360.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  361.      */
  362.     public FieldEquinoctialOrbit(final FieldPVCoordinates<T> pvCoordinates, final Frame frame,
  363.                             final FieldAbsoluteDate<T> date, final T mu)
  364.         throws IllegalArgumentException {
  365.         this(new TimeStampedFieldPVCoordinates<>(date, pvCoordinates), frame, mu);
  366.     }

  368.     /** Constructor from any kind of orbital parameters.
  369.      * @param op orbital parameters to copy
  370.      */
  371.     public FieldEquinoctialOrbit(final FieldOrbit<T> op) {
  372.         super(op.getFrame(), op.getDate(), op.getMu());

  374.         a     = op.getA();
  375.         ex    = op.getEquinoctialEx();
  376.         ey    = op.getEquinoctialEy();
  377.         hx    = op.getHx();
  378.         hy    = op.getHy();
  379.         lv    = op.getLv();

  381.         aDot  = op.getADot();
  382.         exDot = op.getEquinoctialExDot();
  383.         eyDot = op.getEquinoctialEyDot();
  384.         hxDot = op.getHxDot();
  385.         hyDot = op.getHyDot();
  386.         lvDot = op.getLvDot();

  388.         field = a.getField();
  389.         zero  = field.getZero();
  390.         one   = field.getOne();
  391.     }

  393.     /** {@inheritDoc} */
  394.     public OrbitType getType() {
  395.         return OrbitType.EQUINOCTIAL;
  396.     }

  398.     /** {@inheritDoc} */
  399.     public T getA() {
  400.         return a;
  401.     }

  403.     /** {@inheritDoc} */
  404.     public T getADot() {
  405.         return aDot;
  406.     }

  408.     /** {@inheritDoc} */
  409.     public T getEquinoctialEx() {
  410.         return ex;
  411.     }

  413.     /** {@inheritDoc} */
  414.     public T getEquinoctialExDot() {
  415.         return exDot;
  416.     }

  418.     /** {@inheritDoc} */
  419.     public T getEquinoctialEy() {
  420.         return ey;
  421.     }

  423.     /** {@inheritDoc} */
  424.     public T getEquinoctialEyDot() {
  425.         return eyDot;
  426.     }

  428.     /** {@inheritDoc} */
  429.     public T getHx() {
  430.         return hx;
  431.     }

  433.     /** {@inheritDoc} */
  434.     public T getHxDot() {
  435.         return hxDot;
  436.     }

  438.     /** {@inheritDoc} */
  439.     public T getHy() {
  440.         return hy;
  441.     }

  443.     /** {@inheritDoc} */
  444.     public T getHyDot() {
  445.         return hyDot;
  446.     }

  448.     /** {@inheritDoc} */
  449.     public T getLv() {
  450.         return lv;
  451.     }

  453.     /** {@inheritDoc} */
  454.     public T getLvDot() {
  455.         return lvDot;
  456.     }

  458.     /** {@inheritDoc} */
  459.     public T getLE() {
  460.         return trueToEccentric(lv, ex, ey);
  461.     }

  463.     /** {@inheritDoc} */
  464.     public T getLEDot() {

  466.         if (!hasDerivatives()) {
  467.             return null;
  468.         }

  470.         final FieldUnivariateDerivative1<T> lVUD = new FieldUnivariateDerivative1<>(lv, lvDot);
  471.         final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
  472.         final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
  473.         final FieldUnivariateDerivative1<T> lEUD = trueToEccentric(lVUD, exUD, eyUD);
  474.         return lEUD.getDerivative(1);

  476.     }

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

  483.     /** {@inheritDoc} */
  484.     public T getLMDot() {

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

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

  496.     }

  498.     /** Get the longitude argument.
  499.      * @param type type of the angle
  500.      * @return longitude argument (rad)
  501.      */
  502.     public T getL(final PositionAngle type) {
  503.         return (type == PositionAngle.MEAN) ? getLM() :
  504.                                               ((type == PositionAngle.ECCENTRIC) ? getLE() :
  505.                                                                                    getLv());
  506.     }

  508.     /** Get the longitude argument derivative.
  509.      * @param type type of the angle
  510.      * @return longitude argument derivative (rad/s)
  511.      */
  512.     public T getLDot(final PositionAngle type) {
  513.         return (type == PositionAngle.MEAN) ? getLMDot() :
  514.                                               ((type == PositionAngle.ECCENTRIC) ? getLEDot() :
  515.                                                                                    getLvDot());
  516.     }

  518.     /** {@inheritDoc} */
  519.     @Override
  520.     public boolean hasDerivatives() {
  521.         return aDot != null;
  522.     }

  524.     /** Computes the true longitude argument from the eccentric longitude argument.
  525.      * @param lE = E + ω + Ω eccentric longitude argument (rad)
  526.      * @param ex first component of the eccentricity vector
  527.      * @param ey second component of the eccentricity vector
  528.      * @param <T> Type of the field elements
  529.      * @return the true longitude argument
  530.      */
  531.     public static <T extends CalculusFieldElement<T>> T eccentricToTrue(final T lE, final T ex, final T ey) {
  532.         final T epsilon           = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  533.         final FieldSinCos<T> scLE = FastMath.sinCos(lE);
  534.         final T cosLE             = scLE.cos();
  535.         final T sinLE             = scLE.sin();
  536.         final T num               = ex.multiply(sinLE).subtract(ey.multiply(cosLE));
  537.         final T den               = epsilon.add(1).subtract(ex.multiply(cosLE)).subtract(ey.multiply(sinLE));
  538.         return lE.add(num.divide(den).atan().multiply(2));
  539.     }

  541.     /** Computes the eccentric longitude argument from the true longitude argument.
  542.      * @param lv = v + ω + Ω true longitude argument (rad)
  543.      * @param ex first component of the eccentricity vector
  544.      * @param ey second component of the eccentricity vector
  545.      * @param <T> Type of the field elements
  546.      * @return the eccentric longitude argument
  547.      */
  548.     public static <T extends CalculusFieldElement<T>> T trueToEccentric(final T lv, final T ex, final T ey) {
  549.         final T epsilon           = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  550.         final FieldSinCos<T> scLv = FastMath.sinCos(lv);
  551.         final T cosLv             = scLv.cos();
  552.         final T sinLv             = scLv.sin();
  553.         final T num               = ey.multiply(cosLv).subtract(ex.multiply(sinLv));
  554.         final T den               = epsilon.add(1).add(ex.multiply(cosLv)).add(ey.multiply(sinLv));
  555.         return lv.add(num.divide(den).atan().multiply(2));
  556.     }

  558.     /** Computes the eccentric longitude argument from the mean longitude argument.
  559.      * @param lM = M + ω + Ω mean longitude argument (rad)
  560.      * @param ex first component of the eccentricity vector
  561.      * @param ey second component of the eccentricity vector
  562.      * @param <T> Type of the field elements
  563.      * @return the eccentric longitude argument
  564.      */
  565.     public static <T extends CalculusFieldElement<T>> T meanToEccentric(final T lM, final T ex, final T ey) {
  566.         // Generalization of Kepler equation to equinoctial parameters
  567.         // with lE = PA + RAAN + E and
  568.         //      lM = PA + RAAN + M = lE - ex.sin(lE) + ey.cos(lE)
  569.         T lE = lM;
  570.         T shift = lM.getField().getZero();
  571.         T lEmlM = lM.getField().getZero();
  572.         FieldSinCos<T> scLE = FastMath.sinCos(lE);
  573.         int iter = 0;
  574.         do {
  575.             final T f2 = ex.multiply(scLE.sin()).subtract(ey.multiply(scLE.cos()));
  576.             final T f1 = ex.multiply(scLE.cos()).add(ey.multiply(scLE.sin())).negate().add(1);
  577.             final T f0 = lEmlM.subtract(f2);

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

  582.             lEmlM = lEmlM.subtract(shift);
  583.             lE     = lM.add(lEmlM);
  584.             scLE = FastMath.sinCos(lE);

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

  588.         return lE;

  590.     }

  592.     /** Computes the mean longitude argument from the eccentric longitude argument.
  593.      * @param lE = E + ω + Ω mean longitude argument (rad)
  594.      * @param ex first component of the eccentricity vector
  595.      * @param ey second component of the eccentricity vector
  596.      * @param <T> Type of the field elements
  597.      * @return the mean longitude argument
  598.      */
  599.     public static <T extends CalculusFieldElement<T>> T eccentricToMean(final T lE, final T ex, final T ey) {
  600.         final FieldSinCos<T> scLE = FastMath.sinCos(lE);
  601.         return lE.subtract(ex.multiply(scLE.sin())).add(ey.multiply(scLE.cos()));
  602.     }

  604.     /** {@inheritDoc} */
  605.     public T getE() {
  606.         return ex.multiply(ex).add(ey.multiply(ey)).sqrt();
  607.     }

  609.     /** {@inheritDoc} */
  610.     public T getEDot() {

  612.         if (!hasDerivatives()) {
  613.             return null;
  614.         }

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

  618.     }

  620.     /** {@inheritDoc} */
  621.     public T getI() {
  622.         return hx.multiply(hx).add(hy.multiply(hy)).sqrt().atan().multiply(2);
  623.     }

  625.     /** {@inheritDoc} */
  626.     public T getIDot() {

  628.         if (!hasDerivatives()) {
  629.             return null;
  630.         }

  632.         final T h2 = hx.multiply(hx).add(hy.multiply(hy));
  633.         final T h  = h2.sqrt();
  634.         return hx.multiply(hxDot).add(hy.multiply(hyDot)).multiply(2).divide(h.multiply(h2.add(1)));

  636.     }

  638.     /** Compute position and velocity but not acceleration.
  639.      */
  640.     private void computePVWithoutA() {

  642.         if (partialPV != null) {
  643.             // already computed
  644.             return;
  645.         }

  647.         // get equinoctial parameters
  648.         final T lE = getLE();

  650.         // inclination-related intermediate parameters
  651.         final T hx2   = hx.multiply(hx);
  652.         final T hy2   = hy.multiply(hy);
  653.         final T factH = one.divide(hx2.add(1.0).add(hy2));

  655.         // reference axes defining the orbital plane
  656.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  657.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  658.         final T uz = hy.multiply(-2).multiply(factH);

  660.         final T vx = uy;
  661.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  662.         final T vz =  hx.multiply(factH).multiply(2);

  664.         // eccentricity-related intermediate parameters
  665.         final T ex2  = ex.multiply(ex);
  666.         final T exey = ex.multiply(ey);
  667.         final T ey2  = ey.multiply(ey);
  668.         final T e2   = ex2.add(ey2);
  669.         final T eta  = one.subtract(e2).sqrt().add(1);
  670.         final T beta = one.divide(eta);

  672.         // eccentric longitude argument
  673.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  674.         final T cLe    = scLe.cos();
  675.         final T sLe    = scLe.sin();
  676.         final T exCeyS = ex.multiply(cLe).add(ey.multiply(sLe));

  678.         // coordinates of position and velocity in the orbital plane
  679.         final T x      = a.multiply(one.subtract(beta.multiply(ey2)).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(ex));
  680.         final T y      = a.multiply(one.subtract(beta.multiply(ex2)).multiply(sLe).add(beta .multiply(exey).multiply(cLe)).subtract(ey));

  682.         final T factor = getMu().divide(a).sqrt().divide(one.subtract(exCeyS));
  683.         final T xdot   = factor.multiply(sLe.negate().add(beta.multiply(ey).multiply(exCeyS)));
  684.         final T ydot   = factor.multiply(cLe.subtract(beta.multiply(ex).multiply(exCeyS)));

  686.         final FieldVector3D<T> position =
  687.                         new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  688.                                             x.multiply(uy).add(y.multiply(vy)),
  689.                                             x.multiply(uz).add(y.multiply(vz)));
  690.         final FieldVector3D<T> velocity =
  691.                         new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)), xdot.multiply(uy).add(ydot.multiply(vy)), xdot.multiply(uz).add(ydot.multiply(vz)));

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

  695.     }

  697.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  698.      * <p>
  699.      * This method should be called only when {@link #hasDerivatives()} returns true.
  700.      * </p>
  701.      * @return non-Keplerian part of the acceleration
  702.      */
  703.     private FieldVector3D<T> nonKeplerianAcceleration() {

  705.         final T[][] dCdP = MathArrays.buildArray(a.getField(), 6, 6);
  706.         getJacobianWrtParameters(PositionAngle.MEAN, dCdP);

  708.         final T nonKeplerianMeanMotion = getLMDot().subtract(getKeplerianMeanMotion());
  709.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  710.                                  add(dCdP[3][1].multiply(exDot)).
  711.                                  add(dCdP[3][2].multiply(eyDot)).
  712.                                  add(dCdP[3][3].multiply(hxDot)).
  713.                                  add(dCdP[3][4].multiply(hyDot)).
  714.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  715.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  716.                                  add(dCdP[4][1].multiply(exDot)).
  717.                                  add(dCdP[4][2].multiply(eyDot)).
  718.                                  add(dCdP[4][3].multiply(hxDot)).
  719.                                  add(dCdP[4][4].multiply(hyDot)).
  720.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  721.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  722.                                  add(dCdP[5][1].multiply(exDot)).
  723.                                  add(dCdP[5][2].multiply(eyDot)).
  724.                                  add(dCdP[5][3].multiply(hxDot)).
  725.                                  add(dCdP[5][4].multiply(hyDot)).
  726.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  730.     }

  732.     /** {@inheritDoc} */
  733.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  735.         // position and velocity
  736.         computePVWithoutA();

  738.         // acceleration
  739.         final T r2 = partialPV.getPosition().getNormSq();
  740.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(getMu().negate()),
  741.                                                                            partialPV.getPosition());
  742.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  743.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  744.                                               keplerianAcceleration;

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

  748.     }

  750.     /** {@inheritDoc} */
  751.     public FieldEquinoctialOrbit<T> shiftedBy(final double dt) {
  752.         return shiftedBy(getDate().getField().getZero().add(dt));
  753.     }

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

  758.         // use Keplerian-only motion
  759.         final FieldEquinoctialOrbit<T> keplerianShifted = new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy,
  760.                                                                                       getLM().add(getKeplerianMeanMotion().multiply(dt)),
  761.                                                                                       PositionAngle.MEAN, getFrame(),
  762.                                                                                       getDate().shiftedBy(dt), getMu());

  764.         if (hasDerivatives()) {

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

  769.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  770.             keplerianShifted.computePVWithoutA();
  771.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, keplerianShifted.partialPV.getPosition(),
  772.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  773.             final T   fixedR2 = fixedP.getNormSq();
  774.             final T   fixedR  = fixedR2.sqrt();
  775.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, keplerianShifted.partialPV.getVelocity(),
  776.                                                                  dt, nonKeplerianAcceleration);
  777.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  778.                                                                  keplerianShifted.partialPV.getPosition(),
  779.                                                                  one, nonKeplerianAcceleration);

  781.             // build a new orbit, taking non-Keplerian acceleration into account
  782.             return new FieldEquinoctialOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  783.                                                                                    fixedP, fixedV, fixedA),
  784.                                                keplerianShifted.getFrame(), keplerianShifted.getMu());

  786.         } else {
  787.             // Keplerian-only motion is all we can do
  788.             return keplerianShifted;
  789.         }

  791.     }

  793.     /** {@inheritDoc}
  794.      * <p>
  795.      * The interpolated instance is created by polynomial Hermite interpolation
  796.      * on equinoctial elements, without derivatives (which means the interpolation
  797.      * falls back to Lagrange interpolation only).
  798.      * </p>
  799.      * <p>
  800.      * As this implementation of interpolation is polynomial, it should be used only
  801.      * with small samples (about 10-20 points) in order to avoid <a
  802.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  803.      * and numerical problems (including NaN appearing).
  804.      * </p>
  805.      * <p>
  806.      * If orbit interpolation on large samples is needed, using the {@link
  807.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  808.      * low-level interpolation. The Ephemeris class automatically handles selection of
  809.      * a neighboring sub-sample with a predefined number of point from a large global sample
  810.      * in a thread-safe way.
  811.      * </p>
  812.      */
  813.     public FieldEquinoctialOrbit<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldOrbit<T>> sample) {

  815.         // first pass to check if derivatives are available throughout the sample
  816.         final List<FieldOrbit<T>> list = sample.collect(Collectors.toList());
  817.         boolean useDerivatives = true;
  818.         for (final FieldOrbit<T> orbit : list) {
  819.             useDerivatives = useDerivatives && orbit.hasDerivatives();
  820.         }

  822.         // set up an interpolator
  823.         final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<>();

  825.         // second pass to feed interpolator
  826.         FieldAbsoluteDate<T> previousDate = null;
  827.         T                    previousLm   = zero.add(Double.NaN);
  828.         for (final FieldOrbit<T> orbit : list) {
  829.             final FieldEquinoctialOrbit<T> equi = (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.convertType(orbit);
  830.             final T continuousLm;
  831.             if (previousDate == null) {
  832.                 continuousLm = (T) equi.getLM();
  833.             } else {
  834.                 final T dt       = (T) equi.getDate().durationFrom(previousDate);
  835.                 final T keplerLm = previousLm.add((T) equi.getKeplerianMeanMotion().multiply(dt));
  836.                 continuousLm = MathUtils.normalizeAngle((T) equi.getLM(), keplerLm);
  837.             }
  838.             previousDate = equi.getDate();
  839.             previousLm   = continuousLm;
  840.             final T[] toAdd = MathArrays.buildArray(field, 6);
  841.             toAdd[0] = (T) equi.getA();
  842.             toAdd[1] = (T) equi.getEquinoctialEx();
  843.             toAdd[2] = (T) equi.getEquinoctialEy();
  844.             toAdd[3] = (T) equi.getHx();
  845.             toAdd[4] = (T) equi.getHy();
  846.             toAdd[5] = (T) continuousLm;
  847.             if (useDerivatives) {
  848.                 final T[] toAddDot = MathArrays.buildArray(one.getField(), 6);
  849.                 toAddDot[0] = equi.getADot();
  850.                 toAddDot[1] = equi.getEquinoctialExDot();
  851.                 toAddDot[2] = equi.getEquinoctialEyDot();
  852.                 toAddDot[3] = equi.getHxDot();
  853.                 toAddDot[4] = equi.getHyDot();
  854.                 toAddDot[5] = equi.getLMDot();
  855.                 interpolator.addSamplePoint(equi.getDate().durationFrom(date),
  856.                                             toAdd, toAddDot);
  857.             } else {
  858.                 interpolator.addSamplePoint((T) equi.getDate().durationFrom(date),
  859.                                             toAdd);
  860.             }
  861.         }

  863.         // interpolate
  864.         final T[][] interpolated = interpolator.derivatives(zero, 1);

  866.         // build a new interpolated instance
  867.         return new FieldEquinoctialOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  868.                                            interpolated[0][3], interpolated[0][4], interpolated[0][5],
  869.                                            interpolated[1][0], interpolated[1][1], interpolated[1][2],
  870.                                            interpolated[1][3], interpolated[1][4], interpolated[1][5],
  871.                                            PositionAngle.MEAN, getFrame(), date, getMu());

  873.     }

  875.     /** {@inheritDoc} */
  876.     protected T[][] computeJacobianMeanWrtCartesian() {

  878.         final T[][] jacobian = MathArrays.buildArray(field, 6, 6);

  880.         // compute various intermediate parameters
  881.         computePVWithoutA();
  882.         final FieldVector3D<T> position = partialPV.getPosition();
  883.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  884.         final T r2         = position.getNormSq();
  885.         final T r          = r2.sqrt();
  886.         final T r3         = r.multiply(r2);

  888.         final T mu         = getMu();
  889.         final T sqrtMuA    = a.multiply(mu).sqrt();
  890.         final T a2         = a.multiply(a);

  892.         final T e2         = ex.multiply(ex).add(ey.multiply(ey));
  893.         final T oMe2       = one.subtract(e2);
  894.         final T epsilon    = oMe2.sqrt();
  895.         final T beta       = one.divide(epsilon.add(1));
  896.         final T ratio      = epsilon.multiply(beta);

  898.         final T hx2        = hx.multiply(hx);
  899.         final T hy2        = hy.multiply(hy);
  900.         final T hxhy       = hx.multiply(hy);

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

  907.         // coordinates of the spacecraft in the equinoctial frame
  908.         final T x    = FieldVector3D.dotProduct(position, f);
  909.         final T y    = FieldVector3D.dotProduct(position, g);
  910.         final T xDot = FieldVector3D.dotProduct(velocity, f);
  911.         final T yDot = FieldVector3D.dotProduct(velocity, g);

  913.         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
  914.         final T c1  = a.divide(sqrtMuA.multiply(epsilon));
  915.         final T c1N = c1.negate();
  916.         final T c2  = a.multiply(sqrtMuA).multiply(beta).divide(r3);
  917.         final T c3  = sqrtMuA.divide(r3.multiply(epsilon));
  918.         final FieldVector3D<T> drDotSdEx = new FieldVector3D<>(c1.multiply(xDot).multiply(yDot).subtract(c2.multiply(ey).multiply(x)).subtract(c3.multiply(x).multiply(y)), f,
  919.                                                                c1N.multiply(xDot).multiply(xDot).subtract(c2.multiply(ey).multiply(y)).add(c3.multiply(x).multiply(x)), g);

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

  925.         // da
  926.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  927.         final FieldVector3D<T> vectorARDot = new FieldVector3D<>(a2.multiply(2).divide(mu), velocity);
  928.         fillHalfRow(one, vectorAR,    jacobian[0], 0);
  929.         fillHalfRow(one, vectorARDot, jacobian[0], 3);

  931.         // dEx
  932.         final T d1 = a.negate().multiply(ratio).divide(r3);
  933.         final T d2 = (hy.multiply(xDot).subtract(hx.multiply(yDot))).divide(sqrtMuA.multiply(epsilon));
  934.         final T d3 = hx.multiply(y).subtract(hy.multiply(x)).divide(sqrtMuA);
  935.         final FieldVector3D<T> vectorExRDot =
  936.             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);
  937.         fillHalfRow(ex.multiply(d1), position, ey.negate().multiply(d2), w, epsilon.divide(sqrtMuA), drDotSdEy, jacobian[1], 0);
  938.         fillHalfRow(one, vectorExRDot, jacobian[1], 3);

  940.         // dEy
  941.         final FieldVector3D<T> vectorEyRDot =
  942.             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);
  943.         fillHalfRow(ey.multiply(d1), position, ex.multiply(d2), w, epsilon.negate().divide(sqrtMuA), drDotSdEx, jacobian[2], 0);
  944.         fillHalfRow(one, vectorEyRDot, jacobian[2], 3);

  946.         // dHx
  947.         final T h = (hx2.add(1).add(hy2)).divide(sqrtMuA.multiply(2).multiply(epsilon));
  948.         fillHalfRow( h.negate().multiply(xDot), w, jacobian[3], 0);
  949.         fillHalfRow( h.multiply(x),    w, jacobian[3], 3);

  951.        // dHy
  952.         fillHalfRow( h.negate().multiply(yDot), w, jacobian[4], 0);
  953.         fillHalfRow( h.multiply(y),    w, jacobian[4], 3);

  955.         // dLambdaM
  956.         final T l = ratio.negate().divide(sqrtMuA);
  957.         fillHalfRow(one.negate().divide(sqrtMuA), velocity, d2, w, l.multiply(ex), drDotSdEx, l.multiply(ey), drDotSdEy, jacobian[5], 0);
  958.         fillHalfRow(zero.add(-2).divide(sqrtMuA), position, ex.multiply(beta), vectorEyRDot, ey.negate().multiply(beta), vectorExRDot, d3, w, jacobian[5], 3);

  960.         return jacobian;

  962.     }

  964.     /** {@inheritDoc} */
  965.     protected T[][] computeJacobianEccentricWrtCartesian() {

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

  970.         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
  971.         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
  972.         // which is inverted and rewritten as:
  973.         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
  974.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  975.         final T cosLe = scLe.cos();
  976.         final T sinLe = scLe.sin();
  977.         final T aOr   = one.divide(one.subtract(ex.multiply(cosLe)).subtract(ey.multiply(sinLe)));

  979.         // update longitude row
  980.         final T[] rowEx = jacobian[1];
  981.         final T[] rowEy = jacobian[2];
  982.         final T[] rowL  = jacobian[5];

  984.         for (int j = 0; j < 6; ++j) {
  985.             rowL[j] = aOr.multiply(rowL[j].add(sinLe.multiply(rowEx[j])).subtract(cosLe.multiply(rowEy[j])));
  986.         }
  987.         return jacobian;

  989.     }

  991.     /** {@inheritDoc} */
  992.     protected T[][] computeJacobianTrueWrtCartesian() {

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

  997.         // Differentiating the eccentric longitude equation
  998.         // tan((lV - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
  999.         // leads to
  1000.         // cT (dlV - dlE) = cE dlE + cX dex + cY dey
  1001.         // with
  1002.         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
  1003.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
  1004.         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  1005.         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1006.         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1007.         // which can be solved to find the differential of the true longitude
  1008.         // dlV = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
  1009.         final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
  1010.         final T cosLe     = scLe.cos();
  1011.         final T sinLe     = scLe.sin();
  1012.         final T eSinE     = ex.multiply(sinLe).subtract(ey.multiply(cosLe));
  1013.         final T ecosE     = ex.multiply(cosLe).add(ey.multiply(sinLe));
  1014.         final T e2        = ex.multiply(ex).add(ey.multiply(ey));
  1015.         final T epsilon   = one.subtract(e2).sqrt();
  1016.         final T onePeps   = epsilon.add(1);
  1017.         final T d         = onePeps.subtract(ecosE);
  1018.         final T cT        = d.multiply(d).add(eSinE.multiply(eSinE)).divide(2);
  1019.         final T cE        = ecosE.multiply(onePeps).subtract(e2);
  1020.         final T cX        = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinLe.multiply(onePeps));
  1021.         final T cY        = ey.multiply(eSinE).divide(epsilon).add( ex).subtract(cosLe.multiply(onePeps));
  1022.         final T factorLe  = cT.add(cE).divide(cT);
  1023.         final T factorEx  = cX.divide(cT);
  1024.         final T factorEy  = cY.divide(cT);

  1026.         // update longitude row
  1027.         final T[] rowEx = jacobian[1];
  1028.         final T[] rowEy = jacobian[2];
  1029.         final T[] rowL = jacobian[5];
  1030.         for (int j = 0; j < 6; ++j) {
  1031.             rowL[j] = factorLe.multiply(rowL[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
  1032.         }

  1034.         return jacobian;

  1036.     }

  1038.     /** {@inheritDoc} */
  1039.     public void addKeplerContribution(final PositionAngle type, final T gm,
  1040.                                       final T[] pDot) {
  1041.         final T oMe2;
  1042.         final T ksi;
  1043.         final T n               = gm.divide(a).sqrt().divide(a);
  1044.         final FieldSinCos<T> sc = FastMath.sinCos(lv);
  1045.         switch (type) {
  1046.             case MEAN :
  1047.                 pDot[5] = pDot[5].add(n);
  1048.                 break;
  1049.             case ECCENTRIC :
  1050.                 oMe2 = one.subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1051.                 ksi  = ex.multiply(sc.cos()).add(1).add(ey.multiply(sc.sin()));
  1052.                 pDot[5] = pDot[5].add(n.multiply(ksi).divide(oMe2));
  1053.                 break;
  1054.             case TRUE :
  1055.                 oMe2 = one.subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1056.                 ksi  =  ex.multiply(sc.cos()).add(1).add(ey.multiply(sc.sin()));
  1057.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1058.                 break;
  1059.             default :
  1060.                 throw new OrekitInternalError(null);
  1061.         }
  1062.     }

  1064.     /**  Returns a string representation of this equinoctial parameters object.
  1065.      * @return a string representation of this object
  1066.      */
  1067.     public String toString() {
  1068.         return new StringBuilder().append("equinoctial parameters: ").append('{').
  1069.                                   append("a: ").append(a.getReal()).
  1070.                                   append("; ex: ").append(ex.getReal()).append("; ey: ").append(ey.getReal()).
  1071.                                   append("; hx: ").append(hx.getReal()).append("; hy: ").append(hy.getReal()).
  1072.                                   append("; lv: ").append(FastMath.toDegrees(lv.getReal())).
  1073.                                   append(";}").toString();
  1074.     }

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

  1094. }
Created with JaCoCo 0.8.7.202105040129