FieldEquinoctialOrbit.java

  1. /* Copyright 2002-2018 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.HashMap;
  20. import java.util.List;
  21. import java.util.Map;
  22. import java.util.stream.Collectors;
  23. import java.util.stream.Stream;

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


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

  83.     /** Factory for first time derivatives. */
  84.     private static final Map<Field<? extends RealFieldElement<?>>, FDSFactory<? extends RealFieldElement<?>>> FACTORIES =
  85.                     new HashMap<>();

  87.     /** Semi-major axis (m). */
  88.     private final T a;

  90.     /** First component of the eccentricity vector. */
  91.     private final T ex;

  93.     /** Second component of the eccentricity vector. */
  94.     private final T ey;

  96.     /** First component of the inclination vector. */
  97.     private final T hx;

  99.     /** Second component of the inclination vector. */
  100.     private final T hy;

  102.     /** True longitude argument (rad). */
  103.     private final T lv;

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

  108.     /** First component of the eccentricity vector derivative. */
  109.     private final T exDot;

  111.     /** Second component of the eccentricity vector derivative. */
  112.     private final T eyDot;

  114.     /** First component of the inclination vector derivative. */
  115.     private final T hxDot;

  117.     /** Second component of the inclination vector derivative. */
  118.     private final T hyDot;

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

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

  126.     /** Field used by this class.*/
  127.     private Field<T> field;

  129.     /**Zero.*/
  130.     private T zero;

  132.     /**One.*/
  133.     private T one;

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

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

  197.         if (!FACTORIES.containsKey(a.getField())) {
  198.             FACTORIES.put(a.getField(), new FDSFactory<>(a.getField(), 1, 1));
  199.         }

  201.         this.a     = a;
  202.         this.aDot  = aDot;
  203.         this.ex    = ex;
  204.         this.exDot = exDot;
  205.         this.ey    = ey;
  206.         this.eyDot = eyDot;
  207.         this.hx    = hx;
  208.         this.hxDot = hxDot;
  209.         this.hy    = hy;
  210.         this.hyDot = hyDot;

  212.         if (hasDerivatives()) {
  213.             @SuppressWarnings("unchecked")
  214.             final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  215.             final FieldDerivativeStructure<T> exDS = factory.build(ex, exDot);
  216.             final FieldDerivativeStructure<T> eyDS = factory.build(ey, eyDot);
  217.             final FieldDerivativeStructure<T> lDS  = factory.build(l,  lDot);
  218.             final FieldDerivativeStructure<T> lvDS;
  219.             switch (type) {
  220.                 case MEAN :
  221.                     lvDS = eccentricToTrue(meanToEccentric(lDS, exDS, eyDS), exDS, eyDS);
  222.                     break;
  223.                 case ECCENTRIC :
  224.                     lvDS = eccentricToTrue(lDS, exDS, eyDS);
  225.                     break;
  226.                 case TRUE :
  227.                     lvDS = lDS;
  228.                     break;
  229.                 default : // this should never happen
  230.                     throw new OrekitInternalError(null);
  231.             }
  232.             this.lv    = lvDS.getValue();
  233.             this.lvDot = lvDS.getPartialDerivative(1);
  234.         } else {
  235.             switch (type) {
  236.                 case MEAN :
  237.                     this.lv = eccentricToTrue(meanToEccentric(l, ex, ey), ex, ey);
  238.                     break;
  239.                 case ECCENTRIC :
  240.                     this.lv = eccentricToTrue(l, ex, ey);
  241.                     break;
  242.                 case TRUE :
  243.                     this.lv = l;
  244.                     break;
  245.                 default :
  246.                     throw new OrekitInternalError(null);
  247.             }
  248.             this.lvDot = null;
  249.         }

  251.         this.partialPV = null;

  253.     }

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

  274.         field = pvCoordinates.getPosition().getX().getField();
  275.         zero = field.getZero();
  276.         one = field.getOne();

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

  287.         if (rV2OnMu.getReal() > 2) {
  288.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  289.                                                      getClass().getName());
  290.         }

  292.         // compute inclination vector
  293.         final FieldVector3D<T> w = pvCoordinates.getMomentum().normalize();
  294.         final T d = one.divide(one.add(w.getZ()));
  295.         hx =  d.negate().multiply(w.getY());
  296.         hy =  d.multiply(w.getX());

  298.         // compute true longitude argument
  299.         final T cLv = (pvP.getX().subtract(d.multiply(pvP.getZ()).multiply(w.getX()))).divide(r);
  300.         final T sLv = (pvP.getY().subtract(d.multiply(pvP.getZ()).multiply(w.getY()))).divide(r);
  301.         lv = sLv.atan2(cLv);


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

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

  316.         partialPV = pvCoordinates;

  318.         if (!FACTORIES.containsKey(a.getField())) {
  319.             FACTORIES.put(a.getField(), new FDSFactory<>(a.getField(), 1, 1));
  320.         }

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

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

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

  339.             // in order to compute true anomaly derivative, we must compute
  340.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  341.             final T lMDot = getKeplerianMeanMotion().
  342.                             add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  343.             @SuppressWarnings("unchecked")
  344.             final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  345.             final FieldDerivativeStructure<T> exDS = factory.build(ex, exDot);
  346.             final FieldDerivativeStructure<T> eyDS = factory.build(ey, eyDot);
  347.             final FieldDerivativeStructure<T> lMDS = factory.build(getLM(), lMDot);
  348.             final FieldDerivativeStructure<T> lvDS = eccentricToTrue(meanToEccentric(lMDS, exDS, eyDS), exDS, eyDS);
  349.             lvDot = lvDS.getPartialDerivative(1);

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

  363.     }

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

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

  392.         a     = op.getA();
  393.         ex    = op.getEquinoctialEx();
  394.         ey    = op.getEquinoctialEy();
  395.         hx    = op.getHx();
  396.         hy    = op.getHy();
  397.         lv    = op.getLv();

  399.         if (!FACTORIES.containsKey(a.getField())) {
  400.             FACTORIES.put(a.getField(), new FDSFactory<>(a.getField(), 1, 1));
  401.         }

  403.         aDot  = op.getADot();
  404.         exDot = op.getEquinoctialExDot();
  405.         eyDot = op.getEquinoctialEyDot();
  406.         hxDot = op.getHxDot();
  407.         hyDot = op.getHyDot();
  408.         lvDot = op.getLvDot();

  410.         field = a.getField();
  411.         zero  = field.getZero();
  412.         one   = field.getOne();
  413.     }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  492.         @SuppressWarnings("unchecked")
  493.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  494.         final FieldDerivativeStructure<T> lVDS = factory.build(lv, lvDot);
  495.         final FieldDerivativeStructure<T> exDS = factory.build(ex, exDot);
  496.         final FieldDerivativeStructure<T> eyDS = factory.build(ey, eyDot);
  497.         final FieldDerivativeStructure<T> lEDS = trueToEccentric(lVDS, exDS, eyDS);
  498.         return lEDS.getPartialDerivative(1);

  500.     }

  502.     /** {@inheritDoc} */
  503.     public T getLM() {
  504.         return eccentricToMean(trueToEccentric(lv, ex, ey), ex, ey);
  505.     }

  507.     /** {@inheritDoc} */
  508.     public T getLMDot() {

  510.         if (!hasDerivatives()) {
  511.             return null;
  512.         }

  514.         @SuppressWarnings("unchecked")
  515.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(a.getField());
  516.         final FieldDerivativeStructure<T> lVDS = factory.build(lv, lvDot);
  517.         final FieldDerivativeStructure<T> exDS = factory.build(ex, exDot);
  518.         final FieldDerivativeStructure<T> eyDS = factory.build(ey, eyDot);
  519.         final FieldDerivativeStructure<T> lMDS = eccentricToMean(trueToEccentric(lVDS, exDS, eyDS), exDS, eyDS);
  520.         return lMDS.getPartialDerivative(1);

  522.     }

  524.     /** Get the longitude argument.
  525.      * @param type type of the angle
  526.      * @return longitude argument (rad)
  527.      */
  528.     public T getL(final PositionAngle type) {
  529.         return (type == PositionAngle.MEAN) ? getLM() :
  530.                                               ((type == PositionAngle.ECCENTRIC) ? getLE() :
  531.                                                                                    getLv());
  532.     }

  534.     /** Get the longitude argument derivative.
  535.      * @param type type of the angle
  536.      * @return longitude argument derivative (rad/s)
  537.      */
  538.     public T getLDot(final PositionAngle type) {
  539.         return (type == PositionAngle.MEAN) ? getLMDot() :
  540.                                               ((type == PositionAngle.ECCENTRIC) ? getLEDot() :
  541.                                                                                    getLvDot());
  542.     }

  544.     /** {@inheritDoc} */
  545.     @Override
  546.     public boolean hasDerivatives() {
  547.         return aDot != null;
  548.     }

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

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

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

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

  607.             lEmlM = lEmlM.subtract(shift);
  608.             lE     = lM.add(lEmlM);
  609.             cosLE  = lE.cos();
  610.             sinLE  = lE.sin();

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

  614.         return lE;

  616.     }

  618.     /** Computes the mean longitude argument from the eccentric longitude argument.
  619.      * @param lE = E + ω + Ω mean longitude argument (rad)
  620.      * @param ex first component of the eccentricity vector
  621.      * @param ey second component of the eccentricity vector
  622.      * @param <T> Type of the field elements
  623.      * @return the mean longitude argument
  624.      */
  625.     public static <T extends RealFieldElement<T>> T eccentricToMean(final T lE, final T ex, final T ey) {
  626.         return lE.subtract(ex.multiply(lE.sin())).add(ey.multiply(lE.cos()));
  627.     }

  629.     /** Compute position from equinoctial parameters.
  630.      * @param a  semi-major axis (m)
  631.      * @param ex e cos(ω + Ω), first component of eccentricity vector
  632.      * @param ey e sin(ω + Ω), second component of eccentricity vector
  633.      * @param hx tan(i/2) cos(Ω), first component of inclination vector
  634.      * @param hy tan(i/2) sin(Ω), second component of inclination vector
  635.      * @param lv  v + ω + Ω true longitude argument (rad)
  636.      * @param mu central attraction coefficient (m³/s²)
  637.      * @param <T> type of the fiels elements
  638.      * @return position vector
  639.      */
  640.     public static <T extends RealFieldElement<T>> FieldVector3D<T> equinoctialToPosition(final T a, final T ex, final T ey,
  641.                                                                                          final T hx, final T hy, final T lv,
  642.                                                                                          final double mu) {

  644.         final T one  = a.getField().getOne();

  646.         // eccentric longitude argument
  647.         final T lE = trueToEccentric(lv, ex, ey);

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

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

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

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

  671.         // eccentric longitude argument
  672.         final T cLe    = lE.cos();
  673.         final T sLe    = lE.sin();

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

  679.         return new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  680.                                    x.multiply(uy).add(y.multiply(vy)),
  681.                                    x.multiply(uz).add(y.multiply(vz)));

  683.     }

  685.     /** {@inheritDoc} */
  686.     public T getE() {
  687.         return ex.multiply(ex).add(ey.multiply(ey)).sqrt();
  688.     }

  690.     /** {@inheritDoc} */
  691.     public T getEDot() {

  693.         if (!hasDerivatives()) {
  694.             return null;
  695.         }

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

  699.     }

  701.     /** {@inheritDoc} */
  702.     public T getI() {
  703.         return hx.multiply(hx).add(hy.multiply(hy)).sqrt().atan().multiply(2);
  704.     }

  706.     /** {@inheritDoc} */
  707.     public T getIDot() {

  709.         if (!hasDerivatives()) {
  710.             return null;
  711.         }

  713.         final T h2 = hx.multiply(hx).add(hy.multiply(hy));
  714.         final T h  = h2.sqrt();
  715.         return hx.multiply(hxDot).add(hy.multiply(hyDot)).multiply(2).divide(h.multiply(h2.add(1)));

  717.     }

  719.     /** Compute position and velocity but not acceleration.
  720.      */
  721.     private void computePVWithoutA() {

  723.         if (partialPV != null) {
  724.             // already computed
  725.             return;
  726.         }

  728.         // get equinoctial parameters
  729.         final T lE = getLE();

  731.         // inclination-related intermediate parameters
  732.         final T hx2   = hx.multiply(hx);
  733.         final T hy2   = hy.multiply(hy);
  734.         final T factH = one.divide(hx2.add(1.0).add(hy2));

  736.         // reference axes defining the orbital plane
  737.         final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
  738.         final T uy = hx.multiply(hy).multiply(factH).multiply(2);
  739.         final T uz = hy.multiply(-2).multiply(factH);

  741.         final T vx = uy;
  742.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  743.         final T vz =  hx.multiply(factH).multiply(2);

  745.         // eccentricity-related intermediate parameters
  746.         final T ex2  = ex.multiply(ex);
  747.         final T exey = ex.multiply(ey);
  748.         final T ey2  = ey.multiply(ey);
  749.         final T e2   = ex2.add(ey2);
  750.         final T eta  = one.subtract(e2).sqrt().add(1);
  751.         final T beta = one.divide(eta);

  753.         // eccentric longitude argument
  754.         final T cLe    = lE.cos();
  755.         final T sLe    = lE.sin();
  756.         final T exCeyS = ex.multiply(cLe).add(ey.multiply(sLe));

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

  762.         final T factor = zero.add(getMu()).divide(a).sqrt().divide(one.subtract(exCeyS));
  763.         final T xdot   = factor.multiply(sLe.negate().add(beta.multiply(ey).multiply(exCeyS)));
  764.         final T ydot   = factor.multiply(cLe.subtract(beta.multiply(ex).multiply(exCeyS)));

  766.         final FieldVector3D<T> position =
  767.                         new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  768.                                             x.multiply(uy).add(y.multiply(vy)),
  769.                                             x.multiply(uz).add(y.multiply(vz)));
  770.         final FieldVector3D<T> velocity =
  771.                         new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)), xdot.multiply(uy).add(ydot.multiply(vy)), xdot.multiply(uz).add(ydot.multiply(vz)));

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

  775.     }

  777.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  778.      * <p>
  779.      * This method should be called only when {@link #hasDerivatives()} returns true.
  780.      * </p>
  781.      * @return non-Keplerian part of the acceleration
  782.      */
  783.     private FieldVector3D<T> nonKeplerianAcceleration() {

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

  788.         final T nonKeplerianMeanMotion = getLMDot().subtract(getKeplerianMeanMotion());
  789.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  790.                                  add(dCdP[3][1].multiply(exDot)).
  791.                                  add(dCdP[3][2].multiply(eyDot)).
  792.                                  add(dCdP[3][3].multiply(hxDot)).
  793.                                  add(dCdP[3][4].multiply(hyDot)).
  794.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  795.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  796.                                  add(dCdP[4][1].multiply(exDot)).
  797.                                  add(dCdP[4][2].multiply(eyDot)).
  798.                                  add(dCdP[4][3].multiply(hxDot)).
  799.                                  add(dCdP[4][4].multiply(hyDot)).
  800.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  801.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  802.                                  add(dCdP[5][1].multiply(exDot)).
  803.                                  add(dCdP[5][2].multiply(eyDot)).
  804.                                  add(dCdP[5][3].multiply(hxDot)).
  805.                                  add(dCdP[5][4].multiply(hyDot)).
  806.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  810.     }

  812.     /** {@inheritDoc} */
  813.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  815.         // position and velocity
  816.         computePVWithoutA();

  818.         // acceleration
  819.         final T r2 = partialPV.getPosition().getNormSq();
  820.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(-getMu()),
  821.                                                                            partialPV.getPosition());
  822.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  823.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  824.                                               keplerianAcceleration;

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

  828.     }

  830.     /** {@inheritDoc} */
  831.     public FieldEquinoctialOrbit<T> shiftedBy(final double dt) {
  832.         return shiftedBy(getDate().getField().getZero().add(dt));
  833.     }

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

  838.         // use Keplerian-only motion
  839.         final FieldEquinoctialOrbit<T> keplerianShifted = new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy,
  840.                                                                                       getLM().add(getKeplerianMeanMotion().multiply(dt)),
  841.                                                                                       PositionAngle.MEAN, getFrame(),
  842.                                                                                       getDate().shiftedBy(dt), getMu());

  844.         if (hasDerivatives()) {

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

  849.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  850.             keplerianShifted.computePVWithoutA();
  851.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, keplerianShifted.partialPV.getPosition(),
  852.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  853.             final T   fixedR2 = fixedP.getNormSq();
  854.             final T   fixedR  = fixedR2.sqrt();
  855.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, keplerianShifted.partialPV.getVelocity(),
  856.                                                                  dt, nonKeplerianAcceleration);
  857.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(-getMu()),
  858.                                                                  keplerianShifted.partialPV.getPosition(),
  859.                                                                  one, nonKeplerianAcceleration);

  861.             // build a new orbit, taking non-Keplerian acceleration into account
  862.             return new FieldEquinoctialOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  863.                                                                                    fixedP, fixedV, fixedA),
  864.                                                keplerianShifted.getFrame(), keplerianShifted.getMu());

  866.         } else {
  867.             // Keplerian-only motion is all we can do
  868.             return keplerianShifted;
  869.         }

  871.     }

  873.     /** {@inheritDoc}
  874.      * <p>
  875.      * The interpolated instance is created by polynomial Hermite interpolation
  876.      * on equinoctial elements, without derivatives (which means the interpolation
  877.      * falls back to Lagrange interpolation only).
  878.      * </p>
  879.      * <p>
  880.      * As this implementation of interpolation is polynomial, it should be used only
  881.      * with small samples (about 10-20 points) in order to avoid <a
  882.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  883.      * and numerical problems (including NaN appearing).
  884.      * </p>
  885.      * <p>
  886.      * If orbit interpolation on large samples is needed, using the {@link
  887.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  888.      * low-level interpolation. The Ephemeris class automatically handles selection of
  889.      * a neighboring sub-sample with a predefined number of point from a large global sample
  890.      * in a thread-safe way.
  891.      * </p>
  892.      */
  893.     public FieldEquinoctialOrbit<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldOrbit<T>> sample) {

  895.         // first pass to check if derivatives are available throughout the sample
  896.         final List<FieldOrbit<T>> list = sample.collect(Collectors.toList());
  897.         boolean useDerivatives = true;
  898.         for (final FieldOrbit<T> orbit : list) {
  899.             useDerivatives = useDerivatives && orbit.hasDerivatives();
  900.         }

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

  905.         // second pass to feed interpolator
  906.         FieldAbsoluteDate<T> previousDate = null;
  907.         T                    previousLm   = zero.add(Double.NaN);
  908.         for (final FieldOrbit<T> orbit : list) {
  909.             final FieldEquinoctialOrbit<T> equi = (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.convertType(orbit);
  910.             final T continuousLm;
  911.             if (previousDate == null) {
  912.                 continuousLm = (T) equi.getLM();
  913.             } else {
  914.                 final T dt       = (T) equi.getDate().durationFrom(previousDate);
  915.                 final T keplerLm = previousLm.add((T) equi.getKeplerianMeanMotion().multiply(dt));
  916.                 continuousLm = normalizeAngle((T) equi.getLM(), keplerLm);
  917.             }
  918.             previousDate = equi.getDate();
  919.             previousLm   = continuousLm;
  920.             final T[] toAdd = MathArrays.buildArray(field, 6);
  921.             toAdd[0] = (T) equi.getA();
  922.             toAdd[1] = (T) equi.getEquinoctialEx();
  923.             toAdd[2] = (T) equi.getEquinoctialEy();
  924.             toAdd[3] = (T) equi.getHx();
  925.             toAdd[4] = (T) equi.getHy();
  926.             toAdd[5] = (T) continuousLm;
  927.             if (useDerivatives) {
  928.                 final T[] toAddDot = MathArrays.buildArray(one.getField(), 6);
  929.                 toAddDot[0] = equi.getADot();
  930.                 toAddDot[1] = equi.getEquinoctialExDot();
  931.                 toAddDot[2] = equi.getEquinoctialEyDot();
  932.                 toAddDot[3] = equi.getHxDot();
  933.                 toAddDot[4] = equi.getHyDot();
  934.                 toAddDot[5] = equi.getLMDot();
  935.                 interpolator.addSamplePoint(equi.getDate().durationFrom(date),
  936.                                             toAdd, toAddDot);
  937.             } else {
  938.                 interpolator.addSamplePoint((T) equi.getDate().durationFrom(date),
  939.                                             toAdd);
  940.             }
  941.         }

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

  946.         // build a new interpolated instance
  947.         return new FieldEquinoctialOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  948.                                            interpolated[0][3], interpolated[0][4], interpolated[0][5],
  949.                                            interpolated[1][0], interpolated[1][1], interpolated[1][2],
  950.                                            interpolated[1][3], interpolated[1][4], interpolated[1][5],
  951.                                            PositionAngle.MEAN, getFrame(), date, getMu());

  953.     }

  955.     /** {@inheritDoc} */
  956.     protected T[][] computeJacobianMeanWrtCartesian() {

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

  960.         // compute various intermediate parameters
  961.         computePVWithoutA();
  962.         final FieldVector3D<T> position = partialPV.getPosition();
  963.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  964.         final T r2         = position.getNormSq();
  965.         final T r          = r2.sqrt();
  966.         final T r3         = r.multiply(r2);

  968.         final double mu         = getMu();
  969.         final T sqrtMuA    = a.multiply(mu).sqrt();
  970.         final T a2         = a.multiply(a);

  972.         final T e2         = ex.multiply(ex).add(ey.multiply(ey));
  973.         final T oMe2       = one.subtract(e2);
  974.         final T epsilon    = oMe2.sqrt();
  975.         final T beta       = one.divide(epsilon.add(1));
  976.         final T ratio      = epsilon.multiply(beta);

  978.         final T hx2        = hx.multiply(hx);
  979.         final T hy2        = hy.multiply(hy);
  980.         final T hxhy       = hx.multiply(hy);

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

  987.         // coordinates of the spacecraft in the equinoctial frame
  988.         final T x    = FieldVector3D.dotProduct(position, f);
  989.         final T y    = FieldVector3D.dotProduct(position, g);
  990.         final T xDot = FieldVector3D.dotProduct(velocity, f);
  991.         final T yDot = FieldVector3D.dotProduct(velocity, g);

  993.         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
  994.         final T c1  = a.divide(sqrtMuA.multiply(epsilon));
  995.         final T c1N = c1.negate();
  996.         final T c2  = a.multiply(sqrtMuA).multiply(beta).divide(r3);
  997.         final T c3  = sqrtMuA.divide(r3.multiply(epsilon));
  998.         final FieldVector3D<T> drDotSdEx = new FieldVector3D<>(c1.multiply(xDot).multiply(yDot).subtract(c2.multiply(ey).multiply(x)).subtract(c3.multiply(x).multiply(y)), f,
  999.                                                                c1N.multiply(xDot).multiply(xDot).subtract(c2.multiply(ey).multiply(y)).add(c3.multiply(x).multiply(x)), g);

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

  1005.         // da
  1006.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  1007.         final FieldVector3D<T> vectorARDot = new FieldVector3D<>(a2.multiply(2).divide(mu), velocity);
  1008.         fillHalfRow(one, vectorAR,    jacobian[0], 0);
  1009.         fillHalfRow(one, vectorARDot, jacobian[0], 3);

  1011.         // dEx
  1012.         final T d1 = a.negate().multiply(ratio).divide(r3);
  1013.         final T d2 = (hy.multiply(xDot).subtract(hx.multiply(yDot))).divide(sqrtMuA.multiply(epsilon));
  1014.         final T d3 = hx.multiply(y).subtract(hy.multiply(x)).divide(sqrtMuA);
  1015.         final FieldVector3D<T> vectorExRDot =
  1016.             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);
  1017.         fillHalfRow(ex.multiply(d1), position, ey.negate().multiply(d2), w, epsilon.divide(sqrtMuA), drDotSdEy, jacobian[1], 0);
  1018.         fillHalfRow(one, vectorExRDot, jacobian[1], 3);

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

  1026.         // dHx
  1027.         final T h = (hx2.add(1).add(hy2)).divide(sqrtMuA.multiply(2).multiply(epsilon));
  1028.         fillHalfRow( h.negate().multiply(xDot), w, jacobian[3], 0);
  1029.         fillHalfRow( h.multiply(x),    w, jacobian[3], 3);

  1031.        // dHy
  1032.         fillHalfRow( h.negate().multiply(yDot), w, jacobian[4], 0);
  1033.         fillHalfRow( h.multiply(y),    w, jacobian[4], 3);

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

  1040.         return jacobian;

  1042.     }

  1044.     /** {@inheritDoc} */
  1045.     protected T[][] computeJacobianEccentricWrtCartesian() {

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

  1050.         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
  1051.         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
  1052.         // which is inverted and rewritten as:
  1053.         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
  1054.         final T le    = getLE();
  1055.         final T cosLe = le.cos();
  1056.         final T sinLe = le.sin();
  1057.         final T aOr   = one.divide(one.subtract(ex.multiply(cosLe)).subtract(ey.multiply(sinLe)));

  1059.         // update longitude row
  1060.         final T[] rowEx = jacobian[1];
  1061.         final T[] rowEy = jacobian[2];
  1062.         final T[] rowL  = jacobian[5];

  1064.         for (int j = 0; j < 6; ++j) {
  1065.             rowL[j] = aOr.multiply(rowL[j].add(sinLe.multiply(rowEx[j])).subtract(cosLe.multiply(rowEy[j])));
  1066.         }
  1067.         return jacobian;

  1069.     }

  1071.     /** {@inheritDoc} */
  1072.     protected T[][] computeJacobianTrueWrtCartesian() {

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

  1077.         // Differentiating the eccentric longitude equation
  1078.         // tan((lV - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
  1079.         // leads to
  1080.         // cT (dlV - dlE) = cE dlE + cX dex + cY dey
  1081.         // with
  1082.         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
  1083.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
  1084.         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  1085.         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1086.         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  1087.         // which can be solved to find the differential of the true longitude
  1088.         // dlV = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
  1089.         final T le        = getLE();
  1090.         final T cosLe     = le.cos();
  1091.         final T sinLe     = le.sin();
  1092.         final T eSinE     = ex.multiply(sinLe).subtract(ey.multiply(cosLe));
  1093.         final T ecosE     = ex.multiply(cosLe).add(ey.multiply(sinLe));
  1094.         final T e2        = ex.multiply(ex).add(ey.multiply(ey));
  1095.         final T epsilon   = one.subtract(e2).sqrt();
  1096.         final T onePeps   = epsilon.add(1);
  1097.         final T d         = onePeps.subtract(ecosE);
  1098.         final T cT        = d.multiply(d).add(eSinE.multiply(eSinE)).divide(2);
  1099.         final T cE        = ecosE.multiply(onePeps).subtract(e2);
  1100.         final T cX        = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinLe.multiply(onePeps));
  1101.         final T cY        = ey.multiply(eSinE).divide(epsilon).add( ex).subtract(cosLe.multiply(onePeps));
  1102.         final T factorLe  = cT.add(cE).divide(cT);
  1103.         final T factorEx  = cX.divide(cT);
  1104.         final T factorEy  = cY.divide(cT);

  1106.         // update longitude row
  1107.         final T[] rowEx = jacobian[1];
  1108.         final T[] rowEy = jacobian[2];
  1109.         final T[] rowL = jacobian[5];
  1110.         for (int j = 0; j < 6; ++j) {
  1111.             rowL[j] = factorLe.multiply(rowL[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
  1112.         }

  1114.         return jacobian;

  1116.     }

  1118.     /** {@inheritDoc} */
  1119.     public void addKeplerContribution(final PositionAngle type, final double gm,
  1120.                                       final T[] pDot) {
  1121.         final T oMe2;
  1122.         final T ksi;
  1123.         final T n = zero.add(gm).divide(a).sqrt().divide(a);
  1124.         switch (type) {
  1125.             case MEAN :
  1126.                 pDot[5] = pDot[5].add(n);
  1127.                 break;
  1128.             case ECCENTRIC :
  1129.                 oMe2 = one.subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1130.                 ksi  = ex.multiply(lv.cos()).add(1).add(ey.multiply(lv.sin()));
  1131.                 pDot[5] = pDot[5].add(n.multiply(ksi).divide(oMe2));
  1132.                 break;
  1133.             case TRUE :
  1134.                 oMe2 = one.subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1135.                 ksi  =  ex.multiply(lv.cos()).add(1).add(ey.multiply(lv.sin()));
  1136.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1137.                 break;
  1138.             default :
  1139.                 throw new OrekitInternalError(null);
  1140.         }
  1141.     }

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

  1155.     /**
  1156.      * Normalize an angle in a 2&pi; wide interval around a center value.
  1157.      * <p>This method has three main uses:</p>
  1158.      * <ul>
  1159.      *   <li>normalize an angle between 0 and 2&pi;:<br/>
  1160.      *       {@code a = MathUtils.normalizeAngle(a, FastMath.PI);}</li>
  1161.      *   <li>normalize an angle between -&pi; and +&pi;<br/>
  1162.      *       {@code a = MathUtils.normalizeAngle(a, 0.0);}</li>
  1163.      *   <li>compute the angle between two defining angular positions:<br>
  1164.      *       {@code angle = MathUtils.normalizeAngle(end, start) - start;}</li>
  1165.      * </ul>
  1166.      * <p>Note that due to numerical accuracy and since &pi; cannot be represented
  1167.      * exactly, the result interval is <em>closed</em>, it cannot be half-closed
  1168.      * as would be more satisfactory in a purely mathematical view.</p>
  1169.      * @param a angle to normalize
  1170.      * @param center center of the desired 2&pi; interval for the result
  1171.      * @param <T> the type of the field elements
  1172.      * @return a-2k&pi; with integer k and center-&pi; &lt;= a-2k&pi; &lt;= center+&pi;
  1173.      */
  1174.     public static <T extends RealFieldElement<T>> T normalizeAngle(final T a, final T center) {
  1175.         return a.subtract(2 * FastMath.PI * FastMath.floor((a.getReal() + FastMath.PI - center.getReal()) / (2 * FastMath.PI)));
  1176.     }

  1178.     @Override
  1179.     public EquinoctialOrbit toOrbit() {
  1180.         if (hasDerivatives()) {
  1181.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1182.                                         hx.getReal(), hy.getReal(), lv.getReal(),
  1183.                                         aDot.getReal(), exDot.getReal(), eyDot.getReal(),
  1184.                                         hxDot.getReal(), hyDot.getReal(), lvDot.getReal(),
  1185.                                         PositionAngle.TRUE, getFrame(),
  1186.                                         getDate().toAbsoluteDate(), getMu());
  1187.         } else {
  1188.             return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1189.                                         hx.getReal(), hy.getReal(), lv.getReal(),
  1190.                                         PositionAngle.TRUE, getFrame(),
  1191.                                         getDate().toAbsoluteDate(), getMu());
  1192.         }
  1193.     }

  1195. }
Created with JaCoCo 0.7.9.201702052155