1 /* Copyright 2002-2026 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.propagation;
18
19 import java.util.ArrayList;
20 import java.util.Collection;
21 import java.util.Comparator;
22 import java.util.HashMap;
23 import java.util.List;
24 import java.util.Map;
25 import java.util.Optional;
26 import java.util.stream.Collectors;
27
28 import org.hipparchus.CalculusFieldElement;
29 import org.hipparchus.Field;
30 import org.hipparchus.util.MathArrays;
31 import org.hipparchus.util.Pair;
32 import org.orekit.attitudes.AttitudeProvider;
33 import org.orekit.attitudes.FieldAttitude;
34 import org.orekit.attitudes.FieldAttitudeInterpolator;
35 import org.orekit.attitudes.FrameAlignedProvider;
36 import org.orekit.errors.OrekitIllegalArgumentException;
37 import org.orekit.errors.OrekitInternalError;
38 import org.orekit.errors.OrekitMessages;
39 import org.orekit.frames.Frame;
40 import org.orekit.orbits.FieldOrbit;
41 import org.orekit.orbits.FieldOrbitHermiteInterpolator;
42 import org.orekit.time.AbstractFieldTimeInterpolator;
43 import org.orekit.time.AbstractTimeInterpolator;
44 import org.orekit.time.FieldAbsoluteDate;
45 import org.orekit.time.FieldTimeInterpolator;
46 import org.orekit.time.TimeStampedField;
47 import org.orekit.time.TimeStampedFieldHermiteInterpolator;
48 import org.orekit.utils.AngularDerivativesFilter;
49 import org.orekit.utils.CartesianDerivativesFilter;
50 import org.orekit.utils.FieldAbsolutePVCoordinates;
51 import org.orekit.utils.FieldAbsolutePVCoordinatesHermiteInterpolator;
52 import org.orekit.utils.FieldArrayDictionary;
53 import org.orekit.utils.FieldDataDictionary;
54 import org.orekit.utils.FieldPVCoordinatesProvider;
55 import org.orekit.utils.TimeStampedFieldAngularCoordinatesHermiteInterpolator;
56
57 /**
58 * Generic class for spacecraft state interpolator.
59 * <p>
60 * The user can specify what interpolator to use for each attribute of the spacecraft state. However, at least one
61 * interpolator for either orbit or absolute position-velocity-acceleration is needed. All the other interpolators can be
62 * left to null if the user do not want to interpolate these values.
63 *
64 * @param <KK> type of the field element
65 *
66 * @author Luc Maisonobe
67 * @author Vincent Cucchietti
68 * @see FieldSpacecraftState
69 */
70 public class FieldSpacecraftStateInterpolator<KK extends CalculusFieldElement<KK>>
71 extends AbstractFieldTimeInterpolator<FieldSpacecraftState<KK>, KK> {
72
73 /**
74 * Output frame.
75 * <p><b>Must be inertial</b> if interpolating spacecraft states defined by orbit</p>
76 */
77 private final Frame outputFrame;
78
79 /** Orbit interpolator. */
80 private final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolator;
81
82 /** Absolute position-velocity-acceleration interpolator. */
83 private final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolator;
84
85 /** Mass interpolator. */
86 private final FieldTimeInterpolator<TimeStampedField<KK>, KK> massInterpolator;
87
88 /** Attitude interpolator. */
89 private final FieldTimeInterpolator<FieldAttitude<KK>, KK> attitudeInterpolator;
90
91 /** Additional state interpolator. */
92 private final FieldTimeInterpolator<TimeStampedField<KK>, KK> additionalStateInterpolator;
93
94 /**
95 * Simplest constructor to create a default Hermite interpolator for every spacecraft state field.
96 * <p>
97 * The interpolators will have the following configuration :
98 * <ul>
99 * <li>Same frame for coordinates and attitude </li>
100 * <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
101 * <li>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</li>
102 * <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
103 * <li>Use of angular and first time derivative for attitude interpolation</li>
104 * </ul>
105 * <p>
106 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
107 * points (about 10-20 points) in order to avoid <a href="https://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
108 * phenomenon</a> and numerical problems (including NaN appearing).
109 * <p>
110 * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
111 * tabulated spacecraft states defined by orbit, will throw an error otherwise.
112 *
113 * @param outputFrame output frame
114 */
115 public FieldSpacecraftStateInterpolator(final Frame outputFrame) {
116 this(DEFAULT_INTERPOLATION_POINTS, outputFrame);
117 }
118
119 /**
120 * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
121 * <p>
122 * The interpolators will have the following configuration :
123 * <ul>
124 * <li>Same frame for coordinates and attitude </li>
125 * <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
126 * <li>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</li>
127 * <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
128 * <li>Use of angular and first time derivative for attitude interpolation</li>
129 * </ul>
130 * <p>
131 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
132 * points (about 10-20 points) in order to avoid <a href="https://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
133 * phenomenon</a> and numerical problems (including NaN appearing).
134 * <p>
135 * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
136 * tabulated spacecraft states defined by orbit, will throw an error otherwise.
137 *
138 * @param interpolationPoints number of interpolation points
139 * @param outputFrame output frame
140 */
141 public FieldSpacecraftStateInterpolator(final int interpolationPoints, final Frame outputFrame) {
142 this(interpolationPoints, outputFrame, outputFrame);
143 }
144
145 /**
146 * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
147 * <p>
148 * The interpolators will have the following configuration :
149 * <ul>
150 * <li>Same frame for coordinates and attitude </li>
151 * <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
152 * <li>Use of angular and first time derivative for attitude interpolation</li>
153 * </ul>
154 * <p>
155 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
156 * points (about 10-20 points) in order to avoid <a href="https://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
157 * phenomenon</a> and numerical problems (including NaN appearing).
158 * <p>
159 * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
160 * tabulated spacecraft states defined by orbit, will throw an error otherwise.
161 *
162 * @param interpolationPoints number of interpolation points
163 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
164 * @param outputFrame output frame
165 * @since 12.1
166 */
167 public FieldSpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold, final Frame outputFrame) {
168 this(interpolationPoints, extrapolationThreshold, outputFrame, outputFrame);
169 }
170
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>Default extrapolation threshold of {@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s</li>
177 * <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
178 * <li>Use of angular and first time derivative for attitude interpolation</li>
179 * </ul>
180 * <p>
181 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
182 * points (about 10-20 points) in order to avoid <a href="https://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
183 * phenomenon</a> and numerical problems (including NaN appearing).
184 * <p>
185 * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
186 * tabulated spacecraft states defined by orbit, will throw an error otherwise.
187 *
188 * @param interpolationPoints number of interpolation points
189 * @param outputFrame output frame
190 * @param attitudeReferenceFrame reference frame from which attitude is defined
191 */
192 public FieldSpacecraftStateInterpolator(final int interpolationPoints, final Frame outputFrame,
193 final Frame attitudeReferenceFrame) {
194 this(interpolationPoints, AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
195 outputFrame, attitudeReferenceFrame);
196 }
197
198 /**
199 * Constructor to create a customizable Hermite interpolator for every spacecraft state field.
200 * <p>
201 * The interpolators will have the following configuration :
202 * <ul>
203 * <li>Use of position and two time derivatives for absolute position-velocity-acceleration coordinates interpolation</li>
204 * <li>Use of angular and first time derivative for attitude interpolation</li>
205 * </ul>
206 * <p>
207 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
208 * points (about 10-20 points) in order to avoid <a href="https://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
209 * phenomenon</a> and numerical problems (including NaN appearing).
210 * <p>
211 * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
212 * tabulated spacecraft states defined by orbit, will throw an error otherwise.
213 *
214 * @param interpolationPoints number of interpolation points
215 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
216 * @param outputFrame output frame
217 * @param attitudeReferenceFrame reference frame from which attitude is defined
218 */
219 public FieldSpacecraftStateInterpolator(final int interpolationPoints, final double extrapolationThreshold,
220 final Frame outputFrame, final Frame attitudeReferenceFrame) {
221 this(interpolationPoints, extrapolationThreshold, outputFrame, attitudeReferenceFrame,
222 CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RR);
223 }
224
225 /**
226 * Constructor.
227 * <p>
228 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
229 * points (about 10-20 points) in order to avoid <a href="https://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
230 * phenomenon</a> and numerical problems (including NaN appearing).
231 * <p>
232 * <b>BEWARE:</b> output frame <b>must be inertial</b> if this instance is going to interpolate between
233 * tabulated spacecraft states defined by orbit, will throw an error otherwise.
234 *
235 * @param interpolationPoints number of interpolation points
236 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
237 * @param outputFrame output frame
238 * @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
239 * @param attitudeReferenceFrame reference frame from which attitude is defined
240 * @param angularFilter filter for derivatives from the sample to use in attitude interpolation
241 */
242 public FieldSpacecraftStateInterpolator(final int interpolationPoints,
243 final double extrapolationThreshold,
244 final Frame outputFrame,
245 final Frame attitudeReferenceFrame,
246 final CartesianDerivativesFilter pvaFilter,
247 final AngularDerivativesFilter angularFilter) {
248
249 this(interpolationPoints, extrapolationThreshold, outputFrame,
250 new FieldOrbitHermiteInterpolator<>(interpolationPoints, extrapolationThreshold, outputFrame, pvaFilter),
251 new FieldAbsolutePVCoordinatesHermiteInterpolator<>(interpolationPoints, extrapolationThreshold, outputFrame,
252 pvaFilter),
253 new TimeStampedFieldHermiteInterpolator<>(interpolationPoints, extrapolationThreshold),
254 new FieldAttitudeInterpolator<>(attitudeReferenceFrame,
255 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<KK>(
256 interpolationPoints, extrapolationThreshold, angularFilter)),
257 new TimeStampedFieldHermiteInterpolator<>(interpolationPoints, extrapolationThreshold));
258 }
259
260 /**
261 * Constructor.
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 *
266 * @param interpolationPoints number of interpolation points
267 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
268 * @param outputFrame output frame
269 * @param orbitInterpolator orbit interpolator
270 * @param absPVAInterpolator absolute position-velocity-acceleration
271 * @param massInterpolator mass interpolator
272 * @param attitudeInterpolator attitude interpolator
273 * @param additionalStateInterpolator additional state interpolator
274 */
275 public FieldSpacecraftStateInterpolator(final int interpolationPoints,
276 final double extrapolationThreshold,
277 final Frame outputFrame,
278 final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolator,
279 final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolator,
280 final FieldTimeInterpolator<TimeStampedField<KK>, KK> massInterpolator,
281 final FieldTimeInterpolator<FieldAttitude<KK>, KK> attitudeInterpolator,
282 final FieldTimeInterpolator<TimeStampedField<KK>, KK> additionalStateInterpolator) {
283 super(interpolationPoints, extrapolationThreshold);
284 checkAtLeastOneInterpolator(orbitInterpolator, absPVAInterpolator);
285 this.outputFrame = outputFrame;
286 this.orbitInterpolator = orbitInterpolator;
287 this.absPVAInterpolator = absPVAInterpolator;
288 this.massInterpolator = massInterpolator;
289 this.attitudeInterpolator = attitudeInterpolator;
290 this.additionalStateInterpolator = additionalStateInterpolator;
291 }
292
293 /**
294 * {@inheritDoc}
295 * <p>
296 * The additional states that are interpolated are the ones already present in the first neighbor instance. The sample
297 * instances must therefore have at least the same additional states as this neighbor instance. They may have more
298 * additional states, but the extra ones will be ignored.
299 * <p>
300 * All the sample instances <em>must</em> be based on similar trajectory data, i.e. they must either all be based on
301 * orbits or all be based on absolute position-velocity-acceleration. Any inconsistency will trigger an
302 * {@link OrekitIllegalArgumentException}.
303 *
304 * @throws OrekitIllegalArgumentException if there are states defined by orbits and absolute
305 * position-velocity-acceleration coordinates
306 * @throws OrekitIllegalArgumentException if there is no defined interpolator for given sample spacecraft state
307 * definition type
308 */
309 @Override
310 public FieldSpacecraftState<KK> interpolate(final FieldAbsoluteDate<KK> interpolationDate,
311 final Collection<? extends FieldSpacecraftState<KK>> sample) {
312
313 final List<FieldSpacecraftState<KK>> sampleList = new ArrayList<>(sample);
314
315 // Convert to spacecraft state for consistency check
316 final List<SpacecraftState> nonFieldSampleList =
317 sampleList.stream().map(FieldSpacecraftState::toSpacecraftState).collect(Collectors.toList());
318
319 // If sample is empty, an error will be thrown in super method
320 if (!sampleList.isEmpty()) {
321
322 // Check given that given states definition are consistent
323 // (all defined by either orbits or absolute position-velocity-acceleration coordinates)
324 SpacecraftStateInterpolator.checkStatesDefinitionsConsistency(nonFieldSampleList);
325
326 // Check interpolator and sample consistency
327 SpacecraftStateInterpolator.checkSampleAndInterpolatorConsistency(nonFieldSampleList,
328 orbitInterpolator != null,
329 absPVAInterpolator != null);
330 }
331
332 return super.interpolate(interpolationDate, sample);
333 }
334
335 /** {@inheritDoc} */
336 @Override
337 public List<FieldTimeInterpolator<?, KK>> getSubInterpolators() {
338
339 // Add all sub interpolators that are defined
340 final List<FieldTimeInterpolator<?, KK>> subInterpolators = new ArrayList<>();
341
342 addOptionalSubInterpolatorIfDefined(orbitInterpolator, subInterpolators);
343 addOptionalSubInterpolatorIfDefined(absPVAInterpolator, subInterpolators);
344 addOptionalSubInterpolatorIfDefined(massInterpolator, subInterpolators);
345 addOptionalSubInterpolatorIfDefined(attitudeInterpolator, subInterpolators);
346 addOptionalSubInterpolatorIfDefined(additionalStateInterpolator, subInterpolators);
347
348 return subInterpolators;
349 }
350
351 /**
352 * {@inheritDoc}
353 */
354 @Override
355 protected FieldSpacecraftState<KK> interpolate(final InterpolationData interpolationData) {
356
357 // Get interpolation date
358 final FieldAbsoluteDate<KK> interpolationDate = interpolationData.getInterpolationDate();
359
360 // Get first state definition
361 final FieldSpacecraftState<KK> earliestState = interpolationData.getNeighborList().getFirst();
362 final boolean areOrbitDefined = earliestState.isOrbitDefined();
363
364 // Prepare samples
365 final List<FieldAttitude<KK>> attitudes = new ArrayList<>();
366
367 final List<TimeStampedField<KK>> masses = new ArrayList<>();
368
369 final List<FieldDataDictionary<KK>.Entry> additionalEntries = earliestState.getAdditionalDataValues().getData();
370 final Map<String, List<Pair<FieldAbsoluteDate<KK>, Object>>> additionalSample =
371 createAdditionalDataSample(additionalEntries);
372
373 final List<FieldArrayDictionary<KK>.Entry> additionalDotEntries =
374 earliestState.getAdditionalStatesDerivatives().getData();
375 final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalDotSample =
376 createAdditionalStateSample(additionalDotEntries);
377
378 // Fill interpolators with samples
379 final List<FieldSpacecraftState<KK>> samples = interpolationData.getNeighborList();
380 final List<FieldOrbit<KK>> orbitSample = new ArrayList<>();
381 final List<FieldAbsolutePVCoordinates<KK>> absPVASample = new ArrayList<>();
382 for (FieldSpacecraftState<KK> state : samples) {
383 final FieldAbsoluteDate<KK> currentDate = state.getDate();
384
385 // Add orbit sample if state is defined with an orbit
386 if (state.isOrbitDefined()) {
387 orbitSample.add(state.getOrbit());
388 }
389 // Add absolute position-velocity-acceleration sample if state is defined with an absolute position-velocity-acceleration
390 else {
391 absPVASample.add(state.getAbsPVA());
392 }
393
394 // Add mass sample
395 if (massInterpolator != null) {
396 masses.add(new TimeStampedField<>(state.getMass(), state.getDate()));
397 }
398
399 // Add attitude sample if it is interpolated
400 if (attitudeInterpolator != null) {
401 attitudes.add(state.getAttitude());
402 }
403
404 if (additionalStateInterpolator != null) {
405
406 // Add all additional state values if they are interpolated
407 for (final Map.Entry<String, List<Pair<FieldAbsoluteDate<KK>, Object>>> entry : additionalSample.entrySet()) {
408 entry.getValue().add(new Pair<>(currentDate, state.getAdditionalData(entry.getKey())));
409 }
410
411 // Add all additional state derivative values if they are interpolated
412 for (final Map.Entry<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> entry : additionalDotSample.entrySet()) {
413 entry.getValue().add(new Pair<>(currentDate, state.getAdditionalStateDerivative(entry.getKey())));
414 }
415 }
416
417 }
418
419 // Interpolate mass
420 final KK one = interpolationData.getOne();
421 final KK interpolatedMass;
422 if (massInterpolator != null) {
423 interpolatedMass = massInterpolator.interpolate(interpolationDate, masses).getValue();
424 }
425 else {
426 interpolatedMass = one.newInstance(SpacecraftState.DEFAULT_MASS);
427 }
428
429 // Interpolate additional states and derivatives
430 final FieldDataDictionary<KK> interpolatedAdditional;
431 final FieldArrayDictionary<KK> interpolatedAdditionalDot;
432 if (additionalStateInterpolator != null) {
433 interpolatedAdditional = interpolateAdditionalState(interpolationDate, additionalSample).orElse(null);
434 interpolatedAdditionalDot = interpolateAdditionalState(interpolationDate, additionalDotSample).map(FieldDataDictionary::toFieldArrayDictionary).orElse(null);
435 }
436 else {
437 interpolatedAdditional = null;
438 interpolatedAdditionalDot = null;
439 }
440
441 // Interpolate orbit
442 if (areOrbitDefined && orbitInterpolator != null) {
443 final FieldOrbit<KK> interpolatedOrbit =
444 orbitInterpolator.interpolate(interpolationDate, orbitSample);
445
446 final FieldAttitude<KK> interpolatedAttitude =
447 interpolateAttitude(interpolationDate, attitudes, interpolatedOrbit);
448
449 return new FieldSpacecraftState<>(interpolatedOrbit, interpolatedAttitude, interpolatedMass,
450 interpolatedAdditional, interpolatedAdditionalDot);
451 }
452 // Interpolate absolute position-velocity-acceleration
453 else if (!areOrbitDefined && absPVAInterpolator != null) {
454
455 final FieldAbsolutePVCoordinates<KK> interpolatedAbsPva =
456 absPVAInterpolator.interpolate(interpolationDate, absPVASample);
457
458 final FieldAttitude<KK> interpolatedAttitude =
459 interpolateAttitude(interpolationDate, attitudes, interpolatedAbsPva);
460
461 return new FieldSpacecraftState<>(interpolatedAbsPva, interpolatedAttitude, interpolatedMass,
462 interpolatedAdditional, interpolatedAdditionalDot);
463 }
464 // Should never happen
465 else {
466 throw new OrekitInternalError(null);
467 }
468
469 }
470
471 /** Get output frame.
472 * @return output frame
473 */
474 public Frame getOutputFrame() {
475 return outputFrame;
476 }
477
478 /** Get orbit interpolator.
479 * @return optional orbit interpolator
480 *
481 * @see Optional
482 */
483 public Optional<FieldTimeInterpolator<FieldOrbit<KK>, KK>> getOrbitInterpolator() {
484 return Optional.ofNullable(orbitInterpolator);
485 }
486
487 /** Get absolute position-velocity-acceleration interpolator.
488 * @return optional absolute position-velocity-acceleration interpolator
489 *
490 * @see Optional
491 */
492 public Optional<FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK>> getAbsPVAInterpolator() {
493 return Optional.ofNullable(absPVAInterpolator);
494 }
495
496 /** Get mass interpolator.
497 * @return optional mass interpolator
498 *
499 * @see Optional
500 */
501 public Optional<FieldTimeInterpolator<TimeStampedField<KK>, KK>> getMassInterpolator() {
502 return Optional.ofNullable(massInterpolator);
503 }
504
505 /** Get attitude interpolator.
506 * @return optional attitude interpolator
507 *
508 * @see Optional
509 */
510 public Optional<FieldTimeInterpolator<FieldAttitude<KK>, KK>> getAttitudeInterpolator() {
511 return Optional.ofNullable(attitudeInterpolator);
512 }
513
514 /** Get additional state interpolator.
515 * @return optional additional state interpolator
516 *
517 * @see Optional
518 */
519 public Optional<FieldTimeInterpolator<TimeStampedField<KK>, KK>> getAdditionalStateInterpolator() {
520 return Optional.ofNullable(additionalStateInterpolator);
521 }
522
523 /**
524 * Check that at least one interpolator is defined.
525 *
526 * @param orbitInterpolatorToCheck orbit interpolator
527 * @param absPVAInterpolatorToCheck absolute position-velocity-acceleration interpolator
528 */
529 private void checkAtLeastOneInterpolator(final FieldTimeInterpolator<FieldOrbit<KK>, KK> orbitInterpolatorToCheck,
530 final FieldTimeInterpolator<FieldAbsolutePVCoordinates<KK>, KK> absPVAInterpolatorToCheck) {
531 if (orbitInterpolatorToCheck == null && absPVAInterpolatorToCheck == null) {
532 throw new OrekitIllegalArgumentException(OrekitMessages.NO_INTERPOLATOR_FOR_STATE_DEFINITION);
533 }
534 }
535
536 /**
537 * Create empty samples for given additional entries.
538 *
539 * @param additionalEntries tabulated additional entries
540 *
541 * @return empty samples for given additional entries
542 */
543 private Map<String, List<Pair<FieldAbsoluteDate<KK>, Object>>> createAdditionalDataSample(
544 final List<FieldDataDictionary<KK>.Entry> additionalEntries) {
545 final Map<String, List<Pair<FieldAbsoluteDate<KK>, Object>>> additionalSamples =
546 new HashMap<>(additionalEntries.size());
547
548 for (final FieldDataDictionary<KK>.Entry entry : additionalEntries) {
549 additionalSamples.put(entry.getKey(), new ArrayList<>());
550 }
551
552 return additionalSamples;
553 }
554
555 /**
556 * Create empty samples for given additional entries.
557 *
558 * @param additionalEntries tabulated additional entries
559 *
560 * @return empty samples for given additional entries
561 */
562 private Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> createAdditionalStateSample(
563 final List<FieldArrayDictionary<KK>.Entry> additionalEntries) {
564 final Map<String, List<Pair<FieldAbsoluteDate<KK>, KK[]>>> additionalSamples =
565 new HashMap<>(additionalEntries.size());
566
567 for (final FieldArrayDictionary<KK>.Entry entry : additionalEntries) {
568 additionalSamples.put(entry.getKey(), new ArrayList<>());
569 }
570
571 return additionalSamples;
572 }
573
574 /**
575 * Interpolate additional state values.
576 *
577 * @param interpolationDate interpolation date
578 * @param additionalSamples additional state samples
579 * @param <O> type of the data
580 * @return interpolated additional state values
581 */
582 private <O> Optional<FieldDataDictionary<KK>> interpolateAdditionalState(final FieldAbsoluteDate<KK> interpolationDate,
583 final Map<String, List<Pair<FieldAbsoluteDate<KK>, O>>> additionalSamples) {
584 final Field<KK> field = interpolationDate.getField();
585 final Optional<FieldDataDictionary<KK>> interpolatedAdditional;
586
587 if (additionalSamples.isEmpty()) {
588 interpolatedAdditional = Optional.empty();
589 }
590 else {
591 interpolatedAdditional = Optional.of(new FieldDataDictionary<>(field, additionalSamples.size()));
592 for (final Map.Entry<String, List<Pair<FieldAbsoluteDate<KK>, O>>> entry : additionalSamples.entrySet()) {
593
594 // Get current entry
595 final List<Pair<FieldAbsoluteDate<KK>, O>> currentAdditionalSamples = entry.getValue();
596 final O currentInterpolatedAdditional;
597 if (currentAdditionalSamples.getFirst().getValue() instanceof CalculusFieldElement[]) {
598 //noinspection unchecked
599 currentInterpolatedAdditional = (O) interpolateAdditionalSamples(field, interpolationDate, currentAdditionalSamples);
600 } else {
601 currentInterpolatedAdditional = currentAdditionalSamples.stream()
602 .filter(pair -> pair.getKey().isAfter(interpolationDate))
603 .min(Comparator.comparing(Pair::getKey))
604 .get()
605 .getValue();
606 }
607 interpolatedAdditional.get().put(entry.getKey(), currentInterpolatedAdditional);
608 }
609 }
610 return interpolatedAdditional;
611 }
612
613 /**
614 * Interpolates additional samples.
615 * @param field field of the elements
616 * @param interpolationDate interpolation date
617 * @param currentAdditionalSamples additional state samples
618 * @param <O> type of the data
619 * @return interpolated additional samples
620 */
621 @SuppressWarnings("unchecked")
622 private <O> KK[] interpolateAdditionalSamples(final Field<KK> field, final FieldAbsoluteDate<KK> interpolationDate, final List<Pair<FieldAbsoluteDate<KK>, O>> currentAdditionalSamples) {
623
624 // Extract number of values for this specific entry
625 final int nbOfValues = ((KK[]) currentAdditionalSamples.getFirst().getValue()).length;
626
627 // For each value of current additional state entry
628 final KK[] currentInterpolatedAdditional = MathArrays.buildArray(field, nbOfValues);
629 for (int i = 0; i < nbOfValues; i++) {
630
631 // Create final index for lambda expression use
632 final int currentIndex = i;
633
634 // Create sample for specific value of current additional state values
635 final List<TimeStampedField<KK>> currentValueSample = new ArrayList<>();
636
637 currentAdditionalSamples.forEach(currentSamples -> currentValueSample.add(
638 new TimeStampedField<>(((KK[]) currentSamples.getValue())[currentIndex], currentSamples.getFirst())));
639
640 // Interpolate
641 currentInterpolatedAdditional[i] = additionalStateInterpolator.interpolate(interpolationDate, currentValueSample).getValue();
642 }
643 return currentInterpolatedAdditional;
644 }
645
646 /**
647 * Interpolate attitude.
648 * <p>
649 * If no attitude interpolator were defined, create a default inertial provider with respect to the output frame.
650 *
651 * @param interpolationDate interpolation date
652 * @param attitudes attitudes sample
653 * @param pvProvider position-velocity-acceleration coordinates provider
654 *
655 * @return interpolated attitude if attitude interpolator is present, default attitude otherwise
656 */
657 private FieldAttitude<KK> interpolateAttitude(final FieldAbsoluteDate<KK> interpolationDate,
658 final List<FieldAttitude<KK>> attitudes,
659 final FieldPVCoordinatesProvider<KK> pvProvider) {
660 if (attitudes.isEmpty()) {
661 final AttitudeProvider attitudeProvider = new FrameAlignedProvider(outputFrame);
662 return attitudeProvider.getAttitude(pvProvider, interpolationDate, outputFrame);
663 }
664 else {
665 return attitudeInterpolator.interpolate(interpolationDate, attitudes);
666 }
667 }
668 }