EquinoctialOrbit.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 equinoctial orbital parameters, which can support both
  34.  * circular and equatorial orbits.
  35.  * <p>
  36.  * The parameters used internally are the equinoctial elements which can be
  37.  * related to keplerian elements as follows:
  38.  *   <pre>
  39.  *     a
  40.  *     ex = e cos(&omega; + &Omega;)
  41.  *     ey = e sin(&omega; + &Omega;)
  42.  *     hx = tan(i/2) cos(&Omega;)
  43.  *     hy = tan(i/2) sin(&Omega;)
  44.  *     lv = v + &omega; + &Omega;
  45.  *   </pre>
  46.  * where &omega; stands for the Perigee Argument and &Omega; stands for the
  47.  * Right Ascension of the Ascending Node.
  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 either equatorial or circular, the equinoctial
  53.  * parameters are still unambiguously defined whereas some keplerian elements
  54.  * (more precisely &omega; and &Omega;) become ambiguous. For this reason, equinoctial
  55.  * parameters are the recommended way to represent orbits.
  56.  * </p>
  57.  * <p>
  58.  * The instance <code>EquinoctialOrbit</code> is guaranteed to be immutable.
  59.  * </p>
  60.  * @see    Orbit
  61.  * @see    KeplerianOrbit
  62.  * @see    CircularOrbit
  63.  * @see    CartesianOrbit
  64.  * @author Mathieu Rom&eacute;ro
  65.  * @author Luc Maisonobe
  66.  * @author Guylaine Prat
  67.  * @author Fabien Maussion
  68.  * @author V&eacute;ronique Pommier-Maurussane
  69.  */
  70. public class EquinoctialOrbit extends Orbit {

  72.     /** Identifier for mean longitude argument.
  73.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  74.      */
  75.     @Deprecated
  76.     public static final int MEAN_LATITUDE_ARGUMENT = 0;

  78.     /** Identifier for eccentric longitude argument.
  79.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  80.      */
  81.     @Deprecated
  82.     public static final int ECCENTRIC_LATITUDE_ARGUMENT = 1;

  84.     /** Identifier for true longitude argument.
  85.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  86.      */
  87.     @Deprecated
  88.     public static final int TRUE_LATITUDE_ARGUMENT = 2;

  90.     /** Serializable UID. */
  91.     private static final long serialVersionUID = -2000712440570076839L;

  93.     /** Semi-major axis (m). */
  94.     private final double a;

  96.     /** First component of the eccentricity vector. */
  97.     private final double ex;

  99.     /** Second component of the eccentricity vector. */
  100.     private final double ey;

  102.     /** First component of the inclination vector. */
  103.     private final double hx;

  105.     /** Second component of the inclination vector. */
  106.     private final double hy;

  108.     /** True longitude argument (rad). */
  109.     private final double lv;

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

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

  157.     }

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

  199.         switch (type) {
  200.         case MEAN_LATITUDE_ARGUMENT :
  201.             this.lv = eccentricToTrue(meanToEccentric(l));
  202.             break;
  203.         case ECCENTRIC_LATITUDE_ARGUMENT :
  204.             this.lv = eccentricToTrue(l);
  205.             break;
  206.         case TRUE_LATITUDE_ARGUMENT :
  207.             this.lv = l;
  208.             break;
  209.         default :
  210.             this.lv = Double.NaN;
  211.             throw OrekitException.createIllegalArgumentException(
  212.                   OrekitMessages.ANGLE_TYPE_NOT_SUPPORTED,
  213.                   "MEAN_LATITUDE_ARGUMENT", "ECCENTRIC_LATITUDE_ARGUMENT",
  214.                   "TRUE_LATITUDE_ARGUMENT");
  215.         }

  217.     }

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

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

  240.         if (rV2OnMu > 2) {
  241.             throw OrekitException.createIllegalArgumentException(
  242.                   OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName());
  243.         }

  245.         // compute inclination vector
  246.         final Vector3D w = pvCoordinates.getMomentum().normalize();
  247.         final double d = 1.0 / (1 + w.getZ());
  248.         hx = -d * w.getY();
  249.         hy =  d * w.getX();

  251.         // compute true longitude argument
  252.         final double cLv = (pvP.getX() - d * pvP.getZ() * w.getX()) / r;
  253.         final double sLv = (pvP.getY() - d * pvP.getZ() * w.getY()) / r;
  254.         lv = FastMath.atan2(sLv, cLv);


  257.         // compute semi-major axis
  258.         a = r / (2 - rV2OnMu);

  260.         // compute eccentricity vector
  261.         final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a);
  262.         final double eCE = rV2OnMu - 1;
  263.         final double e2  = eCE * eCE + eSE * eSE;
  264.         final double f   = eCE - e2;
  265.         final double g   = FastMath.sqrt(1 - e2) * eSE;
  266.         ex = a * (f * cLv + g * sLv) / r;
  267.         ey = a * (f * sLv - g * cLv) / r;

  269.     }

  271.     /** Constructor from any kind of orbital parameters.
  272.      * @param op orbital parameters to copy
  273.      */
  274.     public EquinoctialOrbit(final Orbit op) {
  275.         super(op.getFrame(), op.getDate(), op.getMu());
  276.         a  = op.getA();
  277.         ex = op.getEquinoctialEx();
  278.         ey = op.getEquinoctialEy();
  279.         hx = op.getHx();
  280.         hy = op.getHy();
  281.         lv = op.getLv();
  282.     }

  284.     /** {@inheritDoc} */
  285.     public OrbitType getType() {
  286.         return OrbitType.EQUINOCTIAL;
  287.     }

  289.     /** {@inheritDoc} */
  290.     public double getA() {
  291.         return a;
  292.     }

  294.     /** {@inheritDoc} */
  295.     public double getEquinoctialEx() {
  296.         return ex;
  297.     }

  299.     /** {@inheritDoc} */
  300.     public double getEquinoctialEy() {
  301.         return ey;
  302.     }

  304.     /** {@inheritDoc} */
  305.     public double getHx() {
  306.         return hx;
  307.     }

  309.     /** {@inheritDoc} */
  310.     public double getHy() {
  311.         return hy;
  312.     }

  314.     /** Get the longitude argument.
  315.      * @param type type of the angle
  316.      * @return longitude argument (rad)
  317.      */
  318.     public double getL(final PositionAngle type) {
  319.         return (type == PositionAngle.MEAN) ? getLM() :
  320.                                               ((type == PositionAngle.ECCENTRIC) ? getLE() :
  321.                                                                                    getLv());
  322.     }

  324.     /** {@inheritDoc} */
  325.     public double getLv() {
  326.         return lv;
  327.     }

  329.     /** {@inheritDoc} */
  330.     public double getLE() {
  331.         final double epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
  332.         final double cosLv   = FastMath.cos(lv);
  333.         final double sinLv   = FastMath.sin(lv);
  334.         final double num     = ey * cosLv - ex * sinLv;
  335.         final double den     = epsilon + 1 + ex * cosLv + ey * sinLv;
  336.         return lv + 2 * FastMath.atan(num / den);
  337.     }

  339.     /** Computes the true longitude argument from the eccentric longitude argument.
  340.      * @param lE = E + &omega; + &Omega; eccentric longitude argument (rad)
  341.      * @return the true longitude argument
  342.      */
  343.     private double eccentricToTrue(final double lE) {
  344.         final double epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
  345.         final double cosLE   = FastMath.cos(lE);
  346.         final double sinLE   = FastMath.sin(lE);
  347.         final double num     = ex * sinLE - ey * cosLE;
  348.         final double den     = epsilon + 1 - ex * cosLE - ey * sinLE;
  349.         return lE + 2 * FastMath.atan(num / den);
  350.     }

  352.     /** {@inheritDoc} */
  353.     public double getLM() {
  354.         final double lE = getLE();
  355.         return lE - ex * FastMath.sin(lE) + ey * FastMath.cos(lE);
  356.     }

  358.     /** Computes the eccentric longitude argument from the mean longitude argument.
  359.      * @param lM = M + &omega; + &Omega; mean longitude argument (rad)
  360.      * @return the eccentric longitude argument
  361.      */
  362.     private double meanToEccentric(final double lM) {
  363.         // Generalization of Kepler equation to equinoctial parameters
  364.         // with lE = PA + RAAN + E and
  365.         //      lM = PA + RAAN + M = lE - ex.sin(lE) + ey.cos(lE)
  366.         double lE = lM;
  367.         double shift = 0.0;
  368.         double lEmlM = 0.0;
  369.         double cosLE = FastMath.cos(lE);
  370.         double sinLE = FastMath.sin(lE);
  371.         int iter = 0;
  372.         do {
  373.             final double f2 = ex * sinLE - ey * cosLE;
  374.             final double f1 = 1.0 - ex * cosLE - ey * sinLE;
  375.             final double f0 = lEmlM - f2;

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

  380.             lEmlM -= shift;
  381.             lE     = lM + lEmlM;
  382.             cosLE  = FastMath.cos(lE);
  383.             sinLE  = FastMath.sin(lE);

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

  387.         return lE;

  389.     }

  391.     /** {@inheritDoc} */
  392.     public double getE() {
  393.         return FastMath.sqrt(ex * ex + ey * ey);
  394.     }

  396.     /** {@inheritDoc} */
  397.     public double getI() {
  398.         return 2 * FastMath.atan(FastMath.sqrt(hx * hx + hy * hy));
  399.     }

  401.     /** {@inheritDoc} */
  402.     protected PVCoordinates initPVCoordinates() {

  404.         // get equinoctial parameters
  405.         final double lE = getLE();

  407.         // inclination-related intermediate parameters
  408.         final double hx2   = hx * hx;
  409.         final double hy2   = hy * hy;
  410.         final double factH = 1. / (1 + hx2 + hy2);

  412.         // reference axes defining the orbital plane
  413.         final double ux = (1 + hx2 - hy2) * factH;
  414.         final double uy =  2 * hx * hy * factH;
  415.         final double uz = -2 * hy * factH;

  417.         final double vx = uy;
  418.         final double vy = (1 - hx2 + hy2) * factH;
  419.         final double vz =  2 * hx * factH;

  421.         // eccentricity-related intermediate parameters
  422.         final double exey = ex * ey;
  423.         final double ex2  = ex * ex;
  424.         final double ey2  = ey * ey;
  425.         final double e2   = ex2 + ey2;
  426.         final double eta  = 1 + FastMath.sqrt(1 - e2);
  427.         final double beta = 1. / eta;

  429.         // eccentric longitude argument
  430.         final double cLe    = FastMath.cos(lE);
  431.         final double sLe    = FastMath.sin(lE);
  432.         final double exCeyS = ex * cLe + ey * sLe;

  434.         // coordinates of position and velocity in the orbital plane
  435.         final double x      = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
  436.         final double y      = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);

  438.         final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
  439.         final double xdot   = factor * (-sLe + beta * ey * exCeyS);
  440.         final double ydot   = factor * ( cLe - beta * ex * exCeyS);

  442.         final Vector3D position =
  443.             new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
  444.         final Vector3D velocity =
  445.             new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz);
  446.         return new PVCoordinates(position, velocity);

  448.     }

  450.     /** {@inheritDoc} */
  451.     public EquinoctialOrbit shiftedBy(final double dt) {
  452.         return new EquinoctialOrbit(a, ex, ey, hx, hy,
  453.                                     getLM() + getKeplerianMeanMotion() * dt,
  454.                                     PositionAngle.MEAN, getFrame(),
  455.                                     getDate().shiftedBy(dt), getMu());
  456.     }

  458.     /** {@inheritDoc}
  459.      * <p>
  460.      * The interpolated instance is created by polynomial Hermite interpolation
  461.      * on equinoctial elements, without derivatives (which means the interpolation
  462.      * falls back to Lagrange interpolation only).
  463.      * </p>
  464.      * <p>
  465.      * As this implementation of interpolation is polynomial, it should be used only
  466.      * with small samples (about 10-20 points) in order to avoid <a
  467.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  468.      * and numerical problems (including NaN appearing).
  469.      * </p>
  470.      * <p>
  471.      * If orbit interpolation on large samples is needed, using the {@link
  472.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  473.      * low-level interpolation. The Ephemeris class automatically handles selection of
  474.      * a neighboring sub-sample with a predefined number of point from a large global sample
  475.      * in a thread-safe way.
  476.      * </p>
  477.      */
  478.     public EquinoctialOrbit interpolate(final AbsoluteDate date, final Collection<Orbit> sample) {

  480.         // set up an interpolator
  481.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  483.         // add sample points
  484.         AbsoluteDate previousDate = null;
  485.         double previousLm = Double.NaN;
  486.         for (final Orbit orbit : sample) {
  487.             final EquinoctialOrbit equi = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit);
  488.             final double continuousLm;
  489.             if (previousDate == null) {
  490.                 continuousLm = equi.getLM();
  491.             } else {
  492.                 final double dt       = equi.getDate().durationFrom(previousDate);
  493.                 final double keplerLm = previousLm + equi.getKeplerianMeanMotion() * dt;
  494.                 continuousLm = MathUtils.normalizeAngle(equi.getLM(), keplerLm);
  495.             }
  496.             previousDate = equi.getDate();
  497.             previousLm   = continuousLm;
  498.             interpolator.addSamplePoint(equi.getDate().durationFrom(date),
  499.                                         new double[] {
  500.                                             equi.getA(),
  501.                                             equi.getEquinoctialEx(),
  502.                                             equi.getEquinoctialEy(),
  503.                                             equi.getHx(),
  504.                                             equi.getHy(),
  505.                                             continuousLm
  506.                                         });
  507.         }

  509.         // interpolate
  510.         final double[] interpolated = interpolator.value(0);

  512.         // build a new interpolated instance
  513.         return new EquinoctialOrbit(interpolated[0], interpolated[1], interpolated[2],
  514.                                     interpolated[3], interpolated[4], interpolated[5],
  515.                                     PositionAngle.MEAN, getFrame(), date, getMu());

  517.     }

  519.     /** {@inheritDoc} */
  520.     protected double[][] computeJacobianMeanWrtCartesian() {

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

  524.         // compute various intermediate parameters
  525.         final Vector3D position = getPVCoordinates().getPosition();
  526.         final Vector3D velocity = getPVCoordinates().getVelocity();
  527.         final double r2         = position.getNormSq();
  528.         final double r          = FastMath.sqrt(r2);
  529.         final double r3         = r * r2;

  531.         final double mu         = getMu();
  532.         final double sqrtMuA    = FastMath.sqrt(a * mu);
  533.         final double a2         = a * a;

  535.         final double e2         = ex * ex + ey * ey;
  536.         final double oMe2       = 1 - e2;
  537.         final double epsilon    = FastMath.sqrt(oMe2);
  538.         final double beta       = 1 / (1 + epsilon);
  539.         final double ratio      = epsilon * beta;

  541.         final double hx2        = hx * hx;
  542.         final double hy2        = hy * hy;
  543.         final double hxhy       = hx * hy;

  545.         // precomputing equinoctial frame unit vectors (f,g,w)
  546.         final Vector3D f  = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
  547.         final Vector3D g  = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
  548.         final Vector3D w  = Vector3D.crossProduct(position, velocity).normalize();

  550.         // coordinates of the spacecraft in the equinoctial frame
  551.         final double x    = Vector3D.dotProduct(position, f);
  552.         final double y    = Vector3D.dotProduct(position, g);
  553.         final double xDot = Vector3D.dotProduct(velocity, f);
  554.         final double yDot = Vector3D.dotProduct(velocity, g);

  556.         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
  557.         final double c1 = a / (sqrtMuA * epsilon);
  558.         final double c2 = a * sqrtMuA * beta / r3;
  559.         final double c3 = sqrtMuA / (r3 * epsilon);
  560.         final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
  561.                                                 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);

  563.         // drDot / dEy = dXDot / dEy * f + dYDot / dEy * g
  564.         final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
  565.                                                 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);

  567.         // da
  568.         final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
  569.         final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
  570.         fillHalfRow(1, vectorAR,    jacobian[0], 0);
  571.         fillHalfRow(1, vectorARDot, jacobian[0], 3);

  573.         // dEx
  574.         final double d1 = -a * ratio / r3;
  575.         final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
  576.         final double d3 = (hx * y - hy * x) / sqrtMuA;
  577.         final Vector3D vectorExRDot =
  578.             new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
  579.         fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
  580.         fillHalfRow(1, vectorExRDot, jacobian[1], 3);

  582.         // dEy
  583.         final Vector3D vectorEyRDot =
  584.             new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
  585.         fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
  586.         fillHalfRow(1, vectorEyRDot, jacobian[2], 3);

  588.         // dHx
  589.         final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
  590.         fillHalfRow(-h * xDot, w, jacobian[3], 0);
  591.         fillHalfRow( h * x,    w, jacobian[3], 3);

  593.        // dHy
  594.         fillHalfRow(-h * yDot, w, jacobian[4], 0);
  595.         fillHalfRow( h * y,    w, jacobian[4], 3);

  597.         // dLambdaM
  598.         final double l = -ratio / sqrtMuA;
  599.         fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
  600.         fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);

  602.         return jacobian;

  604.     }

  606.     /** {@inheritDoc} */
  607.     protected double[][] computeJacobianEccentricWrtCartesian() {

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

  612.         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
  613.         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
  614.         // which is inverted and rewritten as:
  615.         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
  616.         final double le    = getLE();
  617.         final double cosLe = FastMath.cos(le);
  618.         final double sinLe = FastMath.sin(le);
  619.         final double aOr   = 1 / (1 - ex * cosLe - ey * sinLe);

  621.         // update longitude row
  622.         final double[] rowEx = jacobian[1];
  623.         final double[] rowEy = jacobian[2];
  624.         final double[] rowL  = jacobian[5];
  625.         for (int j = 0; j < 6; ++j) {
  626.             rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
  627.         }

  629.         return jacobian;

  631.     }

  633.     /** {@inheritDoc} */
  634.     protected double[][] computeJacobianTrueWrtCartesian() {

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

  639.         // Differentiating the eccentric longitude equation
  640.         // tan((lV - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
  641.         // leads to
  642.         // cT (dlV - dlE) = cE dlE + cX dex + cY dey
  643.         // with
  644.         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
  645.         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
  646.         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
  647.         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  648.         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
  649.         // which can be solved to find the differential of the true longitude
  650.         // dlV = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
  651.         final double le        = getLE();
  652.         final double cosLe     = FastMath.cos(le);
  653.         final double sinLe     = FastMath.sin(le);
  654.         final double eSinE     = ex * sinLe - ey * cosLe;
  655.         final double ecosE     = ex * cosLe + ey * sinLe;
  656.         final double e2        = ex * ex + ey * ey;
  657.         final double epsilon   = FastMath.sqrt(1 - e2);
  658.         final double onePeps   = 1 + epsilon;
  659.         final double d         = onePeps - ecosE;
  660.         final double cT        = (d * d + eSinE * eSinE) / 2;
  661.         final double cE        = ecosE * onePeps - e2;
  662.         final double cX        = ex * eSinE / epsilon - ey + sinLe * onePeps;
  663.         final double cY        = ey * eSinE / epsilon + ex - cosLe * onePeps;
  664.         final double factorLe  = (cT + cE) / cT;
  665.         final double factorEx  = cX / cT;
  666.         final double factorEy  = cY / cT;

  668.         // update longitude row
  669.         final double[] rowEx = jacobian[1];
  670.         final double[] rowEy = jacobian[2];
  671.         final double[] rowL = jacobian[5];
  672.         for (int j = 0; j < 6; ++j) {
  673.             rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
  674.         }

  676.         return jacobian;

  678.     }

  680.     /** {@inheritDoc} */
  681.     public void addKeplerContribution(final PositionAngle type, final double gm,
  682.                                       final double[] pDot) {
  683.         final double oMe2;
  684.         final double ksi;
  685.         final double n = FastMath.sqrt(gm / a) / a;
  686.         switch (type) {
  687.         case MEAN :
  688.             pDot[5] += n;
  689.             break;
  690.         case ECCENTRIC :
  691.             oMe2 = 1 - ex * ex - ey * ey;
  692.             ksi  = 1 + ex * FastMath.cos(lv) + ey * FastMath.sin(lv);
  693.             pDot[5] += n * ksi / oMe2;
  694.             break;
  695.         case TRUE :
  696.             oMe2 = 1 - ex * ex - ey * ey;
  697.             ksi  = 1 + ex * FastMath.cos(lv) + ey * FastMath.sin(lv);
  698.             pDot[5] += n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
  699.             break;
  700.         default :
  701.             throw OrekitException.createInternalError(null);
  702.         }
  703.     }

  705.     /**  Returns a string representation of this equinoctial parameters object.
  706.      * @return a string representation of this object
  707.      */
  708.     public String toString() {
  709.         return new StringBuffer().append("equinoctial parameters: ").append('{').
  710.                                   append("a: ").append(a).
  711.                                   append("; ex: ").append(ex).append("; ey: ").append(ey).
  712.                                   append("; hx: ").append(hx).append("; hy: ").append(hy).
  713.                                   append("; lv: ").append(FastMath.toDegrees(lv)).
  714.                                   append(";}").toString();
  715.     }

  717. }
Created with JaCoCo 0.6.3.201306030806