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;


  20. import java.util.Arrays;
  21. import java.util.HashMap;
  22. import java.util.Map;
  23. import java.util.stream.Stream;

  25. import org.hipparchus.Field;
  26. import org.hipparchus.RealFieldElement;
  27. import org.hipparchus.analysis.differentiation.FDSFactory;
  28. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  29. import org.hipparchus.exception.LocalizedCoreFormats;
  30. import org.hipparchus.exception.MathIllegalStateException;
  31. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  32. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  33. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  34. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  35. import org.hipparchus.util.FastMath;
  36. import org.hipparchus.util.MathArrays;
  37. import org.orekit.errors.OrekitMessages;
  38. import org.orekit.frames.Frame;
  39. import org.orekit.time.AbsoluteDate;
  40. import org.orekit.time.FieldAbsoluteDate;
  41. import org.orekit.utils.CartesianDerivativesFilter;
  42. import org.orekit.utils.FieldPVCoordinates;
  43. import org.orekit.utils.PVCoordinates;
  44. import org.orekit.utils.TimeStampedFieldPVCoordinates;


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

  49.  * <p>
  50.  * The parameters used internally are the Cartesian coordinates:
  51.  *   <ul>
  52.  *     <li>x</li>
  53.  *     <li>y</li>
  54.  *     <li>z</li>
  55.  *     <li>xDot</li>
  56.  *     <li>yDot</li>
  57.  *     <li>zDot</li>
  58.  *   </ul>
  59.  * contained in {@link PVCoordinates}.

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

  83.     /** Factory for first time derivatives. */
  84.     private static final Map<Field<? extends RealFieldElement<?>>, FDSFactory<? extends RealFieldElement<?>>> FACTORIES =
  85.                     new HashMap<>();

  87.     /** Indicator for non-Keplerian acceleration. */
  88.     private final transient boolean hasNonKeplerianAcceleration;

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

  93.     /** Field used by this class.*/
  94.     private final Field<T> field;

  96.     /** Zero. (could be usefull)*/
  97.     private final T zero;

  99.     /** One. (could be useful)*/
  100.     private final T one;

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

  126.         if (!FACTORIES.containsKey(field)) {
  127.             FACTORIES.put(field, new FDSFactory<>(field, 1, 1));
  128.         }

  130.     }

  132.     /** Constructor from Cartesian parameters.
  133.      *
  134.      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
  135.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  136.      * use {@code mu} and the position to compute the acceleration, including
  137.      * {@link #shiftedBy(RealFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  138.      *
  139.      * @param pvaCoordinates the position and velocity of the satellite.
  140.      * @param frame the frame in which the {@link PVCoordinates} are defined
  141.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  142.      * @param date date of the orbital parameters
  143.      * @param mu central attraction coefficient (m³/s²)
  144.      * @exception IllegalArgumentException if frame is not a {@link
  145.      * Frame#isPseudoInertial pseudo-inertial frame}
  146.      */
  147.     public FieldCartesianOrbit(final FieldPVCoordinates<T> pvaCoordinates, final Frame frame,
  148.                                final FieldAbsoluteDate<T> date, final T mu)
  149.         throws IllegalArgumentException {
  150.         this(new TimeStampedFieldPVCoordinates<>(date, pvaCoordinates), frame, mu);
  151.     }

  153.     /** Constructor from any kind of orbital parameters.
  154.      * @param op orbital parameters to copy
  155.      */
  156.     public FieldCartesianOrbit(final FieldOrbit<T> op) {
  157.         super(op.getPVCoordinates(), op.getFrame(), op.getMu());
  158.         hasNonKeplerianAcceleration = op.hasDerivatives();
  159.         if (op instanceof FieldEquinoctialOrbit) {
  160.             equinoctial = (FieldEquinoctialOrbit<T>) op;
  161.         } else if (op instanceof FieldCartesianOrbit) {
  162.             equinoctial = ((FieldCartesianOrbit<T>) op).equinoctial;
  163.         } else {
  164.             equinoctial = null;
  165.         }
  166.         field = op.getA().getField();
  167.         zero = field.getZero();
  168.         one = field.getOne();

  170.         if (!FACTORIES.containsKey(field)) {
  171.             FACTORIES.put(field, new FDSFactory<>(field, 1, 1));
  172.         }

  174.     }

  176.     /** {@inheritDoc} */
  177.     public OrbitType getType() {
  178.         return OrbitType.CARTESIAN;
  179.     }

  181.     /** Lazy evaluation of equinoctial parameters. */
  182.     private void initEquinoctial() {
  183.         if (equinoctial == null) {
  184.             if (hasDerivatives()) {
  185.                 // getPVCoordinates includes accelerations that will be interpreted as derivatives
  186.                 equinoctial = new FieldEquinoctialOrbit<>(getPVCoordinates(), getFrame(), getDate(), getMu());
  187.             } else {
  188.                 // get rid of Keplerian acceleration so we don't assume
  189.                 // we have derivatives when in fact we don't have them
  190.                 equinoctial = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(getPVCoordinates().getPosition(),
  191.                                                                                    getPVCoordinates().getVelocity()),
  192.                                                           getFrame(), getDate(), getMu());
  193.             }
  194.         }
  195.     }

  197.     /** Get position with derivatives.
  198.      * @return position with derivatives
  199.      */
  200.     private FieldVector3D<FieldDerivativeStructure<T>> getPositionDS() {
  201.         final FieldVector3D<T> p = getPVCoordinates().getPosition();
  202.         final FieldVector3D<T> v = getPVCoordinates().getVelocity();
  203.         @SuppressWarnings("unchecked")
  204.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(field);
  205.         return new FieldVector3D<>(factory.build(p.getX(), v.getX()),
  206.                                    factory.build(p.getY(), v.getY()),
  207.                                    factory.build(p.getZ(), v.getZ()));
  208.     }

  210.     /** Get velocity with derivatives.
  211.      * @return velocity with derivatives
  212.      */
  213.     private FieldVector3D<FieldDerivativeStructure<T>> getVelocityDS() {
  214.         final FieldVector3D<T> v = getPVCoordinates().getVelocity();
  215.         final FieldVector3D<T> a = getPVCoordinates().getAcceleration();
  216.         @SuppressWarnings("unchecked")
  217.         final FDSFactory<T> factory = (FDSFactory<T>) FACTORIES.get(field);
  218.         return new FieldVector3D<>(factory.build(v.getX(), a.getX()),
  219.                                    factory.build(v.getY(), a.getY()),
  220.                                    factory.build(v.getZ(), a.getZ()));
  221.     }

  223.     /** {@inheritDoc} */
  224.     public T getA() {
  225.         // lazy evaluation of semi-major axis
  226.         final T r  = getPVCoordinates().getPosition().getNorm();
  227.         final T V2 = getPVCoordinates().getVelocity().getNormSq();
  228.         return r.divide(r.negate().multiply(V2).divide(getMu()).add(2));
  229.     }

  231.     /** {@inheritDoc} */
  232.     public T getADot() {
  233.         if (hasDerivatives()) {
  234.             final FieldDerivativeStructure<T> r  = getPositionDS().getNorm();
  235.             final FieldDerivativeStructure<T> V2 = getVelocityDS().getNormSq();
  236.             final FieldDerivativeStructure<T> a  = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
  237.             return a.getPartialDerivative(1);
  238.         } else {
  239.             return null;
  240.         }
  241.     }

  243.     /** {@inheritDoc} */
  244.     public T getE() {
  245.         final T muA = getA().multiply(getMu());
  246.         if (muA.getReal() > 0) {
  247.             // elliptic or circular orbit
  248.             final FieldVector3D<T> pvP   = getPVCoordinates().getPosition();
  249.             final FieldVector3D<T> pvV   = getPVCoordinates().getVelocity();
  250.             final T rV2OnMu = pvP.getNorm().multiply(pvV.getNormSq()).divide(getMu());
  251.             final T eSE     = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  252.             final T eCE     = rV2OnMu.subtract(1);
  253.             return (eCE.multiply(eCE).add(eSE.multiply(eSE))).sqrt();
  254.         } else {
  255.             // hyperbolic orbit
  256.             final FieldVector3D<T> pvM = getPVCoordinates().getMomentum();
  257.             return pvM.getNormSq().divide(muA).negate().add(1).sqrt();
  258.         }
  259.     }

  261.     /** {@inheritDoc} */
  262.     public T getEDot() {
  263.         if (hasDerivatives()) {
  264.             final FieldVector3D<FieldDerivativeStructure<T>> pvP   = getPositionDS();
  265.             final FieldVector3D<FieldDerivativeStructure<T>> pvV   = getVelocityDS();
  266.             final FieldDerivativeStructure<T> r       = getPositionDS().getNorm();
  267.             final FieldDerivativeStructure<T> V2      = getVelocityDS().getNormSq();
  268.             final FieldDerivativeStructure<T> rV2OnMu = r.multiply(V2).divide(getMu());
  269.             final FieldDerivativeStructure<T> a       = r.divide(rV2OnMu.negate().add(2));
  270.             final FieldDerivativeStructure<T> eSE     = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
  271.             final FieldDerivativeStructure<T> eCE     = rV2OnMu.subtract(1);
  272.             final FieldDerivativeStructure<T> e       = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
  273.             return e.getPartialDerivative(1);
  274.         } else {
  275.             return null;
  276.         }
  277.     }

  279.     /** {@inheritDoc} */
  280.     public T getI() {
  281.         return FieldVector3D.angle(new FieldVector3D<>(zero, zero, one), getPVCoordinates().getMomentum());
  282.     }

  284.     /** {@inheritDoc} */
  285.     public T getIDot() {
  286.         if (hasDerivatives()) {
  287.             final FieldVector3D<FieldDerivativeStructure<T>> momentum =
  288.                             FieldVector3D.crossProduct(getPositionDS(), getVelocityDS());
  289.             final FieldDerivativeStructure<T> i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
  290.             return i.getPartialDerivative(1);
  291.         } else {
  292.             return null;
  293.         }
  294.     }

  296.     /** {@inheritDoc} */
  297.     public T getEquinoctialEx() {
  298.         initEquinoctial();
  299.         return equinoctial.getEquinoctialEx();
  300.     }

  302.     /** {@inheritDoc} */
  303.     public T getEquinoctialExDot() {
  304.         initEquinoctial();
  305.         return equinoctial.getEquinoctialExDot();
  306.     }

  308.     /** {@inheritDoc} */
  309.     public T getEquinoctialEy() {
  310.         initEquinoctial();
  311.         return equinoctial.getEquinoctialEy();
  312.     }

  314.     /** {@inheritDoc} */
  315.     public T getEquinoctialEyDot() {
  316.         initEquinoctial();
  317.         return equinoctial.getEquinoctialEyDot();
  318.     }

  320.     /** {@inheritDoc} */
  321.     public T getHx() {
  322.         final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
  323.         // Check for equatorial retrograde orbit
  324.         final double x = w.getX().getReal();
  325.         final double y = w.getY().getReal();
  326.         final double z = w.getZ().getReal();
  327.         if (((x * x + y * y) == 0) && z < 0) {
  328.             return zero.add(Double.NaN);
  329.         }
  330.         return w.getY().negate().divide(w.getZ().add(1));
  331.     }

  333.     /** {@inheritDoc} */
  334.     public T getHxDot() {
  335.         if (hasDerivatives()) {
  336.             final FieldVector3D<FieldDerivativeStructure<T>> w =
  337.                             FieldVector3D.crossProduct(getPositionDS(), getVelocityDS()).normalize();
  338.             // Check for equatorial retrograde orbit
  339.             final double x = w.getX().getValue().getReal();
  340.             final double y = w.getY().getValue().getReal();
  341.             final double z = w.getZ().getValue().getReal();
  342.             if (((x * x + y * y) == 0) && z < 0) {
  343.                 return zero.add(Double.NaN);
  344.             }
  345.             final FieldDerivativeStructure<T> hx = w.getY().negate().divide(w.getZ().add(1));
  346.             return hx.getPartialDerivative(1);
  347.         } else {
  348.             return null;
  349.         }
  350.     }

  352.     /** {@inheritDoc} */
  353.     public T getHy() {
  354.         final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
  355.         // Check for equatorial retrograde orbit
  356.         final double x = w.getX().getReal();
  357.         final double y = w.getY().getReal();
  358.         final double z = w.getZ().getReal();
  359.         if (((x * x + y * y) == 0) && z < 0) {
  360.             return zero.add(Double.NaN);
  361.         }
  362.         return  w.getX().divide(w.getZ().add(1));
  363.     }

  365.     /** {@inheritDoc} */
  366.     public T getHyDot() {
  367.         if (hasDerivatives()) {
  368.             final FieldVector3D<FieldDerivativeStructure<T>> w =
  369.                             FieldVector3D.crossProduct(getPositionDS(), getVelocityDS()).normalize();
  370.             // Check for equatorial retrograde orbit
  371.             final double x = w.getX().getValue().getReal();
  372.             final double y = w.getY().getValue().getReal();
  373.             final double z = w.getZ().getValue().getReal();
  374.             if (((x * x + y * y) == 0) && z < 0) {
  375.                 return zero.add(Double.NaN);
  376.             }
  377.             final FieldDerivativeStructure<T> hy = w.getX().divide(w.getZ().add(1));
  378.             return hy.getPartialDerivative(1);
  379.         } else {
  380.             return null;
  381.         }
  382.     }

  384.     /** {@inheritDoc} */
  385.     public T getLv() {
  386.         initEquinoctial();
  387.         return equinoctial.getLv();
  388.     }

  390.     /** {@inheritDoc} */
  391.     public T getLvDot() {
  392.         initEquinoctial();
  393.         return equinoctial.getLvDot();
  394.     }

  396.     /** {@inheritDoc} */
  397.     public T getLE() {
  398.         initEquinoctial();
  399.         return equinoctial.getLE();
  400.     }

  402.     /** {@inheritDoc} */
  403.     public T getLEDot() {
  404.         initEquinoctial();
  405.         return equinoctial.getLEDot();
  406.     }

  408.     /** {@inheritDoc} */
  409.     public T getLM() {
  410.         initEquinoctial();
  411.         return equinoctial.getLM();
  412.     }

  414.     /** {@inheritDoc} */
  415.     public T getLMDot() {
  416.         initEquinoctial();
  417.         return equinoctial.getLMDot();
  418.     }

  420.     /** {@inheritDoc} */
  421.     public boolean hasDerivatives() {
  422.         return hasNonKeplerianAcceleration;
  423.     }

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

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

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

  442.     /** {@inheritDoc}
  443.      * <p>
  444.      * The interpolated instance is created by polynomial Hermite interpolation
  445.      * ensuring velocity remains the exact derivative of position.
  446.      * </p>
  447.      * <p>
  448.      * As this implementation of interpolation is polynomial, it should be used only
  449.      * with small samples (about 10-20 points) in order to avoid <a
  450.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  451.      * and numerical problems (including NaN appearing).
  452.      * </p>
  453.      * <p>
  454.      * If orbit interpolation on large samples is needed, using the {@link
  455.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  456.      * low-level interpolation. The Ephemeris class automatically handles selection of
  457.      * a neighboring sub-sample with a predefined number of point from a large global sample
  458.      * in a thread-safe way.
  459.      * </p>
  460.      */
  461.     public FieldCartesianOrbit<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldOrbit<T>> sample) {
  462.         final TimeStampedFieldPVCoordinates<T> interpolated =
  463.                 TimeStampedFieldPVCoordinates.interpolate(date, CartesianDerivativesFilter.USE_PVA,
  464.                                                           sample.map(orbit -> orbit.getPVCoordinates()));
  465.         return new FieldCartesianOrbit<>(interpolated, getFrame(), date, getMu());
  466.     }

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

  474.         // preliminary computation
  475.         final FieldVector3D<T> pvP   = getPVCoordinates().getPosition();
  476.         final FieldVector3D<T> pvV   = getPVCoordinates().getVelocity();
  477.         final T r2      = pvP.getNormSq();
  478.         final T r       = r2.sqrt();
  479.         final T rV2OnMu = r.multiply(pvV.getNormSq()).divide(getMu());
  480.         final T a       = r.divide(rV2OnMu.negate().add(2));
  481.         final T eSE     = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
  482.         final T eCE     = rV2OnMu.subtract(1);
  483.         final T e2      = eCE.multiply(eCE).add(eSE.multiply(eSE));

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

  490.         // the following equations rely on the specific choice of u explained above,
  491.         // some coefficients that vanish to 0 in this case have already been removed here
  492.         final T ex      = eCE.subtract(e2).multiply(a).divide(r);
  493.         final T s       = e2.negate().add(1).sqrt();
  494.         final T ey      = s.negate().multiply(eSE).multiply(a).divide(r);
  495.         final T beta    = s.add(1).reciprocal();
  496.         final T thetaE0 = ey.add(eSE.multiply(beta).multiply(ex)).atan2(r.divide(a).add(ex).subtract(eSE.multiply(beta).multiply(ey)));
  497.         final T thetaM0 = thetaE0.subtract(ex.multiply(thetaE0.sin())).add(ey.multiply(thetaE0.cos()));

  499.         // compute in-plane shifted eccentric argument
  500.         final T sqrtMmuOA = a.reciprocal().multiply(getMu()).sqrt();
  501.         final T thetaM1   = thetaM0.add(sqrtMmuOA.divide(a).multiply(dt));
  502.         final T thetaE1   = meanToEccentric(thetaM1, ex, ey);
  503.         final T cTE       = thetaE1.cos();
  504.         final T sTE       = thetaE1.sin();

  506.         // compute shifted in-plane Cartesian coordinates
  507.         final T exey   = ex.multiply(ey);
  508.         final T exCeyS = ex.multiply(cTE).add(ey.multiply(sTE));
  509.         final T x      = a.multiply(beta.multiply(ey).multiply(ey).negate().add(1).multiply(cTE).add(beta.multiply(exey).multiply(sTE)).subtract(ex));
  510.         final T y      = a.multiply(beta.multiply(ex).multiply(ex).negate().add(1).multiply(sTE).add(beta.multiply(exey).multiply(cTE)).subtract(ey));
  511.         final T factor = sqrtMmuOA.divide(exCeyS.negate().add(1));
  512.         final T xDot   = factor.multiply(beta.multiply(ey).multiply(exCeyS).subtract(sTE));
  513.         final T yDot   = factor.multiply(cTE.subtract(beta.multiply(ex).multiply(exCeyS)));

  515.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, u, y, v);
  516.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, u, yDot, v);
  517.         if (hasNonKeplerianAcceleration) {

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

  523.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  524.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, shiftedP,
  525.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  526.             final T                fixedR2 = fixedP.getNormSq();
  527.             final T                fixedR  = fixedR2.sqrt();
  528.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, shiftedV,
  529.                                                                  dt, nonKeplerianAcceleration);
  530.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR.multiply(fixedR2).reciprocal().multiply(getMu().negate()), shiftedP,
  531.                                                                  one, nonKeplerianAcceleration);

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

  535.         } else {
  536.             // don't include acceleration,
  537.             // so the shifted orbit is not considered to have derivatives
  538.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  539.         }

  541.     }

  543.     /** Compute shifted position and velocity in hyperbolic case.
  544.      * @param dt time shift
  545.      * @return shifted position and velocity
  546.      */
  547.     private FieldPVCoordinates<T> shiftPVHyperbolic(final T dt) {

  549.         final FieldPVCoordinates<T> pv = getPVCoordinates();
  550.         final FieldVector3D<T> pvP   = pv.getPosition();
  551.         final FieldVector3D<T> pvV   = pv.getVelocity();
  552.         final FieldVector3D<T> pvM   = pv.getMomentum();
  553.         final T r2      = pvP.getNormSq();
  554.         final T r       = r2.sqrt();
  555.         final T rV2OnMu = r.multiply(pvV.getNormSq()).divide(getMu());
  556.         final T a       = getA();
  557.         final T muA     = a.multiply(getMu());
  558.         final T e       = one.subtract(FieldVector3D.dotProduct(pvM, pvM).divide(muA)).sqrt();
  559.         final T sqrt    = e.add(1).divide(e.subtract(1)).sqrt();

  561.         // compute mean anomaly
  562.         final T eSH     = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  563.         final T eCH     = rV2OnMu.subtract(1);
  564.         final T H0      = eCH.add(eSH).divide(eCH.subtract(eSH)).log().divide(2);
  565.         final T M0      = e.multiply(H0.sinh()).subtract(H0);

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

  572.         // compute shifted eccentric anomaly
  573.         final T M1      = M0.add(getKeplerianMeanMotion().multiply(dt));
  574.         final T H1      = meanToHyperbolicEccentric(M1, e);

  576.         // compute shifted in-plane Cartesian coordinates
  577.         final T cH     = H1.cosh();
  578.         final T sH     = H1.sinh();
  579.         final T sE2m1  = e.subtract(1).multiply(e.add(1)).sqrt();

  581.         // coordinates of position and velocity in the orbital plane
  582.         final T x      = a.multiply(cH.subtract(e));
  583.         final T y      = a.negate().multiply(sE2m1).multiply(sH);
  584.         final T factor = getMu().divide(a.negate()).sqrt().divide(e.multiply(cH).subtract(1));
  585.         final T xDot   = factor.negate().multiply(sH);
  586.         final T yDot   =  factor.multiply(sE2m1).multiply(cH);

  588.         final FieldVector3D<T> shiftedP = new FieldVector3D<>(x, p, y, q);
  589.         final FieldVector3D<T> shiftedV = new FieldVector3D<>(xDot, p, yDot, q);
  590.         if (hasNonKeplerianAcceleration) {

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

  596.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  597.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, shiftedP,
  598.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  599.             final T                fixedR2 = fixedP.getNormSq();
  600.             final T                fixedR  = fixedR2.sqrt();
  601.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, shiftedV,
  602.                                                                  dt, nonKeplerianAcceleration);
  603.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR.multiply(fixedR2).reciprocal().multiply(getMu().negate()), shiftedP,
  604.                                                                  one, nonKeplerianAcceleration);

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

  608.         } else {
  609.             // don't include acceleration,
  610.             // so the shifted orbit is not considered to have derivatives
  611.             return new FieldPVCoordinates<>(shiftedP, shiftedV);
  612.         }

  614.     }

  616.     /** Computes the eccentric in-plane argument from the mean in-plane argument.
  617.      * @param thetaM = mean in-plane argument (rad)
  618.      * @param ex first component of eccentricity vector
  619.      * @param ey second component of eccentricity vector
  620.      * @return the eccentric in-plane argument.
  621.      */
  622.     private T meanToEccentric(final T thetaM, final T ex, final T ey) {
  623.         // Generalization of Kepler equation to in-plane parameters
  624.         // with thetaE = eta + E and
  625.         //      thetaM = eta + M = thetaE - ex.sin(thetaE) + ey.cos(thetaE)
  626.         // and eta being counted from an arbitrary reference in the orbital plane
  627.         T thetaE        = thetaM;
  628.         T thetaEMthetaM = zero;
  629.         int    iter          = 0;
  630.         do {
  631.             final T cosThetaE = thetaE.cos();
  632.             final T sinThetaE = thetaE.sin();

  634.             final T f2 = ex.multiply(sinThetaE).subtract(ey.multiply(cosThetaE));
  635.             final T f1 = one.subtract(ex.multiply(cosThetaE)).subtract(ey.multiply(sinThetaE));
  636.             final T f0 = thetaEMthetaM.subtract(f2);

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

  641.             thetaEMthetaM = thetaEMthetaM.subtract(shift);
  642.             thetaE         = thetaM.add(thetaEMthetaM);

  644.             if (FastMath.abs(shift.getReal()) <= 1.0e-12) {
  645.                 return thetaE;
  646.             }

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

  650.         throw new MathIllegalStateException(LocalizedCoreFormats.CONVERGENCE_FAILED);

  652.     }

  654.     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
  655.      * <p>
  656.      * The algorithm used here for solving hyperbolic Kepler equation is
  657.      * Danby's iterative method (3rd order) with Vallado's initial guess.
  658.      * </p>
  659.      * @param M mean anomaly (rad)
  660.      * @param ecc eccentricity
  661.      * @return the hyperbolic eccentric anomaly
  662.      */
  663.     private T meanToHyperbolicEccentric(final T M, final T ecc) {

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

  667.         // Initial guess
  668.         T H;
  669.         if (ecc.getReal() < 1.6) {
  670.             if ((-FastMath.PI < M.getReal() && M.getReal() < 0.) || M.getReal() > FastMath.PI) {
  671.                 H = M.subtract(ecc);
  672.             } else {
  673.                 H = M.add(ecc);
  674.             }
  675.         } else {
  676.             if (ecc.getReal() < 3.6 && FastMath.abs(M.getReal()) > FastMath.PI) {
  677.                 H = M.subtract(ecc.copySign(M));
  678.             } else {
  679.                 H = M.divide(ecc.subtract(1.));
  680.             }
  681.         }

  683.         // Iterative computation
  684.         int iter = 0;
  685.         do {
  686.             final T f3  = ecc.multiply(H.cosh());
  687.             final T f2  = ecc.multiply(H.sinh());
  688.             final T f1  = f3.subtract(1.);
  689.             final T f0  = f2.subtract(H).subtract(M);
  690.             final T f12 = f1.multiply(2);
  691.             final T d   = f0.divide(f12);
  692.             final T fdf = f1.subtract(d.multiply(f2));
  693.             final T ds  = f0.divide(fdf);

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

  697.             H = H.subtract(shift);

  699.             if (FastMath.abs(shift.getReal()) <= 1.0e-12) {
  700.                 return H;
  701.             }

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

  705.         throw new MathIllegalStateException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
  706.                                             iter);
  707.     }

  709.     /** Create a 6x6 identity matrix.
  710.      * @return 6x6 identity matrix
  711.      */
  712.     private T[][] create6x6Identity() {
  713.         // this is the fastest way to set the 6x6 identity matrix
  714.         final T[][] identity = MathArrays.buildArray(field, 6, 6);
  715.         for (int i = 0; i < 6; i++) {
  716.             Arrays.fill(identity[i], zero);
  717.             identity[i][i] = one;
  718.         }
  719.         return identity;
  720.     }

  722.     @Override
  723.     protected T[][] computeJacobianMeanWrtCartesian() {
  724.         return create6x6Identity();
  725.     }

  727.     @Override
  728.     protected T[][] computeJacobianEccentricWrtCartesian() {
  729.         return create6x6Identity();
  730.     }

  732.     @Override
  733.     protected T[][] computeJacobianTrueWrtCartesian() {
  734.         return create6x6Identity();
  735.     }

  737.     /** {@inheritDoc} */
  738.     public void addKeplerContribution(final PositionAngle type, final T gm,
  739.                                       final T[] pDot) {

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

  743.         // position derivative is velocity
  744.         final FieldVector3D<T> velocity = pv.getVelocity();
  745.         pDot[0] = pDot[0].add(velocity.getX());
  746.         pDot[1] = pDot[1].add(velocity.getY());
  747.         pDot[2] = pDot[2].add(velocity.getZ());

  749.         // velocity derivative is Newtonian acceleration
  750.         final FieldVector3D<T> position = pv.getPosition();
  751.         final T r2         = position.getNormSq();
  752.         final T coeff      = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
  753.         pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
  754.         pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
  755.         pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));

  757.     }

  759.     /**  Returns a string representation of this Orbit object.
  760.      * @return a string representation of this object
  761.      */
  762.     public String toString() {
  763.         // use only the six defining elements, like the other Orbit.toString() methods
  764.         final String comma = ", ";
  765.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  766.         final Vector3D p = pv.getPosition();
  767.         final Vector3D v = pv.getVelocity();
  768.         return "Cartesian parameters: {P(" +
  769.                 p.getX() + comma +
  770.                 p.getY() + comma +
  771.                 p.getZ() + "), V(" +
  772.                 v.getX() + comma +
  773.                 v.getY() + comma +
  774.                 v.getZ() + ")}";
  775.     }

  777.     @Override
  778.     public CartesianOrbit toOrbit() {
  779.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  780.         final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
  781.         if (hasDerivatives()) {
  782.             // getPVCoordinates includes accelerations that will be interpreted as derivatives
  783.             return new CartesianOrbit(pv, getFrame(), date, getMu().getReal());
  784.         } else {
  785.             // get rid of Keplerian acceleration so we don't assume
  786.             // we have derivatives when in fact we don't have them
  787.             return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
  788.                                       getFrame(), date, getMu().getReal());
  789.         }
  790.     }

  792. }
Created with JaCoCo 0.8.5.201910111838