CircularOrbit.java

  1. /* Copyright 2002-2013 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;

  19. import java.util.Collection;

  21. import org.apache.commons.math3.analysis.interpolation.HermiteInterpolator;
  22. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  23. import org.apache.commons.math3.util.FastMath;
  24. import org.apache.commons.math3.util.MathUtils;
  25. import org.orekit.errors.OrekitException;
  26. import org.orekit.errors.OrekitMessages;
  27. import org.orekit.frames.Frame;
  28. import org.orekit.time.AbsoluteDate;
  29. import org.orekit.utils.PVCoordinates;


  32. /**
  33.  * This class handles circular orbital parameters.

  35.  * <p>
  36.  * The parameters used internally are the circular elements which can be
  37.  * related to keplerian elements as follows:
  38.  *   <ul>
  39.  *     <li>a</li>
  40.  *     <li>e<sub>x</sub> = e cos(&omega;)</li>
  41.  *     <li>e<sub>y</sub> = e sin(&omega;)</li>
  42.  *     <li>i</li>
  43.  *     <li>&Omega;</li>
  44.  *     <li>&alpha;<sub>v</sub> = v + &omega;</li>
  45.  *   </ul>
  46.  * where &Omega; stands for the Right Ascension of the Ascending Node and
  47.  * &alpha;<sub>v</sub> stands for the true latitude argument
  48.  * </p>
  49.  * <p>
  50.  * The conversion equations from and to keplerian elements given above hold only
  51.  * when both sides are unambiguously defined, i.e. when orbit is neither equatorial
  52.  * nor circular. When orbit is circular (but not equatorial), the circular
  53.  * parameters are still unambiguously defined whereas some keplerian elements
  54.  * (more precisely &omega; and &Omega;) become ambiguous. When orbit is equatorial,
  55.  * neither the keplerian nor the circular parameters can be defined unambiguously.
  56.  * {@link EquinoctialOrbit equinoctial orbits} is the recommended way to represent
  57.  * orbits.
  58.  * </p>
  59.  * <p>
  60.  * The instance <code>CircularOrbit</code> is guaranteed to be immutable.
  61.  * </p>
  62.  * @see    Orbit
  63.  * @see    KeplerianOrbit
  64.  * @see    CartesianOrbit
  65.  * @see    EquinoctialOrbit
  66.  * @author Luc Maisonobe
  67.  * @author Fabien Maussion
  68.  * @author V&eacute;ronique Pommier-Maurussane
  69.  */

  71. public class CircularOrbit
  72.     extends Orbit {

  74.     /** Identifier for mean latitude argument.
  75.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  76.      */
  77.     @Deprecated
  78.     public static final int MEAN_LONGITUDE_ARGUMENT = 0;

  80.     /** Identifier for eccentric latitude argument.
  81.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  82.      */
  83.     @Deprecated
  84.     public static final int ECCENTRIC_LONGITUDE_ARGUMENT = 1;

  86.     /** Identifier for true latitude argument.
  87.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  88.      */
  89.     @Deprecated
  90.     public static final int TRUE_LONGITUDE_ARGUMENT = 2;

  92.     /** Serializable UID. */
  93.     private static final long serialVersionUID = 5565190329070485158L;

  95.     /** Semi-major axis (m). */
  96.     private final double a;

  98.     /** First component of the circular eccentricity vector. */
  99.     private final double ex;

  101.     /** Second component of the circular eccentricity vector. */
  102.     private final double ey;

  104.     /** Inclination (rad). */
  105.     private final double i;

  107.     /** Right Ascension of Ascending Node (rad). */
  108.     private final double raan;

  110.     /** True latitude argument (rad). */
  111.     private final double alphaV;

  113.     /** Creates a new instance.
  114.      * @param a  semi-major axis (m)
  115.      * @param ex e cos(&omega;), first component of circular eccentricity vector
  116.      * @param ey e sin(&omega;), second component of circular eccentricity vector
  117.      * @param i inclination (rad)
  118.      * @param raan right ascension of ascending node (&Omega;, rad)
  119.      * @param alpha  an + &omega;, mean, eccentric or true latitude argument (rad)
  120.      * @param type type of latitude argument
  121.      * @param frame the frame in which are defined the parameters
  122.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  123.      * @param date date of the orbital parameters
  124.      * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  125.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  126.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  127.      */
  128.     public CircularOrbit(final double a, final double ex, final double ey,
  129.                          final double i, final double raan,
  130.                          final double alpha, final PositionAngle type,
  131.                          final Frame frame, final AbsoluteDate date, final double mu)
  132.         throws IllegalArgumentException {
  133.         super(frame, date, mu);
  134.         if (ex * ex + ey * ey >= 1.0) {
  135.             throw OrekitException.createIllegalArgumentException(
  136.                   OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName());
  137.         }
  138.         this.a    =  a;
  139.         this.ex   = ex;
  140.         this.ey   = ey;
  141.         this.i    = i;
  142.         this.raan = raan;

  144.         switch (type) {
  145.         case MEAN :
  146.             this.alphaV = eccentricToTrue(meanToEccentric(alpha));
  147.             break;
  148.         case ECCENTRIC :
  149.             this.alphaV = eccentricToTrue(alpha);
  150.             break;
  151.         case TRUE :
  152.             this.alphaV = alpha;
  153.             break;
  154.         default :
  155.             throw OrekitException.createInternalError(null);
  156.         }

  158.     }

  160.     /** Creates a new instance.
  161.      * @param a  semi-major axis (m)
  162.      * @param ex e cos(&omega;), first component of circular eccentricity vector
  163.      * @param ey e sin(&omega;), second component of circular eccentricity vector
  164.      * @param i inclination (rad)
  165.      * @param raan right ascension of ascending node (&Omega;, rad)
  166.      * @param alpha  an + &omega;, mean, eccentric or true latitude argument (rad)
  167.      * @param type type of latitude argument, must be one of {@link #MEAN_LONGITUDE_ARGUMENT},
  168.      * {@link #ECCENTRIC_LONGITUDE_ARGUMENT} or  {@link #TRUE_LONGITUDE_ARGUMENT}
  169.      * @param frame the frame in which are defined the parameters
  170.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  171.      * @param date date of the orbital parameters
  172.      * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  173.      * @exception IllegalArgumentException if the latitude argument type is not
  174.      * one of {@link #MEAN_LONGITUDE_ARGUMENT}, {@link #ECCENTRIC_LONGITUDE_ARGUMENT}
  175.      * or {@link #TRUE_LONGITUDE_ARGUMENT} or if frame is not a {@link
  176.      * Frame#isPseudoInertial pseudo-inertial frame}
  177.      * @see #MEAN_LONGITUDE_ARGUMENT
  178.      * @see #ECCENTRIC_LONGITUDE_ARGUMENT
  179.      * @see #TRUE_LONGITUDE_ARGUMENT
  180.      * @deprecated as of 6.0 replaced by {@link #CircularOrbit(double, double, double,
  181.      * double, double, double, PositionAngle, Frame, AbsoluteDate, double)}
  182.      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
  183.      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
  184.      */
  185.     @Deprecated
  186.     public CircularOrbit(final double a, final double ex, final double ey,
  187.                          final double i, final double raan,
  188.                          final double alpha, final int type,
  189.                          final Frame frame, final AbsoluteDate date, final double mu)
  190.         throws IllegalArgumentException {
  191.         super(frame, date, mu);
  192.         if (ex * ex + ey * ey >= 1.0) {
  193.             throw OrekitException.createIllegalArgumentException(
  194.                   OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName());
  195.         }
  196.         this.a    =  a;
  197.         this.ex   = ex;
  198.         this.ey   = ey;
  199.         this.i    = i;
  200.         this.raan = raan;

  202.         switch (type) {
  203.         case MEAN_LONGITUDE_ARGUMENT :
  204.             this.alphaV = eccentricToTrue(meanToEccentric(alpha));
  205.             break;
  206.         case ECCENTRIC_LONGITUDE_ARGUMENT :
  207.             this.alphaV = eccentricToTrue(alpha);
  208.             break;
  209.         case TRUE_LONGITUDE_ARGUMENT :
  210.             this.alphaV = alpha;
  211.             break;
  212.         default :
  213.             this.alphaV = Double.NaN;
  214.             throw OrekitException.createIllegalArgumentException(
  215.                   OrekitMessages.ANGLE_TYPE_NOT_SUPPORTED,
  216.                   "MEAN_LONGITUDE_ARGUMENT", "ECCENTRIC_LONGITUDE_ARGUMENT",
  217.                   "TRUE_LONGITUDE_ARGUMENT");
  218.         }

  220.     }

  222.     /** Constructor from cartesian parameters.
  223.      * @param pvCoordinates the {@link PVCoordinates} in inertial frame
  224.      * @param frame the frame in which are defined the {@link PVCoordinates}
  225.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  226.      * @param date date of the orbital parameters
  227.      * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  228.      * @exception IllegalArgumentException if frame is not a {@link
  229.      * Frame#isPseudoInertial pseudo-inertial frame}
  230.      */
  231.     public CircularOrbit(final PVCoordinates pvCoordinates, final Frame frame,
  232.                               final AbsoluteDate date, final double mu)
  233.         throws IllegalArgumentException {
  234.         super(pvCoordinates, frame, date, mu);

  236.         // compute semi-major axis
  237.         final Vector3D pvP = pvCoordinates.getPosition();
  238.         final Vector3D pvV = pvCoordinates.getVelocity();
  239.         final double r  = pvP.getNorm();
  240.         final double V2 = pvV.getNormSq();
  241.         final double rV2OnMu = r * V2 / mu;

  243.         if (rV2OnMu > 2) {
  244.             throw OrekitException.createIllegalArgumentException(
  245.                   OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName());
  246.         }

  248.         a = r / (2 - rV2OnMu);

  250.         // compute inclination
  251.         final Vector3D momentum = pvCoordinates.getMomentum();
  252.         i = Vector3D.angle(momentum, Vector3D.PLUS_K);

  254.         // compute right ascension of ascending node
  255.         final Vector3D node  = Vector3D.crossProduct(Vector3D.PLUS_K, momentum);
  256.         raan = FastMath.atan2(node.getY(), node.getX());

  258.         // 2D-coordinates in the canonical frame
  259.         final double cosRaan = FastMath.cos(raan);
  260.         final double sinRaan = FastMath.sin(raan);
  261.         final double cosI    = FastMath.cos(i);
  262.         final double sinI    = FastMath.sin(i);
  263.         final double xP      = pvP.getX();
  264.         final double yP      = pvP.getY();
  265.         final double zP      = pvP.getZ();
  266.         final double x2      = (xP * cosRaan + yP * sinRaan) / a;
  267.         final double y2      = ((yP * cosRaan - xP * sinRaan) * cosI + zP * sinI) / a;

  269.         // compute eccentricity vector
  270.         final double eSE    = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a);
  271.         final double eCE    = rV2OnMu - 1;
  272.         final double e2     = eCE * eCE + eSE * eSE;
  273.         final double f      = eCE - e2;
  274.         final double g      = FastMath.sqrt(1 - e2) * eSE;
  275.         final double aOnR   = a / r;
  276.         final double a2OnR2 = aOnR * aOnR;
  277.         ex = a2OnR2 * (f * x2 + g * y2);
  278.         ey = a2OnR2 * (f * y2 - g * x2);

  280.         // compute latitude argument
  281.         final double beta = 1 / (1 + FastMath.sqrt(1 - ex * ex - ey * ey));
  282.         alphaV = eccentricToTrue(FastMath.atan2(y2 + ey + eSE * beta * ex, x2 + ex - eSE * beta * ey));
  283.     }

  285.     /** Constructor from any kind of orbital parameters.
  286.      * @param op orbital parameters to copy
  287.      */
  288.     public CircularOrbit(final Orbit op) {
  289.         super(op.getFrame(), op.getDate(), op.getMu());
  290.         a    = op.getA();
  291.         i    = op.getI();
  292.         raan = FastMath.atan2(op.getHy(), op.getHx());
  293.         final double cosRaan = FastMath.cos(raan);
  294.         final double sinRaan = FastMath.sin(raan);
  295.         final double equiEx = op.getEquinoctialEx();
  296.         final double equiEy = op.getEquinoctialEy();
  297.         ex   = equiEx * cosRaan + equiEy * sinRaan;
  298.         ey   = equiEy * cosRaan - equiEx * sinRaan;
  299.         this.alphaV = op.getLv() - raan;
  300.     }

  302.     /** {@inheritDoc} */
  303.     public OrbitType getType() {
  304.         return OrbitType.CIRCULAR;
  305.     }

  307.     /** {@inheritDoc} */
  308.     public double getA() {
  309.         return a;
  310.     }

  312.     /** {@inheritDoc} */
  313.     public double getEquinoctialEx() {
  314.         return ex * FastMath.cos(raan) - ey * FastMath.sin(raan);
  315.     }

  317.     /** {@inheritDoc} */
  318.     public double getEquinoctialEy() {
  319.         return ey * FastMath.cos(raan) + ex * FastMath.sin(raan);
  320.     }

  322.     /** Get the first component of the circular eccentricity vector.
  323.      * @return ex = e cos(&omega;), first component of the circular eccentricity vector
  324.      */
  325.     public double getCircularEx() {
  326.         return ex;
  327.     }

  329.     /** Get the second component of the circular eccentricity vector.
  330.      * @return ey = e sin(&omega;), second component of the circular eccentricity vector
  331.      */
  332.     public double getCircularEy() {
  333.         return ey;
  334.     }

  336.     /** {@inheritDoc} */
  337.     public double getHx() {
  338.         return  FastMath.cos(raan) * FastMath.tan(i / 2);
  339.     }

  341.     /** {@inheritDoc} */
  342.     public double getHy() {
  343.         return  FastMath.sin(raan) * FastMath.tan(i / 2);
  344.     }

  346.     /** Get the true latitude argument.
  347.      * @return v + &omega; true latitude argument (rad)
  348.      */
  349.     public double getAlphaV() {
  350.         return alphaV;
  351.     }

  353.     /** Get the latitude argument.
  354.      * @param type type of the angle
  355.      * @return latitude argument (rad)
  356.      */
  357.     public double getAlpha(final PositionAngle type) {
  358.         return (type == PositionAngle.MEAN) ? getAlphaM() :
  359.                                               ((type == PositionAngle.ECCENTRIC) ? getAlphaE() :
  360.                                                                                    getAlphaV());
  361.     }

  363.     /** Get the eccentric latitude argument.
  364.      * @return E + &omega; eccentric latitude argument (rad)
  365.      */
  366.     public double getAlphaE() {
  367.         final double epsilon   = FastMath.sqrt(1 - ex * ex - ey * ey);
  368.         final double cosAlphaV = FastMath.cos(alphaV);
  369.         final double sinAlphaV = FastMath.sin(alphaV);
  370.         return alphaV + 2 * FastMath.atan((ey * cosAlphaV - ex * sinAlphaV) /
  371.                                       (epsilon + 1 + ex * cosAlphaV + ey * sinAlphaV));
  372.     }

  374.     /** Computes the true latitude argument from the eccentric latitude argument.
  375.      * @param alphaE = E + &omega; eccentric latitude argument (rad)
  376.      * @return the true latitude argument.
  377.      */
  378.     private double eccentricToTrue(final double alphaE) {
  379.         final double epsilon   = FastMath.sqrt(1 - ex * ex - ey * ey);
  380.         final double cosAlphaE = FastMath.cos(alphaE);
  381.         final double sinAlphaE = FastMath.sin(alphaE);
  382.         return alphaE + 2 * FastMath.atan((ex * sinAlphaE - ey * cosAlphaE) /
  383.                                       (epsilon + 1 - ex * cosAlphaE - ey * sinAlphaE));
  384.     }

  386.     /** Get the mean latitude argument.
  387.      * @return M + &omega; mean latitude argument (rad)
  388.      */
  389.     public double getAlphaM() {
  390.         final double alphaE = getAlphaE();
  391.         return alphaE - ex * FastMath.sin(alphaE) + ey * FastMath.cos(alphaE);
  392.     }

  394.     /** Computes the eccentric latitude argument from the mean latitude argument.
  395.      * @param alphaM = M + &omega;  mean latitude argument (rad)
  396.      * @return the eccentric latitude argument.
  397.      */
  398.     private double meanToEccentric(final double alphaM) {
  399.         // Generalization of Kepler equation to circular parameters
  400.         // with alphaE = PA + E and
  401.         //      alphaM = PA + M = alphaE - ex.sin(alphaE) + ey.cos(alphaE)
  402.         double alphaE        = alphaM;
  403.         double shift         = 0.0;
  404.         double alphaEMalphaM = 0.0;
  405.         double cosAlphaE     = FastMath.cos(alphaE);
  406.         double sinAlphaE     = FastMath.sin(alphaE);
  407.         int    iter          = 0;
  408.         do {
  409.             final double f2 = ex * sinAlphaE - ey * cosAlphaE;
  410.             final double f1 = 1.0 - ex * cosAlphaE - ey * sinAlphaE;
  411.             final double f0 = alphaEMalphaM - f2;

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

  416.             alphaEMalphaM -= shift;
  417.             alphaE         = alphaM + alphaEMalphaM;
  418.             cosAlphaE      = FastMath.cos(alphaE);
  419.             sinAlphaE      = FastMath.sin(alphaE);

  421.         } while ((++iter < 50) && (FastMath.abs(shift) > 1.0e-12));

  423.         return alphaE;

  425.     }

  427.     /** {@inheritDoc} */
  428.     public double getE() {
  429.         return FastMath.sqrt(ex * ex + ey * ey);
  430.     }

  432.     /** {@inheritDoc} */
  433.     public double getI() {
  434.         return i;
  435.     }

  437.     /** Get the right ascension of the ascending node.
  438.      * @return right ascension of the ascending node (rad)
  439.      */
  440.     public double getRightAscensionOfAscendingNode() {
  441.         return raan;
  442.     }

  444.     /** {@inheritDoc} */
  445.     public double getLv() {
  446.         return alphaV + raan;
  447.     }

  449.     /** {@inheritDoc} */
  450.     public double getLE() {
  451.         return getAlphaE() + raan;
  452.     }

  454.     /** {@inheritDoc} */
  455.     public double getLM() {
  456.         return getAlphaM() + raan;
  457.     }

  459.     /** {@inheritDoc} */
  460.     protected PVCoordinates initPVCoordinates() {

  462.         // get equinoctial parameters
  463.         final double equEx = getEquinoctialEx();
  464.         final double equEy = getEquinoctialEy();
  465.         final double hx = getHx();
  466.         final double hy = getHy();
  467.         final double lE = getLE();

  469.         // inclination-related intermediate parameters
  470.         final double hx2   = hx * hx;
  471.         final double hy2   = hy * hy;
  472.         final double factH = 1. / (1 + hx2 + hy2);

  474.         // reference axes defining the orbital plane
  475.         final double ux = (1 + hx2 - hy2) * factH;
  476.         final double uy =  2 * hx * hy * factH;
  477.         final double uz = -2 * hy * factH;

  479.         final double vx = uy;
  480.         final double vy = (1 - hx2 + hy2) * factH;
  481.         final double vz =  2 * hx * factH;

  483.         // eccentricity-related intermediate parameters
  484.         final double exey = equEx * equEy;
  485.         final double ex2  = equEx * equEx;
  486.         final double ey2  = equEy * equEy;
  487.         final double e2   = ex2 + ey2;
  488.         final double eta  = 1 + FastMath.sqrt(1 - e2);
  489.         final double beta = 1. / eta;

  491.         // eccentric latitude argument
  492.         final double cLe    = FastMath.cos(lE);
  493.         final double sLe    = FastMath.sin(lE);
  494.         final double exCeyS = equEx * cLe + equEy * sLe;

  496.         // coordinates of position and velocity in the orbital plane
  497.         final double x      = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - equEx);
  498.         final double y      = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - equEy);

  500.         final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
  501.         final double xdot   = factor * (-sLe + beta * equEy * exCeyS);
  502.         final double ydot   = factor * ( cLe - beta * equEx * exCeyS);

  504.         final Vector3D position =
  505.             new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
  506.         final Vector3D velocity =
  507.             new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz);
  508.         return new PVCoordinates(position, velocity);

  510.     }

  512.     /** {@inheritDoc} */
  513.     public CircularOrbit shiftedBy(final double dt) {
  514.         return new CircularOrbit(a, ex, ey, i, raan,
  515.                                  getAlphaM() + getKeplerianMeanMotion() * dt,
  516.                                  PositionAngle.MEAN, getFrame(),
  517.                                  getDate().shiftedBy(dt), getMu());
  518.     }

  520.     /** {@inheritDoc}
  521.      * <p>
  522.      * The interpolated instance is created by polynomial Hermite interpolation
  523.      * on circular elements, without derivatives (which means the interpolation
  524.      * falls back to Lagrange interpolation only).
  525.      * </p>
  526.      * <p>
  527.      * As this implementation of interpolation is polynomial, it should be used only
  528.      * with small samples (about 10-20 points) in order to avoid <a
  529.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  530.      * and numerical problems (including NaN appearing).
  531.      * </p>
  532.      * <p>
  533.      * If orbit interpolation on large samples is needed, using the {@link
  534.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  535.      * low-level interpolation. The Ephemeris class automatically handles selection of
  536.      * a neighboring sub-sample with a predefined number of point from a large global sample
  537.      * in a thread-safe way.
  538.      * </p>
  539.      */
  540.     public CircularOrbit interpolate(final AbsoluteDate date, final Collection<Orbit> sample) {

  542.         // set up an interpolator
  543.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  545.         // add sample points
  546.         AbsoluteDate previousDate = null;
  547.         double previousRAAN   = Double.NaN;
  548.         double previousAlphaM = Double.NaN;
  549.         for (final Orbit orbit : sample) {
  550.             final CircularOrbit circ = (CircularOrbit) OrbitType.CIRCULAR.convertType(orbit);
  551.             final double continuousRAAN;
  552.             final double continuousAlphaM;
  553.             if (previousDate == null) {
  554.                 continuousRAAN   = circ.getRightAscensionOfAscendingNode();
  555.                 continuousAlphaM = circ.getAlphaM();
  556.             } else {
  557.                 final double dt       = circ.getDate().durationFrom(previousDate);
  558.                 final double keplerAM = previousAlphaM + circ.getKeplerianMeanMotion() * dt;
  559.                 continuousRAAN   = MathUtils.normalizeAngle(circ.getRightAscensionOfAscendingNode(), previousRAAN);
  560.                 continuousAlphaM = MathUtils.normalizeAngle(circ.getAlphaM(), keplerAM);
  561.             }
  562.             previousDate   = circ.getDate();
  563.             previousRAAN   = continuousRAAN;
  564.             previousAlphaM = continuousAlphaM;
  565.             interpolator.addSamplePoint(circ.getDate().durationFrom(date),
  566.                                         new double[] {
  567.                                             circ.getA(),
  568.                                             circ.getCircularEx(),
  569.                                             circ.getCircularEy(),
  570.                                             circ.getI(),
  571.                                             continuousRAAN,
  572.                                             continuousAlphaM
  573.                                         });
  574.         }

  576.         // interpolate
  577.         final double[] interpolated = interpolator.value(0);

  579.         // build a new interpolated instance
  580.         return new CircularOrbit(interpolated[0], interpolated[1], interpolated[2],
  581.                                  interpolated[3], interpolated[4], interpolated[5],
  582.                                  PositionAngle.MEAN, getFrame(), date, getMu());

  584.     }

  586.     /** {@inheritDoc} */
  587.     protected double[][] computeJacobianMeanWrtCartesian() {

  589.         final double[][] jacobian = new double[6][6];

  591.         // compute various intermediate parameters
  592.         final PVCoordinates pvc = getPVCoordinates();
  593.         final Vector3D position = pvc.getPosition();
  594.         final Vector3D velocity = pvc.getVelocity();

  596.         final double x          = position.getX();
  597.         final double y          = position.getY();
  598.         final double z          = position.getZ();
  599.         final double vx         = velocity.getX();
  600.         final double vy         = velocity.getY();
  601.         final double vz         = velocity.getZ();
  602.         final double pv         = Vector3D.dotProduct(position, velocity);
  603.         final double r2         = position.getNormSq();
  604.         final double r          = FastMath.sqrt(r2);
  605.         final double v2         = velocity.getNormSq();

  607.         final double mu         = getMu();
  608.         final double oOsqrtMuA  = 1 / FastMath.sqrt(mu * a);
  609.         final double rOa        = r / a;
  610.         final double aOr        = a / r;
  611.         final double aOr2       = a / r2;
  612.         final double a2         = a * a;

  614.         final double ex2        = ex * ex;
  615.         final double ey2        = ey * ey;
  616.         final double e2         = ex2 + ey2;
  617.         final double epsilon    = FastMath.sqrt(1 - e2);
  618.         final double beta       = 1 / (1 + epsilon);

  620.         final double eCosE      = 1 - rOa;
  621.         final double eSinE      = pv * oOsqrtMuA;

  623.         final double cosI       = FastMath.cos(i);
  624.         final double sinI       = FastMath.sin(i);
  625.         final double cosRaan    = FastMath.cos(raan);
  626.         final double sinRaan    = FastMath.sin(raan);

  628.         // da
  629.         fillHalfRow(2 * aOr * aOr2, position, jacobian[0], 0);
  630.         fillHalfRow(2 * a2 / mu, velocity, jacobian[0], 3);

  632.         // differentials of the normalized momentum
  633.         final Vector3D danP = new Vector3D(v2, position, -pv, velocity);
  634.         final Vector3D danV = new Vector3D(r2, velocity, -pv, position);
  635.         final double recip  = 1 / pvc.getMomentum().getNorm();
  636.         final double recip2 = recip * recip;
  637.         final Vector3D dwXP = new Vector3D(recip, new Vector3D(  0,  vz, -vy), -recip2 * sinRaan * sinI, danP);
  638.         final Vector3D dwYP = new Vector3D(recip, new Vector3D(-vz,   0,  vx),  recip2 * cosRaan * sinI, danP);
  639.         final Vector3D dwZP = new Vector3D(recip, new Vector3D( vy, -vx,   0), -recip2 * cosI,           danP);
  640.         final Vector3D dwXV = new Vector3D(recip, new Vector3D(  0,  -z,   y), -recip2 * sinRaan * sinI, danV);
  641.         final Vector3D dwYV = new Vector3D(recip, new Vector3D(  z,   0,  -x),  recip2 * cosRaan * sinI, danV);
  642.         final Vector3D dwZV = new Vector3D(recip, new Vector3D( -y,   x,   0), -recip2 * cosI,           danV);

  644.         // di
  645.         fillHalfRow(sinRaan * cosI, dwXP, -cosRaan * cosI, dwYP, -sinI, dwZP, jacobian[3], 0);
  646.         fillHalfRow(sinRaan * cosI, dwXV, -cosRaan * cosI, dwYV, -sinI, dwZV, jacobian[3], 3);

  648.         // dRaan
  649.         fillHalfRow(sinRaan / sinI, dwYP, cosRaan / sinI, dwXP, jacobian[4], 0);
  650.         fillHalfRow(sinRaan / sinI, dwYV, cosRaan / sinI, dwXV, jacobian[4], 3);

  652.         // orbital frame: (p, q, w) p along ascending node, w along momentum
  653.         // the coordinates of the spacecraft in this frame are: (u, v, 0)
  654.         final double u     =  x * cosRaan + y * sinRaan;
  655.         final double cv    = -x * sinRaan + y * cosRaan;
  656.         final double v     = cv * cosI + z * sinI;

  658.         // du
  659.         final Vector3D duP = new Vector3D(cv * cosRaan / sinI, dwXP,
  660.                                           cv * sinRaan / sinI, dwYP,
  661.                                           1, new Vector3D(cosRaan, sinRaan, 0));
  662.         final Vector3D duV = new Vector3D(cv * cosRaan / sinI, dwXV,
  663.                                           cv * sinRaan / sinI, dwYV);

  665.         // dv
  666.         final Vector3D dvP = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXP,
  667.                                           -u * sinRaan * cosI / sinI - cosRaan * z, dwYP,
  668.                                           cv, dwZP,
  669.                                           1, new Vector3D(-sinRaan * cosI, cosRaan * cosI, sinI));
  670.         final Vector3D dvV = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXV,
  671.                                           -u * sinRaan * cosI / sinI - cosRaan * z, dwYV,
  672.                                           cv, dwZV);

  674.         final Vector3D dc1P = new Vector3D(aOr2 * (2 * eSinE * eSinE + 1 - eCosE) / r2, position,
  675.                                             -2 * aOr2 * eSinE * oOsqrtMuA, velocity);
  676.         final Vector3D dc1V = new Vector3D(-2 * aOr2 * eSinE * oOsqrtMuA, position,
  677.                                             2 / mu, velocity);
  678.         final Vector3D dc2P = new Vector3D(aOr2 * eSinE * (eSinE * eSinE - (1 - e2)) / (r2 * epsilon), position,
  679.                                             aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, velocity);
  680.         final Vector3D dc2V = new Vector3D(aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, position,
  681.                                             eSinE / (mu * epsilon), velocity);

  683.         final double cof1   = aOr2 * (eCosE - e2);
  684.         final double cof2   = aOr2 * epsilon * eSinE;
  685.         final Vector3D dexP = new Vector3D(u, dc1P,  v, dc2P, cof1, duP,  cof2, dvP);
  686.         final Vector3D dexV = new Vector3D(u, dc1V,  v, dc2V, cof1, duV,  cof2, dvV);
  687.         final Vector3D deyP = new Vector3D(v, dc1P, -u, dc2P, cof1, dvP, -cof2, duP);
  688.         final Vector3D deyV = new Vector3D(v, dc1V, -u, dc2V, cof1, dvV, -cof2, duV);
  689.         fillHalfRow(1, dexP, jacobian[1], 0);
  690.         fillHalfRow(1, dexV, jacobian[1], 3);
  691.         fillHalfRow(1, deyP, jacobian[2], 0);
  692.         fillHalfRow(1, deyV, jacobian[2], 3);

  694.         final double cle = u / a + ex - eSinE * beta * ey;
  695.         final double sle = v / a + ey + eSinE * beta * ex;
  696.         final double m1  = beta * eCosE;
  697.         final double m2  = 1 - m1 * eCosE;
  698.         final double m3  = (u * ey - v * ex) + eSinE * beta * (u * ex + v * ey);
  699.         final double m4  = -sle + cle * eSinE * beta;
  700.         final double m5  = cle + sle * eSinE * beta;
  701.         fillHalfRow((2 * m3 / r + aOr * eSinE + m1 * eSinE * (1 + m1 - (1 + aOr) * m2) / epsilon) / r2, position,
  702.                     (m1 * m2 / epsilon - 1) * oOsqrtMuA, velocity,
  703.                     m4, dexP, m5, deyP, -sle / a, duP, cle / a, dvP,
  704.                     jacobian[5], 0);
  705.         fillHalfRow((m1 * m2 / epsilon - 1) * oOsqrtMuA, position,
  706.                     (2 * m3 + eSinE * a + m1 * eSinE * r * (eCosE * beta * 2 - aOr * m2) / epsilon) / mu, velocity,
  707.                     m4, dexV, m5, deyV, -sle / a, duV, cle / a, dvV,
  708.                     jacobian[5], 3);

  710.         return jacobian;

  712.     }

  714.     /** {@inheritDoc} */
  715.     protected double[][] computeJacobianEccentricWrtCartesian() {

  717.         // start by computing the Jacobian with mean angle
  718.         final double[][] jacobian = computeJacobianMeanWrtCartesian();

  720.         // Differentiating the Kepler equation aM = aE - ex sin aE + ey cos aE leads to:
  721.         // daM = (1 - ex cos aE - ey sin aE) dE - sin aE dex + cos aE dey
  722.         // which is inverted and rewritten as:
  723.         // daE = a/r daM + sin aE a/r dex - cos aE a/r dey
  724.         final double alphaE = getAlphaE();
  725.         final double cosAe  = FastMath.cos(alphaE);
  726.         final double sinAe  = FastMath.sin(alphaE);
  727.         final double aOr    = 1 / (1 - ex * cosAe - ey * sinAe);

  729.         // update longitude row
  730.         final double[] rowEx = jacobian[1];
  731.         final double[] rowEy = jacobian[2];
  732.         final double[] rowL  = jacobian[5];
  733.         for (int j = 0; j < 6; ++j) {
  734.             rowL[j] = aOr * (rowL[j] + sinAe * rowEx[j] - cosAe * rowEy[j]);
  735.         }

  737.         return jacobian;

  739.     }

  741.     /** {@inheritDoc} */
  742.     protected double[][] computeJacobianTrueWrtCartesian() {

  744.         // start by computing the Jacobian with eccentric angle
  745.         final double[][] jacobian = computeJacobianEccentricWrtCartesian();

  747.         // Differentiating the eccentric latitude equation
  748.         // tan((aV - aE)/2) = [ex sin aE - ey cos aE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos aE - ey sin aE]
  749.         // leads to
  750.         // cT (daV - daE) = cE daE + cX dex + cY dey
  751.         // with
  752.         // cT = [d^2 + (ex sin aE - ey cos aE)^2] / 2
  753.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos aE - ey sin aE
  754.         // cE = (ex cos aE + ey sin aE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  755.         // cX =  sin aE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin aE - ey cos aE) / sqrt(1-ex^2-ey^2)
  756.         // cY = -cos aE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin aE - ey cos aE) / sqrt(1-ex^2-ey^2)
  757.         // which can be solved to find the differential of the true latitude
  758.         // daV = (cT + cE) / cT daE + cX / cT deX + cY / cT deX
  759.         final double alphaE    = getAlphaE();
  760.         final double cosAe     = FastMath.cos(alphaE);
  761.         final double sinAe     = FastMath.sin(alphaE);
  762.         final double eSinE     = ex * sinAe - ey * cosAe;
  763.         final double ecosE     = ex * cosAe + ey * sinAe;
  764.         final double e2        = ex * ex + ey * ey;
  765.         final double epsilon   = FastMath.sqrt(1 - e2);
  766.         final double onePeps   = 1 + epsilon;
  767.         final double d         = onePeps - ecosE;
  768.         final double cT        = (d * d + eSinE * eSinE) / 2;
  769.         final double cE        = ecosE * onePeps - e2;
  770.         final double cX        = ex * eSinE / epsilon - ey + sinAe * onePeps;
  771.         final double cY        = ey * eSinE / epsilon + ex - cosAe * onePeps;
  772.         final double factorLe  = (cT + cE) / cT;
  773.         final double factorEx  = cX / cT;
  774.         final double factorEy  = cY / cT;

  776.         // update latitude row
  777.         final double[] rowEx = jacobian[1];
  778.         final double[] rowEy = jacobian[2];
  779.         final double[] rowA = jacobian[5];
  780.         for (int j = 0; j < 6; ++j) {
  781.             rowA[j] = factorLe * rowA[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
  782.         }

  784.         return jacobian;

  786.     }

  788.     /** {@inheritDoc} */
  789.     public void addKeplerContribution(final PositionAngle type, final double gm,
  790.                                       final double[] pDot) {
  791.         final double oMe2;
  792.         final double ksi;
  793.         final double n = FastMath.sqrt(gm / a) / a;
  794.         switch (type) {
  795.         case MEAN :
  796.             pDot[5] += n;
  797.             break;
  798.         case ECCENTRIC :
  799.             oMe2  = 1 - ex * ex - ey * ey;
  800.             ksi   = 1 + ex * FastMath.cos(alphaV) + ey * FastMath.sin(alphaV);
  801.             pDot[5] += n * ksi / oMe2;
  802.             break;
  803.         case TRUE :
  804.             oMe2  = 1 - ex * ex - ey * ey;
  805.             ksi   = 1 + ex * FastMath.cos(alphaV) + ey * FastMath.sin(alphaV);
  806.             pDot[5] += n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
  807.             break;
  808.         default :
  809.             throw OrekitException.createInternalError(null);
  810.         }
  811.     }

  813.     /**  Returns a string representation of this Orbit object.
  814.      * @return a string representation of this object
  815.      */
  816.     public String toString() {
  817.         return new StringBuffer().append("circular parameters: ").append('{').
  818.                                   append("a: ").append(a).
  819.                                   append(", ex: ").append(ex).append(", ey: ").append(ey).
  820.                                   append(", i: ").append(FastMath.toDegrees(i)).
  821.                                   append(", raan: ").append(FastMath.toDegrees(raan)).
  822.                                   append(", alphaV: ").append(FastMath.toDegrees(alphaV)).
  823.                                   append(";}").toString();
  824.     }

  826. }
Created with JaCoCo 0.6.3.201306030806