FieldCartesianOrbit.java

  1. /* Copyright 2002-2020 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.Field;
  21. import org.hipparchus.RealFieldElement;
  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.errors.OrekitMessages;
  33. import org.orekit.frames.Frame;
  34. import org.orekit.time.AbsoluteDate;
  35. import org.orekit.time.FieldAbsoluteDate;
  36. import org.orekit.utils.CartesianDerivativesFilter;
  37. import org.orekit.utils.FieldPVCoordinates;
  38. import org.orekit.utils.PVCoordinates;
  39. import org.orekit.utils.TimeStampedFieldPVCoordinates;


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  469.     }

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

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

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

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

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

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

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

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

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

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

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

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

  529.     }

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

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

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

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

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

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

  557.         throw new MathIllegalStateException(LocalizedCoreFormats.CONVERGENCE_FAILED);

  558.     }

  559.     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
  560.      * <p>
  561.      * The algorithm used here for solving hyperbolic Kepler equation is
  562.      * Danby's iterative method (3rd order) with Vallado's initial guess.
  563.      * </p>
  564.      * @param M mean anomaly (rad)
  565.      * @param ecc eccentricity
  566.      * @return the hyperbolic eccentric anomaly
  567.      */
  568.     private T meanToHyperbolicEccentric(final T M, final T ecc) {

  569.         // Resolution of hyperbolic Kepler equation for Keplerian parameters

  570.         // Initial guess
  571.         T H;
  572.         if (ecc.getReal() < 1.6) {
  573.             if ((-FastMath.PI < M.getReal() && M.getReal() < 0.) || M.getReal() > FastMath.PI) {
  574.                 H = M.subtract(ecc);
  575.             } else {
  576.                 H = M.add(ecc);
  577.             }
  578.         } else {
  579.             if (ecc.getReal() < 3.6 && FastMath.abs(M.getReal()) > FastMath.PI) {
  580.                 H = M.subtract(ecc.copySign(M));
  581.             } else {
  582.                 H = M.divide(ecc.subtract(1.));
  583.             }
  584.         }

  585.         // Iterative computation
  586.         int iter = 0;
  587.         do {
  588.             final T f3  = ecc.multiply(H.cosh());
  589.             final T f2  = ecc.multiply(H.sinh());
  590.             final T f1  = f3.subtract(1.);
  591.             final T f0  = f2.subtract(H).subtract(M);
  592.             final T f12 = f1.multiply(2);
  593.             final T d   = f0.divide(f12);
  594.             final T fdf = f1.subtract(d.multiply(f2));
  595.             final T ds  = f0.divide(fdf);

  596.             final T shift = f0.divide(fdf.add(ds.multiply(ds).multiply(f3.divide(6.))));

  597.             H = H.subtract(shift);

  598.             if (FastMath.abs(shift.getReal()) <= 1.0e-12) {
  599.                 return H;
  600.             }

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

  602.         throw new MathIllegalStateException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
  603.                                             iter);
  604.     }

  605.     /** Create a 6x6 identity matrix.
  606.      * @return 6x6 identity matrix
  607.      */
  608.     private T[][] create6x6Identity() {
  609.         // this is the fastest way to set the 6x6 identity matrix
  610.         final T[][] identity = MathArrays.buildArray(field, 6, 6);
  611.         for (int i = 0; i < 6; i++) {
  612.             Arrays.fill(identity[i], zero);
  613.             identity[i][i] = one;
  614.         }
  615.         return identity;
  616.     }

  617.     @Override
  618.     protected T[][] computeJacobianMeanWrtCartesian() {
  619.         return create6x6Identity();
  620.     }

  621.     @Override
  622.     protected T[][] computeJacobianEccentricWrtCartesian() {
  623.         return create6x6Identity();
  624.     }

  625.     @Override
  626.     protected T[][] computeJacobianTrueWrtCartesian() {
  627.         return create6x6Identity();
  628.     }

  629.     /** {@inheritDoc} */
  630.     public void addKeplerContribution(final PositionAngle type, final T gm,
  631.                                       final T[] pDot) {

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

  633.         // position derivative is velocity
  634.         final FieldVector3D<T> velocity = pv.getVelocity();
  635.         pDot[0] = pDot[0].add(velocity.getX());
  636.         pDot[1] = pDot[1].add(velocity.getY());
  637.         pDot[2] = pDot[2].add(velocity.getZ());

  638.         // velocity derivative is Newtonian acceleration
  639.         final FieldVector3D<T> position = pv.getPosition();
  640.         final T r2         = position.getNormSq();
  641.         final T coeff      = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
  642.         pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
  643.         pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
  644.         pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));

  645.     }

  646.     /**  Returns a string representation of this Orbit object.
  647.      * @return a string representation of this object
  648.      */
  649.     public String toString() {
  650.         // use only the six defining elements, like the other Orbit.toString() methods
  651.         final String comma = ", ";
  652.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  653.         final Vector3D p = pv.getPosition();
  654.         final Vector3D v = pv.getVelocity();
  655.         return "Cartesian parameters: {P(" +
  656.                 p.getX() + comma +
  657.                 p.getY() + comma +
  658.                 p.getZ() + "), V(" +
  659.                 v.getX() + comma +
  660.                 v.getY() + comma +
  661.                 v.getZ() + ")}";
  662.     }

  663.     @Override
  664.     public CartesianOrbit toOrbit() {
  665.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  666.         final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
  667.         if (hasDerivatives()) {
  668.             // getPVCoordinates includes accelerations that will be interpreted as derivatives
  669.             return new CartesianOrbit(pv, getFrame(), date, getMu().getReal());
  670.         } else {
  671.             // get rid of Keplerian acceleration so we don't assume
  672.             // we have derivatives when in fact we don't have them
  673.             return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
  674.                                       getFrame(), date, getMu().getReal());
  675.         }
  676.     }

  677. }