SpacecraftStateInterpolator.java

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

  18. import org.hipparchus.util.Pair;
  19. import org.orekit.attitudes.Attitude;
  20. import org.orekit.attitudes.AttitudeInterpolator;
  21. import org.orekit.attitudes.AttitudeProvider;
  22. import org.orekit.attitudes.FrameAlignedProvider;
  23. import org.orekit.errors.OrekitIllegalArgumentException;
  24. import org.orekit.errors.OrekitInternalError;
  25. import org.orekit.errors.OrekitMessages;
  26. import org.orekit.frames.Frame;
  27. import org.orekit.orbits.Orbit;
  28. import org.orekit.orbits.OrbitHermiteInterpolator;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.time.AbstractTimeInterpolator;
  31. import org.orekit.time.TimeInterpolator;
  32. import org.orekit.time.TimeStamped;
  33. import org.orekit.time.TimeStampedDouble;
  34. import org.orekit.time.TimeStampedDoubleHermiteInterpolator;
  35. import org.orekit.utils.AbsolutePVCoordinates;
  36. import org.orekit.utils.AbsolutePVCoordinatesHermiteInterpolator;
  37. import org.orekit.utils.AngularDerivativesFilter;
  38. import org.orekit.utils.CartesianDerivativesFilter;
  39. import org.orekit.utils.DoubleArrayDictionary;
  40. import org.orekit.utils.PVCoordinatesProvider;
  41. import org.orekit.utils.TimeStampedAngularCoordinatesHermiteInterpolator;

  42. import java.util.ArrayList;
  43. import java.util.Collection;
  44. import java.util.HashMap;
  45. import java.util.List;
  46. import java.util.Map;
  47. import java.util.Optional;

  48. /**
  49.  * Generic class for spacecraft state interpolator.
  50.  * <p>
  51.  * The user can specify what interpolator to use for each attribute of the spacecraft state. However, at least one
  52.  * interpolator for either orbit or absolute position-velocity-acceleration is needed. All the other interpolators can be
  53.  * left to null if the user do not want to interpolate these values.
  54.  *
  55.  * @author Luc Maisonobe
  56.  * @author Vincent Cucchietti
  57.  * @see SpacecraftState
  58.  */
  59. public class SpacecraftStateInterpolator extends AbstractTimeInterpolator<SpacecraftState> {

  60.     /**
  61.      * Output frame.
  62.      * <p><b>Must be inertial</b> if interpolating spacecraft states defined by orbit</p>
  63.      */
  64.     private final Frame outputFrame;

  65.     /** Orbit interpolator. */
  66.     private final TimeInterpolator<Orbit> orbitInterpolator;

  67.     /** Absolute position-velocity-acceleration interpolator. */
  68.     private final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolator;

  69.     /** Mass interpolator. */
  70.     private final TimeInterpolator<TimeStampedDouble> massInterpolator;

  71.     /** Attitude interpolator. */
  72.     private final TimeInterpolator<Attitude> attitudeInterpolator;

  73.     /** Additional state interpolator. */
  74.     private final TimeInterpolator<TimeStampedDouble> additionalStateInterpolator;

  75.     /**
  76.      * Simplest constructor to create a default Hermite interpolator for every spacecraft state field.
  77.      * <p>
  78.      * The interpolators will have the following configuration :
  79.      * <ul>
  80.      *     <li>Same frame for coordinates and attitude </li>
  81.      *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
  82.      *     <li>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</li>
  83.      *     <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
  84.      *     <li>Use of angular and first time derivative for attitude interpolation</li>
  85.      * </ul>
  86.      * <p>
  87.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  88.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  89.      * phenomenon</a> and numerical problems (including NaN appearing).
  90.      * <p>
  91.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
  92.      * tabulated spacecraft states defined by orbit, will throw an error otherwise.
  93.      *
  94.      * @param outputFrame output frame
  95.      *
  96.      * @see AbstractTimeInterpolator
  97.      */
  98.     public SpacecraftStateInterpolator(final Frame outputFrame) {
  99.         this(DEFAULT_INTERPOLATION_POINTS, outputFrame);
  100.     }

  101.     /**
  102.      * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
  103.      * <p>
  104.      * The interpolators will have the following configuration :
  105.      * <ul>
  106.      *     <li>Same frame for coordinates and attitude </li>
  107.      *     <li>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</li>
  108.      *     <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
  109.      *     <li>Use of angular and first time derivative for attitude interpolation</li>
  110.      * </ul>
  111.      * <p>
  112.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  113.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  114.      * phenomenon</a> and numerical problems (including NaN appearing).
  115.      * <p>
  116.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
  117.      * tabulated spacecraft states defined by orbit, will throw an error otherwise.
  118.      *
  119.      * @param interpolationPoints number of interpolation points
  120.      * @param outputFrame output frame
  121.      *
  122.      * @see AbstractTimeInterpolator
  123.      */
  124.     public SpacecraftStateInterpolator(final int interpolationPoints, final Frame outputFrame) {
  125.         this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, outputFrame, outputFrame);
  126.     }

  127.     /**
  128.      * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
  129.      * <p>
  130.      * The interpolators will have the following configuration :
  131.      * <ul>
  132.      *     <li>Same frame for coordinates and attitude </li>
  133.      *     <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
  134.      *     <li>Use of angular and first time derivative for attitude interpolation</li>
  135.      * </ul>
  136.      * <p>
  137.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  138.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  139.      * phenomenon</a> and numerical problems (including NaN appearing).
  140.      * <p>
  141.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
  142.      * tabulated spacecraft states defined by orbit, will throw an error otherwise.
  143.      *
  144.      * @param interpolationPoints number of interpolation points
  145.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  146.      * @param outputFrame output frame
  147.      * @since 12.1
  148.      * @see AbstractTimeInterpolator
  149.      */
  150.     public SpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold, final Frame outputFrame) {
  151.         this(interpolationPoints, extrapolationThreshold, outputFrame, outputFrame);
  152.     }

  153.     /**
  154.      * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
  155.      * <p>
  156.      * The interpolators will have the following configuration :
  157.      * <ul>
  158.      *     <li>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</li>
  159.      *     <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
  160.      *     <li>Use of angular and first time derivative for attitude interpolation</li>
  161.      * </ul>
  162.      * <p>
  163.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  164.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  165.      * phenomenon</a> and numerical problems (including NaN appearing).
  166.      * <p>
  167.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
  168.      * tabulated spacecraft states defined by orbit, will throw an error otherwise.
  169.      *
  170.      * @param interpolationPoints number of interpolation points
  171.      * @param outputFrame output frame
  172.      * @param attitudeReferenceFrame reference frame from which attitude is defined
  173.      *
  174.      * @see AbstractTimeInterpolator
  175.      */
  176.     public SpacecraftStateInterpolator(final int interpolationPoints, final Frame outputFrame,
  177.                                        final Frame attitudeReferenceFrame) {
  178.         this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, outputFrame, attitudeReferenceFrame,
  179.              CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RR);
  180.     }

  181.     /**
  182.      * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
  183.      * <p>
  184.      * The interpolators will have the following configuration :
  185.      * <ul>
  186.      *     <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
  187.      *     <li>Use of angular and first time derivative for attitude interpolation</li>
  188.      * </ul>
  189.      * <p>
  190.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  191.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  192.      * phenomenon</a> and numerical problems (including NaN appearing).
  193.      * <p>
  194.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
  195.      * tabulated spacecraft states defined by orbit, will throw an error otherwise.
  196.      *
  197.      * @param interpolationPoints number of interpolation points
  198.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  199.      * @param outputFrame output frame
  200.      * @param attitudeReferenceFrame reference frame from which attitude is defined
  201.      */
  202.     public SpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  203.                                        final Frame outputFrame, final Frame attitudeReferenceFrame) {
  204.         this(interpolationPoints, extrapolationThreshold, outputFrame, attitudeReferenceFrame,
  205.              CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RR);
  206.     }

  207.     /**
  208.      * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
  209.      * <p>
  210.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  211.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  212.      * phenomenon</a> and numerical problems (including NaN appearing).
  213.      * <p>
  214.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
  215.      * tabulated spacecraft states defined by orbit, will throw an error otherwise.
  216.      *
  217.      * @param interpolationPoints number of interpolation points
  218.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  219.      * @param outputFrame output frame
  220.      * @param attitudeReferenceFrame reference frame from which attitude is defined
  221.      * @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
  222.      * @param angularFilter filter for derivatives from the sample to use in attitude interpolation
  223.      */
  224.     public SpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  225.                                        final Frame outputFrame, final Frame attitudeReferenceFrame,
  226.                                        final CartesianDerivativesFilter pvaFilter,
  227.                                        final AngularDerivativesFilter angularFilter) {
  228.         this(interpolationPoints, extrapolationThreshold, outputFrame,
  229.              new OrbitHermiteInterpolator(interpolationPoints, extrapolationThreshold, outputFrame, pvaFilter),
  230.              new AbsolutePVCoordinatesHermiteInterpolator(interpolationPoints, extrapolationThreshold, outputFrame,
  231.                                                           pvaFilter),
  232.              new TimeStampedDoubleHermiteInterpolator(interpolationPoints, extrapolationThreshold),
  233.              new AttitudeInterpolator(attitudeReferenceFrame,
  234.                                       new TimeStampedAngularCoordinatesHermiteInterpolator(interpolationPoints,
  235.                                                                                            extrapolationThreshold,
  236.                                                                                            angularFilter)),
  237.              new TimeStampedDoubleHermiteInterpolator(interpolationPoints, extrapolationThreshold));
  238.     }

  239.     /**
  240.      * Constructor with:
  241.      * <ul>
  242.      *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
  243.      *     <li>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</li>
  244.      * </ul>
  245.      * <p>
  246.      * At least one interpolator for either orbit or absolute position-velocity-acceleration is needed. All the other
  247.      * interpolators can be left to null if the user do not want to interpolate these values.
  248.      * <p>
  249.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  250.      * error otherwise.
  251.      * <p>
  252.      * <b>BEWARE:</b> it is up to the user to check the consistency of input interpolators.
  253.      *
  254.      * @param outputFrame output frame (inertial if the user is planning to use the orbit interpolator)
  255.      * @param orbitInterpolator orbit interpolator (can be null if absPVAInterpolator is defined)
  256.      * @param absPVAInterpolator absolute position-velocity-acceleration (can be null if orbitInterpolator is defined)
  257.      * @param massInterpolator mass interpolator (can be null)
  258.      * @param attitudeInterpolator attitude interpolator (can be null)
  259.      * @param additionalStateInterpolator additional state interpolator (can be null)
  260.      *
  261.      * @see AbstractTimeInterpolator
  262.      *
  263.      * @deprecated using this constructor may throw an exception if any given interpolator
  264.      * does not use {@link #DEFAULT_INTERPOLATION_POINTS} and {@link
  265.      * #DEFAULT_EXTRAPOLATION_THRESHOLD_SEC}. Use {@link #SpacecraftStateInterpolator(int,
  266.      * double, Frame, TimeInterpolator, TimeInterpolator, TimeInterpolator,
  267.      * TimeInterpolator, TimeInterpolator)} instead.
  268.      */
  269.     @Deprecated
  270.     public SpacecraftStateInterpolator(final Frame outputFrame, final TimeInterpolator<Orbit> orbitInterpolator,
  271.                                        final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolator,
  272.                                        final TimeInterpolator<TimeStampedDouble> massInterpolator,
  273.                                        final TimeInterpolator<Attitude> attitudeInterpolator,
  274.                                        final TimeInterpolator<TimeStampedDouble> additionalStateInterpolator) {
  275.         super(DEFAULT_INTERPOLATION_POINTS, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC);
  276.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  277.         this.outputFrame                 = outputFrame;
  278.         this.orbitInterpolator           = orbitInterpolator;
  279.         this.absPVAInterpolator          = absPVAInterpolator;
  280.         this.massInterpolator            = massInterpolator;
  281.         this.attitudeInterpolator        = attitudeInterpolator;
  282.         this.additionalStateInterpolator = additionalStateInterpolator;
  283.     }

  284.     /**
  285.      * Constructor.
  286.      * <p>
  287.      * At least one interpolator for either orbit or absolute position-velocity-acceleration is needed. All the other
  288.      * interpolators can be left to null if the user do not want to interpolate these values.
  289.      * <p>
  290.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  291.      * error otherwise.
  292.      * <p>
  293.      * <b>BEWARE:</b> it is up to the user to check the consistency of input interpolators.
  294.      *
  295.      * @param interpolationPoints number of interpolation points
  296.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  297.      * @param outputFrame output frame (inertial if the user is planning to use the orbit interpolator)
  298.      * @param orbitInterpolator orbit interpolator (can be null if absPVAInterpolator is defined)
  299.      * @param absPVAInterpolator absolute position-velocity-acceleration (can be null if orbitInterpolator is defined)
  300.      * @param massInterpolator mass interpolator (can be null)
  301.      * @param attitudeInterpolator attitude interpolator (can be null)
  302.      * @param additionalStateInterpolator additional state interpolator (can be null)
  303.      *
  304.      * @since 12.0.1
  305.      */
  306.     public SpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  307.                                        final Frame outputFrame, final TimeInterpolator<Orbit> orbitInterpolator,
  308.                                        final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolator,
  309.                                        final TimeInterpolator<TimeStampedDouble> massInterpolator,
  310.                                        final TimeInterpolator<Attitude> attitudeInterpolator,
  311.                                        final TimeInterpolator<TimeStampedDouble> additionalStateInterpolator) {
  312.         super(interpolationPoints, extrapolationThreshold);
  313.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  314.         this.outputFrame                 = outputFrame;
  315.         this.orbitInterpolator           = orbitInterpolator;
  316.         this.absPVAInterpolator          = absPVAInterpolator;
  317.         this.massInterpolator            = massInterpolator;
  318.         this.attitudeInterpolator        = attitudeInterpolator;
  319.         this.additionalStateInterpolator = additionalStateInterpolator;
  320.     }

  321.     /**
  322.      * Check that an interpolator exist for given sample state definition.
  323.      *
  324.      * @param sample sample (non empty)
  325.      * @param orbitInterpolatorIsPresent flag defining if an orbit interpolator has been defined for this instance
  326.      * @param absPVInterpolatorIsPresent flag defining if an absolute position-velocity-acceleration interpolator has been
  327.      * defined for this instance
  328.      *
  329.      * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
  330.      * definition type
  331.      */
  332.     public static void checkSampleAndInterpolatorConsistency(final List<SpacecraftState> sample,
  333.                                                              final boolean orbitInterpolatorIsPresent,
  334.                                                              final boolean absPVInterpolatorIsPresent) {
  335.         // Get first state definition
  336.         final SpacecraftState earliestState = sample.get(0);

  337.         if (earliestState.isOrbitDefined() && !orbitInterpolatorIsPresent ||
  338.                 !earliestState.isOrbitDefined() && !absPVInterpolatorIsPresent) {
  339.             throw new OrekitIllegalArgumentException(OrekitMessages.WRONG_INTERPOLATOR_DEFINED_FOR_STATE_INTERPOLATION);
  340.         }
  341.     }

  342.     /**
  343.      * Check that all state are either orbit defined or based on absolute position-velocity-acceleration.
  344.      *
  345.      * @param states spacecraft state sample
  346.      */
  347.     public static void checkStatesDefinitionsConsistency(final List<SpacecraftState> states) {
  348.         // Check all states handle the same additional states and are defined the same way (orbit or absolute PVA)
  349.         final SpacecraftState s0               = states.get(0);
  350.         final boolean         s0IsOrbitDefined = s0.isOrbitDefined();
  351.         for (final SpacecraftState state : states) {
  352.             s0.ensureCompatibleAdditionalStates(state);
  353.             if (s0IsOrbitDefined != state.isOrbitDefined()) {
  354.                 throw new OrekitIllegalArgumentException(OrekitMessages.DIFFERENT_STATE_DEFINITION);
  355.             }
  356.         }
  357.     }

  358.     /**
  359.      * {@inheritDoc}
  360.      * <p>
  361.      * The additional states that are interpolated are the ones already present in the first neighbor instance. The sample
  362.      * instances must therefore have at least the same additional states as this neighbor instance. They may have more
  363.      * additional states, but the extra ones will be ignored.
  364.      * <p>
  365.      * All the sample instances <em>must</em> be based on similar trajectory data, i.e. they must either all be based on
  366.      * orbits or all be based on absolute position-velocity-acceleration. Any inconsistency will trigger an
  367.      * {@link OrekitIllegalArgumentException}.
  368.      *
  369.      * @throws OrekitIllegalArgumentException if there are states defined by orbits and absolute
  370.      * position-velocity-acceleration coordinates
  371.      * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
  372.      * definition type
  373.      */
  374.     @Override
  375.     public SpacecraftState interpolate(final AbsoluteDate interpolationDate, final Collection<SpacecraftState> sample) {

  376.         final List<SpacecraftState> sampleList = new ArrayList<>(sample);

  377.         // If sample is empty, an error will be thrown in super method
  378.         if (!sample.isEmpty()) {

  379.             // Check given that given states definition are consistent
  380.             // (all defined by either orbits or absolute position-velocity-acceleration coordinates)
  381.             checkStatesDefinitionsConsistency(sampleList);

  382.             // Check interpolator and sample consistency
  383.             checkSampleAndInterpolatorConsistency(sampleList, orbitInterpolator != null, absPVAInterpolator != null);
  384.         }

  385.         return super.interpolate(interpolationDate, sample);
  386.     }

  387.     /** {@inheritDoc} */
  388.     @Override
  389.     public List<TimeInterpolator<? extends TimeStamped>> getSubInterpolators() {

  390.         // Add all sub interpolators that are defined
  391.         final List<TimeInterpolator<? extends TimeStamped>> subInterpolators = new ArrayList<>();

  392.         addOptionalSubInterpolatorIfDefined(orbitInterpolator, subInterpolators);
  393.         addOptionalSubInterpolatorIfDefined(absPVAInterpolator, subInterpolators);
  394.         addOptionalSubInterpolatorIfDefined(massInterpolator, subInterpolators);
  395.         addOptionalSubInterpolatorIfDefined(attitudeInterpolator, subInterpolators);
  396.         addOptionalSubInterpolatorIfDefined(additionalStateInterpolator, subInterpolators);

  397.         return subInterpolators;

  398.     }

  399.     /**
  400.      * {@inheritDoc}
  401.      */
  402.     @Override
  403.     protected SpacecraftState interpolate(final InterpolationData interpolationData) {

  404.         // Get first state definition
  405.         final List<SpacecraftState> samples   = interpolationData.getNeighborList();
  406.         final SpacecraftState earliestState   = samples.get(0);
  407.         final boolean         areOrbitDefined = earliestState.isOrbitDefined();

  408.         // Prepare samples
  409.         final List<Attitude> attitudes = new ArrayList<>();

  410.         final List<TimeStampedDouble> masses = new ArrayList<>();

  411.         final List<DoubleArrayDictionary.Entry> additionalEntries = earliestState.getAdditionalStatesValues().getData();
  412.         final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalSample =
  413.                 createAdditionalStateSample(additionalEntries);

  414.         final List<DoubleArrayDictionary.Entry> additionalDotEntries =
  415.                 earliestState.getAdditionalStatesDerivatives().getData();
  416.         final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalDotSample =
  417.                 createAdditionalStateSample(additionalDotEntries);

  418.         // Fill interpolators with samples
  419.         final List<Orbit>                 orbitSample  = new ArrayList<>();
  420.         final List<AbsolutePVCoordinates> absPVASample = new ArrayList<>();
  421.         for (SpacecraftState state : samples) {
  422.             final AbsoluteDate currentDate = state.getDate();

  423.             // Add orbit sample if state is defined with an orbit
  424.             if (state.isOrbitDefined()) {
  425.                 orbitSample.add(state.getOrbit());
  426.             }
  427.             // Add absolute position-velocity-acceleration sample if state is defined with an absolute position-velocity-acceleration
  428.             else {
  429.                 absPVASample.add(state.getAbsPVA());
  430.             }

  431.             // Add mass sample
  432.             if (massInterpolator != null) {
  433.                 masses.add(new TimeStampedDouble(state.getMass(), state.getDate()));
  434.             }

  435.             // Add attitude sample if it is interpolated
  436.             if (attitudeInterpolator != null) {
  437.                 attitudes.add(state.getAttitude());
  438.             }

  439.             if (additionalStateInterpolator != null) {

  440.                 // Add all additional state values if they are interpolated
  441.                 for (final Map.Entry<String, List<Pair<AbsoluteDate, double[]>>> entry : additionalSample.entrySet()) {
  442.                     entry.getValue().add(new Pair<>(currentDate, state.getAdditionalState(entry.getKey())));
  443.                 }

  444.                 // Add all additional state derivative values if they are interpolated
  445.                 for (final Map.Entry<String, List<Pair<AbsoluteDate, double[]>>> entry : additionalDotSample.entrySet()) {
  446.                     entry.getValue().add(new Pair<>(currentDate, state.getAdditionalStateDerivative(entry.getKey())));
  447.                 }
  448.             }
  449.         }

  450.         // Interpolate mass
  451.         final AbsoluteDate interpolationDate = interpolationData.getInterpolationDate();
  452.         final double       interpolatedMass;
  453.         if (massInterpolator != null) {
  454.             interpolatedMass = massInterpolator.interpolate(interpolationDate, masses).getValue();
  455.         } else {
  456.             interpolatedMass = SpacecraftState.DEFAULT_MASS;
  457.         }

  458.         // Interpolate additional states and derivatives
  459.         final DoubleArrayDictionary interpolatedAdditional;
  460.         final DoubleArrayDictionary interpolatedAdditionalDot;
  461.         if (additionalStateInterpolator != null) {
  462.             interpolatedAdditional    = interpolateAdditionalState(interpolationDate, additionalSample);
  463.             interpolatedAdditionalDot = interpolateAdditionalState(interpolationDate, additionalDotSample);
  464.         } else {
  465.             interpolatedAdditional    = null;
  466.             interpolatedAdditionalDot = null;
  467.         }

  468.         // Interpolate orbit
  469.         if (areOrbitDefined && orbitInterpolator != null) {
  470.             final Orbit interpolatedOrbit = orbitInterpolator.interpolate(interpolationDate, orbitSample);

  471.             final Attitude interpolatedAttitude = interpolateAttitude(interpolationDate, attitudes, interpolatedOrbit);

  472.             return new SpacecraftState(interpolatedOrbit, interpolatedAttitude, interpolatedMass, interpolatedAdditional,
  473.                                        interpolatedAdditionalDot);
  474.         }
  475.         // Interpolate absolute position-velocity-acceleration
  476.         else if (!areOrbitDefined && absPVAInterpolator != null) {

  477.             final AbsolutePVCoordinates interpolatedAbsPva = absPVAInterpolator.interpolate(interpolationDate, absPVASample);

  478.             final Attitude interpolatedAttitude = interpolateAttitude(interpolationDate, attitudes, interpolatedAbsPva);

  479.             return new SpacecraftState(interpolatedAbsPva, interpolatedAttitude, interpolatedMass, interpolatedAdditional,
  480.                                        interpolatedAdditionalDot);
  481.         }
  482.         // Should never happen
  483.         else {
  484.             throw new OrekitInternalError(null);
  485.         }

  486.     }

  487.     /**
  488.      * Get output frame.
  489.      *
  490.      * @return output frame
  491.      */
  492.     public Frame getOutputFrame() {
  493.         return outputFrame;
  494.     }

  495.     /**
  496.      * Get orbit interpolator.
  497.      *
  498.      * @return optional orbit interpolator
  499.      *
  500.      * @see Optional
  501.      */
  502.     public Optional<TimeInterpolator<Orbit>> getOrbitInterpolator() {
  503.         return Optional.ofNullable(orbitInterpolator);
  504.     }

  505.     /**
  506.      * Get absolute position-velocity-acceleration interpolator.
  507.      *
  508.      * @return optional absolute position-velocity-acceleration interpolator
  509.      *
  510.      * @see Optional
  511.      */
  512.     public Optional<TimeInterpolator<AbsolutePVCoordinates>> getAbsPVAInterpolator() {
  513.         return Optional.ofNullable(absPVAInterpolator);
  514.     }

  515.     /**
  516.      * Get mass interpolator.
  517.      *
  518.      * @return optional mass interpolator
  519.      *
  520.      * @see Optional
  521.      */
  522.     public Optional<TimeInterpolator<TimeStampedDouble>> getMassInterpolator() {
  523.         return Optional.ofNullable(massInterpolator);
  524.     }

  525.     /**
  526.      * Get attitude interpolator.
  527.      *
  528.      * @return optional attitude interpolator
  529.      *
  530.      * @see Optional
  531.      */
  532.     public Optional<TimeInterpolator<Attitude>> getAttitudeInterpolator() {
  533.         return Optional.ofNullable(attitudeInterpolator);
  534.     }

  535.     /**
  536.      * Get additional state interpolator.
  537.      *
  538.      * @return optional additional state interpolator
  539.      *
  540.      * @see Optional
  541.      */
  542.     public Optional<TimeInterpolator<TimeStampedDouble>> getAdditionalStateInterpolator() {
  543.         return Optional.ofNullable(additionalStateInterpolator);
  544.     }

  545.     /**
  546.      * Check that at least one interpolator is defined.
  547.      *
  548.      * @param orbitInterpolatorToCheck orbit interpolator
  549.      * @param absPVAInterpolatorToCheck absolute position-velocity-acceleration interpolator
  550.      */
  551.     private void checkAtLeastOneInterpolator(final TimeInterpolator<Orbit> orbitInterpolatorToCheck,
  552.                                              final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolatorToCheck) {
  553.         if (orbitInterpolatorToCheck == null && absPVAInterpolatorToCheck == null) {
  554.             throw new OrekitIllegalArgumentException(OrekitMessages.NO_INTERPOLATOR_FOR_STATE_DEFINITION);
  555.         }
  556.     }

  557.     /**
  558.      * Create empty samples for given additional entries.
  559.      *
  560.      * @param additionalEntries tabulated additional entries
  561.      *
  562.      * @return empty samples for given additional entries
  563.      */
  564.     private Map<String, List<Pair<AbsoluteDate, double[]>>> createAdditionalStateSample(
  565.             final List<DoubleArrayDictionary.Entry> additionalEntries) {
  566.         final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalSamples = new HashMap<>(additionalEntries.size());

  567.         for (final DoubleArrayDictionary.Entry entry : additionalEntries) {
  568.             additionalSamples.put(entry.getKey(), new ArrayList<>());
  569.         }

  570.         return additionalSamples;
  571.     }

  572.     /**
  573.      * Interpolate additional state values.
  574.      *
  575.      * @param interpolationDate interpolation date
  576.      * @param additionalSamples additional state samples
  577.      *
  578.      * @return interpolated additional state values
  579.      */
  580.     private DoubleArrayDictionary interpolateAdditionalState(final AbsoluteDate interpolationDate,
  581.                                                              final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalSamples) {
  582.         final DoubleArrayDictionary interpolatedAdditional;

  583.         if (additionalSamples.isEmpty()) {
  584.             interpolatedAdditional = null;
  585.         } else {
  586.             interpolatedAdditional = new DoubleArrayDictionary(additionalSamples.size());
  587.             for (final Map.Entry<String, List<Pair<AbsoluteDate, double[]>>> entry : additionalSamples.entrySet()) {

  588.                 // Get current entry
  589.                 final List<Pair<AbsoluteDate, double[]>> currentAdditionalSamples = entry.getValue();

  590.                 // Extract number of values for this specific entry
  591.                 final int nbOfValues = currentAdditionalSamples.get(0).getValue().length;

  592.                 // For each value of current additional state entry
  593.                 final double[] currentInterpolatedAdditional = new double[nbOfValues];
  594.                 for (int i = 0; i < nbOfValues; i++) {

  595.                     // Create final index for lambda expression use
  596.                     final int currentIndex = i;

  597.                     // Create sample for specific value of current additional state values
  598.                     final List<TimeStampedDouble> currentValueSample = new ArrayList<>();

  599.                     currentAdditionalSamples.forEach(currentSamples -> currentValueSample.add(
  600.                             new TimeStampedDouble(currentSamples.getValue()[currentIndex], currentSamples.getFirst())));

  601.                     // Interpolate
  602.                     currentInterpolatedAdditional[i] =
  603.                             additionalStateInterpolator.interpolate(interpolationDate, currentValueSample).getValue();
  604.                 }

  605.                 interpolatedAdditional.put(entry.getKey(), currentInterpolatedAdditional);
  606.             }
  607.         }
  608.         return interpolatedAdditional;
  609.     }

  610.     /**
  611.      * Interpolate attitude.
  612.      * <p>
  613.      * If no attitude interpolator were defined, create a default inertial provider with respect to the output frame.
  614.      *
  615.      * @param interpolationDate interpolation date
  616.      * @param attitudes attitudes sample
  617.      * @param pvProvider position-velocity-acceleration coordinates provider
  618.      *
  619.      * @return interpolated attitude if attitude interpolator is present, default attitude otherwise
  620.      */
  621.     private Attitude interpolateAttitude(final AbsoluteDate interpolationDate, final List<Attitude> attitudes,
  622.                                          final PVCoordinatesProvider pvProvider) {
  623.         if (attitudes.isEmpty()) {
  624.             final AttitudeProvider attitudeProvider = new FrameAlignedProvider(outputFrame);
  625.             return attitudeProvider.getAttitude(pvProvider, interpolationDate, outputFrame);
  626.         } else {
  627.             return attitudeInterpolator.interpolate(interpolationDate, attitudes);
  628.         }
  629.     }
  630. }