CartesianOrbit.java

/* Copyright 2002-2013 CS Systèmes d'Information
 * Licensed to CS Systèmes d'Information (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.orbits;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

import org.apache.commons.math3.exception.ConvergenceException;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
import org.apache.commons.math3.util.Pair;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinates;


/** This class holds cartesian orbital parameters.

 * <p>
 * The parameters used internally are the cartesian coordinates:
 *   <ul>
 *     <li>x</li>
 *     <li>y</li>
 *     <li>z</li>
 *     <li>xDot</li>
 *     <li>yDot</li>
 *     <li>zDot</li>
 *   </ul>
 * contained in {@link PVCoordinates}.
 * </p>

 * <p>
 * Note that the implementation of this class delegates all non-cartesian related
 * computations ({@link #getA()}, {@link #getEquinoctialEx()}, ...) to an underlying
 * instance of the {@link EquinoctialOrbit} class. This implies that using this class
 * only for analytical computations which are always based on non-cartesian
 * parameters is perfectly possible but somewhat sub-optimal.
 * </p>
 * <p>
 * The instance <code>CartesianOrbit</code> is guaranteed to be immutable.
 * </p>
 * @see    Orbit
 * @see    KeplerianOrbit
 * @see    CircularOrbit
 * @see    EquinoctialOrbit
 * @author Luc Maisonobe
 * @author Guylaine Prat
 * @author Fabien Maussion
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class CartesianOrbit extends Orbit {

    /** Serializable UID. */
    private static final long serialVersionUID = -5411308212620896302L;

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

    /** Constructor from cartesian parameters.
     * @param pvCoordinates the position and velocity of the satellite.
     * @param frame the frame in which the {@link PVCoordinates} are defined
     * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
     * @param date date of the orbital parameters
     * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
     * @exception IllegalArgumentException if frame is not a {@link
     * Frame#isPseudoInertial pseudo-inertial frame}
     */
    public CartesianOrbit(final PVCoordinates pvCoordinates, final Frame frame,
                          final AbsoluteDate date, final double mu)
        throws IllegalArgumentException {
        super(pvCoordinates, frame, date, mu);
        equinoctial = null;
    }

    /** Constructor from any kind of orbital parameters.
     * @param op orbital parameters to copy
     */
    public CartesianOrbit(final Orbit op) {
        super(op.getPVCoordinates(), op.getFrame(), op.getDate(), op.getMu());
        if (op instanceof EquinoctialOrbit) {
            equinoctial = (EquinoctialOrbit) op;
        } else if (op instanceof CartesianOrbit) {
            equinoctial = ((CartesianOrbit) op).equinoctial;
        } else {
            equinoctial = null;
        }
    }

    /** {@inheritDoc} */
    public OrbitType getType() {
        return OrbitType.CARTESIAN;
    }

    /** Lazy evaluation of equinoctial parameters. */
    private void initEquinoctial() {
        if (equinoctial == null) {
            equinoctial = new EquinoctialOrbit(getPVCoordinates(), getFrame(), getDate(), getMu());
        }
    }

    /** {@inheritDoc} */
    public double getA() {
        // lazy evaluation of semi-major axis
        final double r  = getPVCoordinates().getPosition().getNorm();
        final double V2 = getPVCoordinates().getVelocity().getNormSq();
        return r / (2 - r * V2 / getMu());
    }

    /** {@inheritDoc} */
    public double getE() {
        final Vector3D pvP   = getPVCoordinates().getPosition();
        final Vector3D pvV   = getPVCoordinates().getVelocity();
        final double rV2OnMu = pvP.getNorm() * pvV.getNormSq() / getMu();
        final double eSE     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * getA());
        final double eCE     = rV2OnMu - 1;
        return FastMath.sqrt(eCE * eCE + eSE * eSE);
    }

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

    /** {@inheritDoc} */
    public double getEquinoctialEx() {
        initEquinoctial();
        return equinoctial.getEquinoctialEx();
    }

    /** {@inheritDoc} */
    public double getEquinoctialEy() {
        initEquinoctial();
        return equinoctial.getEquinoctialEy();
    }

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

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

    /** {@inheritDoc} */
    public double getLv() {
        initEquinoctial();
        return equinoctial.getLv();
    }

    /** {@inheritDoc} */
    public double getLE() {
        initEquinoctial();
        return equinoctial.getLE();
    }

    /** {@inheritDoc} */
    public double getLM() {
        initEquinoctial();
        return equinoctial.getLM();
    }

    /** {@inheritDoc} */
    protected PVCoordinates initPVCoordinates() {
        // nothing to do here, as the canonical elements are already the cartesian ones
        return getPVCoordinates();
    }

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

    /** {@inheritDoc}
     * <p>
     * The interpolated instance is created by polynomial Hermite interpolation
     * ensuring velocity remains the exact derivative of position.
     * </p>
     * <p>
     * As this implementation of interpolation is polynomial, it should be used only
     * with small samples (about 10-20 points) in order to avoid <a
     * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
     * and numerical problems (including NaN appearing).
     * </p>
     * <p>
     * If orbit interpolation on large samples is needed, using the {@link
     * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
     * low-level interpolation. The Ephemeris class automatically handles selection of
     * a neighboring sub-sample with a predefined number of point from a large global sample
     * in a thread-safe way.
     * </p>
     */
    public CartesianOrbit interpolate(final AbsoluteDate date, final Collection<Orbit> sample) {
        final List<Pair<AbsoluteDate, PVCoordinates>> datedPV =
                new ArrayList<Pair<AbsoluteDate, PVCoordinates>>(sample.size());
        for (final Orbit orbit : sample) {
            datedPV.add(new Pair<AbsoluteDate, PVCoordinates>(orbit.getDate(), orbit.getPVCoordinates()));
        }
        final PVCoordinates interpolated = PVCoordinates.interpolate(date, true, datedPV);
        return new CartesianOrbit(interpolated, getFrame(), date, getMu());
    }

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

        // preliminary computation
        final Vector3D pvP   = getPVCoordinates().getPosition();
        final Vector3D pvV   = getPVCoordinates().getVelocity();
        final double r       = pvP.getNorm();
        final double rV2OnMu = r * pvV.getNormSq() / getMu();
        final double a       = getA();
        final double eSE     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * a);
        final double eCE     = rV2OnMu - 1;
        final double e2      = eCE * eCE + eSE * eSE;

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

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

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

        // compute shifted in-plane cartesian coordinates
        final double exey   = ex * ey;
        final double exCeyS = ex * cTE + ey * sTE;
        final double x      = a * ((1 - beta * ey * ey) * cTE + beta * exey * sTE - ex);
        final double y      = a * ((1 - beta * ex * ex) * sTE + beta * exey * cTE - ey);
        final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
        final double xDot   = factor * (-sTE + beta * ey * exCeyS);
        final double yDot   = factor * ( cTE - beta * ex * exCeyS);

        return new PVCoordinates(new Vector3D(x, u, y, v), new Vector3D(xDot, u, yDot, v));

    }

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

        final PVCoordinates pv = getPVCoordinates();
        final Vector3D pvP   = pv.getPosition();
        final Vector3D pvV   = pv.getVelocity();
        final Vector3D pvM   = pv.getMomentum();
        final double r       = pvP.getNorm();
        final double rV2OnMu = r * pvV.getNormSq() / getMu();
        final double a       = getA();
        final double muA     = getMu() * a;
        final double e       = FastMath.sqrt(1 - Vector3D.dotProduct(pvM, pvM) / muA);
        final double sqrt    = FastMath.sqrt((e + 1) / (e - 1));

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

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

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

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

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

        return new PVCoordinates(new Vector3D(x, p, y, q), new Vector3D(xDot, p, yDot, q));

    }

    /** Computes the eccentric in-plane argument from the mean in-plane argument.
     * @param thetaM = mean in-plane argument (rad)
     * @param ex first component of eccentricity vector
     * @param ey second component of eccentricity vector
     * @return the eccentric in-plane argument.
     */
    private double meanToEccentric(final double thetaM, final double ex, final double ey) {
        // Generalization of Kepler equation to in-plane parameters
        // with thetaE = eta + E and
        //      thetaM = eta + M = thetaE - ex.sin(thetaE) + ey.cos(thetaE)
        // and eta being counted from an arbitrary reference in the orbital plane
        double thetaE        = thetaM;
        double thetaEMthetaM = 0.0;
        int    iter          = 0;
        do {
            final double cosThetaE = FastMath.cos(thetaE);
            final double sinThetaE = FastMath.sin(thetaE);

            final double f2 = ex * sinThetaE - ey * cosThetaE;
            final double f1 = 1.0 - ex * cosThetaE - ey * sinThetaE;
            final double f0 = thetaEMthetaM - f2;

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

            thetaEMthetaM -= shift;
            thetaE         = thetaM + thetaEMthetaM;

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

        } while (++iter < 50);

        throw new ConvergenceException();

    }

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

        // Resolution of hyperbolic Kepler equation for keplerian parameters

        // Initial guess
        double H;
        if (ecc < 1.6) {
            if ((-FastMath.PI < M && M < 0.) || M > FastMath.PI) {
                H = M - ecc;
            } else {
                H = M + ecc;
            }
        } else {
            if (ecc < 3.6 && FastMath.abs(M) > FastMath.PI) {
                H = M - FastMath.copySign(ecc, M);
            } else {
                H = M / (ecc - 1.);
            }
        }

        // Iterative computation
        int iter = 0;
        do {
            final double f3  = ecc * FastMath.cosh(H);
            final double f2  = ecc * FastMath.sinh(H);
            final double f1  = f3 - 1.;
            final double f0  = f2 - H - M;
            final double f12 = 2. * f1;
            final double d   = f0 / f12;
            final double fdf = f1 - d * f2;
            final double ds  = f0 / fdf;

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

            H -= shift;

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

        } while (++iter < 50);

        throw new ConvergenceException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
                                       iter);
    }

    @Override
    public void getJacobianWrtCartesian(final PositionAngle type, final double[][] jacobian) {
        // this is the fastest way to set the 6x6 identity matrix
        for (int i = 0; i < 6; i++) {
            for (int j = 0; j < 6; j++) {
                jacobian[i][j] = 0;
            }
            jacobian[i][i] = 1;
        }
    }

    @Override
    protected double[][] computeJacobianMeanWrtCartesian() {
        // not used
        return null;
    }
    @Override
    protected double[][] computeJacobianEccentricWrtCartesian() {
        // not used
        return null;
    }

    @Override
    protected double[][] computeJacobianTrueWrtCartesian() {
        // not used
        return null;
    }

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

        final PVCoordinates pv = getPVCoordinates();

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

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

    }

    /**  Returns a string representation of this Orbit object.
     * @return a string representation of this object
     */
    public String toString() {
        return "cartesian parameters: " + getPVCoordinates().toString();
    }

    /** Replace the instance with a data transfer object for serialization.
     * <p>
     * This intermediate class serializes all needed parameters,
     * including position-velocity which are <em>not</em> serialized by parent
     * {@link Orbit} class.
     * </p>
     * @return data transfer object that will be serialized
     */
    private Object writeReplace() {
        return new DataTransferObject(getPVCoordinates(), getFrame(), getDate(), getMu());
    }

    /** Internal class used only for serialization. */
    private static class DataTransferObject implements Serializable {

        /** Serializable UID. */
        private static final long serialVersionUID = 4184412866917874790L;

        /** Computed PVCoordinates. */
        private PVCoordinates pvCoordinates;

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

        /** Date of the orbital parameters. */
        private final AbsoluteDate date;

        /** Value of mu used to compute position and velocity (m<sup>3</sup>/s<sup>2</sup>). */
        private final double mu;

        /** Simple constructor.
         * @param pvCoordinates the position and velocity of the satellite.
         * @param frame the frame in which the {@link PVCoordinates} are defined
         * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
         * @param date date of the orbital parameters
         * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
         */
        private DataTransferObject(final PVCoordinates pvCoordinates, final Frame frame,
                                   final AbsoluteDate date, final double mu) {
            this.pvCoordinates = pvCoordinates;
            this.frame         = frame;
            this.date          = date;
            this.mu            = mu;
        }

        /** Replace the deserialized data transfer object with a {@link CartesianOrbit}.
         * @return replacement {@link CartesianOrbit}
         */
        private Object readResolve() {
            // build a new provider, with an empty cache
            return new CartesianOrbit(pvCoordinates, frame, date, mu);
        }

    }

}