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>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</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 outputFrame output frame
  163.      * @param attitudeReferenceFrame reference frame from which attitude is defined
  164.      */
  165.     public FieldSpacecraftStateInterpolator(final int interpolationPoints, final Frame outputFrame,
  166.                                             final Frame attitudeReferenceFrame) {
  167.         this(interpolationPoints, AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
  168.              outputFrame, attitudeReferenceFrame);
  169.     }

  171.     /**
  172.      * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
  173.      * <p>
  174.      * The interpolators will have the following configuration :
  175.      * <ul>
  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 extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  189.      * @param outputFrame output frame
  190.      * @param attitudeReferenceFrame reference frame from which attitude is defined
  191.      */
  192.     public FieldSpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  193.                                             final Frame outputFrame, final Frame attitudeReferenceFrame) {
  194.         this(interpolationPoints, extrapolationThreshold, outputFrame, attitudeReferenceFrame,
  195.              CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RR);
  196.     }

  198.     /**
  199.      * Constructor.
  200.      * <p>
  201.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  202.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  203.      * phenomenon</a> and numerical problems (including NaN appearing).
  204.      * <p>
  205.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
  206.      * tabulated spacecraft states defined by orbit, will throw an error otherwise.
  207.      *
  208.      * @param interpolationPoints number of interpolation points
  209.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  210.      * @param outputFrame output frame
  211.      * @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
  212.      * @param attitudeReferenceFrame reference frame from which attitude is defined
  213.      * @param angularFilter filter for derivatives from the sample to use in attitude interpolation
  214.      */
  215.     public FieldSpacecraftStateInterpolator(final int interpolationPoints,
  216.                                             final double extrapolationThreshold,
  217.                                             final Frame outputFrame,
  218.                                             final Frame attitudeReferenceFrame,
  219.                                             final CartesianDerivativesFilter pvaFilter,
  220.                                             final AngularDerivativesFilter angularFilter) {

  222.         this(interpolationPoints, extrapolationThreshold, outputFrame,
  223.              new FieldOrbitHermiteInterpolator<>(interpolationPoints, extrapolationThreshold, outputFrame, pvaFilter),
  224.              new FieldAbsolutePVCoordinatesHermiteInterpolator<>(interpolationPoints, extrapolationThreshold, outputFrame,
  225.                                                                  pvaFilter),
  226.              new TimeStampedFieldHermiteInterpolator<>(interpolationPoints, extrapolationThreshold),
  227.              new FieldAttitudeInterpolator<>(attitudeReferenceFrame,
  228.                                              new TimeStampedFieldAngularCoordinatesHermiteInterpolator<KK>(
  229.                                                      interpolationPoints, extrapolationThreshold, angularFilter)),
  230.              new TimeStampedFieldHermiteInterpolator<>(interpolationPoints, extrapolationThreshold));
  231.     }

  233.     /**
  234.      * Constructor.
  235.      * <p>
  236.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  237.      * error otherwise.
  238.      *
  239.      * @param outputFrame output frame
  240.      * @param orbitInterpolator orbit interpolator
  241.      * @param absPVAInterpolator absolute position-velocity-acceleration
  242.      * @param massInterpolator mass interpolator
  243.      * @param attitudeInterpolator attitude interpolator
  244.      * @param additionalStateInterpolator additional state interpolator
  245.      *
  246.      * @deprecated using this constructor may throw an exception if any given interpolator
  247.      * does not use {@link #DEFAULT_INTERPOLATION_POINTS} and {@link
  248.      * #DEFAULT_EXTRAPOLATION_THRESHOLD_SEC}. Use {@link
  249.      * #FieldSpacecraftStateInterpolator(int, double, Frame, FieldTimeInterpolator,
  250.      * FieldTimeInterpolator, FieldTimeInterpolator, FieldTimeInterpolator,
  251.      * FieldTimeInterpolator)} instead.
  252.      */
  253.     @Deprecated
  254.     public FieldSpacecraftStateInterpolator(final Frame outputFrame,
  255.                                             final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolator,
  256.                                             final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolator,
  257.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> massInterpolator,
  258.                                             final FieldTimeInterpolator<FieldAttitude<KK>, KK> attitudeInterpolator,
  259.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> additionalStateInterpolator) {
  260.         super(DEFAULT_INTERPOLATION_POINTS, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC);
  261.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  262.         this.outputFrame                 = outputFrame;
  263.         this.orbitInterpolator           = orbitInterpolator;
  264.         this.absPVAInterpolator          = absPVAInterpolator;
  265.         this.massInterpolator            = massInterpolator;
  266.         this.attitudeInterpolator        = attitudeInterpolator;
  267.         this.additionalStateInterpolator = additionalStateInterpolator;
  268.     }

  270.     /**
  271.      * Constructor.
  272.      * <p>
  273.      * <b>BEWARE:</b> output frame <b>must be inertial</b> if interpolated spacecraft states are defined by orbit. Throws an
  274.      * error otherwise.
  275.      *
  276.      * @param interpolationPoints number of interpolation points
  277.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  278.      * @param outputFrame output frame
  279.      * @param orbitInterpolator orbit interpolator
  280.      * @param absPVAInterpolator absolute position-velocity-acceleration
  281.      * @param massInterpolator mass interpolator
  282.      * @param attitudeInterpolator attitude interpolator
  283.      * @param additionalStateInterpolator additional state interpolator
  284.      */
  285.     public FieldSpacecraftStateInterpolator(final int interpolationPoints,
  286.                                             final double extrapolationThreshold,
  287.                                             final Frame outputFrame,
  288.                                             final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolator,
  289.                                             final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolator,
  290.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> massInterpolator,
  291.                                             final FieldTimeInterpolator<FieldAttitude<KK>, KK> attitudeInterpolator,
  292.                                             final FieldTimeInterpolator<TimeStampedField<KK>, KK> additionalStateInterpolator) {
  293.         super(interpolationPoints, extrapolationThreshold);
  294.         checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
  295.         this.outputFrame                 = outputFrame;
  296.         this.orbitInterpolator           = orbitInterpolator;
  297.         this.absPVAInterpolator          = absPVAInterpolator;
  298.         this.massInterpolator            = massInterpolator;
  299.         this.attitudeInterpolator        = attitudeInterpolator;
  300.         this.additionalStateInterpolator = additionalStateInterpolator;
  301.     }

  303.     /**
  304.      * {@inheritDoc}
  305.      * <p>
  306.      * The additional states that are interpolated are the ones already present in the first neighbor instance. The sample
  307.      * instances must therefore have at least the same additional states as this neighbor instance. They may have more
  308.      * additional states, but the extra ones will be ignored.
  309.      * <p>
  310.      * All the sample instances <em>must</em> be based on similar trajectory data, i.e. they must either all be based on
  311.      * orbits or all be based on absolute position-velocity-acceleration. Any inconsistency will trigger an
  312.      * {@link OrekitIllegalArgumentException}.
  313.      *
  314.      * @throws OrekitIllegalArgumentException if there are states defined by orbits and absolute
  315.      * position-velocity-acceleration coordinates
  316.      * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
  317.      * definition type
  318.      */
  319.     @Override
  320.     public FieldSpacecraftState<KK> interpolate(final FieldAbsoluteDate<KK> interpolationDate,
  321.                                                 final Collection<FieldSpacecraftState<KK>> sample) {

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

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

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

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

  336.             // Check interpolator and sample consistency
  337.             SpacecraftStateInterpolator.checkSampleAndInterpolatorConsistency(nonFieldSampleList,
  338.                                                                               orbitInterpolator != null,
  339.                                                                               absPVAInterpolator != null);
  340.         }

  342.         return super.interpolate(interpolationDate, sample);
  343.     }

  345.     /** {@inheritDoc} */
  346.     @Override
  347.     public List<FieldTimeInterpolator<? extends FieldTimeStamped<KK>, KK>> getSubInterpolators() {

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

  352.         addOptionalSubInterpolatorIfDefined(orbitInterpolator, subInterpolators);
  353.         addOptionalSubInterpolatorIfDefined(absPVAInterpolator, subInterpolators);
  354.         addOptionalSubInterpolatorIfDefined(massInterpolator, subInterpolators);
  355.         addOptionalSubInterpolatorIfDefined(attitudeInterpolator, subInterpolators);
  356.         addOptionalSubInterpolatorIfDefined(additionalStateInterpolator, subInterpolators);

  358.         return subInterpolators;
  359.     }

  361.     /**
  362.      * {@inheritDoc}
  363.      */
  364.     @Override
  365.     protected FieldSpacecraftState<KK> interpolate(final InterpolationData interpolationData) {

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

  370.         // Get first state definition
  371.         final FieldSpacecraftState<KK> earliestState   = interpolationData.getNeighborList().get(0);
  372.         final boolean                  areOrbitDefined = earliestState.isOrbitDefined();

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

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

  379.         final List<FieldArrayDictionary<KK>.Entry> additionalEntries =
  380.                 earliestState.getAdditionalStatesValues().getData();
  381.         final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalSample =
  382.                 createAdditionalStateSample(additionalEntries);

  384.         final List<FieldArrayDictionary<KK>.Entry> additionalDotEntries =
  385.                 earliestState.getAdditionalStatesDerivatives().getData();
  386.         final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalDotSample =
  387.                 createAdditionalStateSample(additionalDotEntries);

  389.         // Fill interpolators with samples
  390.         final List<FieldSpacecraftState<KK>>       samples      = interpolationData.getNeighborList();
  391.         final List<FieldOrbit<KK>>                 orbitSample  = new ArrayList<>();
  392.         final List<FieldAbsolutePVCoordinates<KK>> absPVASample = new ArrayList<>();
  393.         for (FieldSpacecraftState<KK> state : samples) {
  394.             final FieldAbsoluteDate<KK> currentDate = state.getDate();

  396.             // Add orbit sample if state is defined with an orbit
  397.             if (state.isOrbitDefined()) {
  398.                 orbitSample.add(state.getOrbit());
  399.             }
  400.             // Add absolute position-velocity-acceleration sample if state is defined with an absolute position-velocity-acceleration
  401.             else {
  402.                 absPVASample.add(state.getAbsPVA());
  403.             }

  405.             // Add mass sample
  406.             if (massInterpolator != null) {
  407.                 masses.add(new TimeStampedField<>(state.getMass(), state.getDate()));
  408.             }

  410.             // Add attitude sample if it is interpolated
  411.             if (attitudeInterpolator != null) {
  412.                 attitudes.add(state.getAttitude());
  413.             }

  415.             if (additionalStateInterpolator != null) {

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

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

  428.         }

  430.         // Interpolate mass
  431.         final KK one = interpolationData.getOne();
  432.         final KK interpolatedMass;
  433.         if (massInterpolator != null) {
  434.             interpolatedMass = massInterpolator.interpolate(interpolationDate, masses).getValue();
  435.         }
  436.         else {
  437.             interpolatedMass = one.multiply(SpacecraftState.DEFAULT_MASS);
  438.         }

  440.         // Interpolate additional states and derivatives
  441.         final FieldArrayDictionary<KK> interpolatedAdditional;
  442.         final FieldArrayDictionary<KK> interpolatedAdditionalDot;
  443.         if (additionalStateInterpolator != null) {
  444.             interpolatedAdditional    = interpolateAdditionalState(interpolationDate, additionalSample);
  445.             interpolatedAdditionalDot = interpolateAdditionalState(interpolationDate, additionalDotSample);
  446.         }
  447.         else {
  448.             interpolatedAdditional    = null;
  449.             interpolatedAdditionalDot = null;
  450.         }

  452.         // Interpolate orbit
  453.         if (areOrbitDefined && orbitInterpolator != null) {
  454.             final FieldOrbit<KK> interpolatedOrbit =
  455.                     orbitInterpolator.interpolate(interpolationDate, orbitSample);

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

  460.             return new FieldSpacecraftState<>(interpolatedOrbit, interpolatedAttitude, interpolatedMass,
  461.                                               interpolatedAdditional, interpolatedAdditionalDot);
  462.         }
  463.         // Interpolate absolute position-velocity-acceleration
  464.         else if (!areOrbitDefined && absPVAInterpolator != null) {

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

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

  472.             return new FieldSpacecraftState<>(interpolatedAbsPva, interpolatedAttitude, interpolatedMass,
  473.                                               interpolatedAdditional, interpolatedAdditionalDot);
  474.         }
  475.         // Should never happen
  476.         else {
  477.             throw new OrekitInternalError(null);
  478.         }

  480.     }

  482.     /** Get output frame.
  483.      * @return output frame
  484.      */
  485.     public Frame getOutputFrame() {
  486.         return outputFrame;
  487.     }

  489.     /** Get orbit interpolator.
  490.      * @return optional orbit interpolator
  491.      *
  492.      * @see Optional
  493.      */
  494.     public Optional<FieldTimeInterpolator<FieldOrbit<KK>, KK>> getOrbitInterpolator() {
  495.         return Optional.ofNullable(orbitInterpolator);
  496.     }

  498.     /** Get absolute position-velocity-acceleration interpolator.
  499.      * @return optional absolute position-velocity-acceleration interpolator
  500.      *
  501.      * @see Optional
  502.      */
  503.     public Optional<FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK>> getAbsPVAInterpolator() {
  504.         return Optional.ofNullable(absPVAInterpolator);
  505.     }

  507.     /** Get mass interpolator.
  508.      * @return optional mass interpolator
  509.      *
  510.      * @see Optional
  511.      */
  512.     public Optional<FieldTimeInterpolator<TimeStampedField<KK>, KK>> getMassInterpolator() {
  513.         return Optional.ofNullable(massInterpolator);
  514.     }

  516.     /** Get attitude interpolator.
  517.      * @return optional attitude interpolator
  518.      *
  519.      * @see Optional
  520.      */
  521.     public Optional<FieldTimeInterpolator<FieldAttitude<KK>, KK>> getAttitudeInterpolator() {
  522.         return Optional.ofNullable(attitudeInterpolator);
  523.     }

  525.     /** Get additional state interpolator.
  526.      * @return optional additional state interpolator
  527.      *
  528.      * @see Optional
  529.      */
  530.     public Optional<FieldTimeInterpolator<TimeStampedField<KK>, KK>> getAdditionalStateInterpolator() {
  531.         return Optional.ofNullable(additionalStateInterpolator);
  532.     }

  534.     /**
  535.      * Check that at least one interpolator is defined.
  536.      *
  537.      * @param orbitInterpolatorToCheck orbit interpolator
  538.      * @param absPVAInterpolatorToCheck absolute position-velocity-acceleration interpolator
  539.      */
  540.     private void checkAtLeastOneInterpolator(final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolatorToCheck,
  541.                                              final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolatorToCheck) {
  542.         if (orbitInterpolatorToCheck == null && absPVAInterpolatorToCheck == null) {
  543.             throw new OrekitIllegalArgumentException(OrekitMessages.NO_INTERPOLATOR_FOR_STATE_DEFINITION);
  544.         }
  545.     }

  547.     /**
  548.      * Create empty samples for given additional entries.
  549.      *
  550.      * @param additionalEntries tabulated additional entries
  551.      *
  552.      * @return empty samples for given additional entries
  553.      */
  554.     private Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> createAdditionalStateSample(
  555.             final List<FieldArrayDictionary<KK>.Entry> additionalEntries) {
  556.         final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalSamples =
  557.                 new HashMap<>(additionalEntries.size());

  559.         for (final FieldArrayDictionary<KK>.Entry entry : additionalEntries) {
  560.             additionalSamples.put(entry.getKey(), new ArrayList<>());
  561.         }

  563.         return additionalSamples;
  564.     }

  566.     /**
  567.      * Interpolate additional state values.
  568.      *
  569.      * @param interpolationDate interpolation date
  570.      * @param additionalSamples additional state samples
  571.      *
  572.      * @return interpolated additional state values
  573.      */
  574.     private FieldArrayDictionary<KK> interpolateAdditionalState(final FieldAbsoluteDate<KK> interpolationDate,
  575.                                                                 final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalSamples) {
  576.         final Field<KK> field = interpolationDate.getField();
  577.         final FieldArrayDictionary<KK> interpolatedAdditional;

  579.         if (additionalSamples.isEmpty()) {
  580.             interpolatedAdditional = null;
  581.         }
  582.         else {
  583.             interpolatedAdditional = new FieldArrayDictionary<>(field, additionalSamples.size());
  584.             for (final Map.Entry<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> entry : additionalSamples.entrySet()) {

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

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

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

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

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

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

  605.                     // Interpolate
  606.                     currentInterpolatedAdditional[i] =
  607.                             additionalStateInterpolator.interpolate(interpolationDate, currentValueSample).getValue();
  608.                 }

  610.                 interpolatedAdditional.put(entry.getKey(), currentInterpolatedAdditional);
  611.             }
  612.         }
  613.         return interpolatedAdditional;
  614.     }

  616.     /**
  617.      * Interpolate attitude.
  618.      * <p>
  619.      * If no attitude interpolator were defined, create a default inertial provider with respect to the output frame.
  620.      *
  621.      * @param interpolationDate interpolation date
  622.      * @param attitudes attitudes sample
  623.      * @param pvProvider position-velocity-acceleration coordinates provider
  624.      *
  625.      * @return interpolated attitude if attitude interpolator is present, default attitude otherwise
  626.      */
  627.     private FieldAttitude<KK> interpolateAttitude(final FieldAbsoluteDate<KK> interpolationDate,
  628.                                                   final List<FieldAttitude<KK>> attitudes,
  629.                                                   final FieldPVCoordinatesProvider<KK> pvProvider) {
  630.         if (attitudes.isEmpty()) {
  631.             final AttitudeProvider attitudeProvider = new FrameAlignedProvider(outputFrame);
  632.             return attitudeProvider.getAttitude(pvProvider, interpolationDate, outputFrame);
  633.         }
  634.         else {
  635.             return attitudeInterpolator.interpolate(interpolationDate, attitudes);
  636.         }
  637.     }
  638. }
Created with JaCoCo 0.8.11.202310140853