TimeStampedPVCoordinates.java

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

  19. import java.io.Serializable;
  20. import java.util.Collection;
  21. import java.util.stream.Stream;

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

  37. /** {@link TimeStamped time-stamped} version of {@link PVCoordinates}.
  38.  * <p>Instances of this class are guaranteed to be immutable.</p>
  39.  * @author Luc Maisonobe
  40.  * @since 7.0
  41.  */
  42. public class TimeStampedPVCoordinates extends PVCoordinates implements TimeStamped {

  44.     /** Serializable UID. */
  45.     private static final long serialVersionUID = 20140723L;

  47.     /** The date. */
  48.     private final AbsoluteDate date;

  50.     /** Builds a TimeStampedPVCoordinates pair.
  51.      * @param date coordinates date
  52.      * @param position the position vector (m)
  53.      * @param velocity the velocity vector (m/s)
  54.      * @param acceleration the acceleration vector (m/s²)
  55.      */
  56.     public TimeStampedPVCoordinates(final AbsoluteDate date,
  57.                                     final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
  58.         super(position, velocity, acceleration);
  59.         this.date = date;
  60.     }

  62.     /**
  63.      * Build from position and velocity. Acceleration is set to zero.
  64.      *
  65.      * @param date coordinates date
  66.      * @param position the position vector (m)
  67.      * @param velocity the velocity vector (m/s)
  68.      */
  69.     public TimeStampedPVCoordinates(final AbsoluteDate date,
  70.                                     final Vector3D position,
  71.                                     final Vector3D velocity) {
  72.         this(date, position, velocity, Vector3D.ZERO);
  73.     }

  75.     /**
  76.      * Build from position velocity acceleration coordinates.
  77.      *
  78.      * @param date coordinates date
  79.      * @param pv position velocity, and acceleration coordinates, in meters and seconds.
  80.      */
  81.     public TimeStampedPVCoordinates(final AbsoluteDate date, final PVCoordinates pv) {
  82.         this(date, pv.getPosition(), pv.getVelocity(), pv.getAcceleration());
  83.     }

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

  100.     /** Subtractive constructor
  101.      * <p>Build a relative TimeStampedPVCoordinates from a start and an end position.</p>
  102.      * <p>The TimeStampedPVCoordinates built will be end - start.</p>
  103.      * @param date date of the built coordinates
  104.      * @param start Starting PVCoordinates
  105.      * @param end ending PVCoordinates
  106.      */
  107.     public TimeStampedPVCoordinates(final AbsoluteDate date,
  108.                                     final PVCoordinates start, final PVCoordinates end) {
  109.         super(end.getPosition().subtract(start.getPosition()),
  110.               end.getVelocity().subtract(start.getVelocity()),
  111.               end.getAcceleration().subtract(start.getAcceleration()));
  112.         this.date = date;
  113.     }

  115.     /** Linear constructor
  116.      * <p>Build a TimeStampedPVCoordinates from two other ones and corresponding scale factors.</p>
  117.      * <p>The TimeStampedPVCoordinates built will be a1 * u1 + a2 * u2</p>
  118.      * @param date date of the built coordinates
  119.      * @param a1 first scale factor
  120.      * @param pv1 first base (unscaled) PVCoordinates
  121.      * @param a2 second scale factor
  122.      * @param pv2 second base (unscaled) PVCoordinates
  123.      */
  124.     public TimeStampedPVCoordinates(final AbsoluteDate date,
  125.                                     final double a1, final PVCoordinates pv1,
  126.                                     final double a2, final PVCoordinates pv2) {
  127.         super(new Vector3D(a1, pv1.getPosition(),     a2, pv2.getPosition()),
  128.               new Vector3D(a1, pv1.getVelocity(),     a2, pv2.getVelocity()),
  129.               new Vector3D(a1, pv1.getAcceleration(), a2, pv2.getAcceleration()));
  130.         this.date = date;
  131.     }

  133.     /** Linear constructor
  134.      * <p>Build a TimeStampedPVCoordinates from three other ones and corresponding scale factors.</p>
  135.      * <p>The TimeStampedPVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  136.      * @param date date of the built coordinates
  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 TimeStampedPVCoordinates(final AbsoluteDate date,
  145.                                     final double a1, final PVCoordinates pv1,
  146.                                     final double a2, final PVCoordinates pv2,
  147.                                     final double a3, final PVCoordinates pv3) {
  148.         super(new Vector3D(a1, pv1.getPosition(),     a2, pv2.getPosition(),     a3, pv3.getPosition()),
  149.               new Vector3D(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),     a3, pv3.getVelocity()),
  150.               new Vector3D(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(), a3, pv3.getAcceleration()));
  151.         this.date = date;
  152.     }

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

  178.     /** Builds a TimeStampedPVCoordinates triplet from  a {@link FieldVector3D}&lt;{@link Derivative}&gt;.
  179.      * <p>
  180.      * The vector components must have time as their only derivation parameter and
  181.      * have consistent derivation orders.
  182.      * </p>
  183.      * @param date date of the built coordinates
  184.      * @param p vector with time-derivatives embedded within the coordinates
  185.      * @param <U> type of the derivative
  186.      */
  187.     public <U extends Derivative<U>> TimeStampedPVCoordinates(final AbsoluteDate date, final FieldVector3D<U> p) {
  188.         super(p);
  189.         this.date = date;
  190.     }

  192.     /** {@inheritDoc} */
  193.     public AbsoluteDate getDate() {
  194.         return date;
  195.     }

  197.     /** Get a time-shifted state.
  198.      * <p>
  199.      * The state can be slightly shifted to close dates. This shift is based on
  200.      * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
  201.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  202.      * for either small time shifts or coarse accuracy.
  203.      * </p>
  204.      * @param dt time shift in seconds
  205.      * @return a new state, shifted with respect to the instance (which is immutable)
  206.      */
  207.     public TimeStampedPVCoordinates shiftedBy(final double dt) {
  208.         final PVCoordinates spv = super.shiftedBy(dt);
  209.         return new TimeStampedPVCoordinates(date.shiftedBy(dt),
  210.                                             spv.getPosition(), spv.getVelocity(), spv.getAcceleration());
  211.     }

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

  233.     /** Interpolate position-velocity.
  234.      * <p>
  235.      * The interpolated instance is created by polynomial Hermite interpolation
  236.      * ensuring velocity remains the exact derivative of position.
  237.      * </p>
  238.      * <p>
  239.      * Note that even if first time derivatives (velocities)
  240.      * from sample can be ignored, the interpolated instance always includes
  241.      * interpolated derivatives. This feature can be used explicitly to
  242.      * compute these derivatives when it would be too complex to compute them
  243.      * from an analytical formula: just compute a few sample points from the
  244.      * explicit formula and set the derivatives to zero in these sample points,
  245.      * then use interpolation to add derivatives consistent with the positions.
  246.      * </p>
  247.      * @param date interpolation date
  248.      * @param filter filter for derivatives from the sample to use in interpolation
  249.      * @param sample sample points on which interpolation should be done
  250.      * @return a new position-velocity, interpolated at specified date
  251.      */
  252.     public static TimeStampedPVCoordinates interpolate(final AbsoluteDate date,
  253.                                                        final CartesianDerivativesFilter filter,
  254.                                                        final Collection<? extends TimeStampedPVCoordinates> sample) {
  255.         return interpolate(date, filter, sample.stream());
  256.     }

  258.     /** Interpolate position-velocity.
  259.      * <p>
  260.      * The interpolated instance is created by polynomial Hermite interpolation
  261.      * ensuring velocity remains the exact derivative of position.
  262.      * </p>
  263.      * <p>
  264.      * Note that even if first time derivatives (velocities)
  265.      * from sample can be ignored, the interpolated instance always includes
  266.      * interpolated derivatives. This feature can be used explicitly to
  267.      * compute these derivatives when it would be too complex to compute them
  268.      * from an analytical formula: just compute a few sample points from the
  269.      * explicit formula and set the derivatives to zero in these sample points,
  270.      * then use interpolation to add derivatives consistent with the positions.
  271.      * </p>
  272.      * @param date interpolation date
  273.      * @param filter filter for derivatives from the sample to use in interpolation
  274.      * @param sample sample points on which interpolation should be done
  275.      * @return a new position-velocity, interpolated at specified date
  276.      * @since 9.0
  277.      */
  278.     public static TimeStampedPVCoordinates interpolate(final AbsoluteDate date,
  279.                                                        final CartesianDerivativesFilter filter,
  280.                                                        final Stream<? extends TimeStampedPVCoordinates> sample) {

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

  285.         // add sample points
  286.         switch (filter) {
  287.             case USE_P :
  288.                 // populate sample with position data, ignoring velocity
  289.                 sample.forEach(pv -> {
  290.                     final Vector3D position = pv.getPosition();
  291.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  292.                                                 position.toArray());
  293.                 });
  294.                 break;
  295.             case USE_PV :
  296.                 // populate sample with position and velocity data
  297.                 sample.forEach(pv -> {
  298.                     final Vector3D position = pv.getPosition();
  299.                     final Vector3D velocity = pv.getVelocity();
  300.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  301.                                                 position.toArray(), velocity.toArray());
  302.                 });
  303.                 break;
  304.             case USE_PVA :
  305.                 // populate sample with position, velocity and acceleration data
  306.                 sample.forEach(pv -> {
  307.                     final Vector3D position     = pv.getPosition();
  308.                     final Vector3D velocity     = pv.getVelocity();
  309.                     final Vector3D acceleration = pv.getAcceleration();
  310.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  311.                                                 position.toArray(), velocity.toArray(), acceleration.toArray());
  312.                 });
  313.                 break;
  314.             default :
  315.                 // this should never happen
  316.                 throw new OrekitInternalError(null);
  317.         }

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

  322.         // build a new interpolated instance
  323.         return new TimeStampedPVCoordinates(date, new Vector3D(p[0]), new Vector3D(p[1]), new Vector3D(p[2]));

  325.     }

  327.     /** Return a string representation of this date, position, velocity, and acceleration.
  328.      *
  329.      * <p>This method uses the {@link DataContext#getDefault() default data context}.
  330.      *
  331.      * @return string representation of this.
  332.      */
  333.     @Override
  334.     @DefaultDataContext
  335.     public String toString() {
  336.         return toString(DataContext.getDefault().getTimeScales().getUTC());
  337.     }

  339.     /**
  340.      * Return a string representation of this date, position, velocity, and acceleration.
  341.      *
  342.      * @param utc time scale used to print the date.
  343.      * @return string representation of this.
  344.      */
  345.     public String toString(final TimeScale utc) {
  346.         final String comma = ", ";
  347.         return new StringBuilder().append('{').
  348.                                   append(date.toString(utc)).append(", P(").
  349.                                   append(getPosition().getX()).append(comma).
  350.                                   append(getPosition().getY()).append(comma).
  351.                                   append(getPosition().getZ()).append("), V(").
  352.                                   append(getVelocity().getX()).append(comma).
  353.                                   append(getVelocity().getY()).append(comma).
  354.                                   append(getVelocity().getZ()).append("), A(").
  355.                                   append(getAcceleration().getX()).append(comma).
  356.                                   append(getAcceleration().getY()).append(comma).
  357.                                   append(getAcceleration().getZ()).append(")}").toString();
  358.     }

  360.     /** Replace the instance with a data transfer object for serialization.
  361.      * @return data transfer object that will be serialized
  362.      */
  363.     @DefaultDataContext
  364.     private Object writeReplace() {
  365.         return new DTO(this);
  366.     }

  368.     /** Internal class used only for serialization. */
  369.     @DefaultDataContext
  370.     private static class DTO implements Serializable {

  372.         /** Serializable UID. */
  373.         private static final long serialVersionUID = 20140723L;

  375.         /** Double values. */
  376.         private double[] d;

  378.         /** Simple constructor.
  379.          * @param pv instance to serialize
  380.          */
  381.         private DTO(final TimeStampedPVCoordinates pv) {

  383.             // decompose date
  384.             final AbsoluteDate j2000Epoch =
  385.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  386.             final double epoch  = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
  387.             final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));

  389.             this.d = new double[] {
  390.                 epoch, offset,
  391.                 pv.getPosition().getX(),     pv.getPosition().getY(),     pv.getPosition().getZ(),
  392.                 pv.getVelocity().getX(),     pv.getVelocity().getY(),     pv.getVelocity().getZ(),
  393.                 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ()
  394.             };

  396.         }

  398.         /** Replace the deserialized data transfer object with a {@link TimeStampedPVCoordinates}.
  399.          * @return replacement {@link TimeStampedPVCoordinates}
  400.          */
  401.         private Object readResolve() {
  402.             final AbsoluteDate j2000Epoch =
  403.                     DataContext.getDefault().getTimeScales().getJ2000Epoch();
  404.             return new TimeStampedPVCoordinates(j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
  405.                                                 new Vector3D(d[2], d[3], d[ 4]),
  406.                                                 new Vector3D(d[5], d[6], d[ 7]),
  407.                                                 new Vector3D(d[8], d[9], d[10]));
  408.         }

  410.     }

  412. }
Created with JaCoCo 0.8.7.202105040129