FieldCartesianOrbit.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;


  18. import java.util.Arrays;
  19. import java.util.stream.Stream;

  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.Field;
  22. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  23. import org.hipparchus.exception.LocalizedCoreFormats;
  24. import org.hipparchus.exception.MathIllegalStateException;
  25. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  26. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  27. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  28. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  29. import org.hipparchus.util.FastMath;
  30. import org.hipparchus.util.FieldSinCos;
  31. import org.hipparchus.util.MathArrays;
  32. import org.orekit.frames.Frame;
  33. import org.orekit.time.AbsoluteDate;
  34. import org.orekit.time.FieldAbsoluteDate;
  35. import org.orekit.utils.CartesianDerivativesFilter;
  36. import org.orekit.utils.FieldPVCoordinates;
  37. import org.orekit.utils.PVCoordinates;
  38. import org.orekit.utils.TimeStampedFieldPVCoordinates;


  39. /** This class holds Cartesian orbital parameters.

  40.  * <p>
  41.  * The parameters used internally are the Cartesian coordinates:
  42.  *   <ul>
  43.  *     <li>x</li>
  44.  *     <li>y</li>
  45.  *     <li>z</li>
  46.  *     <li>xDot</li>
  47.  *     <li>yDot</li>
  48.  *     <li>zDot</li>
  49.  *   </ul>
  50.  * contained in {@link PVCoordinates}.

  51.  * <p>
  52.  * Note that the implementation of this class delegates all non-Cartesian related
  53.  * computations ({@link #getA()}, {@link #getEquinoctialEx()}, ...) to an underlying
  54.  * instance of the {@link EquinoctialOrbit} class. This implies that using this class
  55.  * only for analytical computations which are always based on non-Cartesian
  56.  * parameters is perfectly possible but somewhat sub-optimal.
  57.  * </p>
  58.  * <p>
  59.  * The instance <code>CartesianOrbit</code> is guaranteed to be immutable.
  60.  * </p>
  61.  * @see    Orbit
  62.  * @see    KeplerianOrbit
  63.  * @see    CircularOrbit
  64.  * @see    EquinoctialOrbit
  65.  * @author Luc Maisonobe
  66.  * @author Guylaine Prat
  67.  * @author Fabien Maussion
  68.  * @author V&eacute;ronique Pommier-Maurussane
  69.  * @since 9.0
  70.  */
  71. public class FieldCartesianOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T> {

  72.     /** Indicator for non-Keplerian acceleration. */
  73.     private final transient boolean hasNonKeplerianAcceleration;

  74.     /** Underlying equinoctial orbit to which high-level methods are delegated. */
  75.     private transient FieldEquinoctialOrbit<T> equinoctial;

  76.     /** Field used by this class.*/
  77.     private final Field<T> field;

  78.     /** Zero. (could be usefull)*/
  79.     private final T zero;

  80.     /** One. (could be useful)*/
  81.     private final T one;

  82.     /** Constructor from Cartesian parameters.
  83.      *
  84.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  85.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  86.      * use {@code mu} and the position to compute the acceleration, including
  87.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  88.      *
  89.      * @param pvaCoordinates the position, velocity and acceleration of the satellite.
  90.      * @param frame the frame in which the {@link PVCoordinates} are defined
  91.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  92.      * @param mu central attraction coefficient (m³/s²)
  93.      * @exception IllegalArgumentException if frame is not a {@link
  94.      * Frame#isPseudoInertial pseudo-inertial frame}
  95.      */
  96.     public FieldCartesianOrbit(final TimeStampedFieldPVCoordinates<T> pvaCoordinates,
  97.                                final Frame frame, final T mu)
  98.         throws IllegalArgumentException {
  99.         super(pvaCoordinates, frame, mu);
  100.         hasNonKeplerianAcceleration = hasNonKeplerianAcceleration(pvaCoordinates, mu);
  101.         equinoctial = null;
  102.         field = pvaCoordinates.getPosition().getX().getField();
  103.         zero = field.getZero();
  104.         one = field.getOne();
  105.     }

  106.     /** Constructor from Cartesian parameters.
  107.      *
  108.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  109.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  110.      * use {@code mu} and the position to compute the acceleration, including
  111.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  112.      *
  113.      * @param pvaCoordinates the position and velocity of the satellite.
  114.      * @param frame the frame in which the {@link PVCoordinates} are defined
  115.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  116.      * @param date date of the orbital parameters
  117.      * @param mu central attraction coefficient (m³/s²)
  118.      * @exception IllegalArgumentException if frame is not a {@link
  119.      * Frame#isPseudoInertial pseudo-inertial frame}
  120.      */
  121.     public FieldCartesianOrbit(final FieldPVCoordinates<T> pvaCoordinates, final Frame frame,
  122.                                final FieldAbsoluteDate<T> date, final T mu)
  123.         throws IllegalArgumentException {
  124.         this(new TimeStampedFieldPVCoordinates<>(date, pvaCoordinates), frame, mu);
  125.     }

  126.     /** Constructor from any kind of orbital parameters.
  127.      * @param op orbital parameters to copy
  128.      */
  129.     public FieldCartesianOrbit(final FieldOrbit<T> op) {
  130.         super(op.getPVCoordinates(), op.getFrame(), op.getMu());
  131.         hasNonKeplerianAcceleration = op.hasDerivatives();
  132.         if (op instanceof FieldEquinoctialOrbit) {
  133.             equinoctial = (FieldEquinoctialOrbit<T>) op;
  134.         } else if (op instanceof FieldCartesianOrbit) {
  135.             equinoctial = ((FieldCartesianOrbit<T>) op).equinoctial;
  136.         } else {
  137.             equinoctial = null;
  138.         }
  139.         field = op.getA().getField();
  140.         zero = field.getZero();
  141.         one = field.getOne();
  142.     }

  143.     /** {@inheritDoc} */
  144.     public OrbitType getType() {
  145.         return OrbitType.CARTESIAN;
  146.     }

  147.     /** Lazy evaluation of equinoctial parameters. */
  148.     private void initEquinoctial() {
  149.         if (equinoctial == null) {
  150.             if (hasDerivatives()) {
  151.                 // getPVCoordinates includes accelerations that will be interpreted as derivatives
  152.                 equinoctial = new FieldEquinoctialOrbit<>(getPVCoordinates(), getFrame(), getDate(), getMu());
  153.             } else {
  154.                 // get rid of Keplerian acceleration so we don't assume
  155.                 // we have derivatives when in fact we don't have them
  156.                 equinoctial = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(getPVCoordinates().getPosition(),
  157.                                                                                    getPVCoordinates().getVelocity()),
  158.                                                           getFrame(), getDate(), getMu());
  159.             }
  160.         }
  161.     }

  162.     /** Get the position/velocity with derivatives.
  163.      * @return position/velocity with derivatives
  164.      * @since 10.2
  165.      */
  166.     private FieldPVCoordinates<FieldUnivariateDerivative2<T>> getPVDerivatives() {
  167.         // PVA coordinates
  168.         final FieldPVCoordinates<T> pva = getPVCoordinates();
  169.         final FieldVector3D<T>      p   = pva.getPosition();
  170.         final FieldVector3D<T>      v   = pva.getVelocity();
  171.         final FieldVector3D<T>      a   = pva.getAcceleration();
  172.         // Field coordinates
  173.         final FieldVector3D<FieldUnivariateDerivative2<T>> pG = new FieldVector3D<>(new FieldUnivariateDerivative2<>(p.getX(), v.getX(), a.getX()),
  174.                                                              new FieldUnivariateDerivative2<>(p.getY(), v.getY(), a.getY()),
  175.                                                              new FieldUnivariateDerivative2<>(p.getZ(), v.getZ(), a.getZ()));
  176.         final FieldVector3D<FieldUnivariateDerivative2<T>> vG = new FieldVector3D<>(new FieldUnivariateDerivative2<>(v.getX(), a.getX(), zero),
  177.                                                              new FieldUnivariateDerivative2<>(v.getY(), a.getY(), zero),
  178.                                                              new FieldUnivariateDerivative2<>(v.getZ(), a.getZ(), zero));
  179.         return new FieldPVCoordinates<>(pG, vG);
  180.     }

  181.     /** {@inheritDoc} */
  182.     public T getA() {
  183.         // lazy evaluation of semi-major axis
  184.         final T r  = getPVCoordinates().getPosition().getNorm();
  185.         final T V2 = getPVCoordinates().getVelocity().getNormSq();
  186.         return r.divide(r.negate().multiply(V2).divide(getMu()).add(2));
  187.     }

  188.     /** {@inheritDoc} */
  189.     public T getADot() {
  190.         if (hasDerivatives()) {
  191.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  192.             final FieldUnivariateDerivative2<T> r  = pv.getPosition().getNorm();
  193.             final FieldUnivariateDerivative2<T> V2 = pv.getVelocity().getNormSq();
  194.             final FieldUnivariateDerivative2<T> a  = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
  195.             return a.getDerivative(1);
  196.         } else {
  197.             return null;
  198.         }
  199.     }

  200.     /** {@inheritDoc} */
  201.     public T getE() {
  202.         final T muA = getA().multiply(getMu());
  203.         if (muA.getReal() > 0) {
  204.             // elliptic or circular orbit
  205.             final FieldVector3D<T> pvP   = getPVCoordinates().getPosition();
  206.             final FieldVector3D<T> pvV   = getPVCoordinates().getVelocity();
  207.             final T rV2OnMu = pvP.getNorm().multiply(pvV.getNormSq()).divide(getMu());
  208.             final T eSE     = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  209.             final T eCE     = rV2OnMu.subtract(1);
  210.             return (eCE.multiply(eCE).add(eSE.multiply(eSE))).sqrt();
  211.         } else {
  212.             // hyperbolic orbit
  213.             final FieldVector3D<T> pvM = getPVCoordinates().getMomentum();
  214.             return pvM.getNormSq().divide(muA).negate().add(1).sqrt();
  215.         }
  216.     }

  217.     /** {@inheritDoc} */
  218.     public T getEDot() {
  219.         if (hasDerivatives()) {
  220.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  221.             final FieldUnivariateDerivative2<T> r       = pv.getPosition().getNorm();
  222.             final FieldUnivariateDerivative2<T> V2      = pv.getVelocity().getNormSq();
  223.             final FieldUnivariateDerivative2<T> rV2OnMu = r.multiply(V2).divide(getMu());
  224.             final FieldUnivariateDerivative2<T> a       = r.divide(rV2OnMu.negate().add(2));
  225.             final FieldUnivariateDerivative2<T> eSE     = FieldVector3D.dotProduct(pv.getPosition(), pv.getVelocity()).divide(a.multiply(getMu()).sqrt());
  226.             final FieldUnivariateDerivative2<T> eCE     = rV2OnMu.subtract(1);
  227.             final FieldUnivariateDerivative2<T> e       = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
  228.             return e.getDerivative(1);
  229.         } else {
  230.             return null;
  231.         }
  232.     }

  233.     /** {@inheritDoc} */
  234.     public T getI() {
  235.         return FieldVector3D.angle(new FieldVector3D<>(zero, zero, one), getPVCoordinates().getMomentum());
  236.     }

  237.     /** {@inheritDoc} */
  238.     public T getIDot() {
  239.         if (hasDerivatives()) {
  240.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  241.             final FieldVector3D<FieldUnivariateDerivative2<T>> momentum =
  242.                             FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity());
  243.             final FieldUnivariateDerivative2<T> i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
  244.             return i.getDerivative(1);
  245.         } else {
  246.             return null;
  247.         }
  248.     }

  249.     /** {@inheritDoc} */
  250.     public T getEquinoctialEx() {
  251.         initEquinoctial();
  252.         return equinoctial.getEquinoctialEx();
  253.     }

  254.     /** {@inheritDoc} */
  255.     public T getEquinoctialExDot() {
  256.         initEquinoctial();
  257.         return equinoctial.getEquinoctialExDot();
  258.     }

  259.     /** {@inheritDoc} */
  260.     public T getEquinoctialEy() {
  261.         initEquinoctial();
  262.         return equinoctial.getEquinoctialEy();
  263.     }

  264.     /** {@inheritDoc} */
  265.     public T getEquinoctialEyDot() {
  266.         initEquinoctial();
  267.         return equinoctial.getEquinoctialEyDot();
  268.     }

  269.     /** {@inheritDoc} */
  270.     public T getHx() {
  271.         final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
  272.         // Check for equatorial retrograde orbit
  273.         final double x = w.getX().getReal();
  274.         final double y = w.getY().getReal();
  275.         final double z = w.getZ().getReal();
  276.         if ((x * x + y * y) == 0 && z < 0) {
  277.             return zero.add(Double.NaN);
  278.         }
  279.         return w.getY().negate().divide(w.getZ().add(1));
  280.     }

  281.     /** {@inheritDoc} */
  282.     public T getHxDot() {
  283.         if (hasDerivatives()) {
  284.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  285.             final FieldVector3D<FieldUnivariateDerivative2<T>> w =
  286.                             FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
  287.             // Check for equatorial retrograde orbit
  288.             final double x = w.getX().getValue().getReal();
  289.             final double y = w.getY().getValue().getReal();
  290.             final double z = w.getZ().getValue().getReal();
  291.             if ((x * x + y * y) == 0 && z < 0) {
  292.                 return zero.add(Double.NaN);
  293.             }
  294.             final FieldUnivariateDerivative2<T> hx = w.getY().negate().divide(w.getZ().add(1));
  295.             return hx.getDerivative(1);
  296.         } else {
  297.             return null;
  298.         }
  299.     }

  300.     /** {@inheritDoc} */
  301.     public T getHy() {
  302.         final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
  303.         // Check for equatorial retrograde orbit
  304.         final double x = w.getX().getReal();
  305.         final double y = w.getY().getReal();
  306.         final double z = w.getZ().getReal();
  307.         if ((x * x + y * y) == 0 && z < 0) {
  308.             return zero.add(Double.NaN);
  309.         }
  310.         return  w.getX().divide(w.getZ().add(1));
  311.     }

  312.     /** {@inheritDoc} */
  313.     public T getHyDot() {
  314.         if (hasDerivatives()) {
  315.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  316.             final FieldVector3D<FieldUnivariateDerivative2<T>> w =
  317.                             FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
  318.             // Check for equatorial retrograde orbit
  319.             final double x = w.getX().getValue().getReal();
  320.             final double y = w.getY().getValue().getReal();
  321.             final double z = w.getZ().getValue().getReal();
  322.             if ((x * x + y * y) == 0 && z < 0) {
  323.                 return zero.add(Double.NaN);
  324.             }
  325.             final FieldUnivariateDerivative2<T> hy = w.getX().divide(w.getZ().add(1));
  326.             return hy.getDerivative(1);
  327.         } else {
  328.             return null;
  329.         }
  330.     }

  331.     /** {@inheritDoc} */
  332.     public T getLv() {
  333.         initEquinoctial();
  334.         return equinoctial.getLv();
  335.     }

  336.     /** {@inheritDoc} */
  337.     public T getLvDot() {
  338.         initEquinoctial();
  339.         return equinoctial.getLvDot();
  340.     }

  341.     /** {@inheritDoc} */
  342.     public T getLE() {
  343.         initEquinoctial();
  344.         return equinoctial.getLE();
  345.     }

  346.     /** {@inheritDoc} */
  347.     public T getLEDot() {
  348.         initEquinoctial();
  349.         return equinoctial.getLEDot();
  350.     }

  351.     /** {@inheritDoc} */
  352.     public T getLM() {
  353.         initEquinoctial();
  354.         return equinoctial.getLM();
  355.     }

  356.     /** {@inheritDoc} */
  357.     public T getLMDot() {
  358.         initEquinoctial();
  359.         return equinoctial.getLMDot();
  360.     }

  361.     /** {@inheritDoc} */
  362.     public boolean hasDerivatives() {
  363.         return hasNonKeplerianAcceleration;
  364.     }

  365.     /** {@inheritDoc} */
  366.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {
  367.         // nothing to do here, as the canonical elements are already the Cartesian ones
  368.         return getPVCoordinates();
  369.     }

  370.     /** {@inheritDoc} */
  371.     public FieldCartesianOrbit<T> shiftedBy(final double dt) {
  372.         return shiftedBy(getDate().getField().getZero().add(dt));
  373.     }

  374.     /** {@inheritDoc} */
  375.     public FieldCartesianOrbit<T> shiftedBy(final T dt) {
  376.         final FieldPVCoordinates<T> shiftedPV = (getA().getReal() < 0) ? shiftPVHyperbolic(dt) : shiftPVElliptic(dt);
  377.         return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
  378.     }

  379.     /** {@inheritDoc}
  380.      * <p>
  381.      * The interpolated instance is created by polynomial Hermite interpolation
  382.      * ensuring velocity remains the exact derivative of position.
  383.      * </p>
  384.      * <p>
  385.      * As this implementation of interpolation is polynomial, it should be used only
  386.      * with small samples (about 10-20 points) in order to avoid <a
  387.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  388.      * and numerical problems (including NaN appearing).
  389.      * </p>
  390.      * <p>
  391.      * If orbit interpolation on large samples is needed, using the {@link
  392.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  393.      * low-level interpolation. The Ephemeris class automatically handles selection of
  394.      * a neighboring sub-sample with a predefined number of point from a large global sample
  395.      * in a thread-safe way.
  396.      * </p>
  397.      */
  398.     public FieldCartesianOrbit<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldOrbit<T>> sample) {
  399.         final TimeStampedFieldPVCoordinates<T> interpolated =
  400.                 TimeStampedFieldPVCoordinates.interpolate(date, CartesianDerivativesFilter.USE_PVA,
  401.                                                           sample.map(orbit -> orbit.getPVCoordinates()));
  402.         return new FieldCartesianOrbit<>(interpolated, getFrame(), date, getMu());
  403.     }

  404.     /** Compute shifted position and velocity in elliptic case.
  405.      * @param dt time shift
  406.      * @return shifted position and velocity
  407.      */
  408.     private FieldPVCoordinates<T> shiftPVElliptic(final T dt) {

  409.         // preliminary computation
  410.         final FieldVector3D<T> pvP   = getPVCoordinates().getPosition();
  411.         final FieldVector3D<T> pvV   = getPVCoordinates().getVelocity();
  412.         final T r2      = pvP.getNormSq();
  413.         final T r       = r2.sqrt();
  414.         final T rV2OnMu = r.multiply(pvV.getNormSq()).divide(getMu());
  415.         final T a       = r.divide(rV2OnMu.negate().add(2));
  416.         final T eSE     = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
  417.         final T eCE     = rV2OnMu.subtract(1);
  418.         final T e2      = eCE.multiply(eCE).add(eSE.multiply(eSE));

  419.         // we can use any arbitrary reference 2D frame in the orbital plane
  420.         // in order to simplify some equations below, we use the current position as the u axis
  421.         final FieldVector3D<T> u     = pvP.normalize();
  422.         final FieldVector3D<T> v     = FieldVector3D.crossProduct(FieldVector3D.crossProduct(pvP, pvV), u).normalize();

  423.         // the following equations rely on the specific choice of u explained above,
  424.         // some coefficients that vanish to 0 in this case have already been removed here
  425.         final T              ex        = eCE.subtract(e2).multiply(a).divide(r);
  426.         final T              s         = e2.negate().add(1).sqrt();
  427.         final T              ey        = s.negate().multiply(eSE).multiply(a).divide(r);
  428.         final T              beta      = s.add(1).reciprocal();
  429.         final T              thetaE0   = ey.add(eSE.multiply(beta).multiply(ex)).atan2(r.divide(a).add(ex).subtract(eSE.multiply(beta).multiply(ey)));
  430.         final FieldSinCos<T> scThetaE0 = FastMath.sinCos(thetaE0);
  431.         final T              thetaM0   = thetaE0.subtract(ex.multiply(scThetaE0.sin())).add(ey.multiply(scThetaE0.cos()));

  432.         // compute in-plane shifted eccentric argument
  433.         final T              sqrtMmuOA = a.reciprocal().multiply(getMu()).sqrt();
  434.         final T              thetaM1   = thetaM0.add(sqrtMmuOA.divide(a).multiply(dt));
  435.         final T              thetaE1   = meanToEccentric(thetaM1, ex, ey);
  436.         final FieldSinCos<T> scTE      = FastMath.sinCos(thetaE1);
  437.         final T              cTE       = scTE.cos();
  438.         final T              sTE       = scTE.sin();

  439.         // compute shifted in-plane Cartesian coordinates
  440.         final T exey   = ex.multiply(ey);
  441.         final T exCeyS = ex.multiply(cTE).add(ey.multiply(sTE));
  442.         final T x      = a.multiply(beta.multiply(ey).multiply(ey).negate().add(1).multiply(cTE).add(beta.multiply(exey).multiply(sTE)).subtract(ex));
  443.         final T y      = a.multiply(beta.multiply(ex).multiply(ex).negate().add(1).multiply(sTE).add(beta.multiply(exey).multiply(cTE)).subtract(ey));
  444.         final T factor = sqrtMmuOA.divide(exCeyS.negate().add(1));
  445.         final T xDot   = factor.multiply(beta.multiply(ey).multiply(exCeyS).subtract(sTE));
  446.         final T yDot   = factor.multiply(cTE.subtract(beta.multiply(ex).multiply(exCeyS)));

  447.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, u, y, v);
  448.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, u, yDot, v);
  449.         if (hasNonKeplerianAcceleration) {

  450.             // extract non-Keplerian part of the initial acceleration
  451.             final FieldVector3D<T> nonKeplerianAcceleration = new FieldVector3D<>(one, getPVCoordinates().getAcceleration(),
  452.                                                                                   r.multiply(r2).reciprocal().multiply(getMu()), pvP);

  453.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  454.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, shiftedP,
  455.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  456.             final T                fixedR2 = fixedP.getNormSq();
  457.             final T                fixedR  = fixedR2.sqrt();
  458.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, shiftedV,
  459.                                                                  dt, nonKeplerianAcceleration);
  460.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR.multiply(fixedR2).reciprocal().multiply(getMu().negate()), shiftedP,
  461.                                                                  one, nonKeplerianAcceleration);

  462.             return new FieldPVCoordinates<>(fixedP, fixedV, fixedA);

  463.         } else {
  464.             // don't include acceleration,
  465.             // so the shifted orbit is not considered to have derivatives
  466.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  467.         }

  468.     }

  469.     /** Compute shifted position and velocity in hyperbolic case.
  470.      * @param dt time shift
  471.      * @return shifted position and velocity
  472.      */
  473.     private FieldPVCoordinates<T> shiftPVHyperbolic(final T dt) {

  474.         final FieldPVCoordinates<T> pv = getPVCoordinates();
  475.         final FieldVector3D<T> pvP   = pv.getPosition();
  476.         final FieldVector3D<T> pvV   = pv.getVelocity();
  477.         final FieldVector3D<T> pvM   = pv.getMomentum();
  478.         final T r2      = pvP.getNormSq();
  479.         final T r       = r2.sqrt();
  480.         final T rV2OnMu = r.multiply(pvV.getNormSq()).divide(getMu());
  481.         final T a       = getA();
  482.         final T muA     = a.multiply(getMu());
  483.         final T e       = one.subtract(FieldVector3D.dotProduct(pvM, pvM).divide(muA)).sqrt();
  484.         final T sqrt    = e.add(1).divide(e.subtract(1)).sqrt();

  485.         // compute mean anomaly
  486.         final T eSH     = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  487.         final T eCH     = rV2OnMu.subtract(1);
  488.         final T H0      = eCH.add(eSH).divide(eCH.subtract(eSH)).log().divide(2);
  489.         final T M0      = e.multiply(H0.sinh()).subtract(H0);

  490.         // find canonical 2D frame with p pointing to perigee
  491.         final T v0      = sqrt.multiply(H0.divide(2).tanh()).atan().multiply(2);
  492.         final FieldVector3D<T> p     = new FieldRotation<>(pvM, v0, RotationConvention.FRAME_TRANSFORM).applyTo(pvP).normalize();
  493.         final FieldVector3D<T> q     = FieldVector3D.crossProduct(pvM, p).normalize();

  494.         // compute shifted eccentric anomaly
  495.         final T M1      = M0.add(getKeplerianMeanMotion().multiply(dt));
  496.         final T H1      = FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);

  497.         // compute shifted in-plane Cartesian coordinates
  498.         final T cH     = H1.cosh();
  499.         final T sH     = H1.sinh();
  500.         final T sE2m1  = e.subtract(1).multiply(e.add(1)).sqrt();

  501.         // coordinates of position and velocity in the orbital plane
  502.         final T x      = a.multiply(cH.subtract(e));
  503.         final T y      = a.negate().multiply(sE2m1).multiply(sH);
  504.         final T factor = getMu().divide(a.negate()).sqrt().divide(e.multiply(cH).subtract(1));
  505.         final T xDot   = factor.negate().multiply(sH);
  506.         final T yDot   =  factor.multiply(sE2m1).multiply(cH);

  507.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, p, y, q);
  508.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, p, yDot, q);
  509.         if (hasNonKeplerianAcceleration) {

  510.             // extract non-Keplerian part of the initial acceleration
  511.             final FieldVector3D<T> nonKeplerianAcceleration = new FieldVector3D<>(one, getPVCoordinates().getAcceleration(),
  512.                                                                                   r.multiply(r2).reciprocal().multiply(getMu()), pvP);

  513.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  514.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, shiftedP,
  515.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  516.             final T                fixedR2 = fixedP.getNormSq();
  517.             final T                fixedR  = fixedR2.sqrt();
  518.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, shiftedV,
  519.                                                                  dt, nonKeplerianAcceleration);
  520.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR.multiply(fixedR2).reciprocal().multiply(getMu().negate()), shiftedP,
  521.                                                                  one, nonKeplerianAcceleration);

  522.             return new FieldPVCoordinates<>(fixedP, fixedV, fixedA);

  523.         } else {
  524.             // don't include acceleration,
  525.             // so the shifted orbit is not considered to have derivatives
  526.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  527.         }

  528.     }

  529.     /** Computes the eccentric in-plane argument from the mean in-plane argument.
  530.      * @param thetaM = mean in-plane argument (rad)
  531.      * @param ex first component of eccentricity vector
  532.      * @param ey second component of eccentricity vector
  533.      * @return the eccentric in-plane argument.
  534.      */
  535.     private T meanToEccentric(final T thetaM, final T ex, final T ey) {
  536.         // Generalization of Kepler equation to in-plane parameters
  537.         // with thetaE = eta + E and
  538.         //      thetaM = eta + M = thetaE - ex.sin(thetaE) + ey.cos(thetaE)
  539.         // and eta being counted from an arbitrary reference in the orbital plane
  540.         T thetaE        = thetaM;
  541.         T thetaEMthetaM = zero;
  542.         int    iter          = 0;
  543.         do {
  544.             final FieldSinCos<T> scThetaE = FastMath.sinCos(thetaE);

  545.             final T f2 = ex.multiply(scThetaE.sin()).subtract(ey.multiply(scThetaE.cos()));
  546.             final T f1 = one.subtract(ex.multiply(scThetaE.cos())).subtract(ey.multiply(scThetaE.sin()));
  547.             final T f0 = thetaEMthetaM.subtract(f2);

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

  550.             thetaEMthetaM = thetaEMthetaM.subtract(shift);
  551.             thetaE         = thetaM.add(thetaEMthetaM);

  552.             if (FastMath.abs(shift.getReal()) <= 1.0e-12) {
  553.                 return thetaE;
  554.             }

  555.         } while (++iter < 50);

  556.         throw new MathIllegalStateException(LocalizedCoreFormats.CONVERGENCE_FAILED);

  557.     }

  558.     /** Create a 6x6 identity matrix.
  559.      * @return 6x6 identity matrix
  560.      */
  561.     private T[][] create6x6Identity() {
  562.         // this is the fastest way to set the 6x6 identity matrix
  563.         final T[][] identity = MathArrays.buildArray(field, 6, 6);
  564.         for (int i = 0; i < 6; i++) {
  565.             Arrays.fill(identity[i], zero);
  566.             identity[i][i] = one;
  567.         }
  568.         return identity;
  569.     }

  570.     @Override
  571.     protected T[][] computeJacobianMeanWrtCartesian() {
  572.         return create6x6Identity();
  573.     }

  574.     @Override
  575.     protected T[][] computeJacobianEccentricWrtCartesian() {
  576.         return create6x6Identity();
  577.     }

  578.     @Override
  579.     protected T[][] computeJacobianTrueWrtCartesian() {
  580.         return create6x6Identity();
  581.     }

  582.     /** {@inheritDoc} */
  583.     public void addKeplerContribution(final PositionAngle type, final T gm,
  584.                                       final T[] pDot) {

  585.         final FieldPVCoordinates<T> pv = getPVCoordinates();

  586.         // position derivative is velocity
  587.         final FieldVector3D<T> velocity = pv.getVelocity();
  588.         pDot[0] = pDot[0].add(velocity.getX());
  589.         pDot[1] = pDot[1].add(velocity.getY());
  590.         pDot[2] = pDot[2].add(velocity.getZ());

  591.         // velocity derivative is Newtonian acceleration
  592.         final FieldVector3D<T> position = pv.getPosition();
  593.         final T r2         = position.getNormSq();
  594.         final T coeff      = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
  595.         pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
  596.         pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
  597.         pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));

  598.     }

  599.     /**  Returns a string representation of this Orbit object.
  600.      * @return a string representation of this object
  601.      */
  602.     public String toString() {
  603.         // use only the six defining elements, like the other Orbit.toString() methods
  604.         final String comma = ", ";
  605.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  606.         final Vector3D p = pv.getPosition();
  607.         final Vector3D v = pv.getVelocity();
  608.         return "Cartesian parameters: {P(" +
  609.                 p.getX() + comma +
  610.                 p.getY() + comma +
  611.                 p.getZ() + "), V(" +
  612.                 v.getX() + comma +
  613.                 v.getY() + comma +
  614.                 v.getZ() + ")}";
  615.     }

  616.     @Override
  617.     public CartesianOrbit toOrbit() {
  618.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  619.         final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
  620.         if (hasDerivatives()) {
  621.             // getPVCoordinates includes accelerations that will be interpreted as derivatives
  622.             return new CartesianOrbit(pv, getFrame(), date, getMu().getReal());
  623.         } else {
  624.             // get rid of Keplerian acceleration so we don't assume
  625.             // we have derivatives when in fact we don't have them
  626.             return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
  627.                                       getFrame(), date, getMu().getReal());
  628.         }
  629.     }

  630. }