SpacecraftStateInterpolator.java

  1. /* Copyright 2002-2025 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 java.util.Comparator;
  20. import org.hipparchus.util.Pair;
  21. import org.orekit.attitudes.Attitude;
  22. import org.orekit.attitudes.AttitudeInterpolator;
  23. import org.orekit.attitudes.AttitudeProvider;
  24. import org.orekit.attitudes.FrameAlignedProvider;
  25. import org.orekit.errors.OrekitIllegalArgumentException;
  26. import org.orekit.errors.OrekitInternalError;
  27. import org.orekit.errors.OrekitMessages;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.orbits.Orbit;
  30. import org.orekit.orbits.OrbitHermiteInterpolator;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.time.AbstractTimeInterpolator;
  33. import org.orekit.time.TimeInterpolator;
  34. import org.orekit.time.TimeStamped;
  35. import org.orekit.time.TimeStampedDouble;
  36. import org.orekit.time.TimeStampedDoubleHermiteInterpolator;
  37. import org.orekit.utils.AbsolutePVCoordinates;
  38. import org.orekit.utils.AbsolutePVCoordinatesHermiteInterpolator;
  39. import org.orekit.utils.AngularDerivativesFilter;
  40. import org.orekit.utils.CartesianDerivativesFilter;
  41. import org.orekit.utils.DoubleArrayDictionary;
  42. import org.orekit.utils.DataDictionary;
  43. import org.orekit.utils.PVCoordinatesProvider;
  44. import org.orekit.utils.TimeStampedAngularCoordinatesHermiteInterpolator;

  46. import java.util.ArrayList;
  47. import java.util.Collection;
  48. import java.util.HashMap;
  49. import java.util.List;
  50. import java.util.Map;
  51. import java.util.Optional;

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

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

  72.     /** Orbit interpolator. */
  73.     private final TimeInterpolator<Orbit> orbitInterpolator;

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

  78.     /** Mass interpolator. */
  79.     private final TimeInterpolator<TimeStampedDouble> massInterpolator;

  81.     /** Attitude interpolator. */
  82.     private final TimeInterpolator<Attitude> attitudeInterpolator;

  84.     /** Additional state interpolator. */
  85.     private final TimeInterpolator<TimeStampedDouble> additionalStateInterpolator;

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

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

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

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

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

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

  257.     /**
  258.      * Constructor.
  259.      * <p>
  260.      * At least one interpolator for either orbit or absolute position-velocity-acceleration is needed. All the other
  261.      * interpolators can be left to null if the user do not want to interpolate these values.
  262.      * <p>
  263.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  264.      * error otherwise.
  265.      * <p>
  266.      * <b>BEWARE:</b> it is up to the user to check the consistency of input interpolators.
  267.      *
  268.      * @param interpolationPoints number of interpolation points
  269.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  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.      * @since 12.0.1
  278.      */
  279.     public SpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  280.                                        final Frame outputFrame, final TimeInterpolator<Orbit> orbitInterpolator,
  281.                                        final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolator,
  282.                                        final TimeInterpolator<TimeStampedDouble> massInterpolator,
  283.                                        final TimeInterpolator<Attitude> attitudeInterpolator,
  284.                                        final TimeInterpolator<TimeStampedDouble> additionalStateInterpolator) {
  285.         super(interpolationPoints, extrapolationThreshold);
  286.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  287.         this.outputFrame                 = outputFrame;
  288.         this.orbitInterpolator           = orbitInterpolator;
  289.         this.absPVAInterpolator          = absPVAInterpolator;
  290.         this.massInterpolator            = massInterpolator;
  291.         this.attitudeInterpolator        = attitudeInterpolator;
  292.         this.additionalStateInterpolator = additionalStateInterpolator;
  293.     }

  295.     /**
  296.      * Check that an interpolator exist for given sample state definition.
  297.      *
  298.      * @param sample sample (non empty)
  299.      * @param orbitInterpolatorIsPresent flag defining if an orbit interpolator has been defined for this instance
  300.      * @param absPVInterpolatorIsPresent flag defining if an absolute position-velocity-acceleration interpolator has been
  301.      * defined for this instance
  302.      *
  303.      * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
  304.      * definition type
  305.      */
  306.     public static void checkSampleAndInterpolatorConsistency(final List<SpacecraftState> sample,
  307.                                                              final boolean orbitInterpolatorIsPresent,
  308.                                                              final boolean absPVInterpolatorIsPresent) {
  309.         // Get first state definition
  310.         final SpacecraftState earliestState = sample.get(0);

  312.         if (earliestState.isOrbitDefined() && !orbitInterpolatorIsPresent ||
  313.                 !earliestState.isOrbitDefined() && !absPVInterpolatorIsPresent) {
  314.             throw new OrekitIllegalArgumentException(OrekitMessages.WRONG_INTERPOLATOR_DEFINED_FOR_STATE_INTERPOLATION);
  315.         }
  316.     }

  318.     /**
  319.      * Check that all state are either orbit defined or based on absolute position-velocity-acceleration.
  320.      *
  321.      * @param states spacecraft state sample
  322.      */
  323.     public static void checkStatesDefinitionsConsistency(final List<SpacecraftState> states) {
  324.         // Check all states handle the same additional states and are defined the same way (orbit or absolute PVA)
  325.         final SpacecraftState s0               = states.get(0);
  326.         final boolean         s0IsOrbitDefined = s0.isOrbitDefined();
  327.         for (final SpacecraftState state : states) {
  328.             s0.ensureCompatibleAdditionalStates(state);
  329.             if (s0IsOrbitDefined != state.isOrbitDefined()) {
  330.                 throw new OrekitIllegalArgumentException(OrekitMessages.DIFFERENT_STATE_DEFINITION);
  331.             }
  332.         }
  333.     }

  335.     /**
  336.      * {@inheritDoc}
  337.      * <p>
  338.      * The additional states that are interpolated are the ones already present in the first neighbor instance. The sample
  339.      * instances must therefore have at least the same additional states as this neighbor instance. They may have more
  340.      * additional states, but the extra ones will be ignored.
  341.      * <p>
  342.      * All the sample instances <em>must</em> be based on similar trajectory data, i.e. they must either all be based on
  343.      * orbits or all be based on absolute position-velocity-acceleration. Any inconsistency will trigger an
  344.      * {@link OrekitIllegalArgumentException}.
  345.      *
  346.      * @throws OrekitIllegalArgumentException if there are states defined by orbits and absolute
  347.      * position-velocity-acceleration coordinates
  348.      * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
  349.      * definition type
  350.      */
  351.     @Override
  352.     public SpacecraftState interpolate(final AbsoluteDate interpolationDate, final Collection<SpacecraftState> sample) {

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

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

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

  363.             // Check interpolator and sample consistency
  364.             checkSampleAndInterpolatorConsistency(sampleList, orbitInterpolator != null, absPVAInterpolator != null);
  365.         }

  367.         return super.interpolate(interpolationDate, sample);
  368.     }

  370.     /** {@inheritDoc} */
  371.     @Override
  372.     public List<TimeInterpolator<? extends TimeStamped>> getSubInterpolators() {

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

  377.         addOptionalSubInterpolatorIfDefined(orbitInterpolator, subInterpolators);
  378.         addOptionalSubInterpolatorIfDefined(absPVAInterpolator, subInterpolators);
  379.         addOptionalSubInterpolatorIfDefined(massInterpolator, subInterpolators);
  380.         addOptionalSubInterpolatorIfDefined(attitudeInterpolator, subInterpolators);
  381.         addOptionalSubInterpolatorIfDefined(additionalStateInterpolator, subInterpolators);

  383.         return subInterpolators;

  385.     }

  387.     /**
  388.      * {@inheritDoc}
  389.      */
  390.     @Override
  391.     protected SpacecraftState interpolate(final InterpolationData interpolationData) {

  393.         // Get first state definition
  394.         final List<SpacecraftState> samples   = interpolationData.getNeighborList();
  395.         final SpacecraftState earliestState   = samples.get(0);
  396.         final boolean         areOrbitDefined = earliestState.isOrbitDefined();

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

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

  403.         final List<DataDictionary.Entry> additionalEntries = earliestState.getAdditionalDataValues().getData();
  404.         final Map<String, List<Pair<AbsoluteDate, Object>>> additionalSample =
  405.                 createAdditionalDataSample(additionalEntries);

  407.         final List<DoubleArrayDictionary.Entry> additionalDotEntries =
  408.                 earliestState.getAdditionalStatesDerivatives().getData();
  409.         final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalDotSample =
  410.                 createAdditionalStateSample(additionalDotEntries);

  412.         // Fill interpolators with samples
  413.         final List<Orbit>                 orbitSample  = new ArrayList<>();
  414.         final List<AbsolutePVCoordinates> absPVASample = new ArrayList<>();
  415.         for (SpacecraftState state : samples) {
  416.             final AbsoluteDate currentDate = state.getDate();

  418.             // Add orbit sample if state is defined with an orbit
  419.             if (state.isOrbitDefined()) {
  420.                 orbitSample.add(state.getOrbit());
  421.             }
  422.             // Add absolute position-velocity-acceleration sample if state is defined with an absolute position-velocity-acceleration
  423.             else {
  424.                 absPVASample.add(state.getAbsPVA());
  425.             }

  427.             // Add mass sample
  428.             if (massInterpolator != null) {
  429.                 masses.add(new TimeStampedDouble(state.getMass(), state.getDate()));
  430.             }

  432.             // Add attitude sample if it is interpolated
  433.             if (attitudeInterpolator != null) {
  434.                 attitudes.add(state.getAttitude());
  435.             }

  437.             if (additionalStateInterpolator != null) {

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

  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.         }

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

  460.         // Interpolate additional states and derivatives
  461.         final DataDictionary interpolatedAdditional;
  462.         final DoubleArrayDictionary interpolatedAdditionalDot;
  463.         if (additionalStateInterpolator != null) {
  464.             interpolatedAdditional    = interpolateAdditionalState(interpolationDate, additionalSample).orElse(null);
  465.             interpolatedAdditionalDot = interpolateAdditionalState(interpolationDate, additionalDotSample).map(DataDictionary::toDoubleDictionary).orElse(null);
  466.         } else {
  467.             interpolatedAdditional    = null;
  468.             interpolatedAdditionalDot = null;
  469.         }

  471.         // Interpolate orbit
  472.         if (areOrbitDefined && orbitInterpolator != null) {
  473.             final Orbit interpolatedOrbit = orbitInterpolator.interpolate(interpolationDate, orbitSample);

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

  477.             return new SpacecraftState(interpolatedOrbit, interpolatedAttitude, interpolatedMass, interpolatedAdditional,
  478.                                        interpolatedAdditionalDot);
  479.         }
  480.         // Interpolate absolute position-velocity-acceleration
  481.         else if (!areOrbitDefined && absPVAInterpolator != null) {

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

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

  487.             return new SpacecraftState(interpolatedAbsPva, interpolatedAttitude, interpolatedMass, interpolatedAdditional,
  488.                                        interpolatedAdditionalDot);
  489.         }
  490.         // Should never happen
  491.         else {
  492.             throw new OrekitInternalError(null);
  493.         }

  495.     }

  497.     /**
  498.      * Get output frame.
  499.      *
  500.      * @return output frame
  501.      */
  502.     public Frame getOutputFrame() {
  503.         return outputFrame;
  504.     }

  506.     /**
  507.      * Get orbit interpolator.
  508.      *
  509.      * @return optional orbit interpolator
  510.      *
  511.      * @see Optional
  512.      */
  513.     public Optional<TimeInterpolator<Orbit>> getOrbitInterpolator() {
  514.         return Optional.ofNullable(orbitInterpolator);
  515.     }

  517.     /**
  518.      * Get absolute position-velocity-acceleration interpolator.
  519.      *
  520.      * @return optional absolute position-velocity-acceleration interpolator
  521.      *
  522.      * @see Optional
  523.      */
  524.     public Optional<TimeInterpolator<AbsolutePVCoordinates>> getAbsPVAInterpolator() {
  525.         return Optional.ofNullable(absPVAInterpolator);
  526.     }

  528.     /**
  529.      * Get mass interpolator.
  530.      *
  531.      * @return optional mass interpolator
  532.      *
  533.      * @see Optional
  534.      */
  535.     public Optional<TimeInterpolator<TimeStampedDouble>> getMassInterpolator() {
  536.         return Optional.ofNullable(massInterpolator);
  537.     }

  539.     /**
  540.      * Get attitude interpolator.
  541.      *
  542.      * @return optional attitude interpolator
  543.      *
  544.      * @see Optional
  545.      */
  546.     public Optional<TimeInterpolator<Attitude>> getAttitudeInterpolator() {
  547.         return Optional.ofNullable(attitudeInterpolator);
  548.     }

  550.     /**
  551.      * Get additional state interpolator.
  552.      *
  553.      * @return optional additional state interpolator
  554.      *
  555.      * @see Optional
  556.      */
  557.     public Optional<TimeInterpolator<TimeStampedDouble>> getAdditionalStateInterpolator() {
  558.         return Optional.ofNullable(additionalStateInterpolator);
  559.     }

  561.     /**
  562.      * Check that at least one interpolator is defined.
  563.      *
  564.      * @param orbitInterpolatorToCheck orbit interpolator
  565.      * @param absPVAInterpolatorToCheck absolute position-velocity-acceleration interpolator
  566.      */
  567.     private void checkAtLeastOneInterpolator(final TimeInterpolator<Orbit> orbitInterpolatorToCheck,
  568.                                              final TimeInterpolator<AbsolutePVCoordinates> absPVAInterpolatorToCheck) {
  569.         if (orbitInterpolatorToCheck == null && absPVAInterpolatorToCheck == null) {
  570.             throw new OrekitIllegalArgumentException(OrekitMessages.NO_INTERPOLATOR_FOR_STATE_DEFINITION);
  571.         }
  572.     }

  574.     /**
  575.      * Create empty samples for given additional entries.
  576.      *
  577.      * @param additionalEntries tabulated additional entries
  578.      *
  579.      * @return empty samples for given additional entries
  580.      */
  581.     private Map<String, List<Pair<AbsoluteDate, double[]>>> createAdditionalStateSample(
  582.             final List<DoubleArrayDictionary.Entry> additionalEntries) {
  583.         final Map<String, List<Pair<AbsoluteDate, double[]>>> additionalSamples = new HashMap<>(additionalEntries.size());

  585.         for (final DoubleArrayDictionary.Entry entry : additionalEntries) {
  586.             additionalSamples.put(entry.getKey(), new ArrayList<>());
  587.         }
  588.         return additionalSamples;
  589.     }

  591.     /**
  592.      * Create empty samples for given additional entries.
  593.      *
  594.      * @param additionalEntries tabulated additional entries
  595.      *
  596.      * @return empty samples for given additional entries
  597.      */
  598.     private Map<String, List<Pair<AbsoluteDate, Object>>> createAdditionalDataSample(
  599.             final List<DataDictionary.Entry> additionalEntries) {
  600.         final Map<String, List<Pair<AbsoluteDate, Object>>> additionalSamples = new HashMap<>(additionalEntries.size());

  602.         for (final DataDictionary.Entry entry : additionalEntries) {
  603.             additionalSamples.put(entry.getKey(), new ArrayList<>());
  604.         }
  605.         return additionalSamples;
  606.     }

  608.     /**
  609.      * Interpolate additional values.
  610.      *  Double arrays are interpolated whereas objects values are kept identical until change.
  611.      *
  612.      * @param <T> type of the data
  613.      * @param interpolationDate interpolation date
  614.      * @param additionalSamples additional data samples
  615.      *
  616.      * @return interpolated additional data values
  617.      */
  618.     private <T> Optional<DataDictionary> interpolateAdditionalState(final AbsoluteDate interpolationDate,
  619.                                                                     final Map<String, List<Pair<AbsoluteDate, T>>> additionalSamples) {
  620.         final Optional<DataDictionary> interpolatedAdditional;

  622.         if (additionalSamples.isEmpty()) {
  623.             interpolatedAdditional = Optional.empty();
  624.         } else {
  625.             interpolatedAdditional = Optional.of(new DataDictionary(additionalSamples.size()));
  626.             for (final Map.Entry<String, List<Pair<AbsoluteDate, T>>> entry : additionalSamples.entrySet()) {
  627.                 // Get current entry
  628.                 final List<Pair<AbsoluteDate, T>> currentAdditionalSamples = entry.getValue();
  629.                 final T currentInterpolatedAdditional;
  630.                 if (currentAdditionalSamples.get(0).getValue() instanceof double[]) {
  631.                     currentInterpolatedAdditional = (T) interpolateAdditionalSamples(interpolationDate, currentAdditionalSamples);
  632.                 } else {
  633.                     currentInterpolatedAdditional = currentAdditionalSamples.stream()
  634.                                                                             .filter(pair -> pair.getKey().isAfter(interpolationDate))
  635.                                                                             .min(Comparator.comparing(Pair::getKey))
  636.                                                                             .get()
  637.                                                                             .getValue();
  638.                 }
  639.                 interpolatedAdditional.get().put(entry.getKey(), currentInterpolatedAdditional);
  640.             }
  641.         }
  642.         return interpolatedAdditional;
  643.     }

  645.     private <T> double[] interpolateAdditionalSamples(final AbsoluteDate interpolationDate, final List<Pair<AbsoluteDate, T>> currentAdditionalSamples) {
  646.         // Extract number of values for this specific entry
  647.         final int nbOfValues = ((double[]) currentAdditionalSamples.get(0).getValue()).length;

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

  653.             // Create final index for lambda expression use
  654.             final int currentIndex = i;

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

  659.             currentAdditionalSamples.forEach(
  660.                     currentSamples -> currentValueSample.add(new TimeStampedDouble(((double[]) currentSamples.getValue())[currentIndex], currentSamples.getFirst())));

  662.             // Interpolate
  663.             currentInterpolatedAdditional[i] = additionalStateInterpolator.interpolate(interpolationDate, currentValueSample).getValue();
  664.         }
  665.         return currentInterpolatedAdditional;
  666.     }

  668.     /**
  669.      * Interpolate attitude.
  670.      * <p>
  671.      * If no attitude interpolator were defined, create a default inertial provider with respect to the output frame.
  672.      *
  673.      * @param interpolationDate interpolation date
  674.      * @param attitudes attitudes sample
  675.      * @param pvProvider position-velocity-acceleration coordinates provider
  676.      *
  677.      * @return interpolated attitude if attitude interpolator is present, default attitude otherwise
  678.      */
  679.     private Attitude interpolateAttitude(final AbsoluteDate interpolationDate, final List<Attitude> attitudes,
  680.                                          final PVCoordinatesProvider pvProvider) {
  681.         if (attitudes.isEmpty()) {
  682.             final AttitudeProvider attitudeProvider = new FrameAlignedProvider(outputFrame);
  683.             return attitudeProvider.getAttitude(pvProvider, interpolationDate, outputFrame);
  684.         } else {
  685.             return attitudeInterpolator.interpolate(interpolationDate, attitudes);
  686.         }
  687.     }
  688. }
Created with JaCoCo 0.8.12.202403310830