1   /* Copyright 2022-2025 Romain Serra
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.utils;
18  
19  import org.hipparchus.analysis.differentiation.Gradient;
20  import org.hipparchus.analysis.differentiation.GradientField;
21  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
22  import org.hipparchus.geometry.euclidean.threed.Vector3D;
23  import org.hipparchus.linear.MatrixUtils;
24  import org.hipparchus.linear.RealMatrix;
25  import org.hipparchus.util.MathArrays;
26  import org.junit.jupiter.api.Test;
27  import org.junit.jupiter.params.ParameterizedTest;
28  import org.junit.jupiter.params.provider.EnumSource;
29  import org.junit.jupiter.params.provider.ValueSource;
30  import org.orekit.TestUtils;
31  import org.orekit.attitudes.Attitude;
32  import org.orekit.attitudes.LofOffset;
33  import org.orekit.frames.LOFType;
34  import org.orekit.orbits.EquinoctialOrbit;
35  import org.orekit.orbits.FieldCircularOrbit;
36  import org.orekit.orbits.FieldEquinoctialOrbit;
37  import org.orekit.orbits.FieldKeplerianOrbit;
38  import org.orekit.orbits.FieldOrbit;
39  import org.orekit.orbits.Orbit;
40  import org.orekit.orbits.OrbitType;
41  import org.orekit.orbits.PositionAngleType;
42  import org.orekit.propagation.FieldSpacecraftState;
43  import org.orekit.propagation.SpacecraftState;
44  import org.orekit.time.AbsoluteDate;
45  
46  import static org.junit.jupiter.api.Assertions.*;
47  
48  class DerivativeStateUtilsTest {
49  
50      @ParameterizedTest
51      @EnumSource(OrbitType.class)
52      void testBuildOrbitGradient(final OrbitType orbitType) {
53          // GIVEN
54          final Orbit orbit = orbitType.convertType(TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH));
55          final GradientField field = GradientField.getField(6);
56          final PositionAngleType positionAngleType = PositionAngleType.TRUE;
57          // WHEN
58          final FieldOrbit<Gradient> fieldOrbit = DerivativeStateUtils.buildOrbitGradient(field, orbit);
59          // THEN
60          final double[] expected = new double[6];
61          orbitType.mapOrbitToArray(orbit, positionAngleType, expected, null);
62          final Gradient[] actual = MathArrays.buildArray(field, 6);
63          orbitType.mapOrbitToArray(fieldOrbit, positionAngleType, actual, null);
64          for (int i = 0; i < actual.length; i++) {
65              assertEquals(expected[i], actual[i].getReal());
66          }
67          switch (orbitType) {
68  
69              case CARTESIAN:
70                  assertEquals(1., fieldOrbit.getPosition().getX().getGradient()[0]);
71                  assertEquals(1., fieldOrbit.getPosition().getY().getGradient()[1]);
72                  assertEquals(1., fieldOrbit.getPosition().getZ().getGradient()[2]);
73                  assertEquals(1., fieldOrbit.getVelocity().getX().getGradient()[3]);
74                  assertEquals(1., fieldOrbit.getVelocity().getY().getGradient()[4]);
75                  assertEquals(1., fieldOrbit.getVelocity().getZ().getGradient()[5]);
76                  break;
77  
78              case EQUINOCTIAL:
79                  final FieldEquinoctialOrbit<Gradient> fieldEquinoctialOrbit = (FieldEquinoctialOrbit<Gradient>) fieldOrbit;
80                  assertEquals(1., fieldOrbit.getA().getGradient()[0]);
81                  assertEquals(1., fieldOrbit.getEquinoctialEx().getGradient()[1]);
82                  assertEquals(1., fieldOrbit.getEquinoctialEy().getGradient()[2]);
83                  assertEquals(1., fieldOrbit.getHx().getGradient()[3]);
84                  assertEquals(1., fieldOrbit.getHy().getGradient()[4]);
85                  assertEquals(1., fieldEquinoctialOrbit.getL(fieldEquinoctialOrbit.getCachedPositionAngleType()).getGradient()[5]);
86                  assertEquals(orbit.getADot(), fieldEquinoctialOrbit.getADot().getReal());
87                  assertEquals(orbit.getEquinoctialExDot(), fieldEquinoctialOrbit.getEquinoctialExDot().getReal());
88                  assertEquals(orbit.getEquinoctialEyDot(), fieldEquinoctialOrbit.getEquinoctialEyDot().getReal());
89                  assertEquals(orbit.getHxDot(), fieldEquinoctialOrbit.getHxDot().getReal());
90                  assertEquals(orbit.getHyDot(), fieldEquinoctialOrbit.getHyDot().getReal());
91                  final EquinoctialOrbit equinoctialOrbit = (EquinoctialOrbit) orbit;
92                  assertEquals(equinoctialOrbit.getLDot(equinoctialOrbit.getCachedPositionAngleType()),
93                          fieldEquinoctialOrbit.getLDot(fieldEquinoctialOrbit.getCachedPositionAngleType()).getReal());
94                  break;
95  
96              case CIRCULAR:
97                  final FieldCircularOrbit<Gradient> fieldCircularOrbit = (FieldCircularOrbit<Gradient>) fieldOrbit;
98                  assertEquals(1., fieldCircularOrbit.getA().getGradient()[0]);
99                  assertEquals(1., fieldCircularOrbit.getCircularEx().getGradient()[1]);
100                 assertEquals(1., fieldCircularOrbit.getCircularEy().getGradient()[2]);
101                 assertEquals(1., fieldCircularOrbit.getI().getGradient()[3]);
102                 assertEquals(1., fieldCircularOrbit.getRightAscensionOfAscendingNode().getGradient()[4]);
103                 assertEquals(1., fieldCircularOrbit.getAlpha(fieldCircularOrbit.getCachedPositionAngleType()).getGradient()[5]);
104                 break;
105 
106 
107             case KEPLERIAN:
108                 final FieldKeplerianOrbit<Gradient> fieldKeplerianOrbit = (FieldKeplerianOrbit<Gradient>) fieldOrbit;
109                 assertEquals(1., fieldKeplerianOrbit.getA().getGradient()[0]);
110                 assertEquals(1., fieldKeplerianOrbit.getE().getGradient()[1]);
111                 assertEquals(1., fieldKeplerianOrbit.getI().getGradient()[2]);
112                 assertEquals(1., fieldKeplerianOrbit.getPerigeeArgument().getGradient()[3]);
113                 assertEquals(1., fieldKeplerianOrbit.getRightAscensionOfAscendingNode().getGradient()[4]);
114                 assertEquals(1., fieldKeplerianOrbit.getAnomaly(fieldKeplerianOrbit.getCachedPositionAngleType()).getGradient()[5]);
115                 break;
116         }
117     }
118 
119     @ParameterizedTest
120     @ValueSource(ints = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15})
121     void testBuildOrbitGradientCartesian(final int freeParameters) {
122         // GIVEN
123         final Orbit orbit = OrbitType.CARTESIAN.convertType(TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH));
124         final GradientField field = GradientField.getField(freeParameters);
125         // WHEN
126         final FieldOrbit<Gradient> fieldOrbit = DerivativeStateUtils.buildOrbitGradient(field, orbit);
127         // THEN
128         assertEquals(orbit.getFrame(), fieldOrbit.getFrame());
129         assertEquals(orbit.getDate(), fieldOrbit.getDate().toAbsoluteDate());
130         assertArrayEquals(new double[freeParameters], fieldOrbit.getDate().durationFrom(orbit.getDate()).getGradient());
131         final FieldVector3D<Gradient> fieldPosition = fieldOrbit.getPosition();
132         final Vector3D position = orbit.getPosition();
133         assertEquals(position.getX(), fieldPosition.getX().getReal());
134         assertEquals(position.getY(), fieldPosition.getY().getReal());
135         assertEquals(position.getZ(), fieldPosition.getZ().getReal());
136         assertEquals(1., fieldOrbit.getPosition().getX().getGradient()[0]);
137         for (int i = 1; i < freeParameters; i++) {
138             assertEquals(0., fieldOrbit.getPosition().getX().getGradient()[i]);
139         }
140         if (freeParameters <= 3) {
141             assertArrayEquals(orbit.getVelocity().toArray(),
142                     fieldOrbit.getVelocity().toVector3D().toArray());
143         } else {
144             assertEquals(1., fieldOrbit.getPosition().getY().getGradient()[1]);
145             assertEquals(1., fieldOrbit.getPosition().getZ().getGradient()[2]);
146         }
147     }
148 
149     @ParameterizedTest
150     @ValueSource(ints = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15})
151     void testBuildOrbitGradientPV(final int freeParameters) {
152         // GIVEN
153         final Orbit orbit = OrbitType.CARTESIAN.convertType(TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH));
154         final GradientField field = GradientField.getField(freeParameters);
155         final AbsolutePVCoordinates pvCoordinates = new AbsolutePVCoordinates(orbit.getFrame(), orbit.getDate(),
156                 orbit.getPVCoordinates());
157         // WHEN
158         final FieldAbsolutePVCoordinates<Gradient> fieldPV = DerivativeStateUtils.buildAbsolutePVGradient(field, pvCoordinates);
159         // THEN
160         assertEquals(pvCoordinates.getFrame(), fieldPV.getFrame());
161         assertEquals(pvCoordinates.getDate(), fieldPV.getDate().toAbsoluteDate());
162         assertArrayEquals(new double[freeParameters], fieldPV.getDate().durationFrom(pvCoordinates.getDate()).getGradient());
163         final FieldOrbit<Gradient> fieldOrbit = DerivativeStateUtils.buildOrbitGradient(field, orbit);
164         assertArrayEquals(fieldPV.getPosition().toArray(), fieldOrbit.getPosition().toArray());
165         assertArrayEquals(fieldPV.getVelocity().toArray(),
166                 fieldOrbit.getVelocity().toArray());
167     }
168 
169     @ParameterizedTest
170     @ValueSource(ints = {3, 7})
171     void testBuildStateGradientNullAttitudeProvider(final int freeParameters) {
172         // GIVEN
173         final Orbit orbit = TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH);
174         final GradientField field = GradientField.getField(freeParameters);
175         final SpacecraftState state = new SpacecraftState(orbit);
176         // WHEN
177         final FieldSpacecraftState<Gradient> fieldState = DerivativeStateUtils.buildSpacecraftStateGradient(field, state, null);
178         // THEN
179         assertEquals(state.getMass(), fieldState.getMass().getReal());
180         if (freeParameters == 7) {
181             assertArrayEquals(new double[]{0., 0., 0., 0., 0., 0., 1.}, fieldState.getMass().getGradient());
182         }
183         assertEquals(state.getDate(), fieldState.getDate().toAbsoluteDate());
184         assertEquals(state.getPosition(), fieldState.getPosition().toVector3D());
185         assertEquals(state.getAttitude().getRotationAcceleration(), fieldState.getAttitude().getRotationAcceleration().toVector3D());
186         assertEquals(state.getAttitude().getSpin(), fieldState.getAttitude().getSpin().toVector3D());
187     }
188 
189     @Test
190     void testBuildStateGradientNonNullAttitudeProvider() {
191         // GIVEN
192         final Orbit orbit = TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH);
193         final GradientField field = GradientField.getField(6);
194         final SpacecraftState state = new SpacecraftState(orbit);
195         final LofOffset lofOffset = new LofOffset(orbit.getFrame(), LOFType.TNW);
196         // WHEN
197         final FieldSpacecraftState<Gradient> fieldState = DerivativeStateUtils.buildSpacecraftStateGradient(field, state, lofOffset);
198         // THEN
199         assertEquals(state.getMass(), fieldState.getMass().getReal());
200         assertEquals(state.getDate(), fieldState.getDate().toAbsoluteDate());
201         assertEquals(state.getPosition(), fieldState.getPosition().toVector3D());
202         final Attitude attitude = lofOffset.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
203         assertArrayEquals(attitude.getSpin().toArray(), fieldState.getAttitude().getSpin().toVector3D().toArray(), 1e-12);
204         assertArrayEquals(attitude.getRotationAcceleration().toArray(),
205                 fieldState.getAttitude().getRotationAcceleration().toVector3D().toArray(), 1e-12);
206     }
207 
208     @Test
209     void testBuildSpacecraftStateTransitionGradientAbsolutePV() {
210         // GIVEN
211         final Orbit orbit = TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH);
212         final AbsolutePVCoordinates pvCoordinates = new AbsolutePVCoordinates(orbit.getFrame(), orbit.getDate(),
213                 orbit.getPVCoordinates());
214         final SpacecraftState state = new SpacecraftState(pvCoordinates);
215         final double[][] data = new double[7][];
216         for (int i = 0; i < 7; i++) {
217             data[i] = new double[7];
218             for (int j = 0; j < 7; j++) {
219                 data[i][j] = j + i;
220             }
221         }
222         final RealMatrix partials = MatrixUtils.createRealMatrix(data);
223         // WHEN
224         final FieldSpacecraftState<Gradient> fieldState = DerivativeStateUtils.buildSpacecraftStateTransitionGradient(state, partials, null);
225         // THEN
226         assertEquals(state.getDate(), fieldState.getDate().toAbsoluteDate());
227         assertEquals(state.getMass(), fieldState.getMass().getReal());
228         assertEquals(state.getPosition(), fieldState.getPosition().toVector3D());
229         assertArrayEquals(data[0], fieldState.getPosition().getX().getGradient());
230         assertArrayEquals(data[1], fieldState.getPosition().getY().getGradient());
231         assertArrayEquals(data[2], fieldState.getPosition().getZ().getGradient());
232         assertArrayEquals(data[3], fieldState.getVelocity().getX().getGradient());
233         assertArrayEquals(data[4], fieldState.getVelocity().getY().getGradient());
234         assertArrayEquals(data[5], fieldState.getVelocity().getZ().getGradient());
235         assertArrayEquals(data[6], fieldState.getMass().getGradient());
236     }
237 
238     @Test
239     void testBuildSpacecraftStateTransitionGradientIdentityAbsolutePV() {
240         // GIVEN
241         final Orbit orbit = TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH);
242         final AbsolutePVCoordinates pvCoordinates = new AbsolutePVCoordinates(orbit.getFrame(), orbit.getDate(),
243                 orbit.getPVCoordinates());
244         final SpacecraftState state = new SpacecraftState(pvCoordinates);
245         final RealMatrix identity = MatrixUtils.createRealIdentityMatrix(3);
246         // WHEN
247         final FieldSpacecraftState<Gradient> fieldState = DerivativeStateUtils.buildSpacecraftStateTransitionGradient(state, identity, null);
248         // THEN
249         final GradientField field = GradientField.getField(identity.getColumnDimension());
250         final FieldSpacecraftState<Gradient> expectedFieldState = DerivativeStateUtils.buildSpacecraftStateGradient(field, state, null);
251         assertEquals(expectedFieldState.getMass(), fieldState.getMass());
252         assertEquals(expectedFieldState.getDate(), fieldState.getDate());
253         assertEquals(expectedFieldState.getPosition(), fieldState.getPosition());
254         assertEquals(expectedFieldState.getVelocity(), fieldState.getVelocity());
255         assertEquals(expectedFieldState.getPVCoordinates().getAcceleration(), fieldState.getPVCoordinates().getAcceleration());
256     }
257 
258     @Test
259     void testBuildSpacecraftStateTransitionGradientIdentityOrbit() {
260         // GIVEN
261         final Orbit orbit = TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH);
262         final SpacecraftState state = new SpacecraftState(orbit);
263         final RealMatrix identity = MatrixUtils.createRealIdentityMatrix(3);
264         final LofOffset lofOffset = new LofOffset(orbit.getFrame(), LOFType.QSW);
265         // WHEN
266         final FieldSpacecraftState<Gradient> fieldState = DerivativeStateUtils.buildSpacecraftStateTransitionGradient(state, identity, lofOffset);
267         // THEN
268         final GradientField field = GradientField.getField(identity.getColumnDimension());
269         final FieldSpacecraftState<Gradient> expectedFieldState = DerivativeStateUtils.buildSpacecraftStateGradient(field, state, lofOffset);
270         assertEquals(expectedFieldState.getMass(), fieldState.getMass());
271         assertEquals(expectedFieldState.getDate(), fieldState.getDate());
272         assertEquals(expectedFieldState.getPosition(), fieldState.getPosition());
273         assertEquals(expectedFieldState.getVelocity(), fieldState.getVelocity());
274         assertEquals(expectedFieldState.getPVCoordinates().getAcceleration(), fieldState.getPVCoordinates().getAcceleration());
275     }
276 }
277