FieldAbsolutePVCoordinates.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;

  18. import java.util.stream.Stream;

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  21. import org.hipparchus.analysis.differentiation.FieldDerivative;
  22. import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.orekit.errors.OrekitException;
  25. import org.orekit.errors.OrekitIllegalArgumentException;
  26. import org.orekit.errors.OrekitInternalError;
  27. import org.orekit.errors.OrekitMessages;
  28. import org.orekit.frames.FieldTransform;
  29. import org.orekit.frames.Frame;
  30. import org.orekit.time.FieldAbsoluteDate;
  31. import org.orekit.time.FieldTimeInterpolable;
  32. import org.orekit.time.FieldTimeStamped;

  33. /** Field implementation of AbsolutePVCoordinates.
  34.  * @see AbsolutePVCoordinates
  35.  * @author Vincent Mouraux
  36.  */
  37. public class FieldAbsolutePVCoordinates<T extends CalculusFieldElement<T>> extends TimeStampedFieldPVCoordinates<T>
  38.     implements FieldTimeStamped<T>, FieldTimeInterpolable<FieldAbsolutePVCoordinates<T>, T>,
  39.                FieldPVCoordinatesProvider<T> {

  40.     /** Frame in which are defined the coordinates. */
  41.     private final Frame frame;

  42.     /** Build from position, velocity, acceleration.
  43.      * @param frame the frame in which the coordinates are defined
  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 FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate<T> date,
  50.                                  final FieldVector3D<T> position, final FieldVector3D<T> velocity, final FieldVector3D<T> acceleration) {
  51.         super(date, position, velocity, acceleration);
  52.         this.frame = frame;
  53.     }

  54.     /** Build from position and velocity. Acceleration is set to zero.
  55.      * @param frame the frame in which the coordinates are defined
  56.      * @param date coordinates date
  57.      * @param position the position vector (m)
  58.      * @param velocity the velocity vector (m/s)
  59.      */
  60.     public FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate<T> date,
  61.                                  final FieldVector3D<T> position,
  62.                                  final FieldVector3D<T> velocity) {
  63.         this(frame, date, position, velocity, FieldVector3D.getZero(date.getField()));
  64.     }

  65.     /** Build from frame, date and FieldPVA coordinates.
  66.      * @param frame the frame in which the coordinates are defined
  67.      * @param date date of the coordinates
  68.      * @param pva TimeStampedPVCoordinates
  69.      */
  70.     public FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate<T> date, final FieldPVCoordinates<T> pva) {
  71.         super(date, pva);
  72.         this.frame = frame;
  73.     }

  74.     /** Build from frame and TimeStampedFieldPVCoordinates.
  75.      * @param frame the frame in which the coordinates are defined
  76.      * @param pva TimeStampedFieldPVCoordinates
  77.      */
  78.     public FieldAbsolutePVCoordinates(final Frame frame, final TimeStampedFieldPVCoordinates<T> pva) {
  79.         super(pva.getDate(), pva);
  80.         this.frame = frame;
  81.     }

  82.     /** Multiplicative constructor
  83.      * <p>Build a FieldAbsolutePVCoordinates from another one and a scale factor.</p>
  84.      * <p>The TimeStampedFieldPVCoordinates built will be a * AbsPva</p>
  85.      * @param date date of the built coordinates
  86.      * @param a scale factor
  87.      * @param AbsPva base (unscaled) FieldAbsolutePVCoordinates
  88.      */
  89.     public FieldAbsolutePVCoordinates(final FieldAbsoluteDate<T> date,
  90.                                  final T a, final FieldAbsolutePVCoordinates<T> AbsPva) {
  91.         super(date, a, AbsPva);
  92.         this.frame = AbsPva.frame;
  93.     }

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

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

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

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

  177.     /** Builds a FieldAbsolutePVCoordinates triplet from  a {@link FieldVector3D}&lt;{@link DerivativeStructure}&gt;.
  178.      * <p>
  179.      * The vector components must have time as their only derivation parameter and
  180.      * have consistent derivation orders.
  181.      * </p>
  182.      * @param frame the frame in which the parameters are defined
  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 FieldDerivative<T, U>> FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate<T> date,
  188.                                                                         final FieldVector3D<U> p) {
  189.         super(date, p);
  190.         this.frame = frame;
  191.     }

  192.     /** Ensure that the frames from two FieldAbsolutePVCoordinates are identical.
  193.      * @param absPv1 first FieldAbsolutePVCoordinates
  194.      * @param absPv2 first FieldAbsolutePVCoordinates
  195.      * @param <T> the type of the field elements
  196.      * @throws OrekitIllegalArgumentException if frames are different
  197.      */
  198.     private static <T extends CalculusFieldElement<T>> void ensureIdenticalFrames(final FieldAbsolutePVCoordinates<T> absPv1, final FieldAbsolutePVCoordinates<T> absPv2)
  199.         throws OrekitIllegalArgumentException {
  200.         if (!absPv1.frame.equals(absPv2.frame)) {
  201.             throw new OrekitIllegalArgumentException(OrekitMessages.INCOMPATIBLE_FRAMES,
  202.                                                      absPv1.frame.getName(), absPv2.frame.getName());
  203.         }
  204.     }

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

  219.     /** Get a time-shifted state.
  220.      * <p>
  221.      * The state can be slightly shifted to close dates. This shift is based on
  222.      * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
  223.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  224.      * for either small time shifts or coarse accuracy.
  225.      * </p>
  226.      * @param dt time shift in seconds
  227.      * @return a new state, shifted with respect to the instance (which is immutable)
  228.      */
  229.     public FieldAbsolutePVCoordinates<T> shiftedBy(final double dt) {
  230.         final TimeStampedFieldPVCoordinates<T> spv = super.shiftedBy(dt);
  231.         return new FieldAbsolutePVCoordinates<>(frame, spv);
  232.     }

  233.     /** Create a local provider using simply Taylor expansion through {@link #shiftedBy(double)}.
  234.      * <p>
  235.      * The time evolution is based on a simple Taylor expansion. It is <em>not</em> intended as a
  236.      * replacement for proper orbit propagation (it is not even Keplerian!) but should be sufficient
  237.      * for either small time shifts or coarse accuracy.
  238.      * </p>
  239.      * @return provider based on Taylor expansion, for small time shifts around instance date
  240.      */
  241.     public FieldPVCoordinatesProvider<T> toTaylorProvider() {
  242.         return new FieldPVCoordinatesProvider<T>() {
  243.             /** {@inheritDoc} */
  244.             public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> d,  final Frame f) {
  245.                 final TimeStampedFieldPVCoordinates<T> shifted   = shiftedBy(d.durationFrom(getDate()));
  246.                 final FieldTransform<T>                transform = frame.getTransformTo(f, d);
  247.                 return transform.transformPVCoordinates(shifted);
  248.             }
  249.         };
  250.     }

  251.     /** Get the frame in which the coordinates are defined.
  252.      * @return frame in which the coordinates are defined
  253.      */
  254.     public Frame getFrame() {
  255.         return frame;
  256.     }

  257.     /** Get the TimeStampedFieldPVCoordinates.
  258.      * @return TimeStampedFieldPVCoordinates
  259.      */
  260.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates() {
  261.         return this;
  262.     }

  263.     /** Get the TimeStampedFieldPVCoordinates in a specified frame.
  264.      * @param outputFrame frame in which the position/velocity coordinates shall be computed
  265.      * @return TimeStampedFieldPVCoordinates
  266.      * @exception OrekitException if transformation between frames cannot be computed
  267.      * @see #getPVCoordinates()
  268.      */
  269.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final Frame outputFrame) {
  270.         // If output frame requested is the same as definition frame,
  271.         // PV coordinates are returned directly
  272.         if (outputFrame == frame) {
  273.             return getPVCoordinates();
  274.         }

  275.         // Else, PV coordinates are transformed to output frame
  276.         final FieldTransform<T> t = frame.getTransformTo(outputFrame, getDate());
  277.         return t.transformPVCoordinates(getPVCoordinates());
  278.     }

  279.     @Override
  280.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> otherDate, final Frame outputFrame) {
  281.         return shiftedBy(otherDate.durationFrom(getDate())).getPVCoordinates(outputFrame);
  282.     }

  283.     @Override
  284.     public FieldAbsolutePVCoordinates<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldAbsolutePVCoordinates<T>> sample) {
  285.         return interpolate(getFrame(), date, CartesianDerivativesFilter.USE_PVA, sample);
  286.     }

  287.     /** Interpolate position-velocity.
  288.      * <p>
  289.      * The interpolated instance is created by polynomial Hermite interpolation
  290.      * ensuring velocity remains the exact derivative of position.
  291.      * </p>
  292.      * <p>
  293.      * Note that even if first time derivatives (velocities)
  294.      * from sample can be ignored, the interpolated instance always includes
  295.      * interpolated derivatives. This feature can be used explicitly to
  296.      * compute these derivatives when it would be too complex to compute them
  297.      * from an analytical formula: just compute a few sample points from the
  298.      * explicit formula and set the derivatives to zero in these sample points,
  299.      * then use interpolation to add derivatives consistent with the positions.
  300.      * </p>
  301.      * @param frame frame for the interpolted instance
  302.      * @param date interpolation date
  303.      * @param filter filter for derivatives from the sample to use in interpolation
  304.      * @param sample sample points on which interpolation should be done
  305.      * @param <T> the type of the field elements
  306.      * @return a new position-velocity, interpolated at specified date
  307.      * @exception OrekitIllegalArgumentException if some elements in the sample do not
  308.      * have the same defining frame as other
  309.      */
  310.     public static <T extends CalculusFieldElement<T>> FieldAbsolutePVCoordinates<T> interpolate(final Frame frame, final FieldAbsoluteDate<T> date,
  311.                                                     final CartesianDerivativesFilter filter,
  312.                                                     final Stream<FieldAbsolutePVCoordinates<T>> sample) {


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

  315.         // add sample points
  316.         switch (filter) {
  317.             case USE_P :
  318.                 // populate sample with position data, ignoring velocity
  319.                 sample.forEach(pv -> {
  320.                     final FieldVector3D<T> position = pv.getPosition();
  321.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  322.                                                 position.toArray());
  323.                 });
  324.                 break;
  325.             case USE_PV :
  326.                 // populate sample with position and velocity data
  327.                 sample.forEach(pv -> {
  328.                     final FieldVector3D<T> position = pv.getPosition();
  329.                     final FieldVector3D<T> velocity = pv.getVelocity();
  330.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  331.                                                 position.toArray(), velocity.toArray());
  332.                 });
  333.                 break;
  334.             case USE_PVA :
  335.                 // populate sample with position, velocity and acceleration data
  336.                 sample.forEach(pv -> {
  337.                     final FieldVector3D<T> position     = pv.getPosition();
  338.                     final FieldVector3D<T> velocity     = pv.getVelocity();
  339.                     final FieldVector3D<T> acceleration = pv.getAcceleration();
  340.                     interpolator.addSamplePoint(pv.getDate().durationFrom(date),
  341.                                                 position.toArray(), velocity.toArray(), acceleration.toArray());
  342.                 });
  343.                 break;
  344.             default :
  345.                 // this should never happen
  346.                 throw new OrekitInternalError(null);
  347.         }

  348.         // interpolate
  349.         final T[][] p = interpolator.derivatives(date.getField().getZero(), 2);

  350.         // build a new interpolated instance
  351.         return new FieldAbsolutePVCoordinates<>(frame, date, new FieldVector3D<>(p[0]), new FieldVector3D<>(p[1]), new FieldVector3D<>(p[2]));
  352.     }

  353.     /**
  354.      * Converts to an AbsolutePVCoordinates instance.
  355.      * @return AbsolutePVCoordinates with same properties
  356.      */
  357.     public AbsolutePVCoordinates toAbsolutePVCoordinates() {
  358.         return new AbsolutePVCoordinates(frame, this.getDate()
  359.             .toAbsoluteDate(), this.getPVCoordinates().toPVCoordinates());
  360.     }
  361. }