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;

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

  44. import java.util.ArrayList;
  45. import java.util.Collection;
  46. import java.util.HashMap;
  47. import java.util.List;
  48. import java.util.Map;
  49. import java.util.Optional;

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

  64.     /**
  65.      * Output frame.
  66.      * <p><b>Must be inertial</b> if interpolating spacecraft states defined by orbit</p>
  67.      */
  68.     private final Frame outputFrame;

  70.     /** Orbit interpolator. */
  71.     private final TimeInterpolator<Orbit> orbitInterpolator;

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

  76.     /** Mass interpolator. */
  77.     private final TimeInterpolator<TimeStampedDouble> massInterpolator;

  79.     /** Attitude interpolator. */
  80.     private final TimeInterpolator<Attitude> attitudeInterpolator;

  82.     /** Additional state interpolator. */
  83.     private final TimeInterpolator<TimeStampedDouble> additionalStateInterpolator;

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

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

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

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

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

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

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

  301.     /**
  302.      * Constructor.
  303.      * <p>
  304.      * At least one interpolator for either orbit or absolute position-velocity-acceleration is needed. All the other
  305.      * interpolators can be left to null if the user do not want to interpolate these values.
  306.      * <p>
  307.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  308.      * error otherwise.
  309.      * <p>
  310.      * <b>BEWARE:</b> it is up to the user to check the consistency of input interpolators.
  311.      *
  312.      * @param interpolationPoints number of interpolation points
  313.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  314.      * @param outputFrame output frame (inertial if the user is planning to use the orbit interpolator)
  315.      * @param orbitInterpolator orbit interpolator (can be null if absPVAInterpolator is defined)
  316.      * @param absPVAInterpolator absolute position-velocity-acceleration (can be null if orbitInterpolator is defined)
  317.      * @param massInterpolator mass interpolator (can be null)
  318.      * @param attitudeInterpolator attitude interpolator (can be null)
  319.      * @param additionalStateInterpolator additional state interpolator (can be null)
  320.      *
  321.      * @since 12.0.1
  322.      */
  323.     public SpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  324.                                        final Frame outputFrame, final TimeInterpolator<Orbit> orbitInterpolator,
  325.                                        final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolator,
  326.                                        final TimeInterpolator<TimeStampedDouble> massInterpolator,
  327.                                        final TimeInterpolator<Attitude> attitudeInterpolator,
  328.                                        final TimeInterpolator<TimeStampedDouble> additionalStateInterpolator) {
  329.         super(interpolationPoints, extrapolationThreshold);
  330.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  331.         this.outputFrame                 = outputFrame;
  332.         this.orbitInterpolator           = orbitInterpolator;
  333.         this.absPVAInterpolator          = absPVAInterpolator;
  334.         this.massInterpolator            = massInterpolator;
  335.         this.attitudeInterpolator        = attitudeInterpolator;
  336.         this.additionalStateInterpolator = additionalStateInterpolator;
  337.     }

  339.     /**
  340.      * Check that an interpolator exist for given sample state definition.
  341.      *
  342.      * @param sample sample (non empty)
  343.      * @param orbitInterpolatorIsPresent flag defining if an orbit interpolator has been defined for this instance
  344.      * @param absPVInterpolatorIsPresent flag defining if an absolute position-velocity-acceleration interpolator has been
  345.      * defined for this instance
  346.      *
  347.      * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
  348.      * definition type
  349.      */
  350.     public static void checkSampleAndInterpolatorConsistency(final List<SpacecraftState> sample,
  351.                                                              final boolean orbitInterpolatorIsPresent,
  352.                                                              final boolean absPVInterpolatorIsPresent) {
  353.         // Get first state definition
  354.         final SpacecraftState earliestState = sample.get(0);

  356.         if (earliestState.isOrbitDefined() && !orbitInterpolatorIsPresent ||
  357.                 !earliestState.isOrbitDefined() && !absPVInterpolatorIsPresent) {
  358.             throw new OrekitIllegalArgumentException(OrekitMessages.WRONG_INTERPOLATOR_DEFINED_FOR_STATE_INTERPOLATION);
  359.         }
  360.     }

  362.     /**
  363.      * Check that all state are either orbit defined or based on absolute position-velocity-acceleration.
  364.      *
  365.      * @param states spacecraft state sample
  366.      */
  367.     public static void checkStatesDefinitionsConsistency(final List<SpacecraftState> states) {
  368.         // Check all states handle the same additional states and are defined the same way (orbit or absolute PVA)
  369.         final SpacecraftState s0               = states.get(0);
  370.         final boolean         s0IsOrbitDefined = s0.isOrbitDefined();
  371.         for (final SpacecraftState state : states) {
  372.             s0.ensureCompatibleAdditionalStates(state);
  373.             if (s0IsOrbitDefined != state.isOrbitDefined()) {
  374.                 throw new OrekitIllegalArgumentException(OrekitMessages.DIFFERENT_STATE_DEFINITION);
  375.             }
  376.         }
  377.     }

  379.     /**
  380.      * {@inheritDoc}
  381.      * <p>
  382.      * The additional states that are interpolated are the ones already present in the first neighbor instance. The sample
  383.      * instances must therefore have at least the same additional states as this neighbor instance. They may have more
  384.      * additional states, but the extra ones will be ignored.
  385.      * <p>
  386.      * All the sample instances <em>must</em> be based on similar trajectory data, i.e. they must either all be based on
  387.      * orbits or all be based on absolute position-velocity-acceleration. Any inconsistency will trigger an
  388.      * {@link OrekitIllegalArgumentException}.
  389.      *
  390.      * @throws OrekitIllegalArgumentException if there are states defined by orbits and absolute
  391.      * position-velocity-acceleration coordinates
  392.      * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
  393.      * definition type
  394.      */
  395.     @Override
  396.     public SpacecraftState interpolate(final AbsoluteDate interpolationDate, final Collection<SpacecraftState> sample) {

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

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

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

  407.             // Check interpolator and sample consistency
  408.             checkSampleAndInterpolatorConsistency(sampleList, orbitInterpolator != null, absPVAInterpolator != null);
  409.         }

  411.         return super.interpolate(interpolationDate, sample);
  412.     }

  414.     /** {@inheritDoc} */
  415.     @Override
  416.     public List<TimeInterpolator<? extends TimeStamped>> getSubInterpolators() {

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

  421.         addOptionalSubInterpolatorIfDefined(orbitInterpolator, subInterpolators);
  422.         addOptionalSubInterpolatorIfDefined(absPVAInterpolator, subInterpolators);
  423.         addOptionalSubInterpolatorIfDefined(massInterpolator, subInterpolators);
  424.         addOptionalSubInterpolatorIfDefined(attitudeInterpolator, subInterpolators);
  425.         addOptionalSubInterpolatorIfDefined(additionalStateInterpolator, subInterpolators);

  427.         return subInterpolators;

  429.     }

  431.     /**
  432.      * {@inheritDoc}
  433.      */
  434.     @Override
  435.     protected SpacecraftState interpolate(final InterpolationData interpolationData) {

  437.         // Get first state definition
  438.         final List<SpacecraftState> samples   = interpolationData.getNeighborList();
  439.         final SpacecraftState earliestState   = samples.get(0);
  440.         final boolean         areOrbitDefined = earliestState.isOrbitDefined();

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

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

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

  451.         final List<DoubleArrayDictionary.Entry> additionalDotEntries =
  452.                 earliestState.getAdditionalStatesDerivatives().getData();
  453.         final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalDotSample =
  454.                 createAdditionalStateSample(additionalDotEntries);

  456.         // Fill interpolators with samples
  457.         final List<Orbit>                 orbitSample  = new ArrayList<>();
  458.         final List<AbsolutePVCoordinates> absPVASample = new ArrayList<>();
  459.         for (SpacecraftState state : samples) {
  460.             final AbsoluteDate currentDate = state.getDate();

  462.             // Add orbit sample if state is defined with an orbit
  463.             if (state.isOrbitDefined()) {
  464.                 orbitSample.add(state.getOrbit());
  465.             }
  466.             // Add absolute position-velocity-acceleration sample if state is defined with an absolute position-velocity-acceleration
  467.             else {
  468.                 absPVASample.add(state.getAbsPVA());
  469.             }

  471.             // Add mass sample
  472.             if (massInterpolator != null) {
  473.                 masses.add(new TimeStampedDouble(state.getMass(), state.getDate()));
  474.             }

  476.             // Add attitude sample if it is interpolated
  477.             if (attitudeInterpolator != null) {
  478.                 attitudes.add(state.getAttitude());
  479.             }

  481.             if (additionalStateInterpolator != null) {

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

  488.                 // Add all additional state derivative values if they are interpolated
  489.                 for (final Map.Entry<String, List<Pair<AbsoluteDate, double[]>>> entry : additionalDotSample.entrySet()) {
  490.                     entry.getValue().add(new Pair<>(currentDate, state.getAdditionalStateDerivative(entry.getKey())));
  491.                 }
  492.             }
  493.         }

  495.         // Interpolate mass
  496.         final AbsoluteDate interpolationDate = interpolationData.getInterpolationDate();
  497.         final double       interpolatedMass;
  498.         if (massInterpolator != null) {
  499.             interpolatedMass = massInterpolator.interpolate(interpolationDate, masses).getValue();
  500.         } else {
  501.             interpolatedMass = SpacecraftState.DEFAULT_MASS;
  502.         }

  504.         // Interpolate additional states and derivatives
  505.         final DoubleArrayDictionary interpolatedAdditional;
  506.         final DoubleArrayDictionary interpolatedAdditionalDot;
  507.         if (additionalStateInterpolator != null) {
  508.             interpolatedAdditional    = interpolateAdditionalState(interpolationDate, additionalSample);
  509.             interpolatedAdditionalDot = interpolateAdditionalState(interpolationDate, additionalDotSample);
  510.         } else {
  511.             interpolatedAdditional    = null;
  512.             interpolatedAdditionalDot = null;
  513.         }

  515.         // Interpolate orbit
  516.         if (areOrbitDefined && orbitInterpolator != null) {
  517.             final Orbit interpolatedOrbit = orbitInterpolator.interpolate(interpolationDate, orbitSample);

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

  521.             return new SpacecraftState(interpolatedOrbit, interpolatedAttitude, interpolatedMass, interpolatedAdditional,
  522.                                        interpolatedAdditionalDot);
  523.         }
  524.         // Interpolate absolute position-velocity-acceleration
  525.         else if (!areOrbitDefined && absPVAInterpolator != null) {

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

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

  531.             return new SpacecraftState(interpolatedAbsPva, interpolatedAttitude, interpolatedMass, interpolatedAdditional,
  532.                                        interpolatedAdditionalDot);
  533.         }
  534.         // Should never happen
  535.         else {
  536.             throw new OrekitInternalError(null);
  537.         }

  539.     }

  541.     /**
  542.      * Get output frame.
  543.      *
  544.      * @return output frame
  545.      */
  546.     public Frame getOutputFrame() {
  547.         return outputFrame;
  548.     }

  550.     /**
  551.      * Get orbit interpolator.
  552.      *
  553.      * @return optional orbit interpolator
  554.      *
  555.      * @see Optional
  556.      */
  557.     public Optional<TimeInterpolator<Orbit>> getOrbitInterpolator() {
  558.         return Optional.ofNullable(orbitInterpolator);
  559.     }

  561.     /**
  562.      * Get absolute position-velocity-acceleration interpolator.
  563.      *
  564.      * @return optional absolute position-velocity-acceleration interpolator
  565.      *
  566.      * @see Optional
  567.      */
  568.     public Optional<TimeInterpolator<AbsolutePVCoordinates>> getAbsPVAInterpolator() {
  569.         return Optional.ofNullable(absPVAInterpolator);
  570.     }

  572.     /**
  573.      * Get mass interpolator.
  574.      *
  575.      * @return optional mass interpolator
  576.      *
  577.      * @see Optional
  578.      */
  579.     public Optional<TimeInterpolator<TimeStampedDouble>> getMassInterpolator() {
  580.         return Optional.ofNullable(massInterpolator);
  581.     }

  583.     /**
  584.      * Get attitude interpolator.
  585.      *
  586.      * @return optional attitude interpolator
  587.      *
  588.      * @see Optional
  589.      */
  590.     public Optional<TimeInterpolator<Attitude>> getAttitudeInterpolator() {
  591.         return Optional.ofNullable(attitudeInterpolator);
  592.     }

  594.     /**
  595.      * Get additional state interpolator.
  596.      *
  597.      * @return optional additional state interpolator
  598.      *
  599.      * @see Optional
  600.      */
  601.     public Optional<TimeInterpolator<TimeStampedDouble>> getAdditionalStateInterpolator() {
  602.         return Optional.ofNullable(additionalStateInterpolator);
  603.     }

  605.     /**
  606.      * Check that at least one interpolator is defined.
  607.      *
  608.      * @param orbitInterpolatorToCheck orbit interpolator
  609.      * @param absPVAInterpolatorToCheck absolute position-velocity-acceleration interpolator
  610.      */
  611.     private void checkAtLeastOneInterpolator(final TimeInterpolator<Orbit> orbitInterpolatorToCheck,
  612.                                              final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolatorToCheck) {
  613.         if (orbitInterpolatorToCheck == null && absPVAInterpolatorToCheck == null) {
  614.             throw new OrekitIllegalArgumentException(OrekitMessages.NO_INTERPOLATOR_FOR_STATE_DEFINITION);
  615.         }
  616.     }

  618.     /**
  619.      * Create empty samples for given additional entries.
  620.      *
  621.      * @param additionalEntries tabulated additional entries
  622.      *
  623.      * @return empty samples for given additional entries
  624.      */
  625.     private Map<String, List<Pair<AbsoluteDate, double[]>>> createAdditionalStateSample(
  626.             final List<DoubleArrayDictionary.Entry> additionalEntries) {
  627.         final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalSamples = new HashMap<>(additionalEntries.size());

  629.         for (final DoubleArrayDictionary.Entry entry : additionalEntries) {
  630.             additionalSamples.put(entry.getKey(), new ArrayList<>());
  631.         }

  633.         return additionalSamples;
  634.     }

  636.     /**
  637.      * Interpolate additional state values.
  638.      *
  639.      * @param interpolationDate interpolation date
  640.      * @param additionalSamples additional state samples
  641.      *
  642.      * @return interpolated additional state values
  643.      */
  644.     private DoubleArrayDictionary interpolateAdditionalState(final AbsoluteDate interpolationDate,
  645.                                                              final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalSamples) {
  646.         final DoubleArrayDictionary interpolatedAdditional;

  648.         if (additionalSamples.isEmpty()) {
  649.             interpolatedAdditional = null;
  650.         } else {
  651.             interpolatedAdditional = new DoubleArrayDictionary(additionalSamples.size());
  652.             for (final Map.Entry<String, List<Pair<AbsoluteDate, double[]>>> entry : additionalSamples.entrySet()) {

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

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

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

  664.                     // Create final index for lambda expression use
  665.                     final int currentIndex = i;

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

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

  673.                     // Interpolate
  674.                     currentInterpolatedAdditional[i] =
  675.                             additionalStateInterpolator.interpolate(interpolationDate, currentValueSample).getValue();
  676.                 }

  678.                 interpolatedAdditional.put(entry.getKey(), currentInterpolatedAdditional);
  679.             }
  680.         }
  681.         return interpolatedAdditional;
  682.     }

  684.     /**
  685.      * Interpolate attitude.
  686.      * <p>
  687.      * If no attitude interpolator were defined, create a default inertial provider with respect to the output frame.
  688.      *
  689.      * @param interpolationDate interpolation date
  690.      * @param attitudes attitudes sample
  691.      * @param pvProvider position-velocity-acceleration coordinates provider
  692.      *
  693.      * @return interpolated attitude if attitude interpolator is present, default attitude otherwise
  694.      */
  695.     private Attitude interpolateAttitude(final AbsoluteDate interpolationDate, final List<Attitude> attitudes,
  696.                                          final PVCoordinatesProvider pvProvider) {
  697.         if (attitudes.isEmpty()) {
  698.             final AttitudeProvider attitudeProvider = new FrameAlignedProvider(outputFrame);
  699.             return attitudeProvider.getAttitude(pvProvider, interpolationDate, outputFrame);
  700.         } else {
  701.             return attitudeInterpolator.interpolate(interpolationDate, attitudes);
  702.         }
  703.     }
  704. }
Created with JaCoCo 0.8.12.202403310830