FieldCircularOrbit.java

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

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


  35. /**
  36.  * This class handles circular orbital parameters.

  38.  * <p>
  39.  * The parameters used internally are the circular elements which can be
  40.  * related to Keplerian elements as follows:
  41.  *   <ul>
  42.  *     <li>a</li>
  43.  *     <li>e<sub>x</sub> = e cos(ω)</li>
  44.  *     <li>e<sub>y</sub> = e sin(ω)</li>
  45.  *     <li>i</li>
  46.  *     <li>Ω</li>
  47.  *     <li>α<sub>v</sub> = v + ω</li>
  48.  *   </ul>
  49.  * where Ω stands for the Right Ascension of the Ascending Node and
  50.  * α<sub>v</sub> stands for the true latitude argument
  51.  * <p>
  52.  * The conversion equations from and to Keplerian elements given above hold only
  53.  * when both sides are unambiguously defined, i.e. when orbit is neither equatorial
  54.  * nor circular. When orbit is circular (but not equatorial), the circular
  55.  * parameters are still unambiguously defined whereas some Keplerian elements
  56.  * (more precisely ω and Ω) become ambiguous. When orbit is equatorial,
  57.  * neither the Keplerian nor the circular parameters can be defined unambiguously.
  58.  * {@link EquinoctialOrbit equinoctial orbits} is the recommended way to represent
  59.  * orbits.
  60.  * </p>
  61.  * <p>
  62.  * The instance <code>CircularOrbit</code> is guaranteed to be immutable.
  63.  * </p>
  64.  * @see    Orbit
  65.  * @see    KeplerianOrbit
  66.  * @see    CartesianOrbit
  67.  * @see    EquinoctialOrbit
  68.  * @author Luc Maisonobe
  69.  * @author Fabien Maussion
  70.  * @author V&eacute;ronique Pommier-Maurussane
  71.  * @since 9.0
  72.  * @param <T> type of the field elements
  73.  */

  75. public class FieldCircularOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T> implements PositionAngleBased {

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

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

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

  86.     /** Inclination (rad). */
  87.     private final T i;

  89.     /** Right Ascension of Ascending Node (rad). */
  90.     private final T raan;

  92.     /** True latitude argument (rad). */
  93.     private final T alphaV;

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

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

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

  104.     /** Inclination derivative (rad/s). */
  105.     private final T iDot;

  107.     /** Right Ascension of Ascending Node derivative (rad/s). */
  108.     private final T raanDot;

  110.     /** True latitude argument derivative (rad/s). */
  111.     private final T alphaVDot;

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

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

  141.     /** Creates a new instance.
  142.      * @param a  semi-major axis (m)
  143.      * @param ex e cos(ω), first component of circular eccentricity vector
  144.      * @param ey e sin(ω), second component of circular eccentricity vector
  145.      * @param i inclination (rad)
  146.      * @param raan right ascension of ascending node (Ω, rad)
  147.      * @param alpha  an + ω, mean, eccentric or true latitude argument (rad)
  148.      * @param aDot  semi-major axis derivative (m/s)
  149.      * @param exDot d(e cos(ω))/dt, first component of circular eccentricity vector derivative
  150.      * @param eyDot d(e sin(ω))/dt, second component of circular eccentricity vector derivative
  151.      * @param iDot inclination  derivative(rad/s)
  152.      * @param raanDot right ascension of ascending node derivative (rad/s)
  153.      * @param alphaDot  d(an + ω), mean, eccentric or true latitude argument derivative (rad/s)
  154.      * @param type type of latitude argument
  155.      * @param frame the frame in which are defined the parameters
  156.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  157.      * @param date date of the orbital parameters
  158.      * @param mu central attraction coefficient (m³/s²)
  159.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  160.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  161.      */
  162.     public FieldCircularOrbit(final T a, final T ex, final T ey,
  163.                               final T i, final T raan, final T alpha,
  164.                               final T aDot, final T exDot, final T eyDot,
  165.                               final T iDot, final T raanDot, final T alphaDot,
  166.                               final PositionAngleType type,
  167.                               final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  168.         throws IllegalArgumentException {
  169.         super(frame, date, mu);
  170.         if (ex.getReal() * ex.getReal() + ey.getReal() * ey.getReal() >= 1.0) {
  171.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  172.                                                      getClass().getName());
  173.         }

  175.         this.a       =  a;
  176.         this.aDot    =  aDot;
  177.         this.ex      = ex;
  178.         this.exDot   = exDot;
  179.         this.ey      = ey;
  180.         this.eyDot   = eyDot;
  181.         this.i       = i;
  182.         this.iDot    = iDot;
  183.         this.raan    = raan;
  184.         this.raanDot = raanDot;

  186.         if (hasDerivatives()) {
  187.             final FieldUnivariateDerivative1<T> exUD    = new FieldUnivariateDerivative1<>(ex,    exDot);
  188.             final FieldUnivariateDerivative1<T> eyUD    = new FieldUnivariateDerivative1<>(ey,    eyDot);
  189.             final FieldUnivariateDerivative1<T> alphaUD = new FieldUnivariateDerivative1<>(alpha, alphaDot);
  190.             final FieldUnivariateDerivative1<T> alphavUD;
  191.             switch (type) {
  192.                 case MEAN :
  193.                     alphavUD = eccentricToTrue(meanToEccentric(alphaUD, exUD, eyUD), exUD, eyUD);
  194.                     break;
  195.                 case ECCENTRIC :
  196.                     alphavUD = eccentricToTrue(alphaUD, exUD, eyUD);
  197.                     break;
  198.                 case TRUE :
  199.                     alphavUD = alphaUD;
  200.                     break;
  201.                 default :
  202.                     throw new OrekitInternalError(null);
  203.             }
  204.             this.alphaV    = alphavUD.getValue();
  205.             this.alphaVDot = alphavUD.getDerivative(1);
  206.         } else {
  207.             switch (type) {
  208.                 case MEAN :
  209.                     this.alphaV = eccentricToTrue(meanToEccentric(alpha, ex, ey), ex, ey);
  210.                     break;
  211.                 case ECCENTRIC :
  212.                     this.alphaV = eccentricToTrue(alpha, ex, ey);
  213.                     break;
  214.                 case TRUE :
  215.                     this.alphaV = alpha;
  216.                     break;
  217.                 default :
  218.                     throw new OrekitInternalError(null);
  219.             }
  220.             this.alphaVDot = null;
  221.         }

  223.         this.partialPV = null;

  225.     }

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

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

  255.         a = r.divide(rV2OnMu.negate().add(2));

  257.         if (!isElliptical()) {
  258.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  259.                                                      getClass().getName());
  260.         }

  262.         // compute inclination
  263.         final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
  264.         final FieldVector3D<T> plusK = FieldVector3D.getPlusK(r.getField());
  265.         i = FieldVector3D.angle(momentum, plusK);

  267.         // compute right ascension of ascending node
  268.         final FieldVector3D<T> node  = FieldVector3D.crossProduct(plusK, momentum);
  269.         raan = node.getY().atan2(node.getX());

  271.         // 2D-coordinates in the canonical frame
  272.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  273.         final FieldSinCos<T> scI    = FastMath.sinCos(i);
  274.         final T xP      = pvP.getX();
  275.         final T yP      = pvP.getY();
  276.         final T zP      = pvP.getZ();
  277.         final T x2      = (xP.multiply(scRaan.cos()).add(yP .multiply(scRaan.sin()))).divide(a);
  278.         final T y2      = (yP.multiply(scRaan.cos()).subtract(xP.multiply(scRaan.sin()))).multiply(scI.cos()).add(zP.multiply(scI.sin())).divide(a);

  280.         // compute eccentricity vector
  281.         final T eSE    = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(mu).sqrt());
  282.         final T eCE    = rV2OnMu.subtract(1);
  283.         final T e2     = eCE.multiply(eCE).add(eSE.multiply(eSE));
  284.         final T f      = eCE.subtract(e2);
  285.         final T g      = eSE.multiply(e2.negate().add(1).sqrt());
  286.         final T aOnR   = a.divide(r);
  287.         final T a2OnR2 = aOnR.multiply(aOnR);
  288.         ex = a2OnR2.multiply(f.multiply(x2).add(g.multiply(y2)));
  289.         ey = a2OnR2.multiply(f.multiply(y2).subtract(g.multiply(x2)));

  291.         // compute latitude argument
  292.         final T beta = (ex.multiply(ex).add(ey.multiply(ey)).negate().add(1)).sqrt().add(1).reciprocal();
  293.         alphaV = eccentricToTrue(y2.add(ey).add(eSE.multiply(beta).multiply(ex)).atan2(x2.add(ex).subtract(eSE.multiply(beta).multiply(ey))),
  294.                                  ex, ey);

  296.         partialPV = pvCoordinates;

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

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

  304.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  305.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  306.             final T   aX                       = nonKeplerianAcceleration.getX();
  307.             final T   aY                       = nonKeplerianAcceleration.getY();
  308.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  309.             aDot    = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  310.             exDot   = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  311.             eyDot   = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  312.             iDot    = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  313.             raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  315.             // in order to compute true anomaly derivative, we must compute
  316.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  317.             final T alphaMDot = getKeplerianMeanMotion().
  318.                                 add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  319.             final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex, exDot);
  320.             final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey, eyDot);
  321.             final FieldUnivariateDerivative1<T> alphaMUD = new FieldUnivariateDerivative1<>(getAlphaM(), alphaMDot);
  322.             final FieldUnivariateDerivative1<T> alphavUD = eccentricToTrue(meanToEccentric(alphaMUD, exUD, eyUD), exUD, eyUD);
  323.             alphaVDot = alphavUD.getDerivative(1);

  325.         } else {
  326.             // acceleration is either almost zero or NaN,
  327.             // we assume acceleration was not known
  328.             // we don't set up derivatives
  329.             aDot      = null;
  330.             exDot     = null;
  331.             eyDot     = null;
  332.             iDot      = null;
  333.             raanDot   = null;
  334.             alphaVDot = null;
  335.         }

  337.     }

  339.     /** Constructor from Cartesian parameters.
  340.      *
  341.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  342.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  343.      * use {@code mu} and the position to compute the acceleration, including
  344.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  345.      *
  346.      * @param PVCoordinates the {@link FieldPVCoordinates} in inertial frame
  347.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  348.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  349.      * @param date date of the orbital parameters
  350.      * @param mu central attraction coefficient (m³/s²)
  351.      * @exception IllegalArgumentException if frame is not a {@link
  352.      * Frame#isPseudoInertial pseudo-inertial frame}
  353.      */
  354.     public FieldCircularOrbit(final FieldPVCoordinates<T> PVCoordinates, final Frame frame,
  355.                               final FieldAbsoluteDate<T> date, final T mu)
  356.         throws IllegalArgumentException {
  357.         this(new TimeStampedFieldPVCoordinates<>(date, PVCoordinates), frame, mu);
  358.     }

  360.     /** Constructor from any kind of orbital parameters.
  361.      * @param op orbital parameters to copy
  362.      */
  363.     public FieldCircularOrbit(final FieldOrbit<T> op) {
  364.         super(op.getFrame(), op.getDate(), op.getMu());
  365.         a    = op.getA();
  366.         i    = op.getI();
  367.         final T hx = op.getHx();
  368.         final T hy = op.getHy();
  369.         final T h2 = hx.multiply(hx).add(hy.multiply(hy));
  370.         final T h  = h2.sqrt();
  371.         raan = hy.atan2(hx);
  372.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  373.         final T cosRaan = h.getReal() == 0 ? scRaan.cos() : hx.divide(h);
  374.         final T sinRaan = h.getReal() == 0 ? scRaan.sin() : hy.divide(h);
  375.         final T equiEx = op.getEquinoctialEx();
  376.         final T equiEy = op.getEquinoctialEy();
  377.         ex   = equiEx.multiply(cosRaan).add(equiEy.multiply(sinRaan));
  378.         ey   = equiEy.multiply(cosRaan).subtract(equiEx.multiply(sinRaan));
  379.         alphaV = op.getLv().subtract(raan);

  381.         if (op.hasDerivatives()) {
  382.             aDot      = op.getADot();
  383.             final T      hxDot = op.getHxDot();
  384.             final T      hyDot = op.getHyDot();
  385.             iDot      = cosRaan.multiply(hxDot).add(sinRaan.multiply(hyDot)).multiply(2).divide(h2.add(1));
  386.             raanDot   = hx.multiply(hyDot).subtract(hy.multiply(hxDot)).divide(h2);
  387.             final T equiExDot = op.getEquinoctialExDot();
  388.             final T equiEyDot = op.getEquinoctialEyDot();
  389.             exDot     = equiExDot.add(equiEy.multiply(raanDot)).multiply(cosRaan).
  390.                         add(equiEyDot.subtract(equiEx.multiply(raanDot)).multiply(sinRaan));
  391.             eyDot     = equiEyDot.subtract(equiEx.multiply(raanDot)).multiply(cosRaan).
  392.                         subtract(equiExDot.add(equiEy.multiply(raanDot)).multiply(sinRaan));
  393.             alphaVDot = op.getLvDot().subtract(raanDot);
  394.         } else {
  395.             aDot      = null;
  396.             exDot     = null;
  397.             eyDot     = null;
  398.             iDot      = null;
  399.             raanDot   = null;
  400.             alphaVDot = null;
  401.         }

  403.         partialPV = null;

  405.     }

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

  416.         a    = getZero().add(op.getA());
  417.         i    = getZero().add(op.getI());
  418.         raan = getZero().add(op.getRightAscensionOfAscendingNode());
  419.         ex   = getZero().add(op.getCircularEx());
  420.         ey   = getZero().add(op.getCircularEy());
  421.         alphaV = getZero().add(op.getAlphaV());

  423.         if (op.hasDerivatives()) {
  424.             aDot      = getZero().add(op.getADot());
  425.             iDot      = getZero().add(op.getIDot());
  426.             raanDot   = getZero().add(op.getRightAscensionOfAscendingNodeDot());
  427.             exDot     = getZero().add(op.getCircularExDot());
  428.             eyDot     = getZero().add(op.getCircularEyDot());
  429.             alphaVDot = getZero().add(op.getAlphaVDot());
  430.         } else {
  431.             aDot      = null;
  432.             exDot     = null;
  433.             eyDot     = null;
  434.             iDot      = null;
  435.             raanDot   = null;
  436.             alphaVDot = null;
  437.         }

  439.         partialPV = null;

  441.     }

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

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

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

  463.     /** {@inheritDoc} */
  464.     public T getADot() {
  465.         return aDot;
  466.     }

  468.     /** {@inheritDoc} */
  469.     public T getEquinoctialEx() {
  470.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  471.         return ex.multiply(sc.cos()).subtract(ey.multiply(sc.sin()));
  472.     }

  474.     /** {@inheritDoc} */
  475.     public T getEquinoctialExDot() {

  477.         if (!hasDerivatives()) {
  478.             return null;
  479.         }

  481.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  482.         return exDot.subtract(ey.multiply(raanDot)).multiply(sc.cos()).
  483.                subtract(eyDot.add(ex.multiply(raanDot)).multiply(sc.sin()));

  485.     }

  487.     /** {@inheritDoc} */
  488.     public T getEquinoctialEy() {
  489.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  490.         return ey.multiply(sc.cos()).add(ex.multiply(sc.sin()));
  491.     }

  493.     /** {@inheritDoc} */
  494.     public T getEquinoctialEyDot() {

  496.         if (!hasDerivatives()) {
  497.             return null;
  498.         }

  500.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  501.         return eyDot.add(ex.multiply(raanDot)).multiply(sc.cos()).
  502.                add(exDot.subtract(ey.multiply(raanDot)).multiply(sc.sin()));

  504.     }

  506.     /** Get the first component of the circular eccentricity vector.
  507.      * @return ex = e cos(ω), first component of the circular eccentricity vector
  508.      */
  509.     public T getCircularEx() {
  510.         return ex;
  511.     }

  513.     /** Get the first component of the circular eccentricity vector derivative.
  514.      * @return d(ex)/dt = d(e cos(ω))/dt, first component of the circular eccentricity vector derivative
  515.      */
  516.     public T getCircularExDot() {
  517.         return exDot;
  518.     }

  520.     /** Get the second component of the circular eccentricity vector.
  521.      * @return ey = e sin(ω), second component of the circular eccentricity vector
  522.      */
  523.     public T getCircularEy() {
  524.         return ey;
  525.     }

  527.     /** Get the second component of the circular eccentricity vector derivative.
  528.      * @return d(ey)/dt = d(e sin(ω))/dt, second component of the circular eccentricity vector derivative
  529.      */
  530.     public T getCircularEyDot() {
  531.         return eyDot;
  532.     }

  534.     /** {@inheritDoc} */
  535.     public T getHx() {
  536.         // Check for equatorial retrograde orbit
  537.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  538.             return getZero().add(Double.NaN);
  539.         }
  540.         return raan.cos().multiply(i.divide(2).tan());
  541.     }

  543.     /** {@inheritDoc} */
  544.     public T getHxDot() {

  546.         if (!hasDerivatives()) {
  547.             return null;
  548.         }

  550.         // Check for equatorial retrograde orbit
  551.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  552.             return getZero().add(Double.NaN);
  553.         }

  555.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  556.         final T tan             = i.multiply(0.5).tan();
  557.         return sc.cos().multiply(0.5).multiply(tan.multiply(tan).add(1)).multiply(iDot).
  558.                subtract(sc.sin().multiply(tan).multiply(raanDot));

  560.     }

  562.     /** {@inheritDoc} */
  563.     public T getHy() {
  564.         // Check for equatorial retrograde orbit
  565.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  566.             return getZero().add(Double.NaN);
  567.         }
  568.         return raan.sin().multiply(i.divide(2).tan());
  569.     }

  571.     /** {@inheritDoc} */
  572.     public T getHyDot() {

  574.         if (!hasDerivatives()) {
  575.             return null;
  576.         }

  578.         // Check for equatorial retrograde orbit
  579.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  580.             return getZero().add(Double.NaN);
  581.         }

  583.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  584.         final T tan     = i.multiply(0.5).tan();
  585.         return sc.sin().multiply(0.5).multiply(tan.multiply(tan).add(1)).multiply(iDot).
  586.                add(sc.cos().multiply(tan).multiply(raanDot));

  588.     }

  590.     /** Get the true latitude argument.
  591.      * @return v + ω true latitude argument (rad)
  592.      */
  593.     public T getAlphaV() {
  594.         return alphaV;
  595.     }

  597.     /** Get the true latitude argument derivative.
  598.      * @return d(v + ω)/dt true latitude argument derivative (rad/s)
  599.      */
  600.     public T getAlphaVDot() {
  601.         return alphaVDot;
  602.     }

  604.     /** Get the eccentric latitude argument.
  605.      * @return E + ω eccentric latitude argument (rad)
  606.      */
  607.     public T getAlphaE() {
  608.         return trueToEccentric(alphaV, ex, ey);
  609.     }

  611.     /** Get the eccentric latitude argument derivative.
  612.      * @return d(E + ω)/dt eccentric latitude argument derivative (rad/s)
  613.      */
  614.     public T getAlphaEDot() {

  616.         if (!hasDerivatives()) {
  617.             return null;
  618.         }

  620.         final FieldUnivariateDerivative1<T> alphaVUD = new FieldUnivariateDerivative1<>(alphaV, alphaVDot);
  621.         final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex, exDot);
  622.         final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey, eyDot);
  623.         final FieldUnivariateDerivative1<T> alphaEUD = trueToEccentric(alphaVUD, exUD, eyUD);
  624.         return alphaEUD.getDerivative(1);

  626.     }

  628.     /** Get the mean latitude argument.
  629.      * @return M + ω mean latitude argument (rad)
  630.      */
  631.     public T getAlphaM() {
  632.         return eccentricToMean(trueToEccentric(alphaV, ex, ey), ex, ey);
  633.     }

  635.     /** Get the mean latitude argument derivative.
  636.      * @return d(M + ω)/dt mean latitude argument derivative (rad/s)
  637.      */
  638.     public T getAlphaMDot() {

  640.         if (!hasDerivatives()) {
  641.             return null;
  642.         }

  644.         final FieldUnivariateDerivative1<T> alphaVUD = new FieldUnivariateDerivative1<>(alphaV, alphaVDot);
  645.         final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex, exDot);
  646.         final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey, eyDot);
  647.         final FieldUnivariateDerivative1<T> alphaMUD = eccentricToMean(trueToEccentric(alphaVUD, exUD, eyUD), exUD, eyUD);
  648.         return alphaMUD.getDerivative(1);

  650.     }

  652.     /** Get the latitude argument.
  653.      * @param type type of the angle
  654.      * @return latitude argument (rad)
  655.      */
  656.     public T getAlpha(final PositionAngleType type) {
  657.         return (type == PositionAngleType.MEAN) ? getAlphaM() :
  658.                                               ((type == PositionAngleType.ECCENTRIC) ? getAlphaE() :
  659.                                                                                    getAlphaV());
  660.     }

  662.     /** Get the latitude argument derivative.
  663.      * @param type type of the angle
  664.      * @return latitude argument derivative (rad/s)
  665.      */
  666.     public T getAlphaDot(final PositionAngleType type) {
  667.         return (type == PositionAngleType.MEAN) ? getAlphaMDot() :
  668.                                               ((type == PositionAngleType.ECCENTRIC) ? getAlphaEDot() :
  669.                                                                                    getAlphaVDot());
  670.     }

  672.     /** Computes the true latitude argument from the eccentric latitude argument.
  673.      * @param alphaE = E + ω eccentric latitude argument (rad)
  674.      * @param ex e cos(ω), first component of circular eccentricity vector
  675.      * @param ey e sin(ω), second component of circular eccentricity vector
  676.      * @param <T> Type of the field elements
  677.      * @return the true latitude argument.
  678.      */
  679.     public static <T extends CalculusFieldElement<T>> T eccentricToTrue(final T alphaE, final T ex, final T ey) {
  680.         final T epsilon               = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  681.         final FieldSinCos<T> scAlphaE = FastMath.sinCos(alphaE);
  682.         return alphaE.add(ex.multiply(scAlphaE.sin()).subtract(ey.multiply(scAlphaE.cos())).divide(
  683.                                       epsilon.add(1).subtract(ex.multiply(scAlphaE.cos())).subtract(
  684.                                       ey.multiply(scAlphaE.sin()))).atan().multiply(2));
  685.     }

  687.     /** Computes the eccentric latitude argument from the true latitude argument.
  688.      * @param alphaV = v + ω true latitude argument (rad)
  689.      * @param ex e cos(ω), first component of circular eccentricity vector
  690.      * @param ey e sin(ω), second component of circular eccentricity vector
  691.      * @param <T> Type of the field elements
  692.      * @return the eccentric latitude argument.
  693.      */
  694.     public static <T extends CalculusFieldElement<T>> T trueToEccentric(final T alphaV, final T ex, final T ey) {
  695.         final T epsilon               = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  696.         final FieldSinCos<T> scAlphaV = FastMath.sinCos(alphaV);
  697.         return alphaV.add(ey.multiply(scAlphaV.cos()).subtract(ex.multiply(scAlphaV.sin())).divide
  698.                                       (epsilon.add(1).add(ex.multiply(scAlphaV.cos()).add(ey.multiply(scAlphaV.sin())))).atan().multiply(2));
  699.     }

  701.     /** Computes the eccentric latitude argument from the mean latitude argument.
  702.      * @param alphaM = M + ω  mean latitude argument (rad)
  703.      * @param ex e cos(ω), first component of circular eccentricity vector
  704.      * @param ey e sin(ω), second component of circular eccentricity vector
  705.      * @param <T> Type of the field elements
  706.      * @return the eccentric latitude argument.
  707.      */
  708.     public static <T extends CalculusFieldElement<T>> T meanToEccentric(final T alphaM, final T ex, final T ey) {
  709.         // Generalization of Kepler equation to circular parameters
  710.         // with alphaE = PA + E and
  711.         //      alphaM = PA + M = alphaE - ex.sin(alphaE) + ey.cos(alphaE)

  713.         T alphaE                = alphaM;
  714.         T shift                 = alphaM.getField().getZero();
  715.         T alphaEMalphaM         = alphaM.getField().getZero();
  716.         FieldSinCos<T> scAlphaE = FastMath.sinCos(alphaE);
  717.         int    iter     = 0;
  718.         do {
  719.             final T f2 = ex.multiply(scAlphaE.sin()).subtract(ey.multiply(scAlphaE.cos()));
  720.             final T f1 = ex.negate().multiply(scAlphaE.cos()).subtract(ey.multiply(scAlphaE.sin())).add(1);
  721.             final T f0 = alphaEMalphaM.subtract(f2);

  723.             final T f12 = f1.multiply(2);
  724.             shift = f0.multiply(f12).divide(f1.multiply(f12).subtract(f0.multiply(f2)));

  726.             alphaEMalphaM  = alphaEMalphaM.subtract(shift);
  727.             alphaE         = alphaM.add(alphaEMalphaM);
  728.             scAlphaE       = FastMath.sinCos(alphaE);
  729.         } while (++iter < 50 && FastMath.abs(shift.getReal()) > 1.0e-12);
  730.         return alphaE;

  732.     }

  734.     /** Computes the mean latitude argument from the eccentric latitude argument.
  735.      * @param alphaE = E + ω  eccentric latitude argument (rad)
  736.      * @param ex e cos(ω), first component of circular eccentricity vector
  737.      * @param ey e sin(ω), second component of circular eccentricity vector
  738.      * @param <T> Type of the field elements
  739.      * @return the mean latitude argument.
  740.      */
  741.     public static <T extends CalculusFieldElement<T>> T eccentricToMean(final T alphaE, final T ex, final T ey) {
  742.         final FieldSinCos<T> scAlphaE = FastMath.sinCos(alphaE);
  743.         return alphaE.subtract(ex.multiply(scAlphaE.sin()).subtract(ey.multiply(scAlphaE.cos())));
  744.     }

  746.     /** {@inheritDoc} */
  747.     public T getE() {
  748.         return ex.multiply(ex).add(ey.multiply(ey)).sqrt();
  749.     }

  751.     /** {@inheritDoc} */
  752.     public T getEDot() {

  754.         if (!hasDerivatives()) {
  755.             return null;
  756.         }

  758.         return ex.multiply(exDot).add(ey.multiply(eyDot)).divide(getE());

  760.     }

  762.     /** {@inheritDoc} */
  763.     public T getI() {
  764.         return i;
  765.     }

  767.     /** {@inheritDoc} */
  768.     public T getIDot() {
  769.         return iDot;
  770.     }

  772.     /** Get the right ascension of the ascending node.
  773.      * @return right ascension of the ascending node (rad)
  774.      */
  775.     public T getRightAscensionOfAscendingNode() {
  776.         return raan;
  777.     }

  779.     /** Get the right ascension of the ascending node derivative.
  780.      * @return right ascension of the ascending node derivative (rad/s)
  781.      */
  782.     public T getRightAscensionOfAscendingNodeDot() {
  783.         return raanDot;
  784.     }

  786.     /** {@inheritDoc} */
  787.     public T getLv() {
  788.         return alphaV.add(raan);
  789.     }

  791.     /** {@inheritDoc} */
  792.     public T getLvDot() {
  793.         return hasDerivatives() ? alphaVDot.add(raanDot) : null;
  794.     }

  796.     /** {@inheritDoc} */
  797.     public T getLE() {
  798.         return getAlphaE().add(raan);
  799.     }

  801.     /** {@inheritDoc} */
  802.     public T getLEDot() {
  803.         return hasDerivatives() ? getAlphaEDot().add(raanDot) : null;
  804.     }

  806.     /** {@inheritDoc} */
  807.     public T getLM() {
  808.         return getAlphaM().add(raan);
  809.     }

  811.     /** {@inheritDoc} */
  812.     public T getLMDot() {
  813.         return hasDerivatives() ? getAlphaMDot().add(raanDot) : null;
  814.     }

  816.     /** {@inheritDoc} */
  817.     @Override
  818.     public boolean hasDerivatives() {
  819.         return aDot != null;
  820.     }

  822.     /** Compute position and velocity but not acceleration.
  823.      */
  824.     private void computePVWithoutA() {

  826.         if (partialPV != null) {
  827.             // already computed
  828.             return;
  829.         }

  831.         // get equinoctial parameters
  832.         final T equEx = getEquinoctialEx();
  833.         final T equEy = getEquinoctialEy();
  834.         final T hx = getHx();
  835.         final T hy = getHy();
  836.         final T lE = getLE();
  837.         // inclination-related intermediate parameters
  838.         final T hx2   = hx.multiply(hx);
  839.         final T hy2   = hy.multiply(hy);
  840.         final T factH = (hx2.add(1).add(hy2)).reciprocal();

  842.         // reference axes defining the orbital plane
  843.         final T ux = (hx2.add(1).subtract(hy2)).multiply(factH);
  844.         final T uy =  hx.multiply(2).multiply(hy).multiply(factH);
  845.         final T uz = hy.multiply(-2).multiply(factH);

  847.         final T vx = uy;
  848.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  849.         final T vz =  hx.multiply(factH).multiply(2);

  851.         // eccentricity-related intermediate parameters
  852.         final T exey = equEx.multiply(equEy);
  853.         final T ex2  = equEx.multiply(equEx);
  854.         final T ey2  = equEy.multiply(equEy);
  855.         final T e2   = ex2.add(ey2);
  856.         final T eta  = e2.negate().add(1).sqrt().add(1);
  857.         final T beta = eta.reciprocal();

  859.         // eccentric latitude argument
  860.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  861.         final T cLe    = scLe.cos();
  862.         final T sLe    = scLe.sin();
  863.         final T exCeyS = equEx.multiply(cLe).add(equEy.multiply(sLe));
  864.         // coordinates of position and velocity in the orbital plane
  865.         final T x      = a.multiply(beta.negate().multiply(ey2).add(1).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(equEx));
  866.         final T y      = a.multiply(beta.negate().multiply(ex2).add(1).multiply(sLe).add(beta.multiply(exey).multiply(cLe)).subtract(equEy));

  868.         final T factor = getOne().add(getMu()).divide(a).sqrt().divide(exCeyS.negate().add(1));
  869.         final T xdot   = factor.multiply( beta.multiply(equEy).multiply(exCeyS).subtract(sLe ));
  870.         final T ydot   = factor.multiply( cLe.subtract(beta.multiply(equEx).multiply(exCeyS)));

  872.         final FieldVector3D<T> position = new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  873.                                                               x.multiply(uy).add(y.multiply(vy)),
  874.                                                               x.multiply(uz).add(y.multiply(vz)));
  875.         final FieldVector3D<T> velocity = new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)),
  876.                                                               xdot.multiply(uy).add(ydot.multiply(vy)),
  877.                                                               xdot.multiply(uz).add(ydot.multiply(vz)));

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

  881.     }

  883.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  884.      * <p>
  885.      * This method should be called only when {@link #hasDerivatives()} returns true.
  886.      * </p>
  887.      * @return non-Keplerian part of the acceleration
  888.      */
  889.     private FieldVector3D<T> nonKeplerianAcceleration() {

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

  894.         final T nonKeplerianMeanMotion = getAlphaMDot().subtract(getKeplerianMeanMotion());
  895.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  896.                                  add(dCdP[3][1].multiply(exDot)).
  897.                                  add(dCdP[3][2].multiply(eyDot)).
  898.                                  add(dCdP[3][3].multiply(iDot)).
  899.                                  add(dCdP[3][4].multiply(raanDot)).
  900.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  901.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  902.                                  add(dCdP[4][1].multiply(exDot)).
  903.                                  add(dCdP[4][2].multiply(eyDot)).
  904.                                  add(dCdP[4][3].multiply(iDot)).
  905.                                  add(dCdP[4][4].multiply(raanDot)).
  906.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  907.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  908.                                  add(dCdP[5][1].multiply(exDot)).
  909.                                  add(dCdP[5][2].multiply(eyDot)).
  910.                                  add(dCdP[5][3].multiply(iDot)).
  911.                                  add(dCdP[5][4].multiply(raanDot)).
  912.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  916.     }

  918.     /** {@inheritDoc} */
  919.     protected FieldVector3D<T> initPosition() {
  920.         // get equinoctial parameters
  921.         final T equEx = getEquinoctialEx();
  922.         final T equEy = getEquinoctialEy();
  923.         final T hx = getHx();
  924.         final T hy = getHy();
  925.         final T lE = getLE();
  926.         // inclination-related intermediate parameters
  927.         final T hx2   = hx.multiply(hx);
  928.         final T hy2   = hy.multiply(hy);
  929.         final T factH = (hx2.add(1).add(hy2)).reciprocal();

  931.         // reference axes defining the orbital plane
  932.         final T ux = (hx2.add(1).subtract(hy2)).multiply(factH);
  933.         final T uy =  hx.multiply(2).multiply(hy).multiply(factH);
  934.         final T uz = hy.multiply(-2).multiply(factH);

  936.         final T vx = uy;
  937.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  938.         final T vz =  hx.multiply(factH).multiply(2);

  940.         // eccentricity-related intermediate parameters
  941.         final T exey = equEx.multiply(equEy);
  942.         final T ex2  = equEx.multiply(equEx);
  943.         final T ey2  = equEy.multiply(equEy);
  944.         final T e2   = ex2.add(ey2);
  945.         final T eta  = e2.negate().add(1).sqrt().add(1);
  946.         final T beta = eta.reciprocal();

  948.         // eccentric latitude argument
  949.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  950.         final T cLe    = scLe.cos();
  951.         final T sLe    = scLe.sin();

  953.         // coordinates of position and velocity in the orbital plane
  954.         final T x      = a.multiply(beta.negate().multiply(ey2).add(1).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(equEx));
  955.         final T y      = a.multiply(beta.negate().multiply(ex2).add(1).multiply(sLe).add(beta.multiply(exey).multiply(cLe)).subtract(equEy));

  957.         return new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  958.                                    x.multiply(uy).add(y.multiply(vy)),
  959.                                    x.multiply(uz).add(y.multiply(vz)));

  961.     }

  963.     /** {@inheritDoc} */
  964.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  966.         // position and velocity
  967.         computePVWithoutA();

  969.         // acceleration
  970.         final T r2 = partialPV.getPosition().getNormSq();
  971.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(getMu().negate()),
  972.                                                                            partialPV.getPosition());
  973.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  974.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  975.                                               keplerianAcceleration;

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

  979.     }

  981.     /** {@inheritDoc} */
  982.     public FieldCircularOrbit<T> shiftedBy(final double dt) {
  983.         return shiftedBy(getZero().add(dt));
  984.     }

  986.     /** {@inheritDoc} */
  987.     public FieldCircularOrbit<T> shiftedBy(final T dt) {

  989.         // use Keplerian-only motion
  990.         final FieldCircularOrbit<T> keplerianShifted = new FieldCircularOrbit<>(a, ex, ey, i, raan,
  991.                                                                                 getAlphaM().add(getKeplerianMeanMotion().multiply(dt)),
  992.                                                                                 PositionAngleType.MEAN, getFrame(),
  993.                                                                                 getDate().shiftedBy(dt), getMu());

  995.         if (hasDerivatives()) {

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

  1000.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  1001.             keplerianShifted.computePVWithoutA();
  1002.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getPosition(),
  1003.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  1004.             final T   fixedR2 = fixedP.getNormSq();
  1005.             final T   fixedR  = fixedR2.sqrt();
  1006.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getVelocity(),
  1007.                                                                  dt, nonKeplerianAcceleration);
  1008.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  1009.                                                                  keplerianShifted.partialPV.getPosition(),
  1010.                                                                  getOne(), nonKeplerianAcceleration);

  1012.             // build a new orbit, taking non-Keplerian acceleration into account
  1013.             return new FieldCircularOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  1014.                                                                                 fixedP, fixedV, fixedA),
  1015.                                             keplerianShifted.getFrame(), keplerianShifted.getMu());

  1017.         } else {
  1018.             // Keplerian-only motion is all we can do
  1019.             return keplerianShifted;
  1020.         }

  1022.     }

  1024.     /** {@inheritDoc} */
  1025.     protected T[][] computeJacobianMeanWrtCartesian() {

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

  1029.         // compute various intermediate parameters
  1030.         computePVWithoutA();
  1031.         final FieldVector3D<T> position = partialPV.getPosition();
  1032.         final FieldVector3D<T> velocity = partialPV.getVelocity();

  1034.         final T x          = position.getX();
  1035.         final T y          = position.getY();
  1036.         final T z          = position.getZ();
  1037.         final T vx         = velocity.getX();
  1038.         final T vy         = velocity.getY();
  1039.         final T vz         = velocity.getZ();
  1040.         final T pv         = FieldVector3D.dotProduct(position, velocity);
  1041.         final T r2         = position.getNormSq();
  1042.         final T r          = r2.sqrt();
  1043.         final T v2         = velocity.getNormSq();

  1045.         final T mu         = getMu();
  1046.         final T oOsqrtMuA  = getOne().divide(a.multiply(mu).sqrt());
  1047.         final T rOa        = r.divide(a);
  1048.         final T aOr        = a.divide(r);
  1049.         final T aOr2       = a.divide(r2);
  1050.         final T a2         = a.multiply(a);

  1052.         final T ex2        = ex.multiply(ex);
  1053.         final T ey2        = ey.multiply(ey);
  1054.         final T e2         = ex2.add(ey2);
  1055.         final T epsilon    = e2.negate().add(1.0).sqrt();
  1056.         final T beta       = epsilon.add(1).reciprocal();

  1058.         final T eCosE      = rOa.negate().add(1);
  1059.         final T eSinE      = pv.multiply(oOsqrtMuA);

  1061.         final FieldSinCos<T> scI    = FastMath.sinCos(i);
  1062.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  1063.         final T cosI       = scI.cos();
  1064.         final T sinI       = scI.sin();
  1065.         final T cosRaan    = scRaan.cos();
  1066.         final T sinRaan    = scRaan.sin();

  1068.         // da
  1069.         fillHalfRow(aOr.multiply(2.0).multiply(aOr2), position, jacobian[0], 0);
  1070.         fillHalfRow(a2.multiply(mu.divide(2.).reciprocal()), velocity, jacobian[0], 3);

  1072.         // differentials of the normalized momentum
  1073.         final FieldVector3D<T> danP = new FieldVector3D<>(v2, position, pv.negate(), velocity);
  1074.         final FieldVector3D<T> danV = new FieldVector3D<>(r2, velocity, pv.negate(), position);
  1075.         final T recip   = partialPV.getMomentum().getNorm().reciprocal();
  1076.         final T recip2  = recip.multiply(recip);
  1077.         final T recip2N = recip2.negate();
  1078.         final FieldVector3D<T> dwXP = new FieldVector3D<>(recip,
  1079.                                                           new FieldVector3D<>(getZero(), vz, vy.negate()),
  1080.                                                           recip2N.multiply(sinRaan).multiply(sinI),
  1081.                                                           danP);
  1082.         final FieldVector3D<T> dwYP = new FieldVector3D<>(recip,
  1083.                                                           new FieldVector3D<>(vz.negate(), getZero(), vx),
  1084.                                                           recip2.multiply(cosRaan).multiply(sinI),
  1085.                                                           danP);
  1086.         final FieldVector3D<T> dwZP = new FieldVector3D<>(recip,
  1087.                                                           new FieldVector3D<>(vy, vx.negate(), getZero()),
  1088.                                                           recip2N.multiply(cosI),
  1089.                                                           danP);
  1090.         final FieldVector3D<T> dwXV = new FieldVector3D<>(recip,
  1091.                                                           new FieldVector3D<>(getZero(), z.negate(), y),
  1092.                                                           recip2N.multiply(sinRaan).multiply(sinI),
  1093.                                                           danV);
  1094.         final FieldVector3D<T> dwYV = new FieldVector3D<>(recip,
  1095.                                                           new FieldVector3D<>(z, getZero(), x.negate()),
  1096.                                                           recip2.multiply(cosRaan).multiply(sinI),
  1097.                                                           danV);
  1098.         final FieldVector3D<T> dwZV = new FieldVector3D<>(recip,
  1099.                                                           new FieldVector3D<>(y.negate(), x, getZero()),
  1100.                                                           recip2N.multiply(cosI),
  1101.                                                           danV);

  1103.         // di
  1104.         fillHalfRow(sinRaan.multiply(cosI), dwXP, cosRaan.negate().multiply(cosI), dwYP, sinI.negate(), dwZP, jacobian[3], 0);
  1105.         fillHalfRow(sinRaan.multiply(cosI), dwXV, cosRaan.negate().multiply(cosI), dwYV, sinI.negate(), dwZV, jacobian[3], 3);

  1107.         // dRaan
  1108.         fillHalfRow(sinRaan.divide(sinI), dwYP, cosRaan.divide(sinI), dwXP, jacobian[4], 0);
  1109.         fillHalfRow(sinRaan.divide(sinI), dwYV, cosRaan.divide(sinI), dwXV, jacobian[4], 3);

  1111.         // orbital frame: (p, q, w) p along ascending node, w along momentum
  1112.         // the coordinates of the spacecraft in this frame are: (u, v, 0)
  1113.         final T u     =  x.multiply(cosRaan).add(y.multiply(sinRaan));
  1114.         final T cv    =  x.negate().multiply(sinRaan).add(y.multiply(cosRaan));
  1115.         final T v     = cv.multiply(cosI).add(z.multiply(sinI));

  1117.         // du
  1118.         final FieldVector3D<T> duP = new FieldVector3D<>(cv.multiply(cosRaan).divide(sinI), dwXP,
  1119.                                                          cv.multiply(sinRaan).divide(sinI), dwYP,
  1120.                                                          getOne(), new FieldVector3D<>(cosRaan, sinRaan, getZero()));
  1121.         final FieldVector3D<T> duV = new FieldVector3D<>(cv.multiply(cosRaan).divide(sinI), dwXV,
  1122.                                                          cv.multiply(sinRaan).divide(sinI), dwYV);

  1124.         // dv
  1125.         final FieldVector3D<T> dvP = new FieldVector3D<>(u.negate().multiply(cosRaan).multiply(cosI).divide(sinI).add(sinRaan.multiply(z)), dwXP,
  1126.                                                          u.negate().multiply(sinRaan).multiply(cosI).divide(sinI).subtract(cosRaan.multiply(z)), dwYP,
  1127.                                                          cv, dwZP,
  1128.                                                          getOne(), new FieldVector3D<>(sinRaan.negate().multiply(cosI), cosRaan.multiply(cosI), sinI));
  1129.         final FieldVector3D<T> dvV = new FieldVector3D<>(u.negate().multiply(cosRaan).multiply(cosI).divide(sinI).add(sinRaan.multiply(z)), dwXV,
  1130.                                                          u.negate().multiply(sinRaan).multiply(cosI).divide(sinI).subtract(cosRaan.multiply(z)), dwYV,
  1131.                                                          cv, dwZV);

  1133.         final FieldVector3D<T> dc1P = new FieldVector3D<>(aOr2.multiply(eSinE.multiply(eSinE).multiply(2).add(1).subtract(eCosE)).divide(r2), position,
  1134.                                                           aOr2.multiply(-2).multiply(eSinE).multiply(oOsqrtMuA), velocity);
  1135.         final FieldVector3D<T> dc1V = new FieldVector3D<>(aOr2.multiply(-2).multiply(eSinE).multiply(oOsqrtMuA), position,
  1136.                                                           getZero().add(2).divide(mu), velocity);
  1137.         final FieldVector3D<T> dc2P = new FieldVector3D<>(aOr2.multiply(eSinE).multiply(eSinE.multiply(eSinE).subtract(e2.negate().add(1))).divide(r2.multiply(epsilon)), position,
  1138.                                                           aOr2.multiply(e2.negate().add(1).subtract(eSinE.multiply(eSinE))).multiply(oOsqrtMuA).divide(epsilon), velocity);
  1139.         final FieldVector3D<T> dc2V = new FieldVector3D<>(aOr2.multiply(e2.negate().add(1).subtract(eSinE.multiply(eSinE))).multiply(oOsqrtMuA).divide(epsilon), position,
  1140.                                                           eSinE.divide(epsilon.multiply(mu)), velocity);

  1142.         final T cof1   = aOr2.multiply(eCosE.subtract(e2));
  1143.         final T cof2   = aOr2.multiply(epsilon).multiply(eSinE);
  1144.         final FieldVector3D<T> dexP = new FieldVector3D<>(u, dc1P,  v, dc2P, cof1, duP,  cof2, dvP);
  1145.         final FieldVector3D<T> dexV = new FieldVector3D<>(u, dc1V,  v, dc2V, cof1, duV,  cof2, dvV);
  1146.         final FieldVector3D<T> deyP = new FieldVector3D<>(v, dc1P, u.negate(), dc2P, cof1, dvP, cof2.negate(), duP);
  1147.         final FieldVector3D<T> deyV = new FieldVector3D<>(v, dc1V, u.negate(), dc2V, cof1, dvV, cof2.negate(), duV);
  1148.         fillHalfRow(getOne(), dexP, jacobian[1], 0);
  1149.         fillHalfRow(getOne(), dexV, jacobian[1], 3);
  1150.         fillHalfRow(getOne(), deyP, jacobian[2], 0);
  1151.         fillHalfRow(getOne(), deyV, jacobian[2], 3);

  1153.         final T cle = u.divide(a).add(ex).subtract(eSinE.multiply(beta).multiply(ey));
  1154.         final T sle = v.divide(a).add(ey).add(eSinE.multiply(beta).multiply(ex));
  1155.         final T m1  = beta.multiply(eCosE);
  1156.         final T m2  = m1.multiply(eCosE).negate().add(1);
  1157.         final T m3  = (u.multiply(ey).subtract(v.multiply(ex))).add(eSinE.multiply(beta).multiply(u.multiply(ex).add(v.multiply(ey))));
  1158.         final T m4  = sle.negate().add(cle.multiply(eSinE).multiply(beta));
  1159.         final T m5  = cle.add(sle.multiply(eSinE).multiply(beta));
  1160.         final T kk = m3.multiply(2).divide(r).add(aOr.multiply(eSinE)).add(m1.multiply(eSinE).multiply(m1.add(1).subtract(aOr.add(1).multiply(m2))).divide(epsilon)).divide(r2);
  1161.         final T jj = (m1.multiply(m2).divide(epsilon).subtract(1)).multiply(oOsqrtMuA);
  1162.         fillHalfRow(kk, position,
  1163.                     jj, velocity,
  1164.                     m4, dexP,
  1165.                     m5, deyP,
  1166.                     sle.negate().divide(a), duP,
  1167.                     cle.divide(a), dvP,
  1168.                     jacobian[5], 0);
  1169.         final T ll = (m1.multiply(m2).divide(epsilon ).subtract(1)).multiply(oOsqrtMuA);
  1170.         final T mm = m3.multiply(2).add(eSinE.multiply(a)).add(m1.multiply(eSinE).multiply(r).multiply(eCosE.multiply(beta).multiply(2).subtract(aOr.multiply(m2))).divide(epsilon)).divide(mu);

  1172.         fillHalfRow(ll, position,
  1173.                     mm, velocity,
  1174.                     m4, dexV,
  1175.                     m5, deyV,
  1176.                     sle.negate().divide(a), duV,
  1177.                     cle.divide(a), dvV,
  1178.                     jacobian[5], 3);
  1179.         return jacobian;

  1181.     }

  1183.     /** {@inheritDoc} */
  1184.     protected T[][] computeJacobianEccentricWrtCartesian() {

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

  1189.         // Differentiating the Kepler equation aM = aE - ex sin aE + ey cos aE leads to:
  1190.         // daM = (1 - ex cos aE - ey sin aE) dE - sin aE dex + cos aE dey
  1191.         // which is inverted and rewritten as:
  1192.         // daE = a/r daM + sin aE a/r dex - cos aE a/r dey
  1193.         final T alphaE            = getAlphaE();
  1194.         final FieldSinCos<T> scAe = FastMath.sinCos(alphaE);
  1195.         final T cosAe             = scAe.cos();
  1196.         final T sinAe             = scAe.sin();
  1197.         final T aOr               = getOne().divide(getOne().subtract(ex.multiply(cosAe)).subtract(ey.multiply(sinAe)));

  1199.         // update longitude row
  1200.         final T[] rowEx = jacobian[1];
  1201.         final T[] rowEy = jacobian[2];
  1202.         final T[] rowL  = jacobian[5];
  1203.         for (int j = 0; j < 6; ++j) {
  1204.          // rowL[j] = aOr * (      rowL[j] +   sinAe *        rowEx[j]   -         cosAe *        rowEy[j]);
  1205.             rowL[j] = aOr.multiply(rowL[j].add(sinAe.multiply(rowEx[j])).subtract( cosAe.multiply(rowEy[j])));
  1206.         }
  1207.         jacobian[5] = rowL;
  1208.         return jacobian;

  1210.     }
  1211.     /** {@inheritDoc} */
  1212.     protected T[][] computeJacobianTrueWrtCartesian() {

  1214.         // start by computing the Jacobian with eccentric angle
  1215.         final T[][] jacobian = computeJacobianEccentricWrtCartesian();
  1216.         // Differentiating the eccentric latitude equation
  1217.         // tan((aV - aE)/2) = [ex sin aE - ey cos aE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos aE - ey sin aE]
  1218.         // leads to
  1219.         // cT (daV - daE) = cE daE + cX dex + cY dey
  1220.         // with
  1221.         // cT = [d^2 + (ex sin aE - ey cos aE)^2] / 2
  1222.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos aE - ey sin aE
  1223.         // cE = (ex cos aE + ey sin aE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  1224.         // cX =  sin aE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin aE - ey cos aE) / sqrt(1-ex^2-ey^2)
  1225.         // cY = -cos aE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin aE - ey cos aE) / sqrt(1-ex^2-ey^2)
  1226.         // which can be solved to find the differential of the true latitude
  1227.         // daV = (cT + cE) / cT daE + cX / cT deX + cY / cT deX
  1228.         final T alphaE            = getAlphaE();
  1229.         final FieldSinCos<T> scAe = FastMath.sinCos(alphaE);
  1230.         final T cosAe             = scAe.cos();
  1231.         final T sinAe             = scAe.sin();
  1232.         final T eSinE             = ex.multiply(sinAe).subtract(ey.multiply(cosAe));
  1233.         final T ecosE             = ex.multiply(cosAe).add(ey.multiply(sinAe));
  1234.         final T e2                = ex.multiply(ex).add(ey.multiply(ey));
  1235.         final T epsilon           = (getOne().subtract(e2)).sqrt();
  1236.         final T onePeps           = getOne().add(epsilon);
  1237.         final T d                 = onePeps.subtract(ecosE);
  1238.         final T cT                = (d.multiply(d).add(eSinE.multiply(eSinE))).divide(2);
  1239.         final T cE                = ecosE.multiply(onePeps).subtract(e2);
  1240.         final T cX                = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinAe.multiply(onePeps));
  1241.         final T cY                = ey.multiply(eSinE).divide(epsilon).add(ex).subtract(cosAe.multiply(onePeps));
  1242.         final T factorLe          = (cT.add(cE)).divide(cT);
  1243.         final T factorEx          = cX.divide(cT);
  1244.         final T factorEy          = cY.divide(cT);

  1246.         // update latitude row
  1247.         final T[] rowEx = jacobian[1];
  1248.         final T[] rowEy = jacobian[2];
  1249.         final T[] rowA = jacobian[5];
  1250.         for (int j = 0; j < 6; ++j) {
  1251.             rowA[j] = factorLe.multiply(rowA[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
  1252.         }
  1253.         return jacobian;

  1255.     }

  1257.     /** {@inheritDoc} */
  1258.     public void addKeplerContribution(final PositionAngleType type, final T gm,
  1259.                                       final T[] pDot) {
  1260.         final T oMe2;
  1261.         final T ksi;
  1262.         final T n = a.reciprocal().multiply(gm).sqrt().divide(a);
  1263.         final FieldSinCos<T> sc = FastMath.sinCos(alphaV);
  1264.         switch (type) {
  1265.             case MEAN :
  1266.                 pDot[5] = pDot[5].add(n);
  1267.                 break;
  1268.             case ECCENTRIC :
  1269.                 oMe2  = getOne().subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1270.                 ksi   = getOne().add(ex.multiply(sc.cos())).add(ey.multiply(sc.sin()));
  1271.                 pDot[5] = pDot[5].add(n.multiply(ksi).divide(oMe2));
  1272.                 break;
  1273.             case TRUE :
  1274.                 oMe2  = getOne().subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1275.                 ksi   = getOne().add(ex.multiply(sc.cos())).add(ey.multiply(sc.sin()));
  1276.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1277.                 break;
  1278.             default :
  1279.                 throw new OrekitInternalError(null);
  1280.         }
  1281.     }

  1283.     /**  Returns a string representation of this Orbit object.
  1284.      * @return a string representation of this object
  1285.      */
  1286.     public String toString() {
  1287.         return new StringBuilder().append("circular parameters: ").append('{').
  1288.                                   append("a: ").append(a.getReal()).
  1289.                                   append(", ex: ").append(ex.getReal()).append(", ey: ").append(ey.getReal()).
  1290.                                   append(", i: ").append(FastMath.toDegrees(i.getReal())).
  1291.                                   append(", raan: ").append(FastMath.toDegrees(raan.getReal())).
  1292.                                   append(", alphaV: ").append(FastMath.toDegrees(alphaV.getReal())).
  1293.                                   append(";}").toString();
  1294.     }

  1296.     /** {@inheritDoc} */
  1297.     @Override
  1298.     public PositionAngleType getCachedPositionAngleType() {
  1299.         return PositionAngleType.TRUE;
  1300.     }

  1302.     /** {@inheritDoc} */
  1303.     @Override
  1304.     public boolean hasRates() {
  1305.         return hasDerivatives();
  1306.     }

  1308.     /** {@inheritDoc} */
  1309.     @Override
  1310.     public FieldCircularOrbit<T> removeRates() {
  1311.         final PositionAngleType positionAngleType = getCachedPositionAngleType();
  1312.         return new FieldCircularOrbit<>(getA(), getCircularEx(), getCircularEy(),
  1313.                 getI(), getRightAscensionOfAscendingNode(), getAlpha(positionAngleType),
  1314.                 positionAngleType, getFrame(), getDate(), getMu());
  1315.     }

  1317.     /** {@inheritDoc} */
  1318.     @Override
  1319.     public CircularOrbit toOrbit() {
  1320.         if (hasDerivatives()) {
  1321.             return new CircularOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1322.                                      i.getReal(), raan.getReal(), alphaV.getReal(),
  1323.                                      aDot.getReal(), exDot.getReal(), eyDot.getReal(),
  1324.                                      iDot.getReal(), raanDot.getReal(), alphaVDot.getReal(),
  1325.                                      PositionAngleType.TRUE, getFrame(),
  1326.                                      getDate().toAbsoluteDate(), getMu().getReal());
  1327.         } else {
  1328.             return new CircularOrbit(a.getReal(), ex.getReal(), ey.getReal(),
  1329.                                      i.getReal(), raan.getReal(), alphaV.getReal(),
  1330.                                      PositionAngleType.TRUE, getFrame(),
  1331.                                      getDate().toAbsoluteDate(), getMu().getReal());
  1332.         }
  1333.     }


  1336. }
Created with JaCoCo 0.8.11.202310140853