FieldCartesianOrbit.java

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


  20. import java.util.Arrays;

  22. import org.hipparchus.CalculusFieldElement;
  23. import org.hipparchus.Field;
  24. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  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.FieldPVCoordinates;
  36. import org.orekit.utils.PVCoordinates;
  37. import org.orekit.utils.TimeStampedFieldPVCoordinates;


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

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

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

  78.     /** Indicator for non-Keplerian acceleration. */
  79.     private final transient boolean hasNonKeplerianAcceleration;

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

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

  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.     }

  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.     }

  142.     /** Constructor from Field and CartesianOrbit.
  143.      * <p>Build a FieldCartesianOrbit from non-Field CartesianOrbit.</p>
  144.      * @param field CalculusField to base object on
  145.      * @param op non-field orbit with only "constant" terms
  146.      * @since 12.0
  147.      */
  148.     public FieldCartesianOrbit(final Field<T> field, final CartesianOrbit op) {
  149.         super(new TimeStampedFieldPVCoordinates<>(field, op.getPVCoordinates()), op.getFrame(),
  150.                 field.getZero().add(op.getMu()));
  151.         hasNonKeplerianAcceleration = op.hasDerivatives();
  152.         if (op.isElliptical()) {
  153.             equinoctial = new FieldEquinoctialOrbit<>(field, new EquinoctialOrbit(op));
  154.         } else {
  155.             equinoctial = null;
  156.         }
  157.     }

  159.     /** Constructor from Field and Orbit.
  160.      * <p>Build a FieldCartesianOrbit from any non-Field Orbit.</p>
  161.      * @param field CalculusField to base object on
  162.      * @param op non-field orbit with only "constant" terms
  163.      * @since 12.0
  164.      */
  165.     public FieldCartesianOrbit(final Field<T> field, final Orbit op) {
  166.         this(field, new CartesianOrbit(op));
  167.     }

  169.     /** {@inheritDoc} */
  170.     public OrbitType getType() {
  171.         return OrbitType.CARTESIAN;
  172.     }

  174.     /** Lazy evaluation of equinoctial parameters. */
  175.     private void initEquinoctial() {
  176.         if (equinoctial == null) {
  177.             if (hasDerivatives()) {
  178.                 // getPVCoordinates includes accelerations that will be interpreted as derivatives
  179.                 equinoctial = new FieldEquinoctialOrbit<>(getPVCoordinates(), getFrame(), getDate(), getMu());
  180.             } else {
  181.                 // get rid of Keplerian acceleration so we don't assume
  182.                 // we have derivatives when in fact we don't have them
  183.                 final FieldPVCoordinates<T> pva = getPVCoordinates();
  184.                 equinoctial = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(pva.getPosition(), pva.getVelocity()),
  185.                                                           getFrame(), getDate(), getMu());
  186.             }
  187.         }
  188.     }

  190.     /** Get the position/velocity with derivatives.
  191.      * @return position/velocity with derivatives
  192.      * @since 10.2
  193.      */
  194.     private FieldPVCoordinates<FieldUnivariateDerivative2<T>> getPVDerivatives() {
  195.         // PVA coordinates
  196.         final FieldPVCoordinates<T> pva = getPVCoordinates();
  197.         final FieldVector3D<T>      p   = pva.getPosition();
  198.         final FieldVector3D<T>      v   = pva.getVelocity();
  199.         final FieldVector3D<T>      a   = pva.getAcceleration();
  200.         // Field coordinates
  201.         final FieldVector3D<FieldUnivariateDerivative2<T>> pG = new FieldVector3D<>(new FieldUnivariateDerivative2<>(p.getX(), v.getX(), a.getX()),
  202.                                                              new FieldUnivariateDerivative2<>(p.getY(), v.getY(), a.getY()),
  203.                                                              new FieldUnivariateDerivative2<>(p.getZ(), v.getZ(), a.getZ()));
  204.         final FieldVector3D<FieldUnivariateDerivative2<T>> vG = new FieldVector3D<>(new FieldUnivariateDerivative2<>(v.getX(), a.getX(), getZero()),
  205.                                                              new FieldUnivariateDerivative2<>(v.getY(), a.getY(), getZero()),
  206.                                                              new FieldUnivariateDerivative2<>(v.getZ(), a.getZ(), getZero()));
  207.         return new FieldPVCoordinates<>(pG, vG);
  208.     }

  210.     /** {@inheritDoc} */
  211.     public T getA() {
  212.         // lazy evaluation of semi-major axis
  213.         final FieldPVCoordinates<T> pva = getPVCoordinates();
  214.         final T r  = pva.getPosition().getNorm();
  215.         final T V2 = pva.getVelocity().getNormSq();
  216.         return r.divide(r.negate().multiply(V2).divide(getMu()).add(2));
  217.     }

  219.     /** {@inheritDoc} */
  220.     public T getADot() {
  221.         if (hasDerivatives()) {
  222.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  223.             final FieldUnivariateDerivative2<T> r  = pv.getPosition().getNorm();
  224.             final FieldUnivariateDerivative2<T> V2 = pv.getVelocity().getNormSq();
  225.             final FieldUnivariateDerivative2<T> a  = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
  226.             return a.getDerivative(1);
  227.         } else {
  228.             return null;
  229.         }
  230.     }

  232.     /** {@inheritDoc} */
  233.     public T getE() {
  234.         final T muA = getA().multiply(getMu());
  235.         if (isElliptical()) {
  236.             // elliptic or circular orbit
  237.             final FieldPVCoordinates<T> pva = getPVCoordinates();
  238.             final FieldVector3D<T> pvP      = pva.getPosition();
  239.             final FieldVector3D<T> pvV      = pva.getVelocity();
  240.             final T rV2OnMu = pvP.getNorm().multiply(pvV.getNormSq()).divide(getMu());
  241.             final T eSE     = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  242.             final T eCE     = rV2OnMu.subtract(1);
  243.             return (eCE.multiply(eCE).add(eSE.multiply(eSE))).sqrt();
  244.         } else {
  245.             // hyperbolic orbit
  246.             final FieldVector3D<T> pvM = getPVCoordinates().getMomentum();
  247.             return pvM.getNormSq().divide(muA).negate().add(1).sqrt();
  248.         }
  249.     }

  251.     /** {@inheritDoc} */
  252.     public T getEDot() {
  253.         if (hasDerivatives()) {
  254.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  255.             final FieldUnivariateDerivative2<T> r       = pv.getPosition().getNorm();
  256.             final FieldUnivariateDerivative2<T> V2      = pv.getVelocity().getNormSq();
  257.             final FieldUnivariateDerivative2<T> rV2OnMu = r.multiply(V2).divide(getMu());
  258.             final FieldUnivariateDerivative2<T> a       = r.divide(rV2OnMu.negate().add(2));
  259.             final FieldUnivariateDerivative2<T> eSE     = FieldVector3D.dotProduct(pv.getPosition(), pv.getVelocity()).divide(a.multiply(getMu()).sqrt());
  260.             final FieldUnivariateDerivative2<T> eCE     = rV2OnMu.subtract(1);
  261.             final FieldUnivariateDerivative2<T> e       = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
  262.             return e.getDerivative(1);
  263.         } else {
  264.             return null;
  265.         }
  266.     }

  268.     /** {@inheritDoc} */
  269.     public T getI() {
  270.         return FieldVector3D.angle(new FieldVector3D<>(getZero(), getZero(), getOne()), getPVCoordinates().getMomentum());
  271.     }

  273.     /** {@inheritDoc} */
  274.     public T getIDot() {
  275.         if (hasDerivatives()) {
  276.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  277.             final FieldVector3D<FieldUnivariateDerivative2<T>> momentum =
  278.                             FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity());
  279.             final FieldUnivariateDerivative2<T> i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
  280.             return i.getDerivative(1);
  281.         } else {
  282.             return null;
  283.         }
  284.     }

  286.     /** {@inheritDoc} */
  287.     public T getEquinoctialEx() {
  288.         initEquinoctial();
  289.         return equinoctial.getEquinoctialEx();
  290.     }

  292.     /** {@inheritDoc} */
  293.     public T getEquinoctialExDot() {
  294.         initEquinoctial();
  295.         return equinoctial.getEquinoctialExDot();
  296.     }

  298.     /** {@inheritDoc} */
  299.     public T getEquinoctialEy() {
  300.         initEquinoctial();
  301.         return equinoctial.getEquinoctialEy();
  302.     }

  304.     /** {@inheritDoc} */
  305.     public T getEquinoctialEyDot() {
  306.         initEquinoctial();
  307.         return equinoctial.getEquinoctialEyDot();
  308.     }

  310.     /** {@inheritDoc} */
  311.     public T getHx() {
  312.         final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
  313.         // Check for equatorial retrograde orbit
  314.         final double x = w.getX().getReal();
  315.         final double y = w.getY().getReal();
  316.         final double z = w.getZ().getReal();
  317.         if ((x * x + y * y) == 0 && z < 0) {
  318.             return getZero().add(Double.NaN);
  319.         }
  320.         return w.getY().negate().divide(w.getZ().add(1));
  321.     }

  323.     /** {@inheritDoc} */
  324.     public T getHxDot() {
  325.         if (hasDerivatives()) {
  326.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  327.             final FieldVector3D<FieldUnivariateDerivative2<T>> w =
  328.                             FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
  329.             // Check for equatorial retrograde orbit
  330.             final double x = w.getX().getValue().getReal();
  331.             final double y = w.getY().getValue().getReal();
  332.             final double z = w.getZ().getValue().getReal();
  333.             if ((x * x + y * y) == 0 && z < 0) {
  334.                 return getZero().add(Double.NaN);
  335.             }
  336.             final FieldUnivariateDerivative2<T> hx = w.getY().negate().divide(w.getZ().add(1));
  337.             return hx.getDerivative(1);
  338.         } else {
  339.             return null;
  340.         }
  341.     }

  343.     /** {@inheritDoc} */
  344.     public T getHy() {
  345.         final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
  346.         // Check for equatorial retrograde orbit
  347.         final double x = w.getX().getReal();
  348.         final double y = w.getY().getReal();
  349.         final double z = w.getZ().getReal();
  350.         if ((x * x + y * y) == 0 && z < 0) {
  351.             return getZero().add(Double.NaN);
  352.         }
  353.         return  w.getX().divide(w.getZ().add(1));
  354.     }

  356.     /** {@inheritDoc} */
  357.     public T getHyDot() {
  358.         if (hasDerivatives()) {
  359.             final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
  360.             final FieldVector3D<FieldUnivariateDerivative2<T>> w =
  361.                             FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
  362.             // Check for equatorial retrograde orbit
  363.             final double x = w.getX().getValue().getReal();
  364.             final double y = w.getY().getValue().getReal();
  365.             final double z = w.getZ().getValue().getReal();
  366.             if ((x * x + y * y) == 0 && z < 0) {
  367.                 return getZero().add(Double.NaN);
  368.             }
  369.             final FieldUnivariateDerivative2<T> hy = w.getX().divide(w.getZ().add(1));
  370.             return hy.getDerivative(1);
  371.         } else {
  372.             return null;
  373.         }
  374.     }

  376.     /** {@inheritDoc} */
  377.     public T getLv() {
  378.         initEquinoctial();
  379.         return equinoctial.getLv();
  380.     }

  382.     /** {@inheritDoc} */
  383.     public T getLvDot() {
  384.         initEquinoctial();
  385.         return equinoctial.getLvDot();
  386.     }

  388.     /** {@inheritDoc} */
  389.     public T getLE() {
  390.         initEquinoctial();
  391.         return equinoctial.getLE();
  392.     }

  394.     /** {@inheritDoc} */
  395.     public T getLEDot() {
  396.         initEquinoctial();
  397.         return equinoctial.getLEDot();
  398.     }

  400.     /** {@inheritDoc} */
  401.     public T getLM() {
  402.         initEquinoctial();
  403.         return equinoctial.getLM();
  404.     }

  406.     /** {@inheritDoc} */
  407.     public T getLMDot() {
  408.         initEquinoctial();
  409.         return equinoctial.getLMDot();
  410.     }

  412.     /** {@inheritDoc} */
  413.     public boolean hasDerivatives() {
  414.         return hasNonKeplerianAcceleration;
  415.     }

  417.     /** {@inheritDoc} */
  418.     protected FieldVector3D<T> initPosition() {
  419.         // nothing to do here, as the canonical elements are already the Cartesian ones
  420.         return getPVCoordinates().getPosition();
  421.     }

  423.     /** {@inheritDoc} */
  424.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {
  425.         // nothing to do here, as the canonical elements are already the Cartesian ones
  426.         return getPVCoordinates();
  427.     }

  429.     /** {@inheritDoc} */
  430.     public FieldCartesianOrbit<T> shiftedBy(final double dt) {
  431.         return shiftedBy(getZero().add(dt));
  432.     }

  434.     /** {@inheritDoc} */
  435.     public FieldCartesianOrbit<T> shiftedBy(final T dt) {
  436.         final FieldPVCoordinates<T> shiftedPV = isElliptical() ? shiftPVElliptic(dt) : shiftPVHyperbolic(dt);
  437.         return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
  438.     }

  440.     /** Compute shifted position and velocity in elliptic case.
  441.      * @param dt time shift
  442.      * @return shifted position and velocity
  443.      */
  444.     private FieldPVCoordinates<T> shiftPVElliptic(final T dt) {

  446.         // preliminary computation0
  447.         final FieldPVCoordinates<T> pva = getPVCoordinates();
  448.         final FieldVector3D<T>      pvP = pva.getPosition();
  449.         final FieldVector3D<T>      pvV = pva.getVelocity();
  450.         final FieldVector3D<T>      pvM = pva.getMomentum();
  451.         final T r2                      = pvP.getNormSq();
  452.         final T r                       = r2.sqrt();
  453.         final T rV2OnMu                 = r.multiply(pvV.getNormSq()).divide(getMu());
  454.         final T a                       = r.divide(rV2OnMu.negate().add(2));
  455.         final T muA                     = getMu().multiply(a);

  457.         // compute mean anomaly
  458.         final T eSE              = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  459.         final T eCE              = rV2OnMu.subtract(1);
  460.         final T E0               = FastMath.atan2(eSE, eCE);
  461.         final T M0               = E0.subtract(eSE);

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

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

  471.         // compute shifted eccentric anomaly
  472.         final T M1               = M0.add(getKeplerianMeanMotion().multiply(dt));
  473.         final T E1               = FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, M1);

  475.         // compute shifted in-plane Cartesian coordinates
  476.         final FieldSinCos<T> scE = FastMath.sinCos(E1);
  477.         final T               cE = scE.cos();
  478.         final T               sE = scE.sin();
  479.         final T            sE2m1 = e.negate().add(1).multiply(e.add(1)).sqrt();

  481.         // coordinates of position and velocity in the orbital plane
  482.         final T x        = a.multiply(cE.subtract(e));
  483.         final T y        = a.multiply(sE2m1).multiply(sE);
  484.         final T factor   = a.reciprocal().multiply(getMu()).sqrt().divide(e.multiply(cE).negate().add(1));
  485.         final T xDot     = factor.multiply(sE).negate();
  486.         final T yDot     = factor.multiply(sE2m1).multiply(cE);

  488.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, p, y, q);
  489.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, p, yDot, q);
  490.         if (hasNonKeplerianAcceleration) {

  492.             // extract non-Keplerian part of the initial acceleration
  493.             final FieldVector3D<T> nonKeplerianAcceleration = new FieldVector3D<>(getOne(), getPVCoordinates().getAcceleration(),
  494.                                                                                   r.multiply(r2).reciprocal().multiply(getMu()), pvP);

  496.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  497.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), shiftedP,
  498.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  499.             final T                fixedR2 = fixedP.getNormSq();
  500.             final T                fixedR  = fixedR2.sqrt();
  501.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), shiftedV,
  502.                                                                  dt, nonKeplerianAcceleration);
  503.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR.multiply(fixedR2).reciprocal().multiply(getMu().negate()), shiftedP,
  504.                                                                  getOne(), nonKeplerianAcceleration);

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

  508.         } else {
  509.             // don't include acceleration,
  510.             // so the shifted orbit is not considered to have derivatives
  511.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  512.         }

  514.     }

  516.     /** Compute shifted position and velocity in hyperbolic case.
  517.      * @param dt time shift
  518.      * @return shifted position and velocity
  519.      */
  520.     private FieldPVCoordinates<T> shiftPVHyperbolic(final T dt) {

  522.         final FieldPVCoordinates<T> pv = getPVCoordinates();
  523.         final FieldVector3D<T> pvP   = pv.getPosition();
  524.         final FieldVector3D<T> pvV   = pv.getVelocity();
  525.         final FieldVector3D<T> pvM   = pv.getMomentum();
  526.         final T r2      = pvP.getNormSq();
  527.         final T r       = r2.sqrt();
  528.         final T rV2OnMu = r.multiply(pvV.getNormSq()).divide(getMu());
  529.         final T a       = getA();
  530.         final T muA     = a.multiply(getMu());
  531.         final T e       = getOne().subtract(FieldVector3D.dotProduct(pvM, pvM).divide(muA)).sqrt();
  532.         final T sqrt    = e.add(1).divide(e.subtract(1)).sqrt();

  534.         // compute mean anomaly
  535.         final T eSH     = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  536.         final T eCH     = rV2OnMu.subtract(1);
  537.         final T H0      = eCH.add(eSH).divide(eCH.subtract(eSH)).log().divide(2);
  538.         final T M0      = e.multiply(H0.sinh()).subtract(H0);

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

  545.         // compute shifted eccentric anomaly
  546.         final T M1      = M0.add(getKeplerianMeanMotion().multiply(dt));
  547.         final T H1      = FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);

  549.         // compute shifted in-plane Cartesian coordinates
  550.         final T cH     = H1.cosh();
  551.         final T sH     = H1.sinh();
  552.         final T sE2m1  = e.subtract(1).multiply(e.add(1)).sqrt();

  554.         // coordinates of position and velocity in the orbital plane
  555.         final T x      = a.multiply(cH.subtract(e));
  556.         final T y      = a.negate().multiply(sE2m1).multiply(sH);
  557.         final T factor = getMu().divide(a.negate()).sqrt().divide(e.multiply(cH).subtract(1));
  558.         final T xDot   = factor.negate().multiply(sH);
  559.         final T yDot   =  factor.multiply(sE2m1).multiply(cH);

  561.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, p, y, q);
  562.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, p, yDot, q);
  563.         if (hasNonKeplerianAcceleration) {

  565.             // extract non-Keplerian part of the initial acceleration
  566.             final FieldVector3D<T> nonKeplerianAcceleration = new FieldVector3D<>(getOne(), getPVCoordinates().getAcceleration(),
  567.                                                                                   r.multiply(r2).reciprocal().multiply(getMu()), pvP);

  569.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  570.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), shiftedP,
  571.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  572.             final T                fixedR2 = fixedP.getNormSq();
  573.             final T                fixedR  = fixedR2.sqrt();
  574.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), shiftedV,
  575.                                                                  dt, nonKeplerianAcceleration);
  576.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR.multiply(fixedR2).reciprocal().multiply(getMu().negate()), shiftedP,
  577.                                                                  getOne(), nonKeplerianAcceleration);

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

  581.         } else {
  582.             // don't include acceleration,
  583.             // so the shifted orbit is not considered to have derivatives
  584.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  585.         }

  587.     }

  589.     /** Create a 6x6 identity matrix.
  590.      * @return 6x6 identity matrix
  591.      */
  592.     private T[][] create6x6Identity() {
  593.         // this is the fastest way to set the 6x6 identity matrix
  594.         final T[][] identity = MathArrays.buildArray(getField(), 6, 6);
  595.         for (int i = 0; i < 6; i++) {
  596.             Arrays.fill(identity[i], getZero());
  597.             identity[i][i] = getOne();
  598.         }
  599.         return identity;
  600.     }

  602.     @Override
  603.     protected T[][] computeJacobianMeanWrtCartesian() {
  604.         return create6x6Identity();
  605.     }

  607.     @Override
  608.     protected T[][] computeJacobianEccentricWrtCartesian() {
  609.         return create6x6Identity();
  610.     }

  612.     @Override
  613.     protected T[][] computeJacobianTrueWrtCartesian() {
  614.         return create6x6Identity();
  615.     }

  617.     /** {@inheritDoc} */
  618.     public void addKeplerContribution(final PositionAngleType type, final T gm,
  619.                                       final T[] pDot) {

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

  623.         // position derivative is velocity
  624.         final FieldVector3D<T> velocity = pv.getVelocity();
  625.         pDot[0] = pDot[0].add(velocity.getX());
  626.         pDot[1] = pDot[1].add(velocity.getY());
  627.         pDot[2] = pDot[2].add(velocity.getZ());

  629.         // velocity derivative is Newtonian acceleration
  630.         final FieldVector3D<T> position = pv.getPosition();
  631.         final T r2         = position.getNormSq();
  632.         final T coeff      = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
  633.         pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
  634.         pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
  635.         pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));

  637.     }

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

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

  672. }
Created with JaCoCo 0.8.11.202310140853