TimeStampedFieldPVCoordinates.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.util.Collection;

  21. import org.apache.commons.math3.RealFieldElement;
  22. import org.apache.commons.math3.analysis.interpolation.FieldHermiteInterpolator;
  23. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  24. import org.orekit.errors.OrekitInternalError;
  25. import org.orekit.time.AbsoluteDate;
  26. import org.orekit.time.TimeStamped;

  28. /** {@link TimeStamped time-stamped} version of {@link FieldPVCoordinates}.
  29.  * <p>Instances of this class are guaranteed to be immutable.</p>
  30.  * @param <T> the type of the field elements
  31.  * @author Luc Maisonobe
  32.  * @since 7.0
  33.  */
  34. public class TimeStampedFieldPVCoordinates<T extends RealFieldElement<T>>
  35.     extends FieldPVCoordinates<T> implements TimeStamped {

  37.     /** Serializable UID. */
  38.     private static final long serialVersionUID = 20140611L;

  40.     /** The date. */
  41.     private final AbsoluteDate date;

  43.     /** Builds a PVCoordinates pair.
  44.      * @param date coordinates date
  45.      * @param position the position vector (m)
  46.      * @param velocity the velocity vector (m/s)
  47.      * @param acceleration the acceleration vector (m/s²)
  48.      */
  49.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  50.                                          final FieldVector3D<T> position,
  51.                                          final FieldVector3D<T> velocity,
  52.                                          final FieldVector3D<T> acceleration) {
  53.         super(position, velocity, acceleration);
  54.         this.date = date;
  55.     }

  57.     /** Multiplicative constructor
  58.      * <p>Build a PVCoordinates from another one and a scale factor.</p>
  59.      * <p>The PVCoordinates built will be a * pv</p>
  60.      * @param date date of the built coordinates
  61.      * @param a scale factor
  62.      * @param pv base (unscaled) PVCoordinates
  63.      */
  64.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  65.                                          final double a, final FieldPVCoordinates<T> pv) {
  66.         super(new FieldVector3D<T>(a, pv.getPosition()),
  67.               new FieldVector3D<T>(a, pv.getVelocity()),
  68.               new FieldVector3D<T>(a, pv.getAcceleration()));
  69.         this.date = date;
  70.     }

  72.     /** Multiplicative constructor
  73.      * <p>Build a PVCoordinates from another one and a scale factor.</p>
  74.      * <p>The PVCoordinates built will be a * pv</p>
  75.      * @param date date of the built coordinates
  76.      * @param a scale factor
  77.      * @param pv base (unscaled) PVCoordinates
  78.      */
  79.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  80.                                          final T a, final FieldPVCoordinates<T> pv) {
  81.         super(new FieldVector3D<T>(a, pv.getPosition()),
  82.               new FieldVector3D<T>(a, pv.getVelocity()),
  83.               new FieldVector3D<T>(a, pv.getAcceleration()));
  84.         this.date = date;
  85.     }

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

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

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

  135.     /** Linear constructor
  136.      * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
  137.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
  138.      * @param date date of the built coordinates
  139.      * @param a1 first scale factor
  140.      * @param pv1 first base (unscaled) PVCoordinates
  141.      * @param a2 second scale factor
  142.      * @param pv2 second base (unscaled) PVCoordinates
  143.      */
  144.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  145.                                          final T a1, final FieldPVCoordinates<T> pv1,
  146.                                          final T a2, final FieldPVCoordinates<T> pv2) {
  147.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition()),
  148.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity()),
  149.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration()));
  150.         this.date = date;
  151.     }

  153.     /** Linear constructor
  154.      * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
  155.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
  156.      * @param date date of the built coordinates
  157.      * @param a1 first scale factor
  158.      * @param pv1 first base (unscaled) PVCoordinates
  159.      * @param a2 second scale factor
  160.      * @param pv2 second base (unscaled) PVCoordinates
  161.      */
  162.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  163.                                          final T a1, final PVCoordinates pv1,
  164.                                          final T a2, final PVCoordinates pv2) {
  165.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition()),
  166.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity()),
  167.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration()));
  168.         this.date = date;
  169.     }

  171.     /** Linear constructor
  172.      * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
  173.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  174.      * @param date date of the built coordinates
  175.      * @param a1 first scale factor
  176.      * @param pv1 first base (unscaled) PVCoordinates
  177.      * @param a2 second scale factor
  178.      * @param pv2 second base (unscaled) PVCoordinates
  179.      * @param a3 third scale factor
  180.      * @param pv3 third base (unscaled) PVCoordinates
  181.      */
  182.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  183.                                          final double a1, final FieldPVCoordinates<T> pv1,
  184.                                          final double a2, final FieldPVCoordinates<T> pv2,
  185.                                          final double a3, final FieldPVCoordinates<T> pv3) {
  186.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition(),     a3, pv3.getPosition()),
  187.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),     a3, pv3.getVelocity()),
  188.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(), a3, pv3.getAcceleration()));
  189.         this.date = date;
  190.     }

  192.     /** Linear constructor
  193.      * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
  194.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  195.      * @param date date of the built coordinates
  196.      * @param a1 first scale factor
  197.      * @param pv1 first base (unscaled) PVCoordinates
  198.      * @param a2 second scale factor
  199.      * @param pv2 second base (unscaled) PVCoordinates
  200.      * @param a3 third scale factor
  201.      * @param pv3 third base (unscaled) PVCoordinates
  202.      */
  203.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  204.                                          final T a1, final FieldPVCoordinates<T> pv1,
  205.                                          final T a2, final FieldPVCoordinates<T> pv2,
  206.                                          final T a3, final FieldPVCoordinates<T> pv3) {
  207.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition(),     a3, pv3.getPosition()),
  208.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),     a3, pv3.getVelocity()),
  209.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(), a3, pv3.getAcceleration()));
  210.         this.date = date;
  211.     }

  213.     /** Linear constructor
  214.      * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
  215.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  216.      * @param date date of the built coordinates
  217.      * @param a1 first scale factor
  218.      * @param pv1 first base (unscaled) PVCoordinates
  219.      * @param a2 second scale factor
  220.      * @param pv2 second base (unscaled) PVCoordinates
  221.      * @param a3 third scale factor
  222.      * @param pv3 third base (unscaled) PVCoordinates
  223.      */
  224.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  225.                                          final T a1, final PVCoordinates pv1,
  226.                                          final T a2, final PVCoordinates pv2,
  227.                                          final T a3, final PVCoordinates pv3) {
  228.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition(),     a3, pv3.getPosition()),
  229.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),     a3, pv3.getVelocity()),
  230.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(), a3, pv3.getAcceleration()));
  231.         this.date = date;
  232.     }

  234.     /** Linear constructor
  235.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  236.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  237.      * @param date date of the built coordinates
  238.      * @param a1 first scale factor
  239.      * @param pv1 first base (unscaled) PVCoordinates
  240.      * @param a2 second scale factor
  241.      * @param pv2 second base (unscaled) PVCoordinates
  242.      * @param a3 third scale factor
  243.      * @param pv3 third base (unscaled) PVCoordinates
  244.      * @param a4 fourth scale factor
  245.      * @param pv4 fourth base (unscaled) PVCoordinates
  246.      */
  247.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  248.                                          final double a1, final FieldPVCoordinates<T> pv1,
  249.                                          final double a2, final FieldPVCoordinates<T> pv2,
  250.                                          final double a3, final FieldPVCoordinates<T> pv3,
  251.                                          final double a4, final FieldPVCoordinates<T> pv4) {
  252.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition(),
  253.                                    a3, pv3.getPosition(),     a4, pv4.getPosition()),
  254.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),
  255.                                    a3, pv3.getVelocity(),     a4, pv4.getVelocity()),
  256.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(),
  257.                                    a3, pv3.getAcceleration(), a4, pv4.getAcceleration()));
  258.         this.date = date;
  259.     }

  261.     /** Linear constructor
  262.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  263.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  264.      * @param date date of the built coordinates
  265.      * @param a1 first scale factor
  266.      * @param pv1 first base (unscaled) PVCoordinates
  267.      * @param a2 second scale factor
  268.      * @param pv2 second base (unscaled) PVCoordinates
  269.      * @param a3 third scale factor
  270.      * @param pv3 third base (unscaled) PVCoordinates
  271.      * @param a4 fourth scale factor
  272.      * @param pv4 fourth base (unscaled) PVCoordinates
  273.      */
  274.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  275.                                          final T a1, final FieldPVCoordinates<T> pv1,
  276.                                          final T a2, final FieldPVCoordinates<T> pv2,
  277.                                          final T a3, final FieldPVCoordinates<T> pv3,
  278.                                          final T a4, final FieldPVCoordinates<T> pv4) {
  279.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition(),
  280.                                    a3, pv3.getPosition(),     a4, pv4.getPosition()),
  281.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),
  282.                                    a3, pv3.getVelocity(),     a4, pv4.getVelocity()),
  283.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(),
  284.                                    a3, pv3.getAcceleration(), a4, pv4.getAcceleration()));
  285.         this.date = date;
  286.     }

  288.     /** Linear constructor
  289.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  290.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  291.      * @param date date of the built coordinates
  292.      * @param a1 first scale factor
  293.      * @param pv1 first base (unscaled) PVCoordinates
  294.      * @param a2 second scale factor
  295.      * @param pv2 second base (unscaled) PVCoordinates
  296.      * @param a3 third scale factor
  297.      * @param pv3 third base (unscaled) PVCoordinates
  298.      * @param a4 fourth scale factor
  299.      * @param pv4 fourth base (unscaled) PVCoordinates
  300.      */
  301.     public TimeStampedFieldPVCoordinates(final AbsoluteDate date,
  302.                                          final T a1, final PVCoordinates pv1,
  303.                                          final T a2, final PVCoordinates pv2,
  304.                                          final T a3, final PVCoordinates pv3,
  305.                                          final T a4, final PVCoordinates pv4) {
  306.         super(new FieldVector3D<T>(a1, pv1.getPosition(),     a2, pv2.getPosition(),
  307.                                    a3, pv3.getPosition(),     a4, pv4.getPosition()),
  308.               new FieldVector3D<T>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),
  309.                                    a3, pv3.getVelocity(),     a4, pv4.getVelocity()),
  310.               new FieldVector3D<T>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(),
  311.                                    a3, pv3.getAcceleration(), a4, pv4.getAcceleration()));
  312.         this.date = date;
  313.     }

  315.     /** {@inheritDoc} */
  316.     public AbsoluteDate getDate() {
  317.         return date;
  318.     }

  320.     /** Get a time-shifted state.
  321.      * <p>
  322.      * The state can be slightly shifted to close dates. This shift is based on
  323.      * a simple linear model. It is <em>not</em> intended as a replacement for
  324.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  325.      * for either small time shifts or coarse accuracy.
  326.      * </p>
  327.      * @param dt time shift in seconds
  328.      * @return a new state, shifted with respect to the instance (which is immutable)
  329.      */
  330.     public TimeStampedFieldPVCoordinates<T> shiftedBy(final double dt) {
  331.         final FieldPVCoordinates<T> spv = super.shiftedBy(dt);
  332.         return new TimeStampedFieldPVCoordinates<T>(date.shiftedBy(dt),
  333.                                                     spv.getPosition(), spv.getVelocity(), spv.getAcceleration());
  334.     }

  336.     /** Interpolate position-velocity.
  337.      * <p>
  338.      * The interpolated instance is created by polynomial Hermite interpolation
  339.      * ensuring velocity remains the exact derivative of position.
  340.      * </p>
  341.      * <p>
  342.      * Note that even if first time derivatives (velocities)
  343.      * from sample can be ignored, the interpolated instance always includes
  344.      * interpolated derivatives. This feature can be used explicitly to
  345.      * compute these derivatives when it would be too complex to compute them
  346.      * from an analytical formula: just compute a few sample points from the
  347.      * explicit formula and set the derivatives to zero in these sample points,
  348.      * then use interpolation to add derivatives consistent with the positions.
  349.      * </p>
  350.      * @param date interpolation date
  351.      * @param filter filter for derivatives from the sample to use in interpolation
  352.      * @param sample sample points on which interpolation should be done
  353.      * @param <T> the type of the field elements
  354.      * @return a new position-velocity, interpolated at specified date
  355.      */
  356.     @SuppressWarnings("unchecked")
  357.     public static <T extends RealFieldElement<T>>
  358.         TimeStampedFieldPVCoordinates<T> interpolate(final AbsoluteDate date,
  359.                                                      final CartesianDerivativesFilter filter,
  360.                                                      final Collection<TimeStampedFieldPVCoordinates<T>> sample) {

  362.         // get field properties
  363.         final T prototype = sample.iterator().next().getPosition().getX();
  364.         final T zero      = prototype.getField().getZero();

  366.         // set up an interpolator taking derivatives into account
  367.         final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<T>();

  369.         // add sample points
  370.         switch (filter) {
  371.             case USE_P :
  372.                 // populate sample with position data, ignoring velocity
  373.                 for (final TimeStampedFieldPVCoordinates<T> datedPV : sample) {
  374.                     final FieldVector3D<T> position = datedPV.getPosition();
  375.                     interpolator.addSamplePoint(zero.add(datedPV.getDate().durationFrom(date)),
  376.                                                 position.toArray());
  377.                 }
  378.                 break;
  379.             case USE_PV :
  380.                 // populate sample with position and velocity data
  381.                 for (final TimeStampedFieldPVCoordinates<T> datedPV : sample) {
  382.                     final FieldVector3D<T> position = datedPV.getPosition();
  383.                     final FieldVector3D<T> velocity = datedPV.getVelocity();
  384.                     interpolator.addSamplePoint(zero.add(datedPV.getDate().durationFrom(date)),
  385.                                                 position.toArray(), velocity.toArray());
  386.                 }
  387.                 break;
  388.             case USE_PVA :
  389.                 // populate sample with position, velocity and acceleration data
  390.                 for (final TimeStampedFieldPVCoordinates<T> datedPV : sample) {
  391.                     final FieldVector3D<T> position     = datedPV.getPosition();
  392.                     final FieldVector3D<T> velocity     = datedPV.getVelocity();
  393.                     final FieldVector3D<T> acceleration = datedPV.getAcceleration();
  394.                     interpolator.addSamplePoint(zero.add(datedPV.getDate().durationFrom(date)),
  395.                                                 position.toArray(), velocity.toArray(), acceleration.toArray());
  396.                 }
  397.                 break;
  398.             default :
  399.                 // this should never happen
  400.                 throw new OrekitInternalError(null);
  401.         }

  403.         // interpolate
  404.         final T[][] p = interpolator.derivatives(zero, 2);

  406.         // build a new interpolated instance
  407.         return new TimeStampedFieldPVCoordinates<T>(date,
  408.                                                     new FieldVector3D<T>(p[0]),
  409.                                                     new FieldVector3D<T>(p[1]),
  410.                                                     new FieldVector3D<T>(p[2]));

  412.     }

  414.     /** Return a string representation of this position/velocity pair.
  415.      * @return string representation of this position/velocity pair
  416.      */
  417.     public String toString() {
  418.         final String comma = ", ";
  419.         return new StringBuffer().append('{').append(date).append(", P(").
  420.                                   append(getPosition().getX().getReal()).append(comma).
  421.                                   append(getPosition().getY().getReal()).append(comma).
  422.                                   append(getPosition().getZ().getReal()).append("), V(").
  423.                                   append(getVelocity().getX().getReal()).append(comma).
  424.                                   append(getVelocity().getY().getReal()).append(comma).
  425.                                   append(getVelocity().getZ().getReal()).append("), A(").
  426.                                   append(getAcceleration().getX().getReal()).append(comma).
  427.                                   append(getAcceleration().getY().getReal()).append(comma).
  428.                                   append(getAcceleration().getZ().getReal()).append(")}").toString();
  429.     }

  431. }
Created with JaCoCo 0.7.4.201502262128