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