1   /* Copyright 2002-2025 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.propagation.integration;
18  
19  import java.util.ArrayList;
20  import java.util.List;
21  
22  import org.hipparchus.Field;
23  import org.hipparchus.analysis.differentiation.Gradient;
24  import org.hipparchus.analysis.differentiation.GradientField;
25  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
26  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
27  import org.hipparchus.geometry.euclidean.threed.Vector3D;
28  import org.orekit.attitudes.AttitudeProvider;
29  import org.orekit.attitudes.FieldAttitude;
30  import org.orekit.orbits.OrbitType;
31  import org.orekit.orbits.FieldCartesianOrbit;
32  import org.orekit.orbits.FieldEquinoctialOrbit;
33  import org.orekit.orbits.FieldOrbit;
34  import org.orekit.orbits.PositionAngleType;
35  import org.orekit.propagation.FieldSpacecraftState;
36  import org.orekit.propagation.SpacecraftState;
37  import org.orekit.time.AbsoluteDate;
38  import org.orekit.time.FieldAbsoluteDate;
39  import org.orekit.utils.FieldAngularCoordinates;
40  import org.orekit.utils.FieldPVCoordinates;
41  import org.orekit.utils.FieldAbsolutePVCoordinates;
42  import org.orekit.utils.ParameterDriver;
43  import org.orekit.utils.ParameterDriversProvider;
44  import org.orekit.utils.TimeStampedFieldAngularCoordinates;
45  import org.orekit.utils.TimeStampedFieldPVCoordinates;
46  import org.orekit.utils.TimeSpanMap.Span;
47  
48  /** Converter for states and parameters arrays.
49   *  @author Luc Maisonobe
50   *  @author Bryan Cazabonne
51   *  @since 10.2
52   */
53  public abstract class AbstractGradientConverter {
54  
55      /** Dimension of the state. */
56      private final int freeStateParameters;
57  
58      /** States with various number of additional parameters. */
59      private final List<FieldSpacecraftState<Gradient>> gStates;
60  
61      /** Simple constructor.
62       * @param freeStateParameters number of free parameters
63       */
64      protected AbstractGradientConverter(final int freeStateParameters) {
65          this.freeStateParameters = freeStateParameters;
66          this.gStates             = new ArrayList<>();
67      }
68  
69      /** Get the number of free state parameters.
70       * @return number of free state parameters
71       */
72      public int getFreeStateParameters() {
73          return freeStateParameters;
74      }
75  
76      /** Initialize first state with 0 parameters.
77       * @param zeroParametersState state with zero parameters
78       * @since 11.2
79       */
80      protected void initStates(final FieldSpacecraftState<Gradient> zeroParametersState) {
81          gStates.clear();
82          gStates.add(zeroParametersState);
83      }
84  
85      /** Add zero derivatives.
86       * @param original original scalar
87       * @param freeParameters total number of free parameters in the gradient
88       * @return extended scalar
89       */
90      protected Gradient extend(final Gradient original, final int freeParameters) {
91          final double[] originalDerivatives = original.getGradient();
92          final double[] extendedDerivatives = new double[freeParameters];
93          System.arraycopy(originalDerivatives, 0, extendedDerivatives, 0, originalDerivatives.length);
94          return new Gradient(original.getValue(), extendedDerivatives);
95      }
96  
97      /**
98       * Add zero derivatives.
99       *
100      * @param original       original date
101      * @param freeParameters total number of free parameters in the gradient
102      * @return extended date
103      */
104     protected FieldAbsoluteDate<Gradient> extend(final FieldAbsoluteDate<Gradient> original, final int freeParameters) {
105         final AbsoluteDate date = original.toAbsoluteDate();
106         final Gradient gradient = original.durationFrom(date);
107         return new FieldAbsoluteDate<>(date, extend(gradient, freeParameters));
108     }
109 
110     /** Add zero derivatives.
111      * @param original original vector
112      * @param freeParameters total number of free parameters in the gradient
113      * @return extended vector
114      */
115     protected FieldVector3D<Gradient> extend(final FieldVector3D<Gradient> original, final int freeParameters) {
116         return new FieldVector3D<>(extend(original.getX(), freeParameters),
117                                    extend(original.getY(), freeParameters),
118                                    extend(original.getZ(), freeParameters));
119     }
120 
121     /** Add zero derivatives.
122      * @param original original rotation
123      * @param freeParameters total number of free parameters in the gradient
124      * @return extended rotation
125      */
126     protected FieldRotation<Gradient> extend(final FieldRotation<Gradient> original, final int freeParameters) {
127         return new FieldRotation<>(extend(original.getQ0(), freeParameters),
128                                    extend(original.getQ1(), freeParameters),
129                                    extend(original.getQ2(), freeParameters),
130                                    extend(original.getQ3(), freeParameters),
131                                    false);
132     }
133 
134     /** Process a state into a Gradient version without force model parameter.
135      * @param state state
136      * @param freeStateParameters number of free parameters
137      * @param provider attitude provider
138      * @return Gradient version of the state
139      * @since 12.0
140      */
141     protected static FieldSpacecraftState<Gradient> buildBasicGradientSpacecraftState(final SpacecraftState state,
142                                                                                       final int freeStateParameters,
143                                                                                       final AttitudeProvider provider) {
144 
145         // Derivative field
146         final Field<Gradient> field =  GradientField.getField(freeStateParameters);
147 
148         // position always has derivatives
149         final Vector3D pos = state.getPosition();
150         final FieldVector3D<Gradient> posG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 0, pos.getX()),
151                                                                  Gradient.variable(freeStateParameters, 1, pos.getY()),
152                                                                  Gradient.variable(freeStateParameters, 2, pos.getZ()));
153 
154         // velocity may have derivatives or not
155         final Vector3D vel = state.getPVCoordinates().getVelocity();
156         final FieldVector3D<Gradient> velG;
157         if (freeStateParameters > 3) {
158             velG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 3, vel.getX()),
159                                        Gradient.variable(freeStateParameters, 4, vel.getY()),
160                                        Gradient.variable(freeStateParameters, 5, vel.getZ()));
161         } else {
162             velG = new FieldVector3D<>(field, vel);
163         }
164 
165         // acceleration never has derivatives
166         final Vector3D acc = state.getPVCoordinates().getAcceleration();
167         final FieldVector3D<Gradient> accG = new FieldVector3D<>(field, acc);
168 
169         // mass never has derivatives
170         final Gradient gMass = Gradient.constant(freeStateParameters, state.getMass());
171 
172         final TimeStampedFieldPVCoordinates<Gradient> timeStampedFieldPVCoordinates =
173             new TimeStampedFieldPVCoordinates<>(state.getDate(), posG, velG, accG);
174 
175         final FieldCartesianOrbit<Gradient> gOrbit;
176         final FieldAbsolutePVCoordinates<Gradient> gAbsolutePV;
177         if (state.isOrbitDefined()) {
178             final Gradient gMu = Gradient.constant(freeStateParameters, state.getOrbit().getMu());
179             gOrbit = new FieldCartesianOrbit<>(timeStampedFieldPVCoordinates, state.getFrame(), gMu);
180             gAbsolutePV = null;
181         } else {
182             gOrbit = null;
183             gAbsolutePV = new FieldAbsolutePVCoordinates<>(state.getFrame(), timeStampedFieldPVCoordinates);
184         }
185 
186         final FieldAttitude<Gradient> gAttitude;
187         if (freeStateParameters > 3) {
188             // compute attitude partial derivatives with respect to position/velocity
189             gAttitude = provider.getAttitude((state.isOrbitDefined()) ? gOrbit : gAbsolutePV,
190                                              timeStampedFieldPVCoordinates.getDate(), state.getFrame());
191         } else {
192             // force model does not depend on attitude, don't bother recomputing it
193             gAttitude = new FieldAttitude<>(field, state.getAttitude());
194         }
195 
196         if (state.isOrbitDefined()) {
197             return new FieldSpacecraftState<>(gOrbit, gAttitude).withMass(gMass);
198         } else {
199             return new FieldSpacecraftState<>(gAbsolutePV, gAttitude).withMass(gMass);
200         }
201 
202     }
203 
204     /**
205      * Get the state with the number of parameters consistent with parametric model.
206      * @param parametricModel parametric model
207      * @return state with the number of parameters consistent with parametric model
208      */
209     public FieldSpacecraftState<Gradient> getState(final ParameterDriversProvider parametricModel) {
210 
211         // count the required number of parameters
212         int nbParams = 0;
213         for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
214             if (driver.isSelected()) {
215                 nbParams += driver.getNbOfValues();
216             }
217         }
218 
219         // fill in intermediate slots
220         while (gStates.size() < nbParams + 1) {
221             gStates.add(null);
222         }
223 
224         if (gStates.get(nbParams) == null) {
225             // it is the first time we need this number of parameters
226             // we need to create the state
227             final int freeParameters = freeStateParameters + nbParams;
228             final FieldSpacecraftState<Gradient> s0 = gStates.get(0);
229             final AbsoluteDate date = s0.getDate().toAbsoluteDate();
230 
231             // attitude
232             final FieldAngularCoordinates<Gradient> ac0 = s0.getAttitude().getOrientation();
233             final FieldAttitude<Gradient> gAttitude =
234                     new FieldAttitude<>(s0.getAttitude().getReferenceFrame(),
235                             new TimeStampedFieldAngularCoordinates<>(date,
236                                     extend(ac0.getRotation(), freeParameters),
237                                     extend(ac0.getRotationRate(), freeParameters),
238                                     extend(ac0.getRotationAcceleration(), freeParameters)));
239 
240             // mass
241             final Gradient gMass = extend(s0.getMass(), freeParameters);
242 
243             // orbit or absolute position-velocity coordinates
244             final FieldPVCoordinates<Gradient> pv0 = s0.getPVCoordinates();
245             final TimeStampedFieldPVCoordinates<Gradient> timeStampedFieldPVCoordinates = new TimeStampedFieldPVCoordinates<>(
246                     date,
247                     extend(pv0.getPosition(),     freeParameters),
248                     extend(pv0.getVelocity(),     freeParameters),
249                     extend(pv0.getAcceleration(), freeParameters));
250             final FieldSpacecraftState<Gradient> spacecraftState;
251             if (s0.isOrbitDefined()) {
252                 final FieldOrbit<Gradient> orbit = s0.getOrbit();
253                 if (orbit.getType().equals(OrbitType.EQUINOCTIAL)) {
254                     // for DSST, which always uses EquinoctialOrbit, not CartesianOrbit
255                     spacecraftState =
256                         new FieldSpacecraftState<>(new FieldEquinoctialOrbit<>(extend(orbit.getA(), freeParameters),
257                                                                                extend(orbit.getEquinoctialEx(), freeParameters),
258                                                                                extend(orbit.getEquinoctialEy(), freeParameters),
259                                                                                extend(orbit.getHx(), freeParameters),
260                                                                                extend(orbit.getHy(), freeParameters),
261                                                                                extend(orbit.getLM(), freeParameters),
262                                                                                PositionAngleType.MEAN,
263                                                                                s0.getFrame(),
264                                                                                extend(s0.getDate(), freeParameters),
265                                                                                extend(orbit.getMu(), freeParameters)),
266                             gAttitude).withMass(gMass);
267                 } else {
268                     spacecraftState =
269                         new FieldSpacecraftState<>(new FieldCartesianOrbit<>(timeStampedFieldPVCoordinates,
270                                                                              s0.getFrame(),
271                                                                              extend(orbit.getMu(), freeParameters)),
272                                                    gAttitude).withMass(gMass);
273                 }
274             } else {
275                 spacecraftState =
276                     new FieldSpacecraftState<>(new FieldAbsolutePVCoordinates<>(s0.getFrame(),
277                                                                                 timeStampedFieldPVCoordinates),
278                                                gAttitude).withMass(gMass);
279             }
280 
281             gStates.set(nbParams, spacecraftState);
282 
283         }
284 
285         return gStates.get(nbParams);
286 
287     }
288 
289     /** Get the parametric model parameters, return gradient values for each span of each driver (several gradient
290      * values for each parameter).
291      * Different from {@link #getParametersAtStateDate(FieldSpacecraftState, ParameterDriversProvider)}
292      * which return a Gradient list containing for each driver the gradient value at state date (only 1 gradient
293      * value for each parameter).
294      * @param state state as returned by {@link #getState(ParameterDriversProvider) getState(parametricModel)}
295      * @param parametricModel parametric model associated with the parameters
296      * @return parametric model parameters (for all span of each driver)
297      */
298     public Gradient[] getParameters(final FieldSpacecraftState<Gradient> state,
299                                     final ParameterDriversProvider parametricModel) {
300         final int freeParameters = state.getMass().getFreeParameters();
301         final List<ParameterDriver> drivers = parametricModel.getParametersDrivers();
302         int sizeDrivers = 0;
303         for ( ParameterDriver driver : drivers) {
304             sizeDrivers += driver.getNbOfValues();
305         }
306         final Gradient[] parameters = new Gradient[sizeDrivers];
307         int index = freeStateParameters;
308         int i = 0;
309         for (ParameterDriver driver : drivers) {
310             // Loop on the spans
311             for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
312                 parameters[i++] = driver.isSelected() ?
313                                   Gradient.variable(freeParameters, index++, span.getData()) :
314                                   Gradient.constant(freeParameters, span.getData());
315             }
316         }
317         return parameters;
318     }
319 
320     /** Get the parametric model parameters, return gradient values at state date for each driver (only 1 gradient
321      * value for each parameter).
322      * Different from {@link #getParameters(FieldSpacecraftState, ParameterDriversProvider)}
323      * which return a Gradient list containing for each driver the gradient values for each span value (several gradient
324      * values for each parameter).
325      * @param state state as returned by {@link #getState(ParameterDriversProvider) getState(parametricModel)}
326      * @param parametricModel parametric model associated with the parameters
327      * @return parametric model parameters (for all span of each driver)
328      */
329     public Gradient[] getParametersAtStateDate(final FieldSpacecraftState<Gradient> state,
330             final ParameterDriversProvider parametricModel) {
331         final int freeParameters = state.getMass().getFreeParameters();
332         final List<ParameterDriver> drivers = parametricModel.getParametersDrivers();
333 
334         final Gradient[] parameters = new Gradient[drivers.size()];
335         int index = freeStateParameters;
336         int i = 0;
337         for (ParameterDriver driver : drivers) {
338             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
339                 if (span.getData().equals(driver.getNameSpan(state.getDate().toAbsoluteDate()))) {
340                     parameters[i++] = driver.isSelected() ?
341                                           Gradient.variable(freeParameters, index, driver.getValue(state.getDate().toAbsoluteDate())) :
342                                           Gradient.constant(freeParameters, driver.getValue(state.getDate().toAbsoluteDate()));
343                 }
344                 index = driver.isSelected() ? index + 1 : index;
345             }
346         }
347         return parameters;
348     }
349 
350 
351 }