AbsolutePVCoordinates.java

  1. /* Copyright 2002-2020 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.utils;

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

  20. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  21. import org.hipparchus.analysis.interpolation.HermiteInterpolator;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  24. import org.hipparchus.util.FastMath;
  25. import org.orekit.annotation.DefaultDataContext;
  26. import org.orekit.data.DataContext;
  27. import org.orekit.errors.OrekitException;
  28. import org.orekit.errors.OrekitIllegalArgumentException;
  29. import org.orekit.errors.OrekitInternalError;
  30. import org.orekit.errors.OrekitMessages;
  31. import org.orekit.frames.Frame;
  32. import org.orekit.frames.Transform;
  33. import org.orekit.time.AbsoluteDate;
  34. import org.orekit.time.TimeInterpolable;
  35. import org.orekit.time.TimeStamped;

  36. /** Position - Velocity - Acceleration linked to a date and a frame.
  37.  */
  38. public class AbsolutePVCoordinates extends TimeStampedPVCoordinates
  39.     implements TimeStamped, TimeInterpolable<AbsolutePVCoordinates>,
  40.                Serializable, PVCoordinatesProvider {

  41.     /** Serializable UID. */
  42.     private static final long serialVersionUID = 20150824L;

  43.     /** Frame in which are defined the coordinates. */
  44.     private final Frame frame;

  45.     /** Build from position, velocity, acceleration.
  46.      * @param frame the frame in which the coordinates are defined
  47.      * @param date coordinates date
  48.      * @param position the position vector (m)
  49.      * @param velocity the velocity vector (m/s)
  50.      * @param acceleration the acceleration vector (m/sÂý)
  51.      */
  52.     public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date,
  53.                                  final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
  54.         super(date, position, velocity, acceleration);
  55.         this.frame = frame;
  56.     }

  57.     /** Build from position and velocity. Acceleration is set to zero.
  58.      * @param frame the frame in which the coordinates are defined
  59.      * @param date coordinates date
  60.      * @param position the position vector (m)
  61.      * @param velocity the velocity vector (m/s)
  62.      */
  63.     public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date,
  64.                                  final Vector3D position,
  65.                                  final Vector3D velocity) {
  66.         this(frame, date, position, velocity, Vector3D.ZERO);
  67.     }

  68.     /** Build from frame, date and PVA coordinates.
  69.      * @param frame the frame in which the coordinates are defined
  70.      * @param date date of the coordinates
  71.      * @param pva TimeStampedPVCoordinates
  72.      */
  73.     public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date, final PVCoordinates pva) {
  74.         super(date, pva);
  75.         this.frame = frame;
  76.     }

  77.     /** Build from frame and TimeStampedPVCoordinates.
  78.      * @param frame the frame in which the coordinates are defined
  79.      * @param pva TimeStampedPVCoordinates
  80.      */
  81.     public AbsolutePVCoordinates(final Frame frame, final TimeStampedPVCoordinates pva) {
  82.         super(pva.getDate(), pva);
  83.         this.frame = frame;
  84.     }

  85.     /** Multiplicative constructor
  86.      * <p>Build a AbsolutePVCoordinates from another one and a scale factor.</p>
  87.      * <p>The TimeStampedPVCoordinates built will be a * AbsPva</p>
  88.      * @param date date of the built coordinates
  89.      * @param a scale factor
  90.      * @param AbsPva base (unscaled) AbsolutePVCoordinates
  91.      */
  92.     public AbsolutePVCoordinates(final AbsoluteDate date,
  93.                                  final double a, final AbsolutePVCoordinates AbsPva) {
  94.         super(date, a, AbsPva);
  95.         this.frame = AbsPva.frame;
  96.     }

  97.     /** Subtractive constructor
  98.      * <p>Build a relative AbsolutePVCoordinates from a start and an end position.</p>
  99.      * <p>The AbsolutePVCoordinates built will be end - start.</p>
  100.      * <p>In case start and end use two different pseudo-inertial frames,
  101.      * the new AbsolutePVCoordinates arbitrarily be defined in the start frame. </p>
  102.      * @param date date of the built coordinates
  103.      * @param start Starting AbsolutePVCoordinates
  104.      * @param end ending AbsolutePVCoordinates
  105.      */
  106.     public AbsolutePVCoordinates(final AbsoluteDate date,
  107.                                  final AbsolutePVCoordinates start, final AbsolutePVCoordinates end) {
  108.         super(date, start, end);
  109.         ensureIdenticalFrames(start, end);
  110.         this.frame = start.frame;
  111.     }

  112.     /** Linear constructor
  113.      * <p>Build a AbsolutePVCoordinates from two other ones and corresponding scale factors.</p>
  114.      * <p>The AbsolutePVCoordinates built will be a1 * u1 + a2 * u2</p>
  115.      * <p>In case the AbsolutePVCoordinates use different pseudo-inertial frames,
  116.      * the new AbsolutePVCoordinates arbitrarily be defined in the first frame. </p>
  117.      * @param date date of the built coordinates
  118.      * @param a1 first scale factor
  119.      * @param absPv1 first base (unscaled) AbsolutePVCoordinates
  120.      * @param a2 second scale factor
  121.      * @param absPv2 second base (unscaled) AbsolutePVCoordinates
  122.      */
  123.     public AbsolutePVCoordinates(final AbsoluteDate date,
  124.                                  final double a1, final AbsolutePVCoordinates absPv1,
  125.                                  final double a2, final AbsolutePVCoordinates absPv2) {
  126.         super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates());
  127.         ensureIdenticalFrames(absPv1, absPv2);
  128.         this.frame = absPv1.getFrame();
  129.     }

  130.     /** Linear constructor
  131.      * <p>Build a AbsolutePVCoordinates from three other ones and corresponding scale factors.</p>
  132.      * <p>The AbsolutePVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  133.      * <p>In case the AbsolutePVCoordinates use different pseudo-inertial frames,
  134.      * the new AbsolutePVCoordinates arbitrarily be defined in the first frame. </p>
  135.      * @param date date of the built coordinates
  136.      * @param a1 first scale factor
  137.      * @param absPv1 first base (unscaled) AbsolutePVCoordinates
  138.      * @param a2 second scale factor
  139.      * @param absPv2 second base (unscaled) AbsolutePVCoordinates
  140.      * @param a3 third scale factor
  141.      * @param absPv3 third base (unscaled) AbsolutePVCoordinates
  142.      */
  143.     public AbsolutePVCoordinates(final AbsoluteDate date,
  144.                                  final double a1, final AbsolutePVCoordinates absPv1,
  145.                                  final double a2, final AbsolutePVCoordinates absPv2,
  146.                                  final double a3, final AbsolutePVCoordinates absPv3) {
  147.         super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates(),
  148.                 a3, absPv3.getPVCoordinates());
  149.         ensureIdenticalFrames(absPv1, absPv2);
  150.         ensureIdenticalFrames(absPv1, absPv3);
  151.         this.frame = absPv1.getFrame();
  152.     }

  153.     /** Linear constructor
  154.      * <p>Build a AbsolutePVCoordinates from four other ones and corresponding scale factors.</p>
  155.      * <p>The AbsolutePVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  156.      * <p>In case the AbsolutePVCoordinates use different pseudo-inertial frames,
  157.      * the new AbsolutePVCoordinates arbitrarily be defined in the first frame. </p>
  158.      * @param date date of the built coordinates
  159.      * @param a1 first scale factor
  160.      * @param absPv1 first base (unscaled) AbsolutePVCoordinates
  161.      * @param a2 second scale factor
  162.      * @param absPv2 second base (unscaled) AbsolutePVCoordinates
  163.      * @param a3 third scale factor
  164.      * @param absPv3 third base (unscaled) AbsolutePVCoordinates
  165.      * @param a4 fourth scale factor
  166.      * @param absPv4 fourth base (unscaled) AbsolutePVCoordinates
  167.      */
  168.     public AbsolutePVCoordinates(final AbsoluteDate date,
  169.                                  final double a1, final AbsolutePVCoordinates absPv1,
  170.                                  final double a2, final AbsolutePVCoordinates absPv2,
  171.                                  final double a3, final AbsolutePVCoordinates absPv3,
  172.                                  final double a4, final AbsolutePVCoordinates absPv4) {
  173.         super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates(),
  174.                 a3, absPv3.getPVCoordinates(), a4, absPv4.getPVCoordinates());
  175.         ensureIdenticalFrames(absPv1, absPv2);
  176.         ensureIdenticalFrames(absPv1, absPv3);
  177.         ensureIdenticalFrames(absPv1, absPv4);
  178.         this.frame = absPv1.getFrame();
  179.     }

  180.     /** Builds a AbsolutePVCoordinates triplet from  a {@link FieldVector3D}&lt;{@link DerivativeStructure}&gt;.
  181.      * <p>
  182.      * The vector components must have time as their only derivation parameter and
  183.      * have consistent derivation orders.
  184.      * </p>
  185.      * @param frame the frame in which the parameters are defined
  186.      * @param date date of the built coordinates
  187.      * @param p vector with time-derivatives embedded within the coordinates
  188.      */
  189.     public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date,
  190.             final FieldVector3D<DerivativeStructure> p) {
  191.         super(date, p);
  192.         this.frame = frame;
  193.     }

  194.     /** Ensure that the frames from two AbsolutePVCoordinates are identical.
  195.      * @param absPv1 first AbsolutePVCoordinates
  196.      * @param absPv2 first AbsolutePVCoordinates
  197.      * @throws OrekitIllegalArgumentException if frames are different
  198.      */
  199.     private static void ensureIdenticalFrames(final AbsolutePVCoordinates absPv1, final AbsolutePVCoordinates absPv2)
  200.         throws OrekitIllegalArgumentException {
  201.         if (!absPv1.frame.equals(absPv2.frame)) {
  202.             throw new OrekitIllegalArgumentException(OrekitMessages.INCOMPATIBLE_FRAMES,
  203.                                                      absPv1.frame.getName(), absPv2.frame.getName());
  204.         }
  205.     }

  206.     /** Get a time-shifted state.
  207.      * <p>
  208.      * The state can be slightly shifted to close dates. This shift is based on
  209.      * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
  210.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  211.      * for either small time shifts or coarse accuracy.
  212.      * </p>
  213.      * @param dt time shift in seconds
  214.      * @return a new state, shifted with respect to the instance (which is immutable)
  215.      */
  216.     public AbsolutePVCoordinates shiftedBy(final double dt) {
  217.         final TimeStampedPVCoordinates spv = super.shiftedBy(dt);
  218.         return new AbsolutePVCoordinates(frame, spv);
  219.     }

  220.     /** Create a local provider using simply Taylor expansion through {@link #shiftedBy(double)}.
  221.      * <p>
  222.      * The time evolution is based on a simple Taylor expansion. It is <em>not</em> intended as a
  223.      * replacement for proper orbit propagation (it is not even Keplerian!) but should be sufficient
  224.      * for either small time shifts or coarse accuracy.
  225.      * </p>
  226.      * @return provider based on Taylor expansion, for small time shifts around instance date
  227.      */
  228.     public PVCoordinatesProvider toTaylorProvider() {
  229.         return new PVCoordinatesProvider() {
  230.             /** {@inheritDoc} */
  231.             public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate d,  final Frame f) {
  232.                 final TimeStampedPVCoordinates shifted   = shiftedBy(d.durationFrom(getDate()));
  233.                 final Transform                transform = frame.getTransformTo(f, d);
  234.                 return transform.transformPVCoordinates(shifted);
  235.             }
  236.         };
  237.     }

  238.     /** Get the frame in which the coordinates are defined.
  239.      * @return frame in which the coordinates are defined
  240.      */
  241.     public Frame getFrame() {
  242.         return frame;
  243.     }

  244.     /** Get the TimeStampedPVCoordinates.
  245.      * @return TimeStampedPVCoordinates
  246.      */
  247.     public TimeStampedPVCoordinates getPVCoordinates() {
  248.         return this;
  249.     }

  250.     /** Get the TimeStampedPVCoordinates in a specified frame.
  251.      * @param outputFrame frame in which the position/velocity coordinates shall be computed
  252.      * @return TimeStampedPVCoordinates
  253.      * @exception OrekitException if transformation between frames cannot be computed
  254.      * @see #getPVCoordinates()
  255.      */
  256.     public TimeStampedPVCoordinates getPVCoordinates(final Frame outputFrame) {
  257.         // If output frame requested is the same as definition frame,
  258.         // PV coordinates are returned directly
  259.         if (outputFrame == frame) {
  260.             return getPVCoordinates();
  261.         }

  262.         // Else, PV coordinates are transformed to output frame
  263.         final Transform t = frame.getTransformTo(outputFrame, getDate());
  264.         return t.transformPVCoordinates(getPVCoordinates());
  265.     }

  266.     @Override
  267.     public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate otherDate, final Frame outputFrame) {
  268.         return shiftedBy(otherDate.durationFrom(getDate())).getPVCoordinates(outputFrame);
  269.     }

  270.     @Override
  271.     public AbsolutePVCoordinates interpolate(final AbsoluteDate date, final Stream<AbsolutePVCoordinates> sample) {
  272.         return interpolate(getFrame(), date, CartesianDerivativesFilter.USE_PVA, sample);
  273.     }

  274.     /** Interpolate position-velocity.
  275.      * <p>
  276.      * The interpolated instance is created by polynomial Hermite interpolation
  277.      * ensuring velocity remains the exact derivative of position.
  278.      * </p>
  279.      * <p>
  280.      * Note that even if first time derivatives (velocities)
  281.      * from sample can be ignored, the interpolated instance always includes
  282.      * interpolated derivatives. This feature can be used explicitly to
  283.      * compute these derivatives when it would be too complex to compute them
  284.      * from an analytical formula: just compute a few sample points from the
  285.      * explicit formula and set the derivatives to zero in these sample points,
  286.      * then use interpolation to add derivatives consistent with the positions.
  287.      * </p>
  288.      * @param frame frame for the interpolted instance
  289.      * @param date interpolation date
  290.      * @param filter filter for derivatives from the sample to use in interpolation
  291.      * @param sample sample points on which interpolation should be done
  292.      * @return a new position-velocity, interpolated at specified date
  293.      * @exception OrekitIllegalArgumentException if some elements in the sample do not
  294.      * have the same defining frame as other
  295.      */
  296.     public static AbsolutePVCoordinates interpolate(final Frame frame, final AbsoluteDate date,
  297.                                                     final CartesianDerivativesFilter filter,
  298.                                                     final Stream<AbsolutePVCoordinates> sample) {


  299.         // set up an interpolator taking derivatives into account
  300.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  301.         // add sample points
  302.         switch (filter) {
  303.             case USE_P :
  304.                 // populate sample with position data, ignoring velocity
  305.                 sample.forEach(pv -> {
  306.                     final Vector3D position = pv.getPosition();
  307.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  308.                                                 position.toArray());
  309.                 });
  310.                 break;
  311.             case USE_PV :
  312.                 // populate sample with position and velocity data
  313.                 sample.forEach(pv -> {
  314.                     final Vector3D position = pv.getPosition();
  315.                     final Vector3D velocity = pv.getVelocity();
  316.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  317.                                                 position.toArray(), velocity.toArray());
  318.                 });
  319.                 break;
  320.             case USE_PVA :
  321.                 // populate sample with position, velocity and acceleration data
  322.                 sample.forEach(pv -> {
  323.                     final Vector3D position     = pv.getPosition();
  324.                     final Vector3D velocity     = pv.getVelocity();
  325.                     final Vector3D acceleration = pv.getAcceleration();
  326.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  327.                                                 position.toArray(), velocity.toArray(), acceleration.toArray());
  328.                 });
  329.                 break;
  330.             default :
  331.                 // this should never happen
  332.                 throw new OrekitInternalError(null);
  333.         }

  334.         // interpolate
  335.         final double[][] p = interpolator.derivatives(0.0, 2);

  336.         // build a new interpolated instance
  337.         return new AbsolutePVCoordinates(frame, date, new Vector3D(p[0]), new Vector3D(p[1]), new Vector3D(p[2]));

  338.     }

  339.     /** Replace the instance with a data transfer object for serialization.
  340.      * @return data transfer object that will be serialized
  341.      */
  342.     @DefaultDataContext
  343.     private Object writeReplace() {
  344.         return new DTO(this);
  345.     }

  346.     /** Internal class used only for serialization. */
  347.     @DefaultDataContext
  348.     private static class DTO implements Serializable {

  349.         /** Serializable UID. */
  350.         private static final long serialVersionUID = 20150916L;

  351.         /** Double values. */
  352.         private double[] d;

  353.         /** Frame in which acoordinates are defined. */
  354.         private final Frame frame;

  355.         /** Simple constructor.
  356.          * @param absPva instance to serialize
  357.          */
  358.         private DTO(final AbsolutePVCoordinates absPva) {

  359.             // decompose date
  360.             final AbsoluteDate j2000Epoch =
  361.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  362.             final double epoch  = FastMath.floor(absPva.getDate().durationFrom(j2000Epoch));
  363.             final double offset = absPva.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));

  364.             this.d = new double[] {
  365.                 epoch, offset,
  366.                 absPva.getPosition().getX(),     absPva.getPosition().getY(),     absPva.getPosition().getZ(),
  367.                 absPva.getVelocity().getX(),     absPva.getVelocity().getY(),     absPva.getVelocity().getZ(),
  368.                 absPva.getAcceleration().getX(), absPva.getAcceleration().getY(), absPva.getAcceleration().getZ()
  369.             };
  370.             this.frame = absPva.frame;

  371.         }

  372.         /** Replace the deserialized data transfer object with a {@link AbsolutePVCoordinates}.
  373.          * @return replacement {@link AbsolutePVCoordinates}
  374.          */
  375.         private Object readResolve() {
  376.             final AbsoluteDate j2000Epoch =
  377.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  378.             return new AbsolutePVCoordinates(frame,
  379.                                              j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  380.                                              new Vector3D(d[2], d[3], d[ 4]),
  381.                                              new Vector3D(d[5], d[6], d[ 7]),
  382.                                              new Vector3D(d[8], d[9], d[10]));
  383.         }

  384.     }

  385. }