FieldCircularOrbit.java

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

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

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


  40. /**
  41.  * This class handles circular orbital parameters.

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

  79. public  class FieldCircularOrbit<T extends CalculusFieldElement<T>>
  80.     extends FieldOrbit<T> {

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

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

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

  91.     /** Inclination (rad). */
  92.     private final T i;

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

  97.     /** True latitude argument (rad). */
  98.     private final T alphaV;

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

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

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

  109.     /** Inclination derivative (rad/s). */
  110.     private final T iDot;

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

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

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

  121.     /** one. */
  122.     private final T one;

  124.     /** zero. */
  125.     private final T zero;

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

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

  186.         this.a       =  a;
  187.         this.aDot    =  aDot;
  188.         this.ex      = ex;
  189.         this.exDot   = exDot;
  190.         this.ey      = ey;
  191.         this.eyDot   = eyDot;
  192.         this.i       = i;
  193.         this.iDot    = iDot;
  194.         this.raan    = raan;
  195.         this.raanDot = raanDot;

  197.         one = a.getField().getOne();
  198.         zero = a.getField().getZero();

  200.         if (hasDerivatives()) {
  201.             final FieldUnivariateDerivative1<T> exUD    = new FieldUnivariateDerivative1<>(ex,    exDot);
  202.             final FieldUnivariateDerivative1<T> eyUD    = new FieldUnivariateDerivative1<>(ey,    eyDot);
  203.             final FieldUnivariateDerivative1<T> alphaUD = new FieldUnivariateDerivative1<>(alpha, alphaDot);
  204.             final FieldUnivariateDerivative1<T> alphavUD;
  205.             switch (type) {
  206.                 case MEAN :
  207.                     alphavUD = eccentricToTrue(meanToEccentric(alphaUD, exUD, eyUD), exUD, eyUD);
  208.                     break;
  209.                 case ECCENTRIC :
  210.                     alphavUD = eccentricToTrue(alphaUD, exUD, eyUD);
  211.                     break;
  212.                 case TRUE :
  213.                     alphavUD = alphaUD;
  214.                     break;
  215.                 default :
  216.                     throw new OrekitInternalError(null);
  217.             }
  218.             this.alphaV    = alphavUD.getValue();
  219.             this.alphaVDot = alphavUD.getDerivative(1);
  220.         } else {
  221.             switch (type) {
  222.                 case MEAN :
  223.                     this.alphaV = eccentricToTrue(meanToEccentric(alpha, ex, ey), ex, ey);
  224.                     break;
  225.                 case ECCENTRIC :
  226.                     this.alphaV = eccentricToTrue(alpha, ex, ey);
  227.                     break;
  228.                 case TRUE :
  229.                     this.alphaV = alpha;
  230.                     break;
  231.                 default :
  232.                     throw new OrekitInternalError(null);
  233.             }
  234.             this.alphaVDot = null;
  235.         }

  237.         this.partialPV = null;

  239.     }

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

  260.         // compute semi-major axis
  261.         final FieldVector3D<T> pvP = pvCoordinates.getPosition();
  262.         final FieldVector3D<T> pvV = pvCoordinates.getVelocity();
  263.         final FieldVector3D<T> pvA = pvCoordinates.getAcceleration();
  264.         final T r2 = pvP.getNormSq();
  265.         final T r  = r2.sqrt();
  266.         final T V2 = pvV.getNormSq();
  267.         final T rV2OnMu = r.multiply(V2).divide(mu);

  269.         zero = r.getField().getZero();
  270.         one = r.getField().getOne();

  272.         if (rV2OnMu.getReal() > 2) {
  273.             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
  274.                                                      getClass().getName());
  275.         }

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

  279.         // compute inclination
  280.         final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
  281.         final FieldVector3D<T> plusK = FieldVector3D.getPlusK(r.getField());
  282.         i = FieldVector3D.angle(momentum, plusK);

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

  288.         // 2D-coordinates in the canonical frame
  289.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  290.         final FieldSinCos<T> scI    = FastMath.sinCos(i);
  291.         final T xP      = pvP.getX();
  292.         final T yP      = pvP.getY();
  293.         final T zP      = pvP.getZ();
  294.         final T x2      = (xP.multiply(scRaan.cos()).add(yP .multiply(scRaan.sin()))).divide(a);
  295.         final T y2      = (yP.multiply(scRaan.cos()).subtract(xP.multiply(scRaan.sin()))).multiply(scI.cos()).add(zP.multiply(scI.sin())).divide(a);

  297.         // compute eccentricity vector
  298.         final T eSE    = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(mu).sqrt());
  299.         final T eCE    = rV2OnMu.subtract(1);
  300.         final T e2     = eCE.multiply(eCE).add(eSE.multiply(eSE));
  301.         final T f      = eCE.subtract(e2);
  302.         final T g      = eSE.multiply(e2.negate().add(1).sqrt());
  303.         final T aOnR   = a.divide(r);
  304.         final T a2OnR2 = aOnR.multiply(aOnR);
  305.         ex = a2OnR2.multiply(f.multiply(x2).add(g.multiply(y2)));
  306.         ey = a2OnR2.multiply(f.multiply(y2).subtract(g.multiply(x2)));

  308.         // compute latitude argument
  309.         final T beta = (ex.multiply(ex).add(ey.multiply(ey)).negate().add(1)).sqrt().add(1).reciprocal();
  310.         alphaV = eccentricToTrue(y2.add(ey).add(eSE.multiply(beta).multiply(ex)).atan2(x2.add(ex).subtract(eSE.multiply(beta).multiply(ey))),
  311.                                  ex, ey);

  313.         partialPV = pvCoordinates;

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

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

  321.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  322.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  323.             final T   aX                       = nonKeplerianAcceleration.getX();
  324.             final T   aY                       = nonKeplerianAcceleration.getY();
  325.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  326.             aDot    = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  327.             exDot   = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  328.             eyDot   = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  329.             iDot    = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  330.             raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  332.             // in order to compute true anomaly derivative, we must compute
  333.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  334.             final T alphaMDot = getKeplerianMeanMotion().
  335.                                 add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  336.             final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex, exDot);
  337.             final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey, eyDot);
  338.             final FieldUnivariateDerivative1<T> alphaMUD = new FieldUnivariateDerivative1<>(getAlphaM(), alphaMDot);
  339.             final FieldUnivariateDerivative1<T> alphavUD = eccentricToTrue(meanToEccentric(alphaMUD, exUD, eyUD), exUD, eyUD);
  340.             alphaVDot = alphavUD.getDerivative(1);

  342.         } else {
  343.             // acceleration is either almost zero or NaN,
  344.             // we assume acceleration was not known
  345.             // we don't set up derivatives
  346.             aDot      = null;
  347.             exDot     = null;
  348.             eyDot     = null;
  349.             iDot      = null;
  350.             raanDot   = null;
  351.             alphaVDot = null;
  352.         }

  354.     }

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

  377.     /** Constructor from any kind of orbital parameters.
  378.      * @param op orbital parameters to copy
  379.      */
  380.     public FieldCircularOrbit(final FieldOrbit<T> op) {
  381.         super(op.getFrame(), op.getDate(), op.getMu());
  382.         a    = op.getA();
  383.         i    = op.getI();
  384.         final T hx = op.getHx();
  385.         final T hy = op.getHy();
  386.         final T h2 = hx.multiply(hx).add(hy.multiply(hy));
  387.         final T h  = h2.sqrt();
  388.         raan = hy.atan2(hx);
  389.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  390.         final T cosRaan = h.getReal() == 0 ? scRaan.cos() : hx.divide(h);
  391.         final T sinRaan = h.getReal() == 0 ? scRaan.sin() : hy.divide(h);
  392.         final T equiEx = op.getEquinoctialEx();
  393.         final T equiEy = op.getEquinoctialEy();
  394.         ex   = equiEx.multiply(cosRaan).add(equiEy.multiply(sinRaan));
  395.         ey   = equiEy.multiply(cosRaan).subtract(equiEx.multiply(sinRaan));
  396.         this.alphaV = op.getLv().subtract(raan);

  398.         if (op.hasDerivatives()) {
  399.             aDot      = op.getADot();
  400.             final T      hxDot = op.getHxDot();
  401.             final T      hyDot = op.getHyDot();
  402.             iDot      = cosRaan.multiply(hxDot).add(sinRaan.multiply(hyDot)).multiply(2).divide(h2.add(1));
  403.             raanDot   = hx.multiply(hyDot).subtract(hy.multiply(hxDot)).divide(h2);
  404.             final T equiExDot = op.getEquinoctialExDot();
  405.             final T equiEyDot = op.getEquinoctialEyDot();
  406.             exDot     = equiExDot.add(equiEy.multiply(raanDot)).multiply(cosRaan).
  407.                         add(equiEyDot.subtract(equiEx.multiply(raanDot)).multiply(sinRaan));
  408.             eyDot     = equiEyDot.subtract(equiEx.multiply(raanDot)).multiply(cosRaan).
  409.                         subtract(equiExDot.add(equiEy.multiply(raanDot)).multiply(sinRaan));
  410.             alphaVDot = op.getLvDot().subtract(raanDot);
  411.         } else {
  412.             aDot      = null;
  413.             exDot     = null;
  414.             eyDot     = null;
  415.             iDot      = null;
  416.             raanDot   = null;
  417.             alphaVDot = null;
  418.         }

  420.         partialPV = null;

  422.         one = a.getField().getOne();
  423.         zero = a.getField().getZero();

  425.     }

  427.     /** {@inheritDoc} */
  428.     public OrbitType getType() {
  429.         return OrbitType.CIRCULAR;
  430.     }

  432.     /** {@inheritDoc} */
  433.     public T getA() {
  434.         return a;
  435.     }

  437.     /** {@inheritDoc} */
  438.     public T getADot() {
  439.         return aDot;
  440.     }

  442.     /** {@inheritDoc} */
  443.     public T getEquinoctialEx() {
  444.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  445.         return ex.multiply(sc.cos()).subtract(ey.multiply(sc.sin()));
  446.     }

  448.     /** {@inheritDoc} */
  449.     public T getEquinoctialExDot() {

  451.         if (!hasDerivatives()) {
  452.             return null;
  453.         }

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

  459.     }

  461.     /** {@inheritDoc} */
  462.     public T getEquinoctialEy() {
  463.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  464.         return ey.multiply(sc.cos()).add(ex.multiply(sc.sin()));
  465.     }

  467.     /** {@inheritDoc} */
  468.     public T getEquinoctialEyDot() {

  470.         if (!hasDerivatives()) {
  471.             return null;
  472.         }

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

  478.     }

  480.     /** Get the first component of the circular eccentricity vector.
  481.      * @return ex = e cos(ω), first component of the circular eccentricity vector
  482.      */
  483.     public T getCircularEx() {
  484.         return ex;
  485.     }

  487.     /** Get the first component of the circular eccentricity vector derivative.
  488.      * @return d(ex)/dt = d(e cos(ω))/dt, first component of the circular eccentricity vector derivative
  489.      */
  490.     public T getCircularExDot() {
  491.         return exDot;
  492.     }

  494.     /** Get the second component of the circular eccentricity vector.
  495.      * @return ey = e sin(ω), second component of the circular eccentricity vector
  496.      */
  497.     public T getCircularEy() {
  498.         return ey;
  499.     }

  501.     /** Get the second component of the circular eccentricity vector derivative.
  502.      * @return d(ey)/dt = d(e sin(ω))/dt, second component of the circular eccentricity vector derivative
  503.      */
  504.     public T getCircularEyDot() {
  505.         return eyDot;
  506.     }

  508.     /** {@inheritDoc} */
  509.     public T getHx() {
  510.         // Check for equatorial retrograde orbit
  511.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  512.             return zero.add(Double.NaN);
  513.         }
  514.         return raan.cos().multiply(i.divide(2).tan());
  515.     }

  517.     /** {@inheritDoc} */
  518.     public T getHxDot() {

  520.         if (!hasDerivatives()) {
  521.             return null;
  522.         }

  524.         // Check for equatorial retrograde orbit
  525.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  526.             return zero.add(Double.NaN);
  527.         }

  529.         final FieldSinCos<T> sc = FastMath.sinCos(raan);
  530.         final T tan             = i.multiply(0.5).tan();
  531.         return sc.cos().multiply(0.5).multiply(tan.multiply(tan).add(1)).multiply(iDot).
  532.                subtract(sc.sin().multiply(tan).multiply(raanDot));

  534.     }

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

  545.     /** {@inheritDoc} */
  546.     public T getHyDot() {

  548.         if (!hasDerivatives()) {
  549.             return null;
  550.         }

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

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

  562.     }

  564.     /** Get the true latitude argument.
  565.      * @return v + ω true latitude argument (rad)
  566.      */
  567.     public T getAlphaV() {
  568.         return alphaV;
  569.     }

  571.     /** Get the true latitude argument derivative.
  572.      * @return d(v + ω)/dt true latitude argument derivative (rad/s)
  573.      */
  574.     public T getAlphaVDot() {
  575.         return alphaVDot;
  576.     }

  578.     /** Get the eccentric latitude argument.
  579.      * @return E + ω eccentric latitude argument (rad)
  580.      */
  581.     public T getAlphaE() {
  582.         return trueToEccentric(alphaV, ex, ey);
  583.     }

  585.     /** Get the eccentric latitude argument derivative.
  586.      * @return d(E + ω)/dt eccentric latitude argument derivative (rad/s)
  587.      */
  588.     public T getAlphaEDot() {

  590.         if (!hasDerivatives()) {
  591.             return null;
  592.         }

  594.         final FieldUnivariateDerivative1<T> alphaVUD = new FieldUnivariateDerivative1<>(alphaV, alphaVDot);
  595.         final FieldUnivariateDerivative1<T> exUD     = new FieldUnivariateDerivative1<>(ex, exDot);
  596.         final FieldUnivariateDerivative1<T> eyUD     = new FieldUnivariateDerivative1<>(ey, eyDot);
  597.         final FieldUnivariateDerivative1<T> alphaEUD = trueToEccentric(alphaVUD, exUD, eyUD);
  598.         return alphaEUD.getDerivative(1);

  600.     }

  602.     /** Get the mean latitude argument.
  603.      * @return M + ω mean latitude argument (rad)
  604.      */
  605.     public T getAlphaM() {
  606.         return eccentricToMean(trueToEccentric(alphaV, ex, ey), ex, ey);
  607.     }

  609.     /** Get the mean latitude argument derivative.
  610.      * @return d(M + ω)/dt mean latitude argument derivative (rad/s)
  611.      */
  612.     public T getAlphaMDot() {

  614.         if (!hasDerivatives()) {
  615.             return null;
  616.         }

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

  624.     }

  626.     /** Get the latitude argument.
  627.      * @param type type of the angle
  628.      * @return latitude argument (rad)
  629.      */
  630.     public T getAlpha(final PositionAngle type) {
  631.         return (type == PositionAngle.MEAN) ? getAlphaM() :
  632.                                               ((type == PositionAngle.ECCENTRIC) ? getAlphaE() :
  633.                                                                                    getAlphaV());
  634.     }

  636.     /** Get the latitude argument derivative.
  637.      * @param type type of the angle
  638.      * @return latitude argument derivative (rad/s)
  639.      */
  640.     public T getAlphaDot(final PositionAngle type) {
  641.         return (type == PositionAngle.MEAN) ? getAlphaMDot() :
  642.                                               ((type == PositionAngle.ECCENTRIC) ? getAlphaEDot() :
  643.                                                                                    getAlphaVDot());
  644.     }

  646.     /** Computes the true latitude argument from the eccentric latitude argument.
  647.      * @param alphaE = E + ω eccentric latitude argument (rad)
  648.      * @param ex e cos(ω), first component of circular eccentricity vector
  649.      * @param ey e sin(ω), second component of circular eccentricity vector
  650.      * @param <T> Type of the field elements
  651.      * @return the true latitude argument.
  652.      */
  653.     public static <T extends CalculusFieldElement<T>> T eccentricToTrue(final T alphaE, final T ex, final T ey) {
  654.         final T epsilon               = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  655.         final FieldSinCos<T> scAlphaE = FastMath.sinCos(alphaE);
  656.         return alphaE.add(ex.multiply(scAlphaE.sin()).subtract(ey.multiply(scAlphaE.cos())).divide(
  657.                                       epsilon.add(1).subtract(ex.multiply(scAlphaE.cos())).subtract(
  658.                                       ey.multiply(scAlphaE.sin()))).atan().multiply(2));
  659.     }

  661.     /** Computes the eccentric latitude argument from the true latitude argument.
  662.      * @param alphaV = v + ω true latitude argument (rad)
  663.      * @param ex e cos(ω), first component of circular eccentricity vector
  664.      * @param ey e sin(ω), second component of circular eccentricity vector
  665.      * @param <T> Type of the field elements
  666.      * @return the eccentric latitude argument.
  667.      */
  668.     public static <T extends CalculusFieldElement<T>> T trueToEccentric(final T alphaV, final T ex, final T ey) {
  669.         final T epsilon               = ex.multiply(ex).add(ey.multiply(ey)).negate().add(1).sqrt();
  670.         final FieldSinCos<T> scAlphaV = FastMath.sinCos(alphaV);
  671.         return alphaV.add(ey.multiply(scAlphaV.cos()).subtract(ex.multiply(scAlphaV.sin())).divide
  672.                                       (epsilon.add(1).add(ex.multiply(scAlphaV.cos()).add(ey.multiply(scAlphaV.sin())))).atan().multiply(2));
  673.     }

  675.     /** Computes the eccentric latitude argument from the mean latitude argument.
  676.      * @param alphaM = M + ω  mean latitude argument (rad)
  677.      * @param ex e cos(ω), first component of circular eccentricity vector
  678.      * @param ey e sin(ω), second component of circular eccentricity vector
  679.      * @param <T> Type of the field elements
  680.      * @return the eccentric latitude argument.
  681.      */
  682.     public static <T extends CalculusFieldElement<T>> T meanToEccentric(final T alphaM, final T ex, final T ey) {
  683.         // Generalization of Kepler equation to circular parameters
  684.         // with alphaE = PA + E and
  685.         //      alphaM = PA + M = alphaE - ex.sin(alphaE) + ey.cos(alphaE)

  687.         T alphaE                = alphaM;
  688.         T shift                 = alphaM.getField().getZero();
  689.         T alphaEMalphaM         = alphaM.getField().getZero();
  690.         FieldSinCos<T> scAlphaE = FastMath.sinCos(alphaE);
  691.         int    iter     = 0;
  692.         do {
  693.             final T f2 = ex.multiply(scAlphaE.sin()).subtract(ey.multiply(scAlphaE.cos()));
  694.             final T f1 = ex.negate().multiply(scAlphaE.cos()).subtract(ey.multiply(scAlphaE.sin())).add(1);
  695.             final T f0 = alphaEMalphaM.subtract(f2);

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

  700.             alphaEMalphaM  = alphaEMalphaM.subtract(shift);
  701.             alphaE         = alphaM.add(alphaEMalphaM);
  702.             scAlphaE       = FastMath.sinCos(alphaE);
  703.         } while (++iter < 50 && FastMath.abs(shift.getReal()) > 1.0e-12);
  704.         return alphaE;

  706.     }

  708.     /** Computes the mean latitude argument from the eccentric latitude argument.
  709.      * @param alphaE = E + ω  eccentric latitude argument (rad)
  710.      * @param ex e cos(ω), first component of circular eccentricity vector
  711.      * @param ey e sin(ω), second component of circular eccentricity vector
  712.      * @param <T> Type of the field elements
  713.      * @return the mean latitude argument.
  714.      */
  715.     public static <T extends CalculusFieldElement<T>> T eccentricToMean(final T alphaE, final T ex, final T ey) {
  716.         final FieldSinCos<T> scAlphaE = FastMath.sinCos(alphaE);
  717.         return alphaE.subtract(ex.multiply(scAlphaE.sin()).subtract(ey.multiply(scAlphaE.cos())));
  718.     }

  720.     /** {@inheritDoc} */
  721.     public T getE() {
  722.         return ex.multiply(ex).add(ey.multiply(ey)).sqrt();
  723.     }

  725.     /** {@inheritDoc} */
  726.     public T getEDot() {

  728.         if (!hasDerivatives()) {
  729.             return null;
  730.         }

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

  734.     }

  736.     /** {@inheritDoc} */
  737.     public T getI() {
  738.         return i;
  739.     }

  741.     /** {@inheritDoc} */
  742.     public T getIDot() {
  743.         return iDot;
  744.     }

  746.     /** Get the right ascension of the ascending node.
  747.      * @return right ascension of the ascending node (rad)
  748.      */
  749.     public T getRightAscensionOfAscendingNode() {
  750.         return raan;
  751.     }

  753.     /** Get the right ascension of the ascending node derivative.
  754.      * @return right ascension of the ascending node derivative (rad/s)
  755.      */
  756.     public T getRightAscensionOfAscendingNodeDot() {
  757.         return raanDot;
  758.     }

  760.     /** {@inheritDoc} */
  761.     public T getLv() {
  762.         return alphaV.add(raan);
  763.     }

  765.     /** {@inheritDoc} */
  766.     public T getLvDot() {
  767.         return hasDerivatives() ? alphaVDot.add(raanDot) : null;
  768.     }

  770.     /** {@inheritDoc} */
  771.     public T getLE() {
  772.         return getAlphaE().add(raan);
  773.     }

  775.     /** {@inheritDoc} */
  776.     public T getLEDot() {
  777.         return hasDerivatives() ? getAlphaEDot().add(raanDot) : null;
  778.     }

  780.     /** {@inheritDoc} */
  781.     public T getLM() {
  782.         return getAlphaM().add(raan);
  783.     }

  785.     /** {@inheritDoc} */
  786.     public T getLMDot() {
  787.         return hasDerivatives() ? getAlphaMDot().add(raanDot) : null;
  788.     }

  790.     /** {@inheritDoc} */
  791.     @Override
  792.     public boolean hasDerivatives() {
  793.         return aDot != null;
  794.     }

  796.     /** Compute position and velocity but not acceleration.
  797.      */
  798.     private void computePVWithoutA() {

  800.         if (partialPV != null) {
  801.             // already computed
  802.             return;
  803.         }

  805.         // get equinoctial parameters
  806.         final T equEx = getEquinoctialEx();
  807.         final T equEy = getEquinoctialEy();
  808.         final T hx = getHx();
  809.         final T hy = getHy();
  810.         final T lE = getLE();
  811.         // inclination-related intermediate parameters
  812.         final T hx2   = hx.multiply(hx);
  813.         final T hy2   = hy.multiply(hy);
  814.         final T factH = (hx2.add(1).add(hy2)).reciprocal();

  816.         // reference axes defining the orbital plane
  817.         final T ux = (hx2.add(1).subtract(hy2)).multiply(factH);
  818.         final T uy =  hx.multiply(2).multiply(hy).multiply(factH);
  819.         final T uz = hy.multiply(-2).multiply(factH);

  821.         final T vx = uy;
  822.         final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
  823.         final T vz =  hx.multiply(factH).multiply(2);

  825.         // eccentricity-related intermediate parameters
  826.         final T exey = equEx.multiply(equEy);
  827.         final T ex2  = equEx.multiply(equEx);
  828.         final T ey2  = equEy.multiply(equEy);
  829.         final T e2   = ex2.add(ey2);
  830.         final T eta  = e2.negate().add(1).sqrt().add(1);
  831.         final T beta = eta.reciprocal();

  833.         // eccentric latitude argument
  834.         final FieldSinCos<T> scLe = FastMath.sinCos(lE);
  835.         final T cLe    = scLe.cos();
  836.         final T sLe    = scLe.sin();
  837.         final T exCeyS = equEx.multiply(cLe).add(equEy.multiply(sLe));
  838.         // coordinates of position and velocity in the orbital plane
  839.         final T x      = a.multiply(beta.negate().multiply(ey2).add(1).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(equEx));
  840.         final T y      = a.multiply(beta.negate().multiply(ex2).add(1).multiply(sLe).add(beta.multiply(exey).multiply(cLe)).subtract(equEy));

  842.         final T factor = one.add(getMu()).divide(a).sqrt().divide(exCeyS.negate().add(1));
  843.         final T xdot   = factor.multiply( beta.multiply(equEy).multiply(exCeyS).subtract(sLe ));
  844.         final T ydot   = factor.multiply( cLe.subtract(beta.multiply(equEx).multiply(exCeyS)));

  846.         final FieldVector3D<T> position = new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
  847.                                                               x.multiply(uy).add(y.multiply(vy)),
  848.                                                               x.multiply(uz).add(y.multiply(vz)));
  849.         final FieldVector3D<T> velocity = new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)),
  850.                                                               xdot.multiply(uy).add(ydot.multiply(vy)),
  851.                                                               xdot.multiply(uz).add(ydot.multiply(vz)));

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

  855.     }

  857.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  858.      * <p>
  859.      * This method should be called only when {@link #hasDerivatives()} returns true.
  860.      * </p>
  861.      * @return non-Keplerian part of the acceleration
  862.      */
  863.     private FieldVector3D<T> nonKeplerianAcceleration() {

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

  868.         final T nonKeplerianMeanMotion = getAlphaMDot().subtract(getKeplerianMeanMotion());
  869.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  870.                                  add(dCdP[3][1].multiply(exDot)).
  871.                                  add(dCdP[3][2].multiply(eyDot)).
  872.                                  add(dCdP[3][3].multiply(iDot)).
  873.                                  add(dCdP[3][4].multiply(raanDot)).
  874.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  875.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  876.                                  add(dCdP[4][1].multiply(exDot)).
  877.                                  add(dCdP[4][2].multiply(eyDot)).
  878.                                  add(dCdP[4][3].multiply(iDot)).
  879.                                  add(dCdP[4][4].multiply(raanDot)).
  880.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  881.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  882.                                  add(dCdP[5][1].multiply(exDot)).
  883.                                  add(dCdP[5][2].multiply(eyDot)).
  884.                                  add(dCdP[5][3].multiply(iDot)).
  885.                                  add(dCdP[5][4].multiply(raanDot)).
  886.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

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

  890.     }

  892.     /** {@inheritDoc} */
  893.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  895.         // position and velocity
  896.         computePVWithoutA();

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

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

  908.     }

  910.     /** {@inheritDoc} */
  911.     public FieldCircularOrbit<T> shiftedBy(final double dt) {
  912.         return shiftedBy(getDate().getField().getZero().add(dt));
  913.     }

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

  918.         // use Keplerian-only motion
  919.         final FieldCircularOrbit<T> keplerianShifted = new FieldCircularOrbit<>(a, ex, ey, i, raan,
  920.                                                                                 getAlphaM().add(getKeplerianMeanMotion().multiply(dt)),
  921.                                                                                 PositionAngle.MEAN, getFrame(),
  922.                                                                                 getDate().shiftedBy(dt), getMu());

  924.         if (hasDerivatives()) {

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

  929.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  930.             keplerianShifted.computePVWithoutA();
  931.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, keplerianShifted.partialPV.getPosition(),
  932.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  933.             final T   fixedR2 = fixedP.getNormSq();
  934.             final T   fixedR  = fixedR2.sqrt();
  935.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, keplerianShifted.partialPV.getVelocity(),
  936.                                                                  dt, nonKeplerianAcceleration);
  937.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  938.                                                                  keplerianShifted.partialPV.getPosition(),
  939.                                                                  one, nonKeplerianAcceleration);

  941.             // build a new orbit, taking non-Keplerian acceleration into account
  942.             return new FieldCircularOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  943.                                                                                 fixedP, fixedV, fixedA),
  944.                                             keplerianShifted.getFrame(), keplerianShifted.getMu());

  946.         } else {
  947.             // Keplerian-only motion is all we can do
  948.             return keplerianShifted;
  949.         }

  951.     }

  953.     /** {@inheritDoc}
  954.      * <p>
  955.      * The interpolated instance is created by polynomial Hermite interpolation
  956.      * on circular elements, without derivatives (which means the interpolation
  957.      * falls back to Lagrange interpolation only).
  958.      * </p>
  959.      * <p>
  960.      * As this implementation of interpolation is polynomial, it should be used only
  961.      * with small samples (about 10-20 points) in order to avoid <a
  962.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  963.      * and numerical problems (including NaN appearing).
  964.      * </p>
  965.      * <p>
  966.      * If orbit interpolation on large samples is needed, using the {@link
  967.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  968.      * low-level interpolation. The Ephemeris class automatically handles selection of
  969.      * a neighboring sub-sample with a predefined number of point from a large global sample
  970.      * in a thread-safe way.
  971.      * </p>
  972.      */
  973.     public FieldCircularOrbit<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldOrbit<T>> sample) {

  975.         // first pass to check if derivatives are available throughout the sample
  976.         final List<FieldOrbit<T>> list = sample.collect(Collectors.toList());
  977.         boolean useDerivatives = true;
  978.         for (final FieldOrbit<T> orbit : list) {
  979.             useDerivatives = useDerivatives && orbit.hasDerivatives();
  980.         }

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

  985.         // second pass to feed interpolator
  986.         FieldAbsoluteDate<T> previousDate   = null;
  987.         T                    previousRAAN   = zero.add(Double.NaN);
  988.         T                    previousAlphaM = zero.add(Double.NaN);
  989.         for (final FieldOrbit<T> orbit : list) {
  990.             final FieldCircularOrbit<T> circ = (FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(orbit);
  991.             final T continuousRAAN;
  992.             final T continuousAlphaM;
  993.             if (previousDate == null) {
  994.                 continuousRAAN   = circ.getRightAscensionOfAscendingNode();
  995.                 continuousAlphaM = circ.getAlphaM();
  996.             } else {
  997.                 final T dt       = circ.getDate().durationFrom(previousDate);
  998.                 final T keplerAM = previousAlphaM .add(circ.getKeplerianMeanMotion().multiply(dt));
  999.                 continuousRAAN   = MathUtils.normalizeAngle(circ.getRightAscensionOfAscendingNode(), previousRAAN);
  1000.                 continuousAlphaM = MathUtils.normalizeAngle(circ.getAlphaM(), keplerAM);
  1001.             }
  1002.             previousDate   = circ.getDate();
  1003.             previousRAAN   = continuousRAAN;
  1004.             previousAlphaM = continuousAlphaM;
  1005.             final T[] toAdd = MathArrays.buildArray(one.getField(), 6);
  1006.             toAdd[0] = circ.getA();
  1007.             toAdd[1] = circ.getCircularEx();
  1008.             toAdd[2] = circ.getCircularEy();
  1009.             toAdd[3] = circ.getI();
  1010.             toAdd[4] = continuousRAAN;
  1011.             toAdd[5] = continuousAlphaM;
  1012.             if (useDerivatives) {
  1013.                 final T[] toAddDot = MathArrays.buildArray(one.getField(), 6);
  1014.                 toAddDot[0] = circ.getADot();
  1015.                 toAddDot[1] = circ.getCircularExDot();
  1016.                 toAddDot[2] = circ.getCircularEyDot();
  1017.                 toAddDot[3] = circ.getIDot();
  1018.                 toAddDot[4] = circ.getRightAscensionOfAscendingNodeDot();
  1019.                 toAddDot[5] = circ.getAlphaMDot();
  1020.                 interpolator.addSamplePoint(circ.getDate().durationFrom(date),
  1021.                                             toAdd, toAddDot);
  1022.             } else {
  1023.                 interpolator.addSamplePoint(circ.getDate().durationFrom(date),
  1024.                                             toAdd);
  1025.             }
  1026.         }

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

  1031.         // build a new interpolated instance
  1032.         return new FieldCircularOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  1033.                                         interpolated[0][3], interpolated[0][4], interpolated[0][5],
  1034.                                         interpolated[1][0], interpolated[1][1], interpolated[1][2],
  1035.                                         interpolated[1][3], interpolated[1][4], interpolated[1][5],
  1036.                                         PositionAngle.MEAN, getFrame(), date, getMu());

  1038.     }

  1040.     /** {@inheritDoc} */
  1041.     protected T[][] computeJacobianMeanWrtCartesian() {

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

  1045.         // compute various intermediate parameters
  1046.         computePVWithoutA();
  1047.         final FieldVector3D<T> position = partialPV.getPosition();
  1048.         final FieldVector3D<T> velocity = partialPV.getVelocity();

  1050.         final T x          = position.getX();
  1051.         final T y          = position.getY();
  1052.         final T z          = position.getZ();
  1053.         final T vx         = velocity.getX();
  1054.         final T vy         = velocity.getY();
  1055.         final T vz         = velocity.getZ();
  1056.         final T pv         = FieldVector3D.dotProduct(position, velocity);
  1057.         final T r2         = position.getNormSq();
  1058.         final T r          = r2.sqrt();
  1059.         final T v2         = velocity.getNormSq();

  1061.         final T mu         = getMu();
  1062.         final T oOsqrtMuA  = one.divide(a.multiply(mu).sqrt());
  1063.         final T rOa        = r.divide(a);
  1064.         final T aOr        = a.divide(r);
  1065.         final T aOr2       = a.divide(r2);
  1066.         final T a2         = a.multiply(a);

  1068.         final T ex2        = ex.multiply(ex);
  1069.         final T ey2        = ey.multiply(ey);
  1070.         final T e2         = ex2.add(ey2);
  1071.         final T epsilon    = e2.negate().add(1.0).sqrt();
  1072.         final T beta       = epsilon.add(1).reciprocal();

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

  1077.         final FieldSinCos<T> scI    = FastMath.sinCos(i);
  1078.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  1079.         final T cosI       = scI.cos();
  1080.         final T sinI       = scI.sin();
  1081.         final T cosRaan    = scRaan.cos();
  1082.         final T sinRaan    = scRaan.sin();

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

  1088.         // differentials of the normalized momentum
  1089.         final FieldVector3D<T> danP = new FieldVector3D<>(v2, position, pv.negate(), velocity);
  1090.         final FieldVector3D<T> danV = new FieldVector3D<>(r2, velocity, pv.negate(), position);
  1091.         final T recip   = partialPV.getMomentum().getNorm().reciprocal();
  1092.         final T recip2  = recip.multiply(recip);
  1093.         final T recip2N = recip2.negate();
  1094.         final FieldVector3D<T> dwXP = new FieldVector3D<>(recip,
  1095.                                                           new FieldVector3D<>(zero, vz, vy.negate()),
  1096.                                                           recip2N.multiply(sinRaan).multiply(sinI),
  1097.                                                           danP);
  1098.         final FieldVector3D<T> dwYP = new FieldVector3D<>(recip,
  1099.                                                           new FieldVector3D<>(vz.negate(), zero, vx),
  1100.                                                           recip2.multiply(cosRaan).multiply(sinI),
  1101.                                                           danP);
  1102.         final FieldVector3D<T> dwZP = new FieldVector3D<>(recip,
  1103.                                                           new FieldVector3D<>(vy, vx.negate(), zero),
  1104.                                                           recip2N.multiply(cosI),
  1105.                                                           danP);
  1106.         final FieldVector3D<T> dwXV = new FieldVector3D<>(recip,
  1107.                                                           new FieldVector3D<>(zero, z.negate(), y),
  1108.                                                           recip2N.multiply(sinRaan).multiply(sinI),
  1109.                                                           danV);
  1110.         final FieldVector3D<T> dwYV = new FieldVector3D<>(recip,
  1111.                                                           new FieldVector3D<>(z, zero, x.negate()),
  1112.                                                           recip2.multiply(cosRaan).multiply(sinI),
  1113.                                                           danV);
  1114.         final FieldVector3D<T> dwZV = new FieldVector3D<>(recip,
  1115.                                                           new FieldVector3D<>(y.negate(), x, zero),
  1116.                                                           recip2N.multiply(cosI),
  1117.                                                           danV);

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

  1123.         // dRaan
  1124.         fillHalfRow(sinRaan.divide(sinI), dwYP, cosRaan.divide(sinI), dwXP, jacobian[4], 0);
  1125.         fillHalfRow(sinRaan.divide(sinI), dwYV, cosRaan.divide(sinI), dwXV, jacobian[4], 3);

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

  1133.         // du
  1134.         final FieldVector3D<T> duP = new FieldVector3D<>(cv.multiply(cosRaan).divide(sinI), dwXP,
  1135.                                                          cv.multiply(sinRaan).divide(sinI), dwYP,
  1136.                                                          one, new FieldVector3D<>(cosRaan, sinRaan, zero));
  1137.         final FieldVector3D<T> duV = new FieldVector3D<>(cv.multiply(cosRaan).divide(sinI), dwXV,
  1138.                                                          cv.multiply(sinRaan).divide(sinI), dwYV);

  1140.         // dv
  1141.         final FieldVector3D<T> dvP = new FieldVector3D<>(u.negate().multiply(cosRaan).multiply(cosI).divide(sinI).add(sinRaan.multiply(z)), dwXP,
  1142.                                                          u.negate().multiply(sinRaan).multiply(cosI).divide(sinI).subtract(cosRaan.multiply(z)), dwYP,
  1143.                                                          cv, dwZP,
  1144.                                                          one, new FieldVector3D<>(sinRaan.negate().multiply(cosI), cosRaan.multiply(cosI), sinI));
  1145.         final FieldVector3D<T> dvV = new FieldVector3D<>(u.negate().multiply(cosRaan).multiply(cosI).divide(sinI).add(sinRaan.multiply(z)), dwXV,
  1146.                                                          u.negate().multiply(sinRaan).multiply(cosI).divide(sinI).subtract(cosRaan.multiply(z)), dwYV,
  1147.                                                          cv, dwZV);

  1149.         final FieldVector3D<T> dc1P = new FieldVector3D<>(aOr2.multiply(eSinE.multiply(eSinE).multiply(2).add(1).subtract(eCosE)).divide(r2), position,
  1150.                                                           aOr2.multiply(-2).multiply(eSinE).multiply(oOsqrtMuA), velocity);
  1151.         final FieldVector3D<T> dc1V = new FieldVector3D<>(aOr2.multiply(-2).multiply(eSinE).multiply(oOsqrtMuA), position,
  1152.                                                           zero.add(2).divide(mu), velocity);
  1153.         final FieldVector3D<T> dc2P = new FieldVector3D<>(aOr2.multiply(eSinE).multiply(eSinE.multiply(eSinE).subtract(e2.negate().add(1))).divide(r2.multiply(epsilon)), position,
  1154.                                                           aOr2.multiply(e2.negate().add(1).subtract(eSinE.multiply(eSinE))).multiply(oOsqrtMuA).divide(epsilon), velocity);
  1155.         final FieldVector3D<T> dc2V = new FieldVector3D<>(aOr2.multiply(e2.negate().add(1).subtract(eSinE.multiply(eSinE))).multiply(oOsqrtMuA).divide(epsilon), position,
  1156.                                                           eSinE.divide(epsilon.multiply(mu)), velocity);

  1158.         final T cof1   = aOr2.multiply(eCosE.subtract(e2));
  1159.         final T cof2   = aOr2.multiply(epsilon).multiply(eSinE);
  1160.         final FieldVector3D<T> dexP = new FieldVector3D<>(u, dc1P,  v, dc2P, cof1, duP,  cof2, dvP);
  1161.         final FieldVector3D<T> dexV = new FieldVector3D<>(u, dc1V,  v, dc2V, cof1, duV,  cof2, dvV);
  1162.         final FieldVector3D<T> deyP = new FieldVector3D<>(v, dc1P, u.negate(), dc2P, cof1, dvP, cof2.negate(), duP);
  1163.         final FieldVector3D<T> deyV = new FieldVector3D<>(v, dc1V, u.negate(), dc2V, cof1, dvV, cof2.negate(), duV);
  1164.         fillHalfRow(one, dexP, jacobian[1], 0);
  1165.         fillHalfRow(one, dexV, jacobian[1], 3);
  1166.         fillHalfRow(one, deyP, jacobian[2], 0);
  1167.         fillHalfRow(one, deyV, jacobian[2], 3);

  1169.         final T cle = u.divide(a).add(ex).subtract(eSinE.multiply(beta).multiply(ey));
  1170.         final T sle = v.divide(a).add(ey).add(eSinE.multiply(beta).multiply(ex));
  1171.         final T m1  = beta.multiply(eCosE);
  1172.         final T m2  = m1.multiply(eCosE).negate().add(1);
  1173.         final T m3  = (u.multiply(ey).subtract(v.multiply(ex))).add(eSinE.multiply(beta).multiply(u.multiply(ex).add(v.multiply(ey))));
  1174.         final T m4  = sle.negate().add(cle.multiply(eSinE).multiply(beta));
  1175.         final T m5  = cle.add(sle.multiply(eSinE).multiply(beta));
  1176.         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);
  1177.         final T jj = (m1.multiply(m2).divide(epsilon).subtract(1)).multiply(oOsqrtMuA);
  1178.         fillHalfRow(kk, position,
  1179.                     jj, velocity,
  1180.                     m4, dexP,
  1181.                     m5, deyP,
  1182.                     sle.negate().divide(a), duP,
  1183.                     cle.divide(a), dvP,
  1184.                     jacobian[5], 0);
  1185.         final T ll = (m1.multiply(m2).divide(epsilon ).subtract(1)).multiply(oOsqrtMuA);
  1186.         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);

  1188.         fillHalfRow(ll, position,
  1189.                     mm, velocity,
  1190.                     m4, dexV,
  1191.                     m5, deyV,
  1192.                     sle.negate().divide(a), duV,
  1193.                     cle.divide(a), dvV,
  1194.                     jacobian[5], 3);
  1195.         return jacobian;

  1197.     }

  1199.     /** {@inheritDoc} */
  1200.     protected T[][] computeJacobianEccentricWrtCartesian() {

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

  1205.         // Differentiating the Kepler equation aM = aE - ex sin aE + ey cos aE leads to:
  1206.         // daM = (1 - ex cos aE - ey sin aE) dE - sin aE dex + cos aE dey
  1207.         // which is inverted and rewritten as:
  1208.         // daE = a/r daM + sin aE a/r dex - cos aE a/r dey
  1209.         final T alphaE            = getAlphaE();
  1210.         final FieldSinCos<T> scAe = FastMath.sinCos(alphaE);
  1211.         final T cosAe             = scAe.cos();
  1212.         final T sinAe             = scAe.sin();
  1213.         final T aOr               = one.divide(one.subtract(ex.multiply(cosAe)).subtract(ey.multiply(sinAe)));

  1215.         // update longitude row
  1216.         final T[] rowEx = jacobian[1];
  1217.         final T[] rowEy = jacobian[2];
  1218.         final T[] rowL  = jacobian[5];
  1219.         for (int j = 0; j < 6; ++j) {
  1220.          // rowL[j] = aOr * (      rowL[j] +   sinAe *        rowEx[j]   -         cosAe *        rowEy[j]);
  1221.             rowL[j] = aOr.multiply(rowL[j].add(sinAe.multiply(rowEx[j])).subtract( cosAe.multiply(rowEy[j])));
  1222.         }
  1223.         jacobian[5] = rowL;
  1224.         return jacobian;

  1226.     }
  1227.     /** {@inheritDoc} */
  1228.     protected T[][] computeJacobianTrueWrtCartesian() {

  1230.         // start by computing the Jacobian with eccentric angle
  1231.         final T[][] jacobian = computeJacobianEccentricWrtCartesian();
  1232.         // Differentiating the eccentric latitude equation
  1233.         // tan((aV - aE)/2) = [ex sin aE - ey cos aE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos aE - ey sin aE]
  1234.         // leads to
  1235.         // cT (daV - daE) = cE daE + cX dex + cY dey
  1236.         // with
  1237.         // cT = [d^2 + (ex sin aE - ey cos aE)^2] / 2
  1238.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos aE - ey sin aE
  1239.         // cE = (ex cos aE + ey sin aE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  1240.         // cX =  sin aE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin aE - ey cos aE) / sqrt(1-ex^2-ey^2)
  1241.         // cY = -cos aE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin aE - ey cos aE) / sqrt(1-ex^2-ey^2)
  1242.         // which can be solved to find the differential of the true latitude
  1243.         // daV = (cT + cE) / cT daE + cX / cT deX + cY / cT deX
  1244.         final T alphaE            = getAlphaE();
  1245.         final FieldSinCos<T> scAe = FastMath.sinCos(alphaE);
  1246.         final T cosAe             = scAe.cos();
  1247.         final T sinAe             = scAe.sin();
  1248.         final T eSinE             = ex.multiply(sinAe).subtract(ey.multiply(cosAe));
  1249.         final T ecosE             = ex.multiply(cosAe).add(ey.multiply(sinAe));
  1250.         final T e2                = ex.multiply(ex).add(ey.multiply(ey));
  1251.         final T epsilon           = (one.subtract(e2)).sqrt();
  1252.         final T onePeps           = one.add(epsilon);
  1253.         final T d                 = onePeps.subtract(ecosE);
  1254.         final T cT                = (d.multiply(d).add(eSinE.multiply(eSinE))).divide(2);
  1255.         final T cE                = ecosE.multiply(onePeps).subtract(e2);
  1256.         final T cX                = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinAe.multiply(onePeps));
  1257.         final T cY                = ey.multiply(eSinE).divide(epsilon).add(ex).subtract(cosAe.multiply(onePeps));
  1258.         final T factorLe          = (cT.add(cE)).divide(cT);
  1259.         final T factorEx          = cX.divide(cT);
  1260.         final T factorEy          = cY.divide(cT);

  1262.         // update latitude row
  1263.         final T[] rowEx = jacobian[1];
  1264.         final T[] rowEy = jacobian[2];
  1265.         final T[] rowA = jacobian[5];
  1266.         for (int j = 0; j < 6; ++j) {
  1267.             rowA[j] = factorLe.multiply(rowA[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
  1268.         }
  1269.         return jacobian;

  1271.     }

  1273.     /** {@inheritDoc} */
  1274.     public void addKeplerContribution(final PositionAngle type, final T gm,
  1275.                                       final T[] pDot) {
  1276.         final T oMe2;
  1277.         final T ksi;
  1278.         final T n = a.reciprocal().multiply(gm).sqrt().divide(a);
  1279.         final FieldSinCos<T> sc = FastMath.sinCos(alphaV);
  1280.         switch (type) {
  1281.             case MEAN :
  1282.                 pDot[5] = pDot[5].add(n);
  1283.                 break;
  1284.             case ECCENTRIC :
  1285.                 oMe2  = one.subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1286.                 ksi   = one.add(ex.multiply(sc.cos())).add(ey.multiply(sc.sin()));
  1287.                 pDot[5] = pDot[5].add(n.multiply(ksi).divide(oMe2));
  1288.                 break;
  1289.             case TRUE :
  1290.                 oMe2  = one.subtract(ex.multiply(ex)).subtract(ey.multiply(ey));
  1291.                 ksi   = one.add(ex.multiply(sc.cos())).add(ey.multiply(sc.sin()));
  1292.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1293.                 break;
  1294.             default :
  1295.                 throw new OrekitInternalError(null);
  1296.         }
  1297.     }

  1299.     /**  Returns a string representation of this Orbit object.
  1300.      * @return a string representation of this object
  1301.      */
  1302.     public String toString() {
  1303.         return new StringBuilder().append("circular parameters: ").append('{').
  1304.                                   append("a: ").append(a.getReal()).
  1305.                                   append(", ex: ").append(ex.getReal()).append(", ey: ").append(ey.getReal()).
  1306.                                   append(", i: ").append(FastMath.toDegrees(i.getReal())).
  1307.                                   append(", raan: ").append(FastMath.toDegrees(raan.getReal())).
  1308.                                   append(", alphaV: ").append(FastMath.toDegrees(alphaV.getReal())).
  1309.                                   append(";}").toString();
  1310.     }

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


  1331. }
Created with JaCoCo 0.8.7.202105040129