1   /* Copyright 2002-2026 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.propagation;
18  
19  
20  import org.hipparchus.CalculusFieldElement;
21  import org.hipparchus.Field;
22  import org.hipparchus.exception.LocalizedCoreFormats;
23  import org.hipparchus.exception.MathIllegalArgumentException;
24  import org.hipparchus.exception.MathIllegalStateException;
25  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26  import org.hipparchus.util.MathArrays;
27  import org.orekit.attitudes.FieldAttitude;
28  import org.orekit.errors.OrekitException;
29  import org.orekit.errors.OrekitIllegalArgumentException;
30  import org.orekit.errors.OrekitIllegalStateException;
31  import org.orekit.errors.OrekitMessages;
32  import org.orekit.frames.FieldStaticTransform;
33  import org.orekit.frames.FieldTransform;
34  import org.orekit.frames.Frame;
35  import org.orekit.orbits.FieldOrbit;
36  import org.orekit.orbits.Orbit;
37  import org.orekit.time.FieldAbsoluteDate;
38  import org.orekit.time.FieldTimeShiftable;
39  import org.orekit.time.FieldTimeStamped;
40  import org.orekit.utils.DataDictionary;
41  import org.orekit.utils.DoubleArrayDictionary;
42  import org.orekit.utils.FieldAbsolutePVCoordinates;
43  import org.orekit.utils.FieldArrayDictionary;
44  import org.orekit.utils.FieldDataDictionary;
45  import org.orekit.utils.FieldPVCoordinates;
46  import org.orekit.utils.TimeStampedFieldPVCoordinates;
47  import org.orekit.utils.TimeStampedPVCoordinates;
48  
49  /** This class is the representation of a complete state holding orbit, attitude
50   * and mass information at a given date, meant primarily for propagation.
51   *
52   * <p>It contains an {@link FieldOrbit}, or a {@link FieldAbsolutePVCoordinates} if there
53   * is no definite central body, plus the current mass and attitude at the intrinsic
54   * {@link FieldAbsoluteDate}. Quantities are guaranteed to be consistent in terms
55   * of date and reference frame. The spacecraft state may also contain additional
56   * states, which are simply named double arrays which can hold any user-defined
57   * data.
58   * </p>
59   * <p>
60   * The state can be slightly shifted to close dates. This actual shift varies
61   * between {@link FieldOrbit} and {@link FieldAbsolutePVCoordinates}.
62   * For attitude it is a linear extrapolation taking the spin rate into account
63   * and no mass change. It is <em>not</em> intended as a replacement for proper
64   * orbit and attitude propagation but should be sufficient for either small
65   * time shifts or coarse accuracy.
66   * </p>
67   * <p>
68   * The instance {@code FieldSpacecraftState} is guaranteed to be immutable.
69   * </p>
70   * @see org.orekit.propagation.numerical.NumericalPropagator
71   * @see SpacecraftState
72   * @author Fabien Maussion
73   * @author V&eacute;ronique Pommier-Maurussane
74   * @author Luc Maisonobe
75   * @author Vincent Mouraux
76   * @param <T> type of the field elements
77   */
78  public class FieldSpacecraftState <T extends CalculusFieldElement<T>>
79      implements FieldTimeStamped<T>, FieldTimeShiftable<FieldSpacecraftState<T>, T> {
80  
81      /** Default mass. */
82      private static final double DEFAULT_MASS = 1000.0;
83  
84      /**
85       * tolerance on date comparison in {@link #checkConsistency(FieldOrbit, FieldAttitude)}. 100 ns
86       * corresponds to sub-mm accuracy at LEO orbital velocities.
87       */
88      private static final double DATE_INCONSISTENCY_THRESHOLD = 100e-9;
89  
90      /** Orbital state. */
91      private final FieldOrbit<T> orbit;
92  
93      /** Trajectory state, when it is not an orbit. */
94      private final FieldAbsolutePVCoordinates<T> absPva;
95  
96      /** FieldAttitude<T>. */
97      private final FieldAttitude<T> attitude;
98  
99      /** Current mass (kg). */
100     private final T mass;
101 
102     /** Current mass rate (kg/s). */
103     private final T massRate;
104 
105     /** Additional data, can be any object (String, T[], etc.). */
106     private final FieldDataDictionary<T> additional;
107 
108     /** Additional states derivatives.
109      * @since 11.1
110      */
111     private final FieldArrayDictionary<T> additionalDot;
112 
113     /** Build a spacecraft state from orbit only.
114      * <p>FieldAttitude and mass are set to unspecified non-null arbitrary values.</p>
115      * @param orbit the orbit
116      */
117     public FieldSpacecraftState(final FieldOrbit<T> orbit) {
118         this(orbit, SpacecraftState.getDefaultAttitudeProvider(orbit.getFrame())
119                         .getAttitude(orbit, orbit.getDate(), orbit.getFrame()),
120              orbit.getA().getField().getZero().newInstance(DEFAULT_MASS), null, null);
121     }
122 
123     /** Build a spacecraft state from orbit and attitude. Kept for performance.
124      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
125      * @param orbit the orbit
126      * @param attitude attitude
127      * @exception IllegalArgumentException if orbit and attitude dates
128      * or frames are not equal
129      */
130     public FieldSpacecraftState(final FieldOrbit<T> orbit, final FieldAttitude<T> attitude)
131         throws IllegalArgumentException {
132         this(orbit, attitude, orbit.getA().getField().getZero().newInstance(DEFAULT_MASS), null, null);
133         checkConsistency(orbit, attitude);
134     }
135 
136     /** Build a spacecraft state from orbit, attitude, mass, additional states and derivatives.
137      * @param orbit the orbit
138      * @param attitude attitude
139      * @param mass the mass (kg)
140      * @param additional additional states (may be null if no additional states are available)
141      * @param additionalDot additional states derivatives(may be null if no additional states derivative sare available)
142      * @exception IllegalArgumentException if orbit and attitude dates
143      * or frames are not equal
144      * @since 11.1
145      */
146     public FieldSpacecraftState(final FieldOrbit<T> orbit, final FieldAttitude<T> attitude, final T mass,
147                                 final FieldDataDictionary<T> additional,
148                                 final FieldArrayDictionary<T> additionalDot)
149         throws IllegalArgumentException {
150         this(orbit, null, attitude, mass, mass.getField().getZero(), additional, additionalDot, true);
151     }
152 
153     /** Build a spacecraft state from orbit, attitude, mass, additional states and derivatives.
154      * @param orbit the orbit
155      * @param attitude attitude
156      * @param mass the mass (kg)
157      * @param massRate the mass rate (kg/s)
158      * @param additional additional states (may be null if no additional states are available)
159      * @param additionalDot additional states derivatives(may be null if no additional states derivative sare available)
160      * @exception IllegalArgumentException if orbit and attitude dates
161      * or frames are not equal
162      * @since 14.0
163      */
164     public FieldSpacecraftState(final FieldOrbit<T> orbit, final FieldAttitude<T> attitude, final T mass,
165                                 final T massRate, final FieldDataDictionary<T> additional,
166                                 final FieldArrayDictionary<T> additionalDot)
167             throws IllegalArgumentException {
168         this(orbit, null, attitude, mass, massRate, additional, additionalDot, true);
169         checkConsistency(orbit, attitude);
170     }
171 
172     /** Convert a {@link FieldSpacecraftState}.
173      * @param field field to which the elements belong
174      * @param state state to convert
175      */
176     public FieldSpacecraftState(final Field<T> field, final SpacecraftState state) {
177 
178         if (state.isOrbitDefined()) {
179 
180             final Orbit nonFieldOrbit = state.getOrbit();
181             this.orbit    = nonFieldOrbit.getType().convertToFieldOrbit(field, nonFieldOrbit);
182             this.absPva   = null;
183 
184         } else {
185             final TimeStampedPVCoordinates tspva = state.getPVCoordinates();
186             final FieldVector3D<T> position = new FieldVector3D<>(field, tspva.getPosition());
187             final FieldVector3D<T> velocity = new FieldVector3D<>(field, tspva.getVelocity());
188             final FieldVector3D<T> acceleration = new FieldVector3D<>(field, tspva.getAcceleration());
189             final FieldPVCoordinates<T> pva = new FieldPVCoordinates<>(position, velocity, acceleration);
190             final FieldAbsoluteDate<T> dateF = new FieldAbsoluteDate<>(field, state.getDate());
191             this.orbit  = null;
192             this.absPva = new FieldAbsolutePVCoordinates<>(state.getFrame(), dateF, pva);
193         }
194 
195         this.attitude = new FieldAttitude<>(field, state.getAttitude());
196         this.mass     = field.getZero().newInstance(state.getMass());
197         this.massRate = this.mass.newInstance(state.getMassRate());
198 
199         final DataDictionary additionalD = state.getAdditionalDataValues();
200         if (additionalD.size() == 0) {
201             this.additional = new FieldDataDictionary<>(field);
202         } else {
203             this.additional = new FieldDataDictionary<>(field, additionalD.size());
204             for (final DataDictionary.Entry entry : additionalD.getData()) {
205                 if (entry.getValue() instanceof double[]) {
206                     final double[] realValues = (double[]) entry.getValue();
207                     final T[] fieldArray = MathArrays.buildArray(field, realValues.length);
208                     for (int i = 0; i < fieldArray.length; i++) {
209                         fieldArray[i] = field.getZero().add(realValues[i]);
210                     }
211                     this.additional.put(entry.getKey(), fieldArray);
212                 } else {
213                     this.additional.put(entry.getKey(), entry.getValue());
214                 }
215             }
216         }
217         final DoubleArrayDictionary additionalDotD = state.getAdditionalStatesDerivatives();
218         if (additionalDotD.size() == 0) {
219             this.additionalDot = new FieldArrayDictionary<>(field);
220         } else {
221             this.additionalDot = new FieldArrayDictionary<>(field, additionalDotD.size());
222             for (final DoubleArrayDictionary.Entry entry : additionalDotD.getData()) {
223                 this.additionalDot.put(entry.getKey(), entry.getValue());
224             }
225         }
226 
227     }
228 
229     /** Build a spacecraft state from absolute coordinates only.
230      * <p>Attitude and mass are set to unspecified non-null arbitrary values.</p>
231      * @param absPva position-velocity-acceleration
232      */
233     public FieldSpacecraftState(final FieldAbsolutePVCoordinates<T> absPva) {
234         this(absPva,
235              SpacecraftState.getDefaultAttitudeProvider(absPva.getFrame()).
236                      getAttitude(absPva, absPva.getDate(), absPva.getFrame()),
237              absPva.getDate().getField().getZero().newInstance(DEFAULT_MASS), null, null);
238     }
239 
240     /** Build a spacecraft state from absolute coordinates and attitude. Kept for performance.
241      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
242      * @param absPva position-velocity-acceleration
243      * @param attitude attitude
244      * @exception IllegalArgumentException if orbit and attitude dates
245      * or frames are not equal
246      */
247     public FieldSpacecraftState(final FieldAbsolutePVCoordinates<T> absPva, final FieldAttitude<T> attitude)
248         throws IllegalArgumentException {
249         this(absPva, attitude, absPva.getDate().getField().getZero().newInstance(DEFAULT_MASS), null, null);
250         checkConsistency(absPva, attitude);
251     }
252 
253     /** Build a spacecraft state from absolute coordinates, attitude and mass.
254      * @param absPva position-velocity-acceleration
255      * @param attitude attitude
256      * @param mass the mass (kg)
257      * @param additional additional states (may be null if no additional states are available)
258      * @param additionalDot additional states derivatives(may be null if no additional states derivatives are available)
259      * @exception IllegalArgumentException if orbit and attitude dates
260      * or frames are not equal
261      * @since 11.1
262      */
263     public FieldSpacecraftState(final FieldAbsolutePVCoordinates<T> absPva, final FieldAttitude<T> attitude, final T mass,
264                                 final FieldDataDictionary<T> additional, final FieldArrayDictionary<T> additionalDot)
265         throws IllegalArgumentException {
266         this(absPva, attitude, mass, mass.getField().getZero(), additional, additionalDot);
267         checkConsistency(absPva, attitude);
268     }
269 
270     /** Build a spacecraft state from absolute coordinates, attitude, mass and mass rate.
271      * @param absPva position-velocity-acceleration
272      * @param attitude attitude
273      * @param mass the mass (kg)
274      * @param massRate mass rate (kg/s)
275      * @param additional additional states (may be null if no additional states are available)
276      * @param additionalDot additional states derivatives(may be null if no additional states derivatives are available)
277      * @exception IllegalArgumentException if orbit and attitude dates or frames are not equal
278      * @since 14.0
279      */
280     public FieldSpacecraftState(final FieldAbsolutePVCoordinates<T> absPva, final FieldAttitude<T> attitude,
281                                 final T mass, final T massRate,
282                                 final FieldDataDictionary<T> additional, final FieldArrayDictionary<T> additionalDot)
283             throws IllegalArgumentException {
284         this(null, absPva, attitude, mass, massRate, additional, additionalDot, true);
285         checkConsistency(absPva, attitude);
286     }
287 
288     /** Full, private constructor.
289      * @param orbit the orbit
290      * @param absPva absolute position-velocity
291      * @param attitude attitude
292      * @param mass the mass (kg)
293      * @param massRate the mass rate (kg/s)
294      * @param additional additional data (may be null if no additional states are available)
295      * @param additionalDot additional states derivatives (may be null if no additional states derivatives are available)
296      * @param deepCopy flag to copy dictionaries (additional data and derivatives)
297      * @exception IllegalArgumentException if orbit and attitude dates
298      * or frames are not equal
299      * @since 13.1.1
300      */
301     private FieldSpacecraftState(final FieldOrbit<T> orbit, final FieldAbsolutePVCoordinates<T> absPva,
302                                  final FieldAttitude<T> attitude, final T mass, final T massRate,
303                                  final FieldDataDictionary<T> additional, final FieldArrayDictionary<T> additionalDot,
304                                  final boolean deepCopy)
305             throws IllegalArgumentException {
306         this.orbit      = orbit;
307         this.absPva     = absPva;
308         this.attitude   = attitude;
309         this.mass       = mass;
310         this.massRate   = massRate;
311         if (deepCopy) {
312             this.additional = additional == null ? new FieldDataDictionary<>(mass.getField()) : new FieldDataDictionary<>(additional);
313             this.additionalDot = additionalDot == null ? new FieldArrayDictionary<>(mass.getField()) : new FieldArrayDictionary<>(additionalDot);
314         } else {
315             this.additional = additional == null ? new FieldDataDictionary<>(mass.getField()) : additional;
316             this.additionalDot = additionalDot == null ? new FieldArrayDictionary<>(mass.getField()) : additionalDot;
317         }
318     }
319 
320     /**
321      * Create a new instance with input mass.
322      * @param newMass mass
323      * @return new state
324      * @since 13.0
325      */
326     public FieldSpacecraftState<T> withMass(final T newMass) {
327         return new FieldSpacecraftState<>(orbit, absPva, attitude, newMass, massRate, additional, additionalDot, false);
328     }
329 
330     /**
331      * Create a new instance with input mass.
332      * @param newMassRate mass rate
333      * @return new state
334      * @since 14.0
335      */
336     public FieldSpacecraftState<T> withMassRate(final T newMassRate) {
337         return new FieldSpacecraftState<>(orbit, absPva, attitude, mass, newMassRate, additional, additionalDot, false);
338     }
339 
340     /**
341      * Create a new instance with input attitude.
342      * @param newAttitude attitude
343      * @return new state
344      * @since 13.0
345      */
346     public FieldSpacecraftState<T> withAttitude(final FieldAttitude<T> newAttitude) {
347         if (isOrbitDefined()) {
348             checkConsistency(orbit, newAttitude);
349         } else {
350             checkConsistency(absPva, newAttitude);
351         }
352         return new FieldSpacecraftState<>(orbit, absPva, newAttitude, mass, massRate, additional, additionalDot, false);
353     }
354 
355     /**
356      * Create a new instance with input additional data.
357      * @param newAdditional data
358      * @return new state
359      * @since 13.0
360      */
361     public FieldSpacecraftState<T> withAdditionalData(final FieldDataDictionary<T> newAdditional) {
362         return new FieldSpacecraftState<>(orbit, absPva, attitude, mass, massRate, newAdditional, additionalDot, false);
363     }
364 
365     /**
366      * Create a new instance with input additional data.
367      * @param newAdditionalDot additional derivatives
368      * @return new state
369      * @since 13.0
370      */
371     public FieldSpacecraftState<T> withAdditionalStatesDerivatives(final FieldArrayDictionary<T> newAdditionalDot) {
372         return new FieldSpacecraftState<>(orbit, absPva, attitude, mass, massRate, additional, newAdditionalDot, false);
373     }
374 
375     /** Add an additional data.
376      * <p>
377      * {@link FieldSpacecraftState SpacecraftState} instances are immutable,
378      * so this method does <em>not</em> change the instance, but rather
379      * creates a new instance, which has the same orbit, attitude, mass
380      * and additional states as the original instance, except it also
381      * has the specified state. If the original instance already had an
382      * additional state with the same name, it will be overridden. If it
383      * did not have any additional state with that name, the new instance
384      * will have one more additional state than the original instance.
385      * </p>
386      * @param name name of the additional data (names containing "orekit"
387      *      * with any case are reserved for the library internal use)
388      * @param value value of the additional data
389      * @return a new instance, with the additional data added
390      * @see #hasAdditionalData(String)
391      * @see #getAdditionalData(String)
392      * @see #getAdditionalDataValues()
393      */
394     @SuppressWarnings("unchecked") // cast including generic type is checked and unitary tested
395     public final FieldSpacecraftState<T> addAdditionalData(final String name, final Object value) {
396         final FieldDataDictionary<T> newDict = new FieldDataDictionary<>(additional);
397         if (value instanceof CalculusFieldElement[]) {
398             final CalculusFieldElement<T>[] valueArray = (CalculusFieldElement<T>[]) value;
399             newDict.put(name, valueArray.clone());
400         } else if (value instanceof CalculusFieldElement) {
401             final CalculusFieldElement<T>[] valueArray = MathArrays.buildArray(mass.getField(), 1);
402             valueArray[0] = (CalculusFieldElement<T>) value;
403             newDict.put(name, valueArray);
404         } else {
405             newDict.put(name, value);
406         }
407         return withAdditionalData(newDict);
408     }
409 
410     /** Add an additional state derivative.
411     * {@link FieldSpacecraftState FieldSpacecraftState} instances are immutable,
412      * so this method does <em>not</em> change the instance, but rather
413      * creates a new instance, which has the same components as the original
414      * instance, except it also has the specified state derivative. If the
415      * original instance already had an additional state derivative with the
416      * same name, it will be overridden. If it did not have any additional
417      * state derivative with that name, the new instance will have one more
418      * additional state derivative than the original instance.
419      * @param name name of the additional state derivative
420      * @param value value of the additional state derivative
421      * @return a new instance, with the additional state derivative added
422      * @see #hasAdditionalStateDerivative(String)
423      * @see #getAdditionalStateDerivative(String)
424      * @see #getAdditionalStatesDerivatives()
425      */
426     @SafeVarargs
427     public final FieldSpacecraftState<T> addAdditionalStateDerivative(final String name, final T... value) {
428         final FieldArrayDictionary<T> newDict = new FieldArrayDictionary<>(additionalDot);
429         newDict.put(name, value.clone());
430         return withAdditionalStatesDerivatives(newDict);
431     }
432 
433     /** Check orbit and attitude dates are equal.
434      * @param orbitN the orbit
435      * @param attitudeN attitude
436      * @exception IllegalArgumentException if orbit and attitude dates
437      * are not equal
438      */
439     private void checkConsistency(final FieldOrbit<T> orbitN, final FieldAttitude<T> attitudeN)
440         throws IllegalArgumentException {
441         checkDateAndFrameConsistency(attitudeN, orbitN.getDate(), orbitN.getFrame());
442     }
443 
444     /** Check if the state contains an orbit part.
445      * <p>
446      * A state contains either an {@link FieldAbsolutePVCoordinates absolute
447      * position-velocity-acceleration} or an {@link FieldOrbit orbit}.
448      * </p>
449      * @return true if state contains an orbit (in which case {@link #getOrbit()}
450      * will not throw an exception), or false if the state contains an
451      * absolut position-velocity-acceleration (in which case {@link #getAbsPVA()}
452      * will not throw an exception)
453      */
454     public boolean isOrbitDefined() {
455         return orbit != null;
456     }
457 
458     /**
459      * Check FieldAbsolutePVCoordinates and attitude dates are equal.
460      * @param absPva   position-velocity-acceleration
461      * @param attitude attitude
462      * @param <T>      the type of the field elements
463      * @exception IllegalArgumentException if orbit and attitude dates are not equal
464      */
465     private static <T extends CalculusFieldElement<T>> void checkConsistency(final FieldAbsolutePVCoordinates<T> absPva, final FieldAttitude<T> attitude)
466         throws IllegalArgumentException {
467         checkDateAndFrameConsistency(attitude, absPva.getDate(), absPva.getFrame());
468     }
469 
470     /** Check attitude frame and epoch.
471      * @param attitude attitude
472      * @param date epoch to verify
473      * @param frame frame to verify
474      * @param <T> type of the elements
475      */
476     private static <T extends CalculusFieldElement<T>> void checkDateAndFrameConsistency(final FieldAttitude<T> attitude,
477                                                                                          final FieldAbsoluteDate<T> date,
478                                                                                          final Frame frame) {
479         if (date.durationFrom(attitude.getDate()).abs().getReal() >
480                 DATE_INCONSISTENCY_THRESHOLD) {
481             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_AND_ATTITUDE_DATES_MISMATCH,
482                     date, attitude.getDate());
483         }
484         if (frame != attitude.getReferenceFrame()) {
485             throw new OrekitIllegalArgumentException(OrekitMessages.FRAMES_MISMATCH,
486                     frame.getName(),
487                     attitude.getReferenceFrame().getName());
488         }
489     }
490 
491     /** Get a time-shifted state.
492      * <p>
493      * The state can be slightly shifted to close dates. This shift is based on
494      * a simple Keplerian model for orbit and Taylor series for absolute position-velocity.
495      * A linear extrapolation for attitude is used, taking the spin rate into account.
496      * The mass is changed linearly, as well as additional states
497      * if their derivatives are available. It is <em>not</em> intended as a replacement for proper orbit
498      * and attitude propagation but should be sufficient for small time shifts
499      * or coarse accuracy.
500      * </p>
501      * <p>
502      * As a rough order of magnitude, the following table shows the extrapolation
503      * errors obtained between this simple shift method and an {@link
504      * org.orekit.propagation.numerical.FieldNumericalPropagator numerical
505      * propagator} for a low Earth Sun Synchronous Orbit, with a 20x20 gravity field,
506      * Sun and Moon third bodies attractions, drag and solar radiation pressure.
507      * Beware that these results will be different for other orbits.
508      * </p>
509      * <table border="1">
510      * <caption>Extrapolation Error</caption>
511      * <tr style="background-color: #ccccff;"><th>interpolation time (s)</th>
512      * <th>position error without derivatives (m)</th><th>position error with derivatives (m)</th></tr>
513      * <tr><td style="background-color: #eeeeff; padding:5px"> 60</td><td>  18</td><td> 1.1</td></tr>
514      * <tr><td style="background-color: #eeeeff; padding:5px">120</td><td>  72</td><td> 9.1</td></tr>
515      * <tr><td style="background-color: #eeeeff; padding:5px">300</td><td> 447</td><td> 140</td></tr>
516      * <tr><td style="background-color: #eeeeff; padding:5px">600</td><td>1601</td><td>1067</td></tr>
517      * <tr><td style="background-color: #eeeeff; padding:5px">900</td><td>3141</td><td>3307</td></tr>
518      * </table>
519      * @param dt time shift in seconds
520      * @return a new state, shifted with respect to the instance (which is immutable)
521      */
522     @Override
523     public FieldSpacecraftState<T> shiftedBy(final double dt) {
524         if (isOrbitDefined()) {
525             return new FieldSpacecraftState<>(orbit.shiftedBy(dt), null, attitude.shiftedBy(dt), mass.add(massRate.multiply(dt)),
526                                               massRate, shiftAdditional(dt), new FieldArrayDictionary<>(additionalDot), false);
527         } else {
528             return new FieldSpacecraftState<>(null, absPva.shiftedBy(dt), attitude.shiftedBy(dt), mass.add(massRate.multiply(dt)),
529                                               massRate, shiftAdditional(dt), new FieldArrayDictionary<>(additionalDot), false);
530         }
531     }
532 
533     /** Get a time-shifted state.
534      * <p>
535      * The state can be slightly shifted to close dates. This shift is based on
536      * a simple Keplerian model for orbit and Taylor series for absolute position-velocity.
537      * A linear extrapolation for attitude is used, taking the spin rate into account.
538      * The mass is changed linearly, as well as additional states
539      * if their derivatives are available. It is <em>not</em> intended as a replacement for proper orbit
540      * and attitude propagation but should be sufficient for small time shifts
541      * or coarse accuracy.
542      * </p>
543      * <p>
544      * As a rough order of magnitude, the following table shows the extrapolation
545      * errors obtained between this simple shift method and an {@link
546      * org.orekit.propagation.numerical.FieldNumericalPropagator numerical
547      * propagator} for a low Earth Sun Synchronous Orbit, with a 20x20 gravity field,
548      * Sun and Moon third bodies attractions, drag and solar radiation pressure.
549      * Beware that these results will be different for other orbits.
550      * </p>
551      * <table border="1">
552      * <caption>Extrapolation Error</caption>
553      * <tr style="background-color: #ccccff;"><th>interpolation time (s)</th>
554      * <th>position error without derivatives (m)</th><th>position error with derivatives (m)</th></tr>
555      * <tr><td style="background-color: #eeeeff; padding:5px"> 60</td><td>  18</td><td> 1.1</td></tr>
556      * <tr><td style="background-color: #eeeeff; padding:5px">120</td><td>  72</td><td> 9.1</td></tr>
557      * <tr><td style="background-color: #eeeeff; padding:5px">300</td><td> 447</td><td> 140</td></tr>
558      * <tr><td style="background-color: #eeeeff; padding:5px">600</td><td>1601</td><td>1067</td></tr>
559      * <tr><td style="background-color: #eeeeff; padding:5px">900</td><td>3141</td><td>3307</td></tr>
560      * </table>
561      * @param dt time shift in seconds
562      * @return a new state, shifted with respect to the instance (which is immutable)
563      */
564     @Override
565     public FieldSpacecraftState<T> shiftedBy(final T dt) {
566         if (isOrbitDefined()) {
567             return new FieldSpacecraftState<>(orbit.shiftedBy(dt), null, attitude.shiftedBy(dt), mass.add(massRate.multiply(dt)),
568                                               massRate, shiftAdditional(dt), new FieldArrayDictionary<>(additionalDot), false);
569         } else {
570             return new FieldSpacecraftState<>(null, absPva.shiftedBy(dt), attitude.shiftedBy(dt), mass.add(massRate.multiply(dt)),
571                                               massRate, shiftAdditional(dt), new FieldArrayDictionary<>(additionalDot), false);
572         }
573     }
574 
575     /** Shift additional data.
576      * @param dt time shift in seconds
577      * @return shifted additional data
578      * @since 11.1.1
579      */
580     private FieldDataDictionary<T> shiftAdditional(final double dt) {
581 
582         // fast handling when there are no derivatives at all
583         if (additionalDot.size() == 0) {
584             return additional;
585         }
586 
587         // there are derivatives, we need to take them into account in the additional state
588         final FieldDataDictionary<T> shifted = new FieldDataDictionary<>(additional);
589         for (final FieldArrayDictionary<T>.Entry dotEntry : additionalDot.getData()) {
590             final FieldDataDictionary<T>.Entry entry = shifted.getEntry(dotEntry.getKey());
591             if (entry != null) {
592                 entry.scaledIncrement(dt, dotEntry);
593             }
594         }
595 
596         return shifted;
597 
598     }
599 
600     /** Shift additional states.
601      * @param dt time shift in seconds
602      * @return shifted additional states
603      * @since 11.1.1
604      */
605     private FieldDataDictionary<T> shiftAdditional(final T dt) {
606 
607         // fast handling when there are no derivatives at all
608         if (additionalDot.size() == 0) {
609             return additional;
610         }
611 
612         // there are derivatives, we need to take them into account in the additional state
613         final FieldDataDictionary<T> shifted = new FieldDataDictionary<>(additional);
614         for (final FieldArrayDictionary<T>.Entry dotEntry : additionalDot.getData()) {
615             final FieldDataDictionary<T>.Entry entry = shifted.getEntry(dotEntry.getKey());
616             if (entry != null) {
617                 entry.scaledIncrement(dt, dotEntry);
618             }
619         }
620 
621         return shifted;
622 
623     }
624 
625     /** Get the absolute position-velocity-acceleration.
626      * <p>
627      * A state contains either an {@link FieldAbsolutePVCoordinates absolute
628      * position-velocity-acceleration} or an {@link FieldOrbit orbit}. Which
629      * one is present can be checked using {@link #isOrbitDefined()}.
630      * </p>
631      * @return absolute position-velocity-acceleration
632      * @exception OrekitIllegalStateException if position-velocity-acceleration is null,
633      * which mean the state rather contains an {@link FieldOrbit}
634      * @see #isOrbitDefined()
635      * @see #getOrbit()
636      */
637     public FieldAbsolutePVCoordinates<T> getAbsPVA() throws OrekitIllegalStateException {
638         if (isOrbitDefined()) {
639             throw new OrekitIllegalStateException(OrekitMessages.UNDEFINED_ABSOLUTE_PVCOORDINATES);
640         }
641         return absPva;
642     }
643 
644     /** Get the current orbit.
645      * <p>
646      * A state contains either an {@link FieldAbsolutePVCoordinates absolute
647      * position-velocity-acceleration} or an {@link FieldOrbit orbit}. Which
648      * one is present can be checked using {@link #isOrbitDefined()}.
649      * </p>
650      * @return the orbit
651      * @exception OrekitIllegalStateException if orbit is null,
652      * which means the state rather contains an {@link FieldAbsolutePVCoordinates absolute
653      * position-velocity-acceleration}
654      * @see #isOrbitDefined()
655      * @see #getAbsPVA()
656      */
657     public FieldOrbit<T> getOrbit() throws OrekitIllegalStateException {
658         if (orbit == null) {
659             throw new OrekitIllegalStateException(OrekitMessages.UNDEFINED_ORBIT);
660         }
661         return orbit;
662     }
663 
664     /** {@inheritDoc} */
665     @Override
666     public FieldAbsoluteDate<T> getDate() {
667         return isOrbitDefined() ? orbit.getDate() : absPva.getDate();
668     }
669 
670     /** Get the defining frame.
671      * @return the frame in which state is defined
672      */
673     public Frame getFrame() {
674         return isOrbitDefined() ? orbit.getFrame() : absPva.getFrame();
675     }
676 
677 
678     /** Check if an additional data is available.
679      * @param name name of the additional data
680      * @return true if the additional data is available
681      * @see #addAdditionalData(String, Object)
682      * @see #getAdditionalData(String)
683      * @see #getAdditionalDataValues()
684      */
685     public boolean hasAdditionalData(final String name) {
686         return additional.getEntry(name) != null;
687     }
688 
689     /** Check if an additional state derivative is available.
690      * @param name name of the additional state derivative
691      * @return true if the additional state derivative is available
692      * @see #addAdditionalStateDerivative(String, CalculusFieldElement...)
693      * @see #getAdditionalStateDerivative(String)
694      * @see #getAdditionalStatesDerivatives()
695      */
696     public boolean hasAdditionalStateDerivative(final String name) {
697         return additionalDot.getEntry(name) != null;
698     }
699 
700     /** Check if two instances have the same set of additional states available.
701      * <p>
702      * Only the names and dimensions of the additional states are compared,
703      * not their values.
704      * </p>
705      * @param state state to compare to instance
706      * @exception MathIllegalArgumentException if an additional state does not have
707      * the same dimension in both states
708      */
709     @SuppressWarnings("unchecked") // cast including generic type is checked and unitary tested
710     public void ensureCompatibleAdditionalStates(final FieldSpacecraftState<T> state)
711         throws MathIllegalArgumentException {
712 
713         // check instance additional states is a subset of the other one
714         for (final FieldDataDictionary<T>.Entry entry : additional.getData()) {
715             final Object other = state.additional.get(entry.getKey());
716             if (other == null) {
717                 throw new OrekitException(OrekitMessages.UNKNOWN_ADDITIONAL_DATA,
718                                           entry.getKey());
719             }
720             if (other instanceof CalculusFieldElement[]) {
721                 final CalculusFieldElement<T>[] arrayOther = (CalculusFieldElement<T>[]) other;
722                 final CalculusFieldElement<T>[] arrayEntry = (CalculusFieldElement<T>[]) entry.getValue();
723                 if (arrayEntry.length != arrayOther.length) {
724                     throw new MathIllegalStateException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, arrayOther.length, arrayEntry.length);
725                 }
726             }
727         }
728 
729         // check instance additional states derivatives is a subset of the other one
730         for (final FieldArrayDictionary<T>.Entry entry : additionalDot.getData()) {
731             final T[] other = state.additionalDot.get(entry.getKey());
732             if (other == null) {
733                 throw new OrekitException(OrekitMessages.UNKNOWN_ADDITIONAL_DATA,
734                                           entry.getKey());
735             }
736             if (other.length != entry.getValue().length) {
737                 throw new MathIllegalStateException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
738                                                     other.length, entry.getValue().length);
739             }
740         }
741 
742         if (state.additional.size() > additional.size()) {
743             // the other state has more additional states
744             for (final FieldDataDictionary<T>.Entry entry : state.additional.getData()) {
745                 if (additional.getEntry(entry.getKey()) == null) {
746                     throw new OrekitException(OrekitMessages.UNKNOWN_ADDITIONAL_DATA,
747                                               entry.getKey());
748                 }
749             }
750         }
751 
752         if (state.additionalDot.size() > additionalDot.size()) {
753             // the other state has more additional states
754             for (final FieldArrayDictionary<T>.Entry entry : state.additionalDot.getData()) {
755                 if (additionalDot.getEntry(entry.getKey()) == null) {
756                     throw new OrekitException(OrekitMessages.UNKNOWN_ADDITIONAL_DATA,
757                                               entry.getKey());
758                 }
759             }
760         }
761 
762     }
763 
764     /**
765      * Get an additional state.
766      *
767      * @param name name of the additional state
768      * @return value of the additional state
769      * @see #hasAdditionalData(String)
770      * @see #getAdditionalDataValues()
771      */
772     @SuppressWarnings("unchecked") // cast including generic type is checked and unitary tested
773     public T[] getAdditionalState(final String name) {
774         final Object data = getAdditionalData(name);
775         if (data instanceof CalculusFieldElement[]) {
776             return (T[]) data;
777         } else if (data instanceof CalculusFieldElement) {
778             final T[] values = MathArrays.buildArray(mass.getField(), 1);
779             values[0] = (T) data;
780             return values;
781         } else {
782             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_BAD_TYPE, name);
783         }
784     }
785 
786 
787     /**
788      * Get an additional data.
789      *
790      * @param name name of the additional state
791      * @return value of the additional state
792      * @see #addAdditionalData(String, Object)
793      * @see #hasAdditionalData(String)
794      * @see #getAdditionalDataValues()
795      * @since 13.0
796      */
797     public Object getAdditionalData(final String name) {
798         final FieldDataDictionary<T>.Entry entry = additional.getEntry(name);
799         if (entry == null) {
800             throw new OrekitException(OrekitMessages.UNKNOWN_ADDITIONAL_DATA, name);
801         }
802         return entry.getValue();
803     }
804 
805     /** Get an additional state derivative.
806      * @param name name of the additional state derivative
807      * @return value of the additional state derivative
808      * @see #addAdditionalStateDerivative(String, CalculusFieldElement...)
809      * @see #hasAdditionalStateDerivative(String)
810      * @see #getAdditionalStatesDerivatives()
811      * @since 11.1
812      */
813     public T[] getAdditionalStateDerivative(final String name) {
814         final FieldArrayDictionary<T>.Entry entry = additionalDot.getEntry(name);
815         if (entry == null) {
816             throw new OrekitException(OrekitMessages.UNKNOWN_ADDITIONAL_DATA, name);
817         }
818         return entry.getValue();
819     }
820 
821     /** Get an unmodifiable map of additional states.
822      * @return unmodifiable map of additional states
823      * @see #addAdditionalData(String, Object)
824      * @see #hasAdditionalData(String)
825      * @see #getAdditionalData(String)
826      * @since 11.1
827      */
828     public FieldDataDictionary<T> getAdditionalDataValues() {
829         return additional;
830     }
831 
832     /** Get an unmodifiable map of additional states derivatives.
833      * @return unmodifiable map of additional states derivatives
834      * @see #addAdditionalStateDerivative(String, CalculusFieldElement...)
835      * @see #hasAdditionalStateDerivative(String)
836      * @see #getAdditionalStateDerivative(String)
837     * @since 11.1
838       */
839     public FieldArrayDictionary<T> getAdditionalStatesDerivatives() {
840         return additionalDot.unmodifiableView();
841     }
842 
843     /** Compute the transform from state defining frame to spacecraft frame.
844      * <p>The spacecraft frame origin is at the point defined by the orbit,
845      * and its orientation is defined by the attitude.</p>
846      * @return transform from specified frame to current spacecraft frame
847      */
848     public FieldTransform<T> toTransform() {
849         final TimeStampedFieldPVCoordinates<T> pv = getPVCoordinates();
850         return new FieldTransform<>(pv.getDate(),
851                                     new FieldTransform<>(pv.getDate(), pv.negate()),
852                                     new FieldTransform<>(pv.getDate(), attitude.getOrientation()));
853     }
854 
855     /** Compute the static transform from state defining frame to spacecraft frame.
856      * @return static transform from specified frame to current spacecraft frame
857      * @see #toTransform()
858      * @since 12.0
859      */
860     public FieldStaticTransform<T> toStaticTransform() {
861         return FieldStaticTransform.of(getDate(), getPosition().negate(), attitude.getRotation());
862     }
863 
864     /** Get the position in state definition frame.
865      * @return position in state definition frame
866      * @since 12.0
867      */
868     public FieldVector3D<T> getPosition() {
869         return isOrbitDefined() ? orbit.getPosition() : absPva.getPosition();
870     }
871 
872     /** Get the velocity in state definition frame.
873      * @return velocity in state definition frame
874      * @since 13.1
875      */
876     public FieldVector3D<T> getVelocity() {
877         return isOrbitDefined() ? orbit.getVelocity() : absPva.getVelocity();
878     }
879 
880     /** Get the {@link TimeStampedFieldPVCoordinates} in orbit definition frame.
881      * <p>
882      * Compute the position and velocity of the satellite. This method caches its
883      * results, and recompute them only when the method is called with a new value
884      * for mu. The result is provided as a reference to the internally cached
885      * {@link TimeStampedFieldPVCoordinates}, so the caller is responsible to copy it in a separate
886      * {@link TimeStampedFieldPVCoordinates} if it needs to keep the value for a while.
887      * </p>
888      * @return pvCoordinates in orbit definition frame
889      */
890     public TimeStampedFieldPVCoordinates<T> getPVCoordinates() {
891         return isOrbitDefined() ? orbit.getPVCoordinates() : absPva.getPVCoordinates();
892     }
893 
894     /** Get the position in given output frame.
895      * @param outputFrame frame in which position should be defined
896      * @return position in given output frame
897      * @since 12.0
898      * @see #getPVCoordinates(Frame)
899      */
900     public FieldVector3D<T> getPosition(final Frame outputFrame) {
901         return isOrbitDefined() ? orbit.getPosition(outputFrame) : absPva.getPosition(outputFrame);
902     }
903 
904     /** Get the {@link TimeStampedFieldPVCoordinates} in given output frame.
905      * <p>
906      * Compute the position and velocity of the satellite. This method caches its
907      * results, and recompute them only when the method is called with a new value
908      * for mu. The result is provided as a reference to the internally cached
909      * {@link TimeStampedFieldPVCoordinates}, so the caller is responsible to copy it in a separate
910      * {@link TimeStampedFieldPVCoordinates} if it needs to keep the value for a while.
911      * </p>
912      * @param outputFrame frame in which coordinates should be defined
913      * @return pvCoordinates in orbit definition frame
914      */
915     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final Frame outputFrame) {
916         return isOrbitDefined() ? orbit.getPVCoordinates(outputFrame) : absPva.getPVCoordinates(outputFrame);
917     }
918 
919     /** Get the attitude.
920      * @return the attitude.
921      */
922     public FieldAttitude<T> getAttitude() {
923         return attitude;
924     }
925 
926     /** Gets the current mass.
927      * @return the mass (kg)
928      */
929     public T getMass() {
930         return mass;
931     }
932 
933     /** Gets the current mass rate.
934      * @return the mass (kg/S)
935      * @since 14.0
936      */
937     public T getMassRate() {
938         return massRate;
939     }
940 
941     /**To convert a FieldSpacecraftState instance into a SpacecraftState instance.
942      *
943      * @return SpacecraftState instance with the same properties
944      */
945     @SuppressWarnings("unchecked") // cast including generic type is checked and unitary tested
946     public SpacecraftState toSpacecraftState() {
947         final DataDictionary dictionary;
948         if (additional.size() == 0) {
949             dictionary = new DataDictionary();
950         } else {
951             dictionary = new DataDictionary(additional.size());
952             for (final FieldDataDictionary<T>.Entry entry : additional.getData()) {
953                 if (entry.getValue() instanceof CalculusFieldElement[]) {
954                     final CalculusFieldElement<T>[] entryArray  = (CalculusFieldElement<T>[]) entry.getValue();
955                     final double[] realArray = new double[entryArray.length];
956                     for (int k = 0; k < realArray.length; ++k) {
957                         realArray[k] = entryArray[k].getReal();
958                     }
959                     dictionary.put(entry.getKey(), realArray);
960                 } else {
961                     dictionary.put(entry.getKey(), entry.getValue());
962                 }
963             }
964         }
965         final DoubleArrayDictionary dictionaryDot;
966         if (additionalDot.size() == 0) {
967             dictionaryDot = new DoubleArrayDictionary();
968         } else {
969             dictionaryDot = new DoubleArrayDictionary(additionalDot.size());
970             for (final FieldArrayDictionary<T>.Entry entry : additionalDot.getData()) {
971                 final double[] array = new double[entry.getValue().length];
972                 for (int k = 0; k < array.length; ++k) {
973                     array[k] = entry.getValue()[k].getReal();
974                 }
975                 dictionaryDot.put(entry.getKey(), array);
976             }
977         }
978         if (isOrbitDefined()) {
979             return new SpacecraftState(orbit.toOrbit(), attitude.toAttitude(),
980                                        mass.getReal(), dictionary, dictionaryDot);
981         } else {
982             return new SpacecraftState(absPva.toAbsolutePVCoordinates(),
983                                        attitude.toAttitude(), mass.getReal(),
984                                        dictionary, dictionaryDot);
985         }
986     }
987 
988     @Override
989     public String toString() {
990         return "FieldSpacecraftState{" +
991                 "orbit=" + orbit +
992                 ", absPva=" + absPva +
993                 ", attitude=" + attitude +
994                 ", mass=" + mass +
995                 ", massRate=" + massRate +
996                 ", additional=" + additional +
997                 ", additionalDot=" + additionalDot +
998                 '}';
999     }
1000 
1001 }