CartesianOrbit.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;

  19. import java.io.Serializable;
  20. import java.util.stream.Stream;

  22. import org.hipparchus.analysis.differentiation.DSFactory;
  23. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  24. import org.hipparchus.exception.LocalizedCoreFormats;
  25. import org.hipparchus.exception.MathIllegalStateException;
  26. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  27. import org.hipparchus.geometry.euclidean.threed.Rotation;
  28. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  29. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  30. import org.hipparchus.util.FastMath;
  31. import org.orekit.annotation.DefaultDataContext;
  32. import org.orekit.data.DataContext;
  33. import org.orekit.errors.OrekitMessages;
  34. import org.orekit.frames.Frame;
  35. import org.orekit.time.AbsoluteDate;
  36. import org.orekit.utils.CartesianDerivativesFilter;
  37. import org.orekit.utils.PVCoordinates;
  38. import org.orekit.utils.TimeStampedPVCoordinates;


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

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

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

  77.     /** Serializable UID. */
  78.     private static final long serialVersionUID = 20170414L;

  80.     /** Factory for first time derivatives. */
  81.     private static final DSFactory FACTORY = new DSFactory(1, 1);

  83.     /** Indicator for non-Keplerian derivatives. */
  84.     private final transient boolean hasNonKeplerianAcceleration;

  86.     /** Underlying equinoctial orbit to which high-level methods are delegated. */
  87.     private transient EquinoctialOrbit equinoctial;

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

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

  132.     /** Constructor from any kind of orbital parameters.
  133.      * @param op orbital parameters to copy
  134.      */
  135.     public CartesianOrbit(final Orbit op) {
  136.         super(op.getPVCoordinates(), op.getFrame(), op.getMu());
  137.         hasNonKeplerianAcceleration = op.hasDerivatives();
  138.         if (op instanceof EquinoctialOrbit) {
  139.             equinoctial = (EquinoctialOrbit) op;
  140.         } else if (op instanceof CartesianOrbit) {
  141.             equinoctial = ((CartesianOrbit) op).equinoctial;
  142.         } else {
  143.             equinoctial = null;
  144.         }
  145.     }

  147.     /** {@inheritDoc} */
  148.     public OrbitType getType() {
  149.         return OrbitType.CARTESIAN;
  150.     }

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

  168.     /** Get position with derivatives.
  169.      * @return position with derivatives
  170.      */
  171.     private FieldVector3D<DerivativeStructure> getPositionDS() {
  172.         final Vector3D p = getPVCoordinates().getPosition();
  173.         final Vector3D v = getPVCoordinates().getVelocity();
  174.         return new FieldVector3D<>(FACTORY.build(p.getX(), v.getX()),
  175.                                    FACTORY.build(p.getY(), v.getY()),
  176.                                    FACTORY.build(p.getZ(), v.getZ()));
  177.     }

  179.     /** Get velocity with derivatives.
  180.      * @return velocity with derivatives
  181.      */
  182.     private FieldVector3D<DerivativeStructure> getVelocityDS() {
  183.         final Vector3D v = getPVCoordinates().getVelocity();
  184.         final Vector3D a = getPVCoordinates().getAcceleration();
  185.         return new FieldVector3D<>(FACTORY.build(v.getX(), a.getX()),
  186.                                    FACTORY.build(v.getY(), a.getY()),
  187.                                    FACTORY.build(v.getZ(), a.getZ()));
  188.     }

  190.     /** {@inheritDoc} */
  191.     public double getA() {
  192.         final double r  = getPVCoordinates().getPosition().getNorm();
  193.         final double V2 = getPVCoordinates().getVelocity().getNormSq();
  194.         return r / (2 - r * V2 / getMu());
  195.     }

  197.     /** {@inheritDoc} */
  198.     public double getADot() {
  199.         if (hasDerivatives()) {
  200.             final DerivativeStructure r  = getPositionDS().getNorm();
  201.             final DerivativeStructure V2 = getVelocityDS().getNormSq();
  202.             final DerivativeStructure a  = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
  203.             return a.getPartialDerivative(1);
  204.         } else {
  205.             return Double.NaN;
  206.         }
  207.     }

  209.     /** {@inheritDoc} */
  210.     public double getE() {
  211.         final double muA = getMu() * getA();
  212.         if (muA > 0) {
  213.             // elliptic or circular orbit
  214.             final Vector3D pvP   = getPVCoordinates().getPosition();
  215.             final Vector3D pvV   = getPVCoordinates().getVelocity();
  216.             final double rV2OnMu = pvP.getNorm() * pvV.getNormSq() / getMu();
  217.             final double eSE     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
  218.             final double eCE     = rV2OnMu - 1;
  219.             return FastMath.sqrt(eCE * eCE + eSE * eSE);
  220.         } else {
  221.             // hyperbolic orbit
  222.             final Vector3D pvM = getPVCoordinates().getMomentum();
  223.             return FastMath.sqrt(1 - pvM.getNormSq() / muA);
  224.         }
  225.     }

  227.     /** {@inheritDoc} */
  228.     public double getEDot() {
  229.         if (hasDerivatives()) {
  230.             final FieldVector3D<DerivativeStructure> pvP   = getPositionDS();
  231.             final FieldVector3D<DerivativeStructure> pvV   = getVelocityDS();
  232.             final DerivativeStructure r       = getPositionDS().getNorm();
  233.             final DerivativeStructure V2      = getVelocityDS().getNormSq();
  234.             final DerivativeStructure rV2OnMu = r.multiply(V2).divide(getMu());
  235.             final DerivativeStructure a       = r.divide(rV2OnMu.negate().add(2));
  236.             final DerivativeStructure eSE     = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
  237.             final DerivativeStructure eCE     = rV2OnMu.subtract(1);
  238.             final DerivativeStructure e       = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
  239.             return e.getPartialDerivative(1);
  240.         } else {
  241.             return Double.NaN;
  242.         }
  243.     }

  245.     /** {@inheritDoc} */
  246.     public double getI() {
  247.         return Vector3D.angle(Vector3D.PLUS_K, getPVCoordinates().getMomentum());
  248.     }

  250.     /** {@inheritDoc} */
  251.     public double getIDot() {
  252.         if (hasDerivatives()) {
  253.             final FieldVector3D<DerivativeStructure> momentum =
  254.                             FieldVector3D.crossProduct(getPositionDS(), getVelocityDS());
  255.             final DerivativeStructure i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
  256.             return i.getPartialDerivative(1);
  257.         } else {
  258.             return Double.NaN;
  259.         }
  260.     }

  262.     /** {@inheritDoc} */
  263.     public double getEquinoctialEx() {
  264.         initEquinoctial();
  265.         return equinoctial.getEquinoctialEx();
  266.     }

  268.     /** {@inheritDoc} */
  269.     public double getEquinoctialExDot() {
  270.         initEquinoctial();
  271.         return equinoctial.getEquinoctialExDot();
  272.     }

  274.     /** {@inheritDoc} */
  275.     public double getEquinoctialEy() {
  276.         initEquinoctial();
  277.         return equinoctial.getEquinoctialEy();
  278.     }

  280.     /** {@inheritDoc} */
  281.     public double getEquinoctialEyDot() {
  282.         initEquinoctial();
  283.         return equinoctial.getEquinoctialEyDot();
  284.     }

  286.     /** {@inheritDoc} */
  287.     public double getHx() {
  288.         final Vector3D w = getPVCoordinates().getMomentum().normalize();
  289.         // Check for equatorial retrograde orbit
  290.         if (((w.getX() * w.getX() + w.getY() * w.getY()) == 0) && w.getZ() < 0) {
  291.             return Double.NaN;
  292.         }
  293.         return -w.getY() / (1 + w.getZ());
  294.     }

  296.     /** {@inheritDoc} */
  297.     public double getHxDot() {
  298.         if (hasDerivatives()) {
  299.             final FieldVector3D<DerivativeStructure> w =
  300.                             FieldVector3D.crossProduct(getPositionDS(), getVelocityDS()).normalize();
  301.             // Check for equatorial retrograde orbit
  302.             final double x = w.getX().getValue();
  303.             final double y = w.getY().getValue();
  304.             final double z = w.getZ().getValue();
  305.             if (((x * x + y * y) == 0) && z < 0) {
  306.                 return Double.NaN;
  307.             }
  308.             final DerivativeStructure hx = w.getY().negate().divide(w.getZ().add(1));
  309.             return hx.getPartialDerivative(1);
  310.         } else {
  311.             return Double.NaN;
  312.         }
  313.     }

  315.     /** {@inheritDoc} */
  316.     public double getHy() {
  317.         final Vector3D w = getPVCoordinates().getMomentum().normalize();
  318.         // Check for equatorial retrograde orbit
  319.         if (((w.getX() * w.getX() + w.getY() * w.getY()) == 0) && w.getZ() < 0) {
  320.             return Double.NaN;
  321.         }
  322.         return  w.getX() / (1 + w.getZ());
  323.     }

  325.     /** {@inheritDoc} */
  326.     public double getHyDot() {
  327.         if (hasDerivatives()) {
  328.             final FieldVector3D<DerivativeStructure> w =
  329.                             FieldVector3D.crossProduct(getPositionDS(), getVelocityDS()).normalize();
  330.             // Check for equatorial retrograde orbit
  331.             final double x = w.getX().getValue();
  332.             final double y = w.getY().getValue();
  333.             final double z = w.getZ().getValue();
  334.             if (((x * x + y * y) == 0) && z < 0) {
  335.                 return Double.NaN;
  336.             }
  337.             final DerivativeStructure hy = w.getX().divide(w.getZ().add(1));
  338.             return hy.getPartialDerivative(1);
  339.         } else {
  340.             return Double.NaN;
  341.         }
  342.     }

  344.     /** {@inheritDoc} */
  345.     public double getLv() {
  346.         initEquinoctial();
  347.         return equinoctial.getLv();
  348.     }

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

  356.     /** {@inheritDoc} */
  357.     public double getLE() {
  358.         initEquinoctial();
  359.         return equinoctial.getLE();
  360.     }

  362.     /** {@inheritDoc} */
  363.     public double getLEDot() {
  364.         initEquinoctial();
  365.         return equinoctial.getLEDot();
  366.     }

  368.     /** {@inheritDoc} */
  369.     public double getLM() {
  370.         initEquinoctial();
  371.         return equinoctial.getLM();
  372.     }

  374.     /** {@inheritDoc} */
  375.     public double getLMDot() {
  376.         initEquinoctial();
  377.         return equinoctial.getLMDot();
  378.     }

  380.     /** {@inheritDoc} */
  381.     public boolean hasDerivatives() {
  382.         return hasNonKeplerianAcceleration;
  383.     }

  385.     /** {@inheritDoc} */
  386.     protected TimeStampedPVCoordinates initPVCoordinates() {
  387.         // nothing to do here, as the canonical elements are already the Cartesian ones
  388.         return getPVCoordinates();
  389.     }

  391.     /** {@inheritDoc} */
  392.     public CartesianOrbit shiftedBy(final double dt) {
  393.         final PVCoordinates shiftedPV = (getA() < 0) ? shiftPVHyperbolic(dt) : shiftPVElliptic(dt);
  394.         return new CartesianOrbit(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
  395.     }

  397.     /** {@inheritDoc}
  398.      * <p>
  399.      * The interpolated instance is created by polynomial Hermite interpolation
  400.      * ensuring velocity remains the exact derivative of position.
  401.      * </p>
  402.      * <p>
  403.      * As this implementation of interpolation is polynomial, it should be used only
  404.      * with small samples (about 10-20 points) in order to avoid <a
  405.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  406.      * and numerical problems (including NaN appearing).
  407.      * </p>
  408.      * <p>
  409.      * If orbit interpolation on large samples is needed, using the {@link
  410.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  411.      * low-level interpolation. The Ephemeris class automatically handles selection of
  412.      * a neighboring sub-sample with a predefined number of point from a large global sample
  413.      * in a thread-safe way.
  414.      * </p>
  415.      */
  416.     public CartesianOrbit interpolate(final AbsoluteDate date, final Stream<Orbit> sample) {
  417.         final TimeStampedPVCoordinates interpolated =
  418.                 TimeStampedPVCoordinates.interpolate(date, CartesianDerivativesFilter.USE_PVA,
  419.                                                      sample.map(orbit -> orbit.getPVCoordinates()));
  420.         return new CartesianOrbit(interpolated, getFrame(), date, getMu());
  421.     }

  423.     /** Compute shifted position and velocity in elliptic case.
  424.      * @param dt time shift
  425.      * @return shifted position and velocity
  426.      */
  427.     private PVCoordinates shiftPVElliptic(final double dt) {

  429.         // preliminary computation
  430.         final Vector3D pvP   = getPVCoordinates().getPosition();
  431.         final Vector3D pvV   = getPVCoordinates().getVelocity();
  432.         final double r2      = pvP.getNormSq();
  433.         final double r       = FastMath.sqrt(r2);
  434.         final double rV2OnMu = r * pvV.getNormSq() / getMu();
  435.         final double a       = getA();
  436.         final double eSE     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * a);
  437.         final double eCE     = rV2OnMu - 1;
  438.         final double e2      = eCE * eCE + eSE * eSE;

  440.         // we can use any arbitrary reference 2D frame in the orbital plane
  441.         // in order to simplify some equations below, we use the current position as the u axis
  442.         final Vector3D u     = pvP.normalize();
  443.         final Vector3D v     = Vector3D.crossProduct(getPVCoordinates().getMomentum(), u).normalize();

  445.         // the following equations rely on the specific choice of u explained above,
  446.         // some coefficients that vanish to 0 in this case have already been removed here
  447.         final double ex      = (eCE - e2) * a / r;
  448.         final double ey      = -FastMath.sqrt(1 - e2) * eSE * a / r;
  449.         final double beta    = 1 / (1 + FastMath.sqrt(1 - e2));
  450.         final double thetaE0 = FastMath.atan2(ey + eSE * beta * ex, r / a + ex - eSE * beta * ey);
  451.         final double thetaM0 = thetaE0 - ex * FastMath.sin(thetaE0) + ey * FastMath.cos(thetaE0);

  453.         // compute in-plane shifted eccentric argument
  454.         final double thetaM1 = thetaM0 + getKeplerianMeanMotion() * dt;
  455.         final double thetaE1 = meanToEccentric(thetaM1, ex, ey);
  456.         final double cTE     = FastMath.cos(thetaE1);
  457.         final double sTE     = FastMath.sin(thetaE1);

  459.         // compute shifted in-plane Cartesian coordinates
  460.         final double exey   = ex * ey;
  461.         final double exCeyS = ex * cTE + ey * sTE;
  462.         final double x      = a * ((1 - beta * ey * ey) * cTE + beta * exey * sTE - ex);
  463.         final double y      = a * ((1 - beta * ex * ex) * sTE + beta * exey * cTE - ey);
  464.         final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
  465.         final double xDot   = factor * (-sTE + beta * ey * exCeyS);
  466.         final double yDot   = factor * ( cTE - beta * ex * exCeyS);

  468.         final Vector3D shiftedP = new Vector3D(x, u, y, v);
  469.         final Vector3D shiftedV = new Vector3D(xDot, u, yDot, v);
  470.         if (hasNonKeplerianAcceleration) {

  472.             // extract non-Keplerian part of the initial acceleration
  473.             final Vector3D nonKeplerianAcceleration = new Vector3D(1, getPVCoordinates().getAcceleration(),
  474.                                                                    getMu() / (r2 * r), pvP);

  476.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  477.             final Vector3D fixedP   = new Vector3D(1, shiftedP,
  478.                                                    0.5 * dt * dt, nonKeplerianAcceleration);
  479.             final double   fixedR2 = fixedP.getNormSq();
  480.             final double   fixedR  = FastMath.sqrt(fixedR2);
  481.             final Vector3D fixedV  = new Vector3D(1, shiftedV,
  482.                                                   dt, nonKeplerianAcceleration);
  483.             final Vector3D fixedA  = new Vector3D(-getMu() / (fixedR2 * fixedR), shiftedP,
  484.                                                   1, nonKeplerianAcceleration);

  486.             return new PVCoordinates(fixedP, fixedV, fixedA);

  488.         } else {
  489.             // don't include acceleration,
  490.             // so the shifted orbit is not considered to have derivatives
  491.             return new PVCoordinates(shiftedP, shiftedV);
  492.         }

  494.     }

  496.     /** Compute shifted position and velocity in hyperbolic case.
  497.      * @param dt time shift
  498.      * @return shifted position and velocity
  499.      */
  500.     private PVCoordinates shiftPVHyperbolic(final double dt) {

  502.         final PVCoordinates pv = getPVCoordinates();
  503.         final Vector3D pvP   = pv.getPosition();
  504.         final Vector3D pvV   = pv.getVelocity();
  505.         final Vector3D pvM   = pv.getMomentum();
  506.         final double r2      = pvP.getNormSq();
  507.         final double r       = FastMath.sqrt(r2);
  508.         final double rV2OnMu = r * pvV.getNormSq() / getMu();
  509.         final double a       = getA();
  510.         final double muA     = getMu() * a;
  511.         final double e       = FastMath.sqrt(1 - Vector3D.dotProduct(pvM, pvM) / muA);
  512.         final double sqrt    = FastMath.sqrt((e + 1) / (e - 1));

  514.         // compute mean anomaly
  515.         final double eSH     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA);
  516.         final double eCH     = rV2OnMu - 1;
  517.         final double H0      = FastMath.log((eCH + eSH) / (eCH - eSH)) / 2;
  518.         final double M0      = e * FastMath.sinh(H0) - H0;

  520.         // find canonical 2D frame with p pointing to perigee
  521.         final double v0      = 2 * FastMath.atan(sqrt * FastMath.tanh(H0 / 2));
  522.         final Vector3D p     = new Rotation(pvM, v0, RotationConvention.FRAME_TRANSFORM).applyTo(pvP).normalize();
  523.         final Vector3D q     = Vector3D.crossProduct(pvM, p).normalize();

  525.         // compute shifted eccentric anomaly
  526.         final double M1      = M0 + getKeplerianMeanMotion() * dt;
  527.         final double H1      = meanToHyperbolicEccentric(M1, e);

  529.         // compute shifted in-plane Cartesian coordinates
  530.         final double cH     = FastMath.cosh(H1);
  531.         final double sH     = FastMath.sinh(H1);
  532.         final double sE2m1  = FastMath.sqrt((e - 1) * (e + 1));

  534.         // coordinates of position and velocity in the orbital plane
  535.         final double x      = a * (cH - e);
  536.         final double y      = -a * sE2m1 * sH;
  537.         final double factor = FastMath.sqrt(getMu() / -a) / (e * cH - 1);
  538.         final double xDot   = -factor * sH;
  539.         final double yDot   =  factor * sE2m1 * cH;

  541.         final Vector3D shiftedP = new Vector3D(x, p, y, q);
  542.         final Vector3D shiftedV = new Vector3D(xDot, p, yDot, q);
  543.         if (hasNonKeplerianAcceleration) {

  545.             // extract non-Keplerian part of the initial acceleration
  546.             final Vector3D nonKeplerianAcceleration = new Vector3D(1, getPVCoordinates().getAcceleration(),
  547.                                                                    getMu() / (r2 * r), pvP);

  549.             // add the quadratic motion due to the non-Keplerian acceleration to the Keplerian motion
  550.             final Vector3D fixedP   = new Vector3D(1, shiftedP,
  551.                                                    0.5 * dt * dt, nonKeplerianAcceleration);
  552.             final double   fixedR2 = fixedP.getNormSq();
  553.             final double   fixedR  = FastMath.sqrt(fixedR2);
  554.             final Vector3D fixedV  = new Vector3D(1, shiftedV,
  555.                                                   dt, nonKeplerianAcceleration);
  556.             final Vector3D fixedA  = new Vector3D(-getMu() / (fixedR2 * fixedR), shiftedP,
  557.                                                   1, nonKeplerianAcceleration);

  559.             return new PVCoordinates(fixedP, fixedV, fixedA);

  561.         } else {
  562.             // don't include acceleration,
  563.             // so the shifted orbit is not considered to have derivatives
  564.             return new PVCoordinates(shiftedP, shiftedV);
  565.         }

  567.     }

  569.     /** Computes the eccentric in-plane argument from the mean in-plane argument.
  570.      * @param thetaM = mean in-plane argument (rad)
  571.      * @param ex first component of eccentricity vector
  572.      * @param ey second component of eccentricity vector
  573.      * @return the eccentric in-plane argument.
  574.      */
  575.     private double meanToEccentric(final double thetaM, final double ex, final double ey) {
  576.         // Generalization of Kepler equation to in-plane parameters
  577.         // with thetaE = eta + E and
  578.         //      thetaM = eta + M = thetaE - ex.sin(thetaE) + ey.cos(thetaE)
  579.         // and eta being counted from an arbitrary reference in the orbital plane
  580.         double thetaE        = thetaM;
  581.         double thetaEMthetaM = 0.0;
  582.         int    iter          = 0;
  583.         do {
  584.             final double cosThetaE = FastMath.cos(thetaE);
  585.             final double sinThetaE = FastMath.sin(thetaE);

  587.             final double f2 = ex * sinThetaE - ey * cosThetaE;
  588.             final double f1 = 1.0 - ex * cosThetaE - ey * sinThetaE;
  589.             final double f0 = thetaEMthetaM - f2;

  591.             final double f12 = 2.0 * f1;
  592.             final double shift = f0 * f12 / (f1 * f12 - f0 * f2);

  594.             thetaEMthetaM -= shift;
  595.             thetaE         = thetaM + thetaEMthetaM;

  597.             if (FastMath.abs(shift) <= 1.0e-12) {
  598.                 return thetaE;
  599.             }

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

  603.         throw new MathIllegalStateException(LocalizedCoreFormats.CONVERGENCE_FAILED);

  605.     }

  607.     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
  608.      * <p>
  609.      * The algorithm used here for solving hyperbolic Kepler equation is
  610.      * Danby's iterative method (3rd order) with Vallado's initial guess.
  611.      * </p>
  612.      * @param M mean anomaly (rad)
  613.      * @param ecc eccentricity
  614.      * @return the hyperbolic eccentric anomaly
  615.      */
  616.     private double meanToHyperbolicEccentric(final double M, final double ecc) {

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

  620.         // Initial guess
  621.         double H;
  622.         if (ecc < 1.6) {
  623.             if ((-FastMath.PI < M && M < 0.) || M > FastMath.PI) {
  624.                 H = M - ecc;
  625.             } else {
  626.                 H = M + ecc;
  627.             }
  628.         } else {
  629.             if (ecc < 3.6 && FastMath.abs(M) > FastMath.PI) {
  630.                 H = M - FastMath.copySign(ecc, M);
  631.             } else {
  632.                 H = M / (ecc - 1.);
  633.             }
  634.         }

  636.         // Iterative computation
  637.         int iter = 0;
  638.         do {
  639.             final double f3  = ecc * FastMath.cosh(H);
  640.             final double f2  = ecc * FastMath.sinh(H);
  641.             final double f1  = f3 - 1.;
  642.             final double f0  = f2 - H - M;
  643.             final double f12 = 2. * f1;
  644.             final double d   = f0 / f12;
  645.             final double fdf = f1 - d * f2;
  646.             final double ds  = f0 / fdf;

  648.             final double shift = f0 / (fdf + ds * ds * f3 / 6.);

  650.             H -= shift;

  652.             if (FastMath.abs(shift) <= 1.0e-12) {
  653.                 return H;
  654.             }

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

  658.         throw new MathIllegalStateException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
  659.                                             iter);
  660.     }

  662.     /** Create a 6x6 identity matrix.
  663.      * @return 6x6 identity matrix
  664.      */
  665.     private double[][] create6x6Identity() {
  666.         // this is the fastest way to set the 6x6 identity matrix
  667.         final double[][] identity = new double[6][6];
  668.         for (int i = 0; i < 6; i++) {
  669.             identity[i][i] = 1.0;
  670.         }
  671.         return identity;
  672.     }

  674.     @Override
  675.     protected double[][] computeJacobianMeanWrtCartesian() {
  676.         return create6x6Identity();
  677.     }

  679.     @Override
  680.     protected double[][] computeJacobianEccentricWrtCartesian() {
  681.         return create6x6Identity();
  682.     }

  684.     @Override
  685.     protected double[][] computeJacobianTrueWrtCartesian() {
  686.         return create6x6Identity();
  687.     }

  689.     /** {@inheritDoc} */
  690.     public void addKeplerContribution(final PositionAngle type, final double gm,
  691.                                       final double[] pDot) {

  693.         final PVCoordinates pv = getPVCoordinates();

  695.         // position derivative is velocity
  696.         final Vector3D velocity = pv.getVelocity();
  697.         pDot[0] += velocity.getX();
  698.         pDot[1] += velocity.getY();
  699.         pDot[2] += velocity.getZ();

  701.         // velocity derivative is Newtonian acceleration
  702.         final Vector3D position = pv.getPosition();
  703.         final double r2         = position.getNormSq();
  704.         final double coeff      = -gm / (r2 * FastMath.sqrt(r2));
  705.         pDot[3] += coeff * position.getX();
  706.         pDot[4] += coeff * position.getY();
  707.         pDot[5] += coeff * position.getZ();

  709.     }

  711.     /**  Returns a string representation of this Orbit object.
  712.      * @return a string representation of this object
  713.      */
  714.     public String toString() {
  715.         // use only the six defining elements, like the other Orbit.toString() methods
  716.         final String comma = ", ";
  717.         final PVCoordinates pv = getPVCoordinates();
  718.         final Vector3D p = pv.getPosition();
  719.         final Vector3D v = pv.getVelocity();
  720.         return "Cartesian parameters: {P(" +
  721.                 p.getX() + comma +
  722.                 p.getY() + comma +
  723.                 p.getZ() + "), V(" +
  724.                 v.getX() + comma +
  725.                 v.getY() + comma +
  726.                 v.getZ() + ")}";
  727.     }

  729.     /** Replace the instance with a data transfer object for serialization.
  730.      * <p>
  731.      * This intermediate class serializes all needed parameters,
  732.      * including position-velocity which are <em>not</em> serialized by parent
  733.      * {@link Orbit} class.
  734.      * </p>
  735.      * @return data transfer object that will be serialized
  736.      */
  737.     @DefaultDataContext
  738.     private Object writeReplace() {
  739.         return new DTO(this);
  740.     }

  742.     /** Internal class used only for serialization. */
  743.     @DefaultDataContext
  744.     private static class DTO implements Serializable {

  746.         /** Serializable UID. */
  747.         private static final long serialVersionUID = 20170414L;

  749.         /** Double values. */
  750.         private double[] d;

  752.         /** Frame in which are defined the orbital parameters. */
  753.         private final Frame frame;

  755.         /** Simple constructor.
  756.          * @param orbit instance to serialize
  757.          */
  758.         private DTO(final CartesianOrbit orbit) {

  760.             final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();

  762.             // decompose date
  763.             final AbsoluteDate j2000Epoch =
  764.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  765.             final double epoch  = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
  766.             final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));

  768.             if (orbit.hasDerivatives()) {
  769.                 this.d = new double[] {
  770.                     epoch, offset, orbit.getMu(),
  771.                     pv.getPosition().getX(),     pv.getPosition().getY(),     pv.getPosition().getZ(),
  772.                     pv.getVelocity().getX(),     pv.getVelocity().getY(),     pv.getVelocity().getZ(),
  773.                     pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ()
  774.                 };
  775.             } else {
  776.                 this.d = new double[] {
  777.                     epoch, offset, orbit.getMu(),
  778.                     pv.getPosition().getX(),     pv.getPosition().getY(),     pv.getPosition().getZ(),
  779.                     pv.getVelocity().getX(),     pv.getVelocity().getY(),     pv.getVelocity().getZ()
  780.                 };
  781.             }

  783.             this.frame = orbit.getFrame();

  785.         }

  787.         /** Replace the deserialized data transfer object with a {@link CartesianOrbit}.
  788.          * @return replacement {@link CartesianOrbit}
  789.          */
  790.         private Object readResolve() {
  791.             final AbsoluteDate j2000Epoch =
  792.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  793.             if (d.length >= 12) {
  794.                 // we have derivatives
  795.                 return new CartesianOrbit(new TimeStampedPVCoordinates(j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  796.                                                                        new Vector3D(d[3], d[ 4], d[ 5]),
  797.                                                                        new Vector3D(d[6], d[ 7], d[ 8]),
  798.                                                                        new Vector3D(d[9], d[10], d[11])),
  799.                                           frame, d[2]);
  800.             } else {
  801.                 // we don't have derivatives
  802.                 return new CartesianOrbit(new TimeStampedPVCoordinates(j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  803.                                                                        new Vector3D(d[3], d[ 4], d[ 5]),
  804.                                                                        new Vector3D(d[6], d[ 7], d[ 8])),
  805.                                           frame, d[2]);
  806.             }
  807.         }

  809.     }

  811. }
Created with JaCoCo 0.8.5.201910111838