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