FieldSpacecraftStateInterpolator.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.CalculusFieldElement;
  20. import org.hipparchus.Field;
  21. import org.hipparchus.util.MathArrays;
  22. import org.hipparchus.util.Pair;
  23. import org.orekit.attitudes.AttitudeProvider;
  24. import org.orekit.attitudes.FieldAttitude;
  25. import org.orekit.attitudes.FieldAttitudeInterpolator;
  26. import org.orekit.attitudes.FrameAlignedProvider;
  27. import org.orekit.errors.OrekitIllegalArgumentException;
  28. import org.orekit.errors.OrekitInternalError;
  29. import org.orekit.errors.OrekitMessages;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.orbits.FieldOrbit;
  32. import org.orekit.orbits.FieldOrbitHermiteInterpolator;
  33. import org.orekit.time.AbstractFieldTimeInterpolator;
  34. import org.orekit.time.AbstractTimeInterpolator;
  35. import org.orekit.time.FieldAbsoluteDate;
  36. import org.orekit.time.FieldTimeInterpolator;
  37. import org.orekit.time.FieldTimeStamped;
  38. import org.orekit.time.TimeStampedField;
  39. import org.orekit.time.TimeStampedFieldHermiteInterpolator;
  40. import org.orekit.utils.AngularDerivativesFilter;
  41. import org.orekit.utils.CartesianDerivativesFilter;
  42. import org.orekit.utils.FieldAbsolutePVCoordinates;
  43. import org.orekit.utils.FieldAbsolutePVCoordinatesHermiteInterpolator;
  44. import org.orekit.utils.FieldArrayDictionary;
  45. import org.orekit.utils.FieldPVCoordinatesProvider;
  46. import org.orekit.utils.TimeStampedFieldAngularCoordinatesHermiteInterpolator;

  48. import java.util.ArrayList;
  49. import java.util.Collection;
  50. import java.util.HashMap;
  51. import java.util.List;
  52. import java.util.Map;
  53. import java.util.Optional;
  54. import java.util.stream.Collectors;

  56. /**
  57.  * Generic class for spacecraft state interpolator.
  58.  * <p>
  59.  * The user can specify what interpolator to use for each attribute of the spacecraft state.  However, at least one
  60.  * interpolator for either orbit or absolute position-velocity-acceleration is needed. All the other interpolators can be
  61.  * left to null if the user do not want to interpolate these values.
  62.  *
  63.  * @param <KK> type of the field element
  64.  *
  65.  * @author Luc Maisonobe
  66.  * @author Vincent Cucchietti
  67.  * @see SpacecraftState
  68.  */
  69. public class FieldSpacecraftStateInterpolator<KK extends CalculusFieldElement<KK>>
  70.         extends AbstractFieldTimeInterpolator<FieldSpacecraftState<KK>, KK> {

  72.     /**
  73.      * Output frame.
  74.      * <p><b>Must be inertial</b> if interpolating spacecraft states defined by orbit</p>
  75.      */
  76.     private final Frame outputFrame;

  78.     /** Orbit interpolator. */
  79.     private final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolator;

  81.     /** Absolute position-velocity-acceleration interpolator. */
  82.     private final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolator;

  84.     /** Mass interpolator. */
  85.     private final FieldTimeInterpolator<TimeStampedField<KK>, KK> massInterpolator;

  87.     /** Attitude interpolator. */
  88.     private final FieldTimeInterpolator<FieldAttitude<KK>, KK> attitudeInterpolator;

  90.     /** Additional state interpolator. */
  91.     private final FieldTimeInterpolator<TimeStampedField<KK>, KK> additionalStateInterpolator;

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

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

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

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

  248.         this(interpolationPoints, extrapolationThreshold, outputFrame,
  249.              new FieldOrbitHermiteInterpolator<>(interpolationPoints, extrapolationThreshold, outputFrame, pvaFilter),
  250.              new FieldAbsolutePVCoordinatesHermiteInterpolator<>(interpolationPoints, extrapolationThreshold, outputFrame,
  251.                                                                  pvaFilter),
  252.              new TimeStampedFieldHermiteInterpolator<>(interpolationPoints, extrapolationThreshold),
  253.              new FieldAttitudeInterpolator<>(attitudeReferenceFrame,
  254.                                              new TimeStampedFieldAngularCoordinatesHermiteInterpolator<KK>(
  255.                                                      interpolationPoints, extrapolationThreshold, angularFilter)),
  256.              new TimeStampedFieldHermiteInterpolator<>(interpolationPoints, extrapolationThreshold));
  257.     }

  259.     /**
  260.      * Constructor.
  261.      * <p>
  262.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  263.      * error otherwise.
  264.      *
  265.      * @param outputFrame output frame
  266.      * @param orbitInterpolator orbit interpolator
  267.      * @param absPVAInterpolator absolute position-velocity-acceleration
  268.      * @param massInterpolator mass interpolator
  269.      * @param attitudeInterpolator attitude interpolator
  270.      * @param additionalStateInterpolator additional state interpolator
  271.      *
  272.      * @deprecated using this constructor may throw an exception if any given interpolator
  273.      * does not use {@link #DEFAULT_INTERPOLATION_POINTS} and {@link
  274.      * #DEFAULT_EXTRAPOLATION_THRESHOLD_SEC}. Use {@link
  275.      * #FieldSpacecraftStateInterpolator(int, double, Frame, FieldTimeInterpolator,
  276.      * FieldTimeInterpolator, FieldTimeInterpolator, FieldTimeInterpolator,
  277.      * FieldTimeInterpolator)} instead.
  278.      */
  279.     @Deprecated
  280.     public FieldSpacecraftStateInterpolator(final Frame outputFrame,
  281.                                             final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolator,
  282.                                             final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolator,
  283.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> massInterpolator,
  284.                                             final FieldTimeInterpolator<FieldAttitude<KK>, KK> attitudeInterpolator,
  285.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> additionalStateInterpolator) {
  286.         super(DEFAULT_INTERPOLATION_POINTS, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC);
  287.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  288.         this.outputFrame                 = outputFrame;
  289.         this.orbitInterpolator           = orbitInterpolator;
  290.         this.absPVAInterpolator          = absPVAInterpolator;
  291.         this.massInterpolator            = massInterpolator;
  292.         this.attitudeInterpolator        = attitudeInterpolator;
  293.         this.additionalStateInterpolator = additionalStateInterpolator;
  294.     }

  296.     /**
  297.      * Constructor.
  298.      * <p>
  299.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  300.      * error otherwise.
  301.      *
  302.      * @param interpolationPoints number of interpolation points
  303.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  304.      * @param outputFrame output frame
  305.      * @param orbitInterpolator orbit interpolator
  306.      * @param absPVAInterpolator absolute position-velocity-acceleration
  307.      * @param massInterpolator mass interpolator
  308.      * @param attitudeInterpolator attitude interpolator
  309.      * @param additionalStateInterpolator additional state interpolator
  310.      */
  311.     public FieldSpacecraftStateInterpolator(final int interpolationPoints,
  312.                                             final double extrapolationThreshold,
  313.                                             final Frame outputFrame,
  314.                                             final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolator,
  315.                                             final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolator,
  316.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> massInterpolator,
  317.                                             final FieldTimeInterpolator<FieldAttitude<KK>, KK> attitudeInterpolator,
  318.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> additionalStateInterpolator) {
  319.         super(interpolationPoints, extrapolationThreshold);
  320.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  321.         this.outputFrame                 = outputFrame;
  322.         this.orbitInterpolator           = orbitInterpolator;
  323.         this.absPVAInterpolator          = absPVAInterpolator;
  324.         this.massInterpolator            = massInterpolator;
  325.         this.attitudeInterpolator        = attitudeInterpolator;
  326.         this.additionalStateInterpolator = additionalStateInterpolator;
  327.     }

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

  349.         final List<FieldSpacecraftState<KK>> sampleList = new ArrayList<>(sample);

  351.         // Convert to spacecraft state for consistency check
  352.         final List<SpacecraftState> nonFieldSampleList =
  353.                 sampleList.stream().map(FieldSpacecraftState::toSpacecraftState).collect(Collectors.toList());

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

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

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

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

  371.     /** {@inheritDoc} */
  372.     @Override
  373.     public List<FieldTimeInterpolator<? extends FieldTimeStamped<KK>, KK>> getSubInterpolators() {

  375.         // Add all sub interpolators that are defined
  376.         final List<FieldTimeInterpolator<? extends FieldTimeStamped<KK>, KK>> subInterpolators = new ArrayList<>();

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

  384.         return subInterpolators;
  385.     }

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

  393.         // Get interpolation date
  394.         final FieldAbsoluteDate<KK> interpolationDate = interpolationData.getInterpolationDate();

  396.         // Get first state definition
  397.         final FieldSpacecraftState<KK> earliestState   = interpolationData.getNeighborList().get(0);
  398.         final boolean                  areOrbitDefined = earliestState.isOrbitDefined();

  400.         // Prepare samples
  401.         final List<FieldAttitude<KK>> attitudes = new ArrayList<>();

  403.         final List<TimeStampedField<KK>> masses = new ArrayList<>();

  405.         final List<FieldArrayDictionary<KK>.Entry> additionalEntries =
  406.                 earliestState.getAdditionalStatesValues().getData();
  407.         final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalSample =
  408.                 createAdditionalStateSample(additionalEntries);

  410.         final List<FieldArrayDictionary<KK>.Entry> additionalDotEntries =
  411.                 earliestState.getAdditionalStatesDerivatives().getData();
  412.         final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalDotSample =
  413.                 createAdditionalStateSample(additionalDotEntries);

  415.         // Fill interpolators with samples
  416.         final List<FieldSpacecraftState<KK>>       samples      = interpolationData.getNeighborList();
  417.         final List<FieldOrbit<KK>>                 orbitSample  = new ArrayList<>();
  418.         final List<FieldAbsolutePVCoordinates<KK>> absPVASample = new ArrayList<>();
  419.         for (FieldSpacecraftState<KK> state : samples) {
  420.             final FieldAbsoluteDate<KK> currentDate = state.getDate();

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

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

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

  441.             if (additionalStateInterpolator != null) {

  443.                 // Add all additional state values if they are interpolated
  444.                 for (final Map.Entry<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> entry : additionalSample.entrySet()) {
  445.                     entry.getValue().add(new Pair<>(currentDate, state.getAdditionalState(entry.getKey())));
  446.                 }

  448.                 // Add all additional state derivative values if they are interpolated
  449.                 for (final Map.Entry<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> entry : additionalDotSample.entrySet()) {
  450.                     entry.getValue().add(new Pair<>(currentDate, state.getAdditionalStateDerivative(entry.getKey())));
  451.                 }
  452.             }

  454.         }

  456.         // Interpolate mass
  457.         final KK one = interpolationData.getOne();
  458.         final KK interpolatedMass;
  459.         if (massInterpolator != null) {
  460.             interpolatedMass = massInterpolator.interpolate(interpolationDate, masses).getValue();
  461.         }
  462.         else {
  463.             interpolatedMass = one.newInstance(SpacecraftState.DEFAULT_MASS);
  464.         }

  466.         // Interpolate additional states and derivatives
  467.         final FieldArrayDictionary<KK> interpolatedAdditional;
  468.         final FieldArrayDictionary<KK> interpolatedAdditionalDot;
  469.         if (additionalStateInterpolator != null) {
  470.             interpolatedAdditional    = interpolateAdditionalState(interpolationDate, additionalSample);
  471.             interpolatedAdditionalDot = interpolateAdditionalState(interpolationDate, additionalDotSample);
  472.         }
  473.         else {
  474.             interpolatedAdditional    = null;
  475.             interpolatedAdditionalDot = null;
  476.         }

  478.         // Interpolate orbit
  479.         if (areOrbitDefined && orbitInterpolator != null) {
  480.             final FieldOrbit<KK> interpolatedOrbit =
  481.                     orbitInterpolator.interpolate(interpolationDate, orbitSample);

  483.             final FieldAttitude<KK> interpolatedAttitude =
  484.                     interpolateAttitude(interpolationDate, attitudes, interpolatedOrbit);

  486.             return new FieldSpacecraftState<>(interpolatedOrbit, interpolatedAttitude, interpolatedMass,
  487.                                               interpolatedAdditional, interpolatedAdditionalDot);
  488.         }
  489.         // Interpolate absolute position-velocity-acceleration
  490.         else if (!areOrbitDefined && absPVAInterpolator != null) {

  492.             final FieldAbsolutePVCoordinates<KK> interpolatedAbsPva =
  493.                     absPVAInterpolator.interpolate(interpolationDate, absPVASample);

  495.             final FieldAttitude<KK> interpolatedAttitude =
  496.                     interpolateAttitude(interpolationDate, attitudes, interpolatedAbsPva);

  498.             return new FieldSpacecraftState<>(interpolatedAbsPva, interpolatedAttitude, interpolatedMass,
  499.                                               interpolatedAdditional, interpolatedAdditionalDot);
  500.         }
  501.         // Should never happen
  502.         else {
  503.             throw new OrekitInternalError(null);
  504.         }

  506.     }

  508.     /** Get output frame.
  509.      * @return output frame
  510.      */
  511.     public Frame getOutputFrame() {
  512.         return outputFrame;
  513.     }

  515.     /** Get orbit interpolator.
  516.      * @return optional orbit interpolator
  517.      *
  518.      * @see Optional
  519.      */
  520.     public Optional<FieldTimeInterpolator<FieldOrbit<KK>, KK>> getOrbitInterpolator() {
  521.         return Optional.ofNullable(orbitInterpolator);
  522.     }

  524.     /** Get absolute position-velocity-acceleration interpolator.
  525.      * @return optional absolute position-velocity-acceleration interpolator
  526.      *
  527.      * @see Optional
  528.      */
  529.     public Optional<FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK>> getAbsPVAInterpolator() {
  530.         return Optional.ofNullable(absPVAInterpolator);
  531.     }

  533.     /** Get mass interpolator.
  534.      * @return optional mass interpolator
  535.      *
  536.      * @see Optional
  537.      */
  538.     public Optional<FieldTimeInterpolator<TimeStampedField<KK>, KK>> getMassInterpolator() {
  539.         return Optional.ofNullable(massInterpolator);
  540.     }

  542.     /** Get attitude interpolator.
  543.      * @return optional attitude interpolator
  544.      *
  545.      * @see Optional
  546.      */
  547.     public Optional<FieldTimeInterpolator<FieldAttitude<KK>, KK>> getAttitudeInterpolator() {
  548.         return Optional.ofNullable(attitudeInterpolator);
  549.     }

  551.     /** Get additional state interpolator.
  552.      * @return optional additional state interpolator
  553.      *
  554.      * @see Optional
  555.      */
  556.     public Optional<FieldTimeInterpolator<TimeStampedField<KK>, KK>> getAdditionalStateInterpolator() {
  557.         return Optional.ofNullable(additionalStateInterpolator);
  558.     }

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

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

  585.         for (final FieldArrayDictionary<KK>.Entry entry : additionalEntries) {
  586.             additionalSamples.put(entry.getKey(), new ArrayList<>());
  587.         }

  589.         return additionalSamples;
  590.     }

  592.     /**
  593.      * Interpolate additional state values.
  594.      *
  595.      * @param interpolationDate interpolation date
  596.      * @param additionalSamples additional state samples
  597.      *
  598.      * @return interpolated additional state values
  599.      */
  600.     private FieldArrayDictionary<KK> interpolateAdditionalState(final FieldAbsoluteDate<KK> interpolationDate,
  601.                                                                 final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalSamples) {
  602.         final Field<KK> field = interpolationDate.getField();
  603.         final FieldArrayDictionary<KK> interpolatedAdditional;

  605.         if (additionalSamples.isEmpty()) {
  606.             interpolatedAdditional = null;
  607.         }
  608.         else {
  609.             interpolatedAdditional = new FieldArrayDictionary<>(field, additionalSamples.size());
  610.             for (final Map.Entry<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> entry : additionalSamples.entrySet()) {

  612.                 // Get current entry
  613.                 final List<Pair<FieldAbsoluteDate<KK>, KK[]>> currentAdditionalSamples = entry.getValue();

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

  618.                 // For each value of current additional state entry
  619.                 final KK[] currentInterpolatedAdditional = MathArrays.buildArray(field, nbOfValues);
  620.                 for (int i = 0; i < nbOfValues; i++) {

  622.                     // Create final index for lambda expression use
  623.                     final int currentIndex = i;

  625.                     // Create sample for specific value of current additional state values
  626.                     final List<TimeStampedField<KK>> currentValueSample = new ArrayList<>();

  628.                     currentAdditionalSamples.forEach(currentSamples -> currentValueSample.add(
  629.                             new TimeStampedField<>(currentSamples.getValue()[currentIndex], currentSamples.getFirst())));

  631.                     // Interpolate
  632.                     currentInterpolatedAdditional[i] =
  633.                             additionalStateInterpolator.interpolate(interpolationDate, currentValueSample).getValue();
  634.                 }

  636.                 interpolatedAdditional.put(entry.getKey(), currentInterpolatedAdditional);
  637.             }
  638.         }
  639.         return interpolatedAdditional;
  640.     }

  642.     /**
  643.      * Interpolate attitude.
  644.      * <p>
  645.      * If no attitude interpolator were defined, create a default inertial provider with respect to the output frame.
  646.      *
  647.      * @param interpolationDate interpolation date
  648.      * @param attitudes attitudes sample
  649.      * @param pvProvider position-velocity-acceleration coordinates provider
  650.      *
  651.      * @return interpolated attitude if attitude interpolator is present, default attitude otherwise
  652.      */
  653.     private FieldAttitude<KK> interpolateAttitude(final FieldAbsoluteDate<KK> interpolationDate,
  654.                                                   final List<FieldAttitude<KK>> attitudes,
  655.                                                   final FieldPVCoordinatesProvider<KK> pvProvider) {
  656.         if (attitudes.isEmpty()) {
  657.             final AttitudeProvider attitudeProvider = new FrameAlignedProvider(outputFrame);
  658.             return attitudeProvider.getAttitude(pvProvider, interpolationDate, outputFrame);
  659.         }
  660.         else {
  661.             return attitudeInterpolator.interpolate(interpolationDate, attitudes);
  662.         }
  663.     }
  664. }
Created with JaCoCo 0.8.12.202403310830