FieldCartesianOrbit.java

  1. /* Copyright 2002-2018 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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}.
  60.  * </p>

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

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

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

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

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

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

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

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

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

  131.     }

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

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

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

  175.     }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  542.     }

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

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

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

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

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

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

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

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

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

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

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

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

  615.     }

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

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

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

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

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

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

  651.         throw new MathIllegalStateException(LocalizedCoreFormats.CONVERGENCE_FAILED);

  653.     }

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

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

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

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

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

  698.             H = H.subtract(shift);

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

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

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

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

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

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

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

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

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

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

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

  758.     }

  760.     /**  Returns a string representation of this Orbit object.
  761.      * @return a string representation of this object
  762.      */
  763.     public String toString() {
  764.         return "Cartesian parameters: " + getPVCoordinates().toString();
  765.     }

  767.     @Override
  768.     public CartesianOrbit toOrbit() {
  769.         final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
  770.         final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
  771.         if (hasDerivatives()) {
  772.             // getPVCoordinates includes accelerations that will be interpreted as derivatives
  773.             return new CartesianOrbit(pv, getFrame(), date, getMu());
  774.         } else {
  775.             // get rid of Keplerian acceleration so we don't assume
  776.             // we have derivatives when in fact we don't have them
  777.             return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
  778.                                       getFrame(), date, getMu());
  779.         }
  780.     }

  782. }
Created with JaCoCo 0.7.9.201702052155