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