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.geometry.euclidean.threed.FieldRotation;
  24. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  25. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.util.FastMath;
  28. import org.hipparchus.util.FieldSinCos;
  29. import org.hipparchus.util.MathArrays;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.time.FieldAbsoluteDate;
  33. import org.orekit.utils.CartesianDerivativesFilter;
  34. import org.orekit.utils.FieldPVCoordinates;
  35. import org.orekit.utils.PVCoordinates;
  36. import org.orekit.utils.TimeStampedFieldPVCoordinates;


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  408.         final FieldPVCoordinates<T> pva = getPVCoordinates();
  409.         final FieldVector3D<T>      pvP = pva.getPosition();
  410.         final FieldVector3D<T>      pvV = pva.getVelocity();
  411.         final FieldVector3D<T>      pvM = pva.getMomentum();
  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 muA                     = getMu().multiply(a);

  417.         // compute mean anomaly
  418.         final T eSE              = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  419.         final T eCE              = rV2OnMu.subtract(1);
  420.         final T E0               = FastMath.atan2(eSE, eCE);
  421.         final T M0               = E0.subtract(eSE);

  422.         final T e                       = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
  423.         final T sqrt                    = e.add(1).divide(e.negate().add(1)).sqrt();

  424.         // find canonical 2D frame with p pointing to perigee
  425.         final T v0               = sqrt.multiply(FastMath.tan(E0.divide(2))).atan().multiply(2);
  426.         final FieldVector3D<T> p = new FieldRotation<>(pvM, v0, RotationConvention.FRAME_TRANSFORM).applyTo(pvP).normalize();
  427.         final FieldVector3D<T> q = FieldVector3D.crossProduct(pvM, p).normalize();

  428.         // compute shifted eccentric anomaly
  429.         final T M1               = M0.add(getKeplerianMeanMotion().multiply(dt));
  430.         final T E1               = FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, M1);

  431.         // compute shifted in-plane Cartesian coordinates
  432.         final FieldSinCos<T> scE = FastMath.sinCos(E1);
  433.         final T               cE = scE.cos();
  434.         final T               sE = scE.sin();
  435.         final T            sE2m1 = e.negate().add(1).multiply(e.add(1)).sqrt();

  436.         // coordinates of position and velocity in the orbital plane
  437.         final T x        = a.multiply(cE.subtract(e));
  438.         final T y        = a.multiply(sE2m1).multiply(sE);
  439.         final T factor   = a.reciprocal().multiply(getMu()).sqrt().divide(e.multiply(cE).negate().add(1));
  440.         final T xDot     = factor.multiply(sE).negate();
  441.         final T yDot     = factor.multiply(sE2m1).multiply(cE);

  442.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, p, y, q);
  443.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, p, yDot, q);
  444.         if (hasNonKeplerianAcceleration) {

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

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

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

  458.         } else {
  459.             // don't include acceleration,
  460.             // so the shifted orbit is not considered to have derivatives
  461.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  462.         }

  463.     }

  464.     /** Compute shifted position and velocity in hyperbolic case.
  465.      * @param dt time shift
  466.      * @return shifted position and velocity
  467.      */
  468.     private FieldPVCoordinates<T> shiftPVHyperbolic(final T dt) {

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

  480.         // compute mean anomaly
  481.         final T eSH     = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  482.         final T eCH     = rV2OnMu.subtract(1);
  483.         final T H0      = eCH.add(eSH).divide(eCH.subtract(eSH)).log().divide(2);
  484.         final T M0      = e.multiply(H0.sinh()).subtract(H0);

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

  489.         // compute shifted eccentric anomaly
  490.         final T M1      = M0.add(getKeplerianMeanMotion().multiply(dt));
  491.         final T H1      = FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);

  492.         // compute shifted in-plane Cartesian coordinates
  493.         final T cH     = H1.cosh();
  494.         final T sH     = H1.sinh();
  495.         final T sE2m1  = e.subtract(1).multiply(e.add(1)).sqrt();

  496.         // coordinates of position and velocity in the orbital plane
  497.         final T x      = a.multiply(cH.subtract(e));
  498.         final T y      = a.negate().multiply(sE2m1).multiply(sH);
  499.         final T factor = getMu().divide(a.negate()).sqrt().divide(e.multiply(cH).subtract(1));
  500.         final T xDot   = factor.negate().multiply(sH);
  501.         final T yDot   =  factor.multiply(sE2m1).multiply(cH);

  502.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, p, y, q);
  503.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, p, yDot, q);
  504.         if (hasNonKeplerianAcceleration) {

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

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

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

  518.         } else {
  519.             // don't include acceleration,
  520.             // so the shifted orbit is not considered to have derivatives
  521.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  522.         }

  523.     }

  524.     /** Create a 6x6 identity matrix.
  525.      * @return 6x6 identity matrix
  526.      */
  527.     private T[][] create6x6Identity() {
  528.         // this is the fastest way to set the 6x6 identity matrix
  529.         final T[][] identity = MathArrays.buildArray(field, 6, 6);
  530.         for (int i = 0; i < 6; i++) {
  531.             Arrays.fill(identity[i], zero);
  532.             identity[i][i] = one;
  533.         }
  534.         return identity;
  535.     }

  536.     @Override
  537.     protected T[][] computeJacobianMeanWrtCartesian() {
  538.         return create6x6Identity();
  539.     }

  540.     @Override
  541.     protected T[][] computeJacobianEccentricWrtCartesian() {
  542.         return create6x6Identity();
  543.     }

  544.     @Override
  545.     protected T[][] computeJacobianTrueWrtCartesian() {
  546.         return create6x6Identity();
  547.     }

  548.     /** {@inheritDoc} */
  549.     public void addKeplerContribution(final PositionAngle type, final T gm,
  550.                                       final T[] pDot) {

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

  552.         // position derivative is velocity
  553.         final FieldVector3D<T> velocity = pv.getVelocity();
  554.         pDot[0] = pDot[0].add(velocity.getX());
  555.         pDot[1] = pDot[1].add(velocity.getY());
  556.         pDot[2] = pDot[2].add(velocity.getZ());

  557.         // velocity derivative is Newtonian acceleration
  558.         final FieldVector3D<T> position = pv.getPosition();
  559.         final T r2         = position.getNormSq();
  560.         final T coeff      = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
  561.         pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
  562.         pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
  563.         pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));

  564.     }

  565.     /**  Returns a string representation of this Orbit object.
  566.      * @return a string representation of this object
  567.      */
  568.     public String toString() {
  569.         // use only the six defining elements, like the other Orbit.toString() methods
  570.         final String comma = ", ";
  571.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  572.         final Vector3D p = pv.getPosition();
  573.         final Vector3D v = pv.getVelocity();
  574.         return "Cartesian parameters: {P(" +
  575.                 p.getX() + comma +
  576.                 p.getY() + comma +
  577.                 p.getZ() + "), V(" +
  578.                 v.getX() + comma +
  579.                 v.getY() + comma +
  580.                 v.getZ() + ")}";
  581.     }

  582.     @Override
  583.     public CartesianOrbit toOrbit() {
  584.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  585.         final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
  586.         if (hasDerivatives()) {
  587.             // getPVCoordinates includes accelerations that will be interpreted as derivatives
  588.             return new CartesianOrbit(pv, getFrame(), date, getMu().getReal());
  589.         } else {
  590.             // get rid of Keplerian acceleration so we don't assume
  591.             // we have derivatives when in fact we don't have them
  592.             return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
  593.                                       getFrame(), date, getMu().getReal());
  594.         }
  595.     }

  596. }