PVCoordinates.java

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

  19. import java.io.Serializable;
  20. import java.util.ArrayList;
  21. import java.util.Collection;
  22. import java.util.List;

  24. import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
  25. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  26. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  27. import org.apache.commons.math3.util.Pair;
  28. import org.orekit.errors.OrekitException;
  29. import org.orekit.errors.OrekitMessages;
  30. import org.orekit.time.AbsoluteDate;
  31. import org.orekit.time.TimeShiftable;

  33. /** Simple container for Position/Velocity/Acceleration triplets.
  34.  * <p>
  35.  * The state can be slightly shifted to close dates. This shift is based on
  36.  * a simple quadratic model. It is <em>not</em> intended as a replacement for
  37.  * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  38.  * for either small time shifts or coarse accuracy.
  39.  * </p>
  40.  * <p>
  41.  * This class is the angular counterpart to {@link AngularCoordinates}.
  42.  * </p>
  43.  * <p>Instances of this class are guaranteed to be immutable.</p>
  44.  * @author Fabien Maussion
  45.  * @author Luc Maisonobe
  46.  */
  47. public class PVCoordinates implements TimeShiftable<PVCoordinates>, Serializable {

  49.     /** Fixed position/velocity at origin (both p, v and a are zero vectors). */
  50.     public static final PVCoordinates ZERO = new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);

  52.     /** Serializable UID. */
  53.     private static final long serialVersionUID = 20140407L;

  55.     /** The position. */
  56.     private final Vector3D position;

  58.     /** The velocity. */
  59.     private final Vector3D velocity;

  61.     /** The acceleration. */
  62.     private final Vector3D acceleration;

  64.     /** Simple constructor.
  65.      * <p> Set the Coordinates to default : (0 0 0), (0 0 0), (0 0 0).</p>
  66.      */
  67.     public PVCoordinates() {
  68.         position     = Vector3D.ZERO;
  69.         velocity     = Vector3D.ZERO;
  70.         acceleration = Vector3D.ZERO;
  71.     }

  73.     /** Builds a PVCoordinates triplet with zero acceleration.
  74.      * <p>Acceleration is set to zero</p>
  75.      * @param position the position vector (m)
  76.      * @param velocity the velocity vector (m/s)
  77.      */
  78.     public PVCoordinates(final Vector3D position, final Vector3D velocity) {
  79.         this.position     = position;
  80.         this.velocity     = velocity;
  81.         this.acceleration = Vector3D.ZERO;
  82.     }

  84.     /** Builds a PVCoordinates triplet.
  85.      * @param position the position vector (m)
  86.      * @param velocity the velocity vector (m/s)
  87.      * @param acceleration the acceleration vector (m/s²)
  88.      */
  89.     public PVCoordinates(final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
  90.         this.position     = position;
  91.         this.velocity     = velocity;
  92.         this.acceleration = acceleration;
  93.     }

  95.     /** Multiplicative constructor.
  96.      * <p>Build a PVCoordinates from another one and a scale factor.</p>
  97.      * <p>The PVCoordinates built will be a * pv</p>
  98.      * @param a scale factor
  99.      * @param pv base (unscaled) PVCoordinates
  100.      */
  101.     public PVCoordinates(final double a, final PVCoordinates pv) {
  102.         position     = new Vector3D(a, pv.position);
  103.         velocity     = new Vector3D(a, pv.velocity);
  104.         acceleration = new Vector3D(a, pv.acceleration);
  105.     }

  107.     /** Subtractive constructor.
  108.      * <p>Build a relative PVCoordinates from a start and an end position.</p>
  109.      * <p>The PVCoordinates built will be end - start.</p>
  110.      * @param start Starting PVCoordinates
  111.      * @param end ending PVCoordinates
  112.      */
  113.     public PVCoordinates(final PVCoordinates start, final PVCoordinates end) {
  114.         this.position     = end.position.subtract(start.position);
  115.         this.velocity     = end.velocity.subtract(start.velocity);
  116.         this.acceleration = end.acceleration.subtract(start.acceleration);
  117.     }

  119.     /** Linear constructor.
  120.      * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
  121.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
  122.      * @param a1 first scale factor
  123.      * @param pv1 first base (unscaled) PVCoordinates
  124.      * @param a2 second scale factor
  125.      * @param pv2 second base (unscaled) PVCoordinates
  126.      */
  127.     public PVCoordinates(final double a1, final PVCoordinates pv1,
  128.                          final double a2, final PVCoordinates pv2) {
  129.         position     = new Vector3D(a1, pv1.position,     a2, pv2.position);
  130.         velocity     = new Vector3D(a1, pv1.velocity,     a2, pv2.velocity);
  131.         acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration);
  132.     }

  134.     /** Linear constructor.
  135.      * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
  136.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  137.      * @param a1 first scale factor
  138.      * @param pv1 first base (unscaled) PVCoordinates
  139.      * @param a2 second scale factor
  140.      * @param pv2 second base (unscaled) PVCoordinates
  141.      * @param a3 third scale factor
  142.      * @param pv3 third base (unscaled) PVCoordinates
  143.      */
  144.     public PVCoordinates(final double a1, final PVCoordinates pv1,
  145.                          final double a2, final PVCoordinates pv2,
  146.                          final double a3, final PVCoordinates pv3) {
  147.         position     = new Vector3D(a1, pv1.position,     a2, pv2.position,     a3, pv3.position);
  148.         velocity     = new Vector3D(a1, pv1.velocity,     a2, pv2.velocity,     a3, pv3.velocity);
  149.         acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration);
  150.     }

  152.     /** Linear constructor.
  153.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  154.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  155.      * @param a1 first scale factor
  156.      * @param pv1 first base (unscaled) PVCoordinates
  157.      * @param a2 second scale factor
  158.      * @param pv2 second base (unscaled) PVCoordinates
  159.      * @param a3 third scale factor
  160.      * @param pv3 third base (unscaled) PVCoordinates
  161.      * @param a4 fourth scale factor
  162.      * @param pv4 fourth base (unscaled) PVCoordinates
  163.      */
  164.     public PVCoordinates(final double a1, final PVCoordinates pv1,
  165.                          final double a2, final PVCoordinates pv2,
  166.                          final double a3, final PVCoordinates pv3,
  167.                          final double a4, final PVCoordinates pv4) {
  168.         position     = new Vector3D(a1, pv1.position,     a2, pv2.position,
  169.                                     a3, pv3.position,     a4, pv4.position);
  170.         velocity     = new Vector3D(a1, pv1.velocity,     a2, pv2.velocity,
  171.                                     a3, pv3.velocity,     a4, pv4.velocity);
  172.         acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration,
  173.                                     a3, pv3.acceleration, a4, pv4.acceleration);
  174.     }

  176.     /** Builds a PVCoordinates triplet from  a {@link FieldVector3D}&lt;{@link DerivativeStructure}&gt;.
  177.      * <p>
  178.      * The vector components must have time as their only derivation parameter and
  179.      * have consistent derivation orders.
  180.      * </p>
  181.      * @param p vector with time-derivatives embedded within the coordinates
  182.      */
  183.     public PVCoordinates(final FieldVector3D<DerivativeStructure> p) {
  184.         position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal());
  185.         if (p.getX().getOrder() >= 1) {
  186.             velocity = new Vector3D(p.getX().getPartialDerivative(1),
  187.                                     p.getY().getPartialDerivative(1),
  188.                                     p.getZ().getPartialDerivative(1));
  189.             if (p.getX().getOrder() >= 2) {
  190.                 acceleration = new Vector3D(p.getX().getPartialDerivative(2),
  191.                                             p.getY().getPartialDerivative(2),
  192.                                             p.getZ().getPartialDerivative(2));
  193.             } else {
  194.                 acceleration = Vector3D.ZERO;
  195.             }
  196.         } else {
  197.             velocity     = Vector3D.ZERO;
  198.             acceleration = Vector3D.ZERO;
  199.         }
  200.     }

  202.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link DerivativeStructure}&gt;.
  203.      * <p>
  204.      * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
  205.      * to the user-specified order.
  206.      * </p>
  207.      * @param order derivation order for the vector components (must be either 0, 1 or 2)
  208.      * @return vector with time-derivatives embedded within the coordinates
  209.      * @exception OrekitException if the user specified order is too large
  210.      */
  211.     public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order)
  212.         throws OrekitException {

  214.         final DerivativeStructure x;
  215.         final DerivativeStructure y;
  216.         final DerivativeStructure z;
  217.         switch(order) {
  218.             case 0 :
  219.                 x = new DerivativeStructure(1, 0, position.getX());
  220.                 y = new DerivativeStructure(1, 0, position.getY());
  221.                 z = new DerivativeStructure(1, 0, position.getZ());
  222.                 break;
  223.             case 1 :
  224.                 x = new DerivativeStructure(1, 1, position.getX(), velocity.getX());
  225.                 y = new DerivativeStructure(1, 1, position.getY(), velocity.getY());
  226.                 z = new DerivativeStructure(1, 1, position.getZ(), velocity.getZ());
  227.                 break;
  228.             case 2 :
  229.                 x = new DerivativeStructure(1, 2, position.getX(), velocity.getX(), acceleration.getX());
  230.                 y = new DerivativeStructure(1, 2, position.getY(), velocity.getY(), acceleration.getY());
  231.                 z = new DerivativeStructure(1, 2, position.getZ(), velocity.getZ(), acceleration.getZ());
  232.                 break;
  233.             default :
  234.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  235.         }

  237.         return new FieldVector3D<DerivativeStructure>(x, y, z);

  239.     }

  241.     /** Estimate velocity between two positions.
  242.      * <p>Estimation is based on a simple fixed velocity translation
  243.      * during the time interval between the two positions.</p>
  244.      * @param start start position
  245.      * @param end end position
  246.      * @param dt time elapsed between the dates of the two positions
  247.      * @return velocity allowing to go from start to end positions
  248.      */
  249.     public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
  250.         final double scale = 1.0 / dt;
  251.         return new Vector3D(scale, end, -scale, start);
  252.     }

  254.     /** Get a time-shifted state.
  255.      * <p>
  256.      * The state can be slightly shifted to close dates. This shift is based on
  257.      * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
  258.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  259.      * for either small time shifts or coarse accuracy.
  260.      * </p>
  261.      * @param dt time shift in seconds
  262.      * @return a new state, shifted with respect to the instance (which is immutable)
  263.      */
  264.     public PVCoordinates shiftedBy(final double dt) {
  265.         return new PVCoordinates(new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
  266.                                  new Vector3D(1, velocity, dt, acceleration),
  267.                                  acceleration);
  268.     }

  270.     /** Interpolate position-velocity.
  271.      * <p>
  272.      * The interpolated instance is created by polynomial Hermite interpolation
  273.      * ensuring velocity remains the exact derivative of position.
  274.      * </p>
  275.      * <p>
  276.      * Note that even if first time derivatives (velocities)
  277.      * from sample can be ignored, the interpolated instance always includes
  278.      * interpolated derivatives. This feature can be used explicitly to
  279.      * compute these derivatives when it would be too complex to compute them
  280.      * from an analytical formula: just compute a few sample points from the
  281.      * explicit formula and set the derivatives to zero in these sample points,
  282.      * then use interpolation to add derivatives consistent with the positions.
  283.      * </p>
  284.      * @param date interpolation date
  285.      * @param useVelocities if true, use sample points velocities,
  286.      * otherwise ignore them and use only positions
  287.      * @param sample sample points on which interpolation should be done
  288.      * @return a new position-velocity, interpolated at specified date
  289.      * @deprecated since 7.0 replaced with {@link TimeStampedPVCoordinates#interpolate(AbsoluteDate, CartesianDerivativesFilter, Collection)}
  290.      */
  291.     @Deprecated
  292.     public static PVCoordinates interpolate(final AbsoluteDate date, final boolean useVelocities,
  293.                                             final Collection<Pair<AbsoluteDate, PVCoordinates>> sample) {
  294.         final List<TimeStampedPVCoordinates> list = new ArrayList<TimeStampedPVCoordinates>(sample.size());
  295.         for (final Pair<AbsoluteDate, PVCoordinates> pair : sample) {
  296.             list.add(new TimeStampedPVCoordinates(pair.getFirst(),
  297.                                                   pair.getSecond().getPosition(),
  298.                                                   pair.getSecond().getVelocity(),
  299.                                                   pair.getSecond().getAcceleration()));
  300.         }
  301.         return TimeStampedPVCoordinates.interpolate(date,
  302.                                                     useVelocities ? CartesianDerivativesFilter.USE_PV : CartesianDerivativesFilter.USE_P,
  303.                                                     list);
  304.     }

  306.     /** Gets the position.
  307.      * @return the position vector (m).
  308.      */
  309.     public Vector3D getPosition() {
  310.         return position;
  311.     }

  313.     /** Gets the velocity.
  314.      * @return the velocity vector (m/s).
  315.      */
  316.     public Vector3D getVelocity() {
  317.         return velocity;
  318.     }

  320.     /** Gets the acceleration.
  321.      * @return the acceleration vector (m/s²).
  322.      */
  323.     public Vector3D getAcceleration() {
  324.         return acceleration;
  325.     }

  327.     /** Gets the momentum.
  328.      * <p>This vector is the p &otimes; v where p is position, v is velocity
  329.      * and &otimes; is cross product. To get the real physical angular momentum
  330.      * you need to multiply this vector by the mass.</p>
  331.      * <p>The returned vector is recomputed each time this method is called, it
  332.      * is not cached.</p>
  333.      * @return a new instance of the momentum vector (m²/s).
  334.      */
  335.     public Vector3D getMomentum() {
  336.         return Vector3D.crossProduct(position, velocity);
  337.     }

  339.     /**
  340.      * Get the angular velocity (spin) of this point as seen from the origin.
  341.      * <p/>
  342.      * The angular velocity vector is parallel to the {@link #getMomentum() angular
  343.      * momentum} and is computed by ω = p &times; v / ||p||²
  344.      *
  345.      * @return the angular velocity vector
  346.      * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
  347.      *      Wikipedia</a>
  348.      */
  349.     public Vector3D getAngularVelocity() {
  350.         return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
  351.     }

  353.     /** Get the opposite of the instance.
  354.      * @return a new position-velocity which is opposite to the instance
  355.      */
  356.     public PVCoordinates negate() {
  357.         return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
  358.     }

  360.     /** Normalize the position part of the instance.
  361.      * <p>
  362.      * The computed coordinates first component (position) will be a
  363.      * normalized vector, the second component (velocity) will be the
  364.      * derivative of the first component (hence it will generally not
  365.      * be normalized), and the third component (acceleration) will be the
  366.      * derivative of the second component (hence it will generally not
  367.      * be normalized).
  368.      * </p>
  369.      * @return a new instance, with first component normalized and
  370.      * remaining component computed to have consistent derivatives
  371.      */
  372.     public PVCoordinates normalize() {
  373.         final double   inv     = 1.0 / position.getNorm();
  374.         final Vector3D u       = new Vector3D(inv, position);
  375.         final Vector3D v       = new Vector3D(inv, velocity);
  376.         final Vector3D w       = new Vector3D(inv, acceleration);
  377.         final double   uv      = Vector3D.dotProduct(u, v);
  378.         final double   v2      = Vector3D.dotProduct(v, v);
  379.         final double   uw      = Vector3D.dotProduct(u, w);
  380.         final Vector3D uDot    = new Vector3D(1, v, -uv, u);
  381.         final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
  382.         return new PVCoordinates(u, uDot, uDotDot);
  383.     }

  385.     /** Compute the cross-product of two instances.
  386.      * @param pv1 first instances
  387.      * @param pv2 second instances
  388.      * @return the cross product v1 ^ v2 as a new instance
  389.      */
  390.     public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) {
  391.         final Vector3D p1 = pv1.position;
  392.         final Vector3D v1 = pv1.velocity;
  393.         final Vector3D a1 = pv1.acceleration;
  394.         final Vector3D p2 = pv2.position;
  395.         final Vector3D v2 = pv2.velocity;
  396.         final Vector3D a2 = pv2.acceleration;
  397.         return new PVCoordinates(Vector3D.crossProduct(p1, p2),
  398.                                  new Vector3D(1, Vector3D.crossProduct(p1, v2),
  399.                                               1, Vector3D.crossProduct(v1, p2)),
  400.                                  new Vector3D(1, Vector3D.crossProduct(p1, a2),
  401.                                               2, Vector3D.crossProduct(v1, v2),
  402.                                               1, Vector3D.crossProduct(a1, p2)));
  403.     }

  405.     /** Return a string representation of this position/velocity pair.
  406.      * @return string representation of this position/velocity pair
  407.      */
  408.     public String toString() {
  409.         final String comma = ", ";
  410.         return new StringBuffer().append('{').append("P(").
  411.                 append(position.getX()).append(comma).
  412.                 append(position.getY()).append(comma).
  413.                 append(position.getZ()).append("), V(").
  414.                 append(velocity.getX()).append(comma).
  415.                 append(velocity.getY()).append(comma).
  416.                 append(velocity.getZ()).append("), A(").
  417.                 append(acceleration.getX()).append(comma).
  418.                 append(acceleration.getY()).append(comma).
  419.                 append(acceleration.getZ()).append(")}").toString();
  420.     }

  422.     /** Replace the instance with a data transfer object for serialization.
  423.      * @return data transfer object that will be serialized
  424.      */
  425.     private Object writeReplace() {
  426.         return new DTO(this);
  427.     }

  429.     /** Internal class used only for serialization. */
  430.     private static class DTO implements Serializable {

  432.         /** Serializable UID. */
  433.         private static final long serialVersionUID = 20140723L;

  435.         /** Double values. */
  436.         private double[] d;

  438.         /** Simple constructor.
  439.          * @param pv instance to serialize
  440.          */
  441.         private DTO(final PVCoordinates pv) {
  442.             this.d = new double[] {
  443.                 pv.getPosition().getX(),     pv.getPosition().getY(),     pv.getPosition().getZ(),
  444.                 pv.getVelocity().getX(),     pv.getVelocity().getY(),     pv.getVelocity().getZ(),
  445.                 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
  446.             };
  447.         }

  449.         /** Replace the deserialized data transfer object with a {@link PVCoordinates}.
  450.          * @return replacement {@link PVCoordinates}
  451.          */
  452.         private Object readResolve() {
  453.             return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
  454.                                      new Vector3D(d[3], d[4], d[5]),
  455.                                      new Vector3D(d[6], d[7], d[8]));
  456.         }

  458.     }

  460. }
Created with JaCoCo 0.7.4.201502262128