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.forces.gravity;
18  
19  import org.hipparchus.Field;
20  import org.hipparchus.analysis.differentiation.DSFactory;
21  import org.hipparchus.analysis.differentiation.DerivativeStructure;
22  import org.hipparchus.analysis.differentiation.Gradient;
23  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24  import org.hipparchus.geometry.euclidean.threed.Vector3D;
25  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
26  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
27  import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
28  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
29  import org.hipparchus.util.FastMath;
30  import org.junit.jupiter.api.Assertions;
31  import org.junit.jupiter.api.BeforeEach;
32  import org.junit.jupiter.api.Test;
33  import org.orekit.Utils;
34  import org.orekit.attitudes.LofOffset;
35  import org.orekit.bodies.CelestialBody;
36  import org.orekit.bodies.CelestialBodyFactory;
37  import org.orekit.forces.AbstractLegacyForceModelTest;
38  import org.orekit.forces.ForceModel;
39  import org.orekit.frames.Frame;
40  import org.orekit.frames.FramesFactory;
41  import org.orekit.frames.LOFType;
42  import org.orekit.orbits.CartesianOrbit;
43  import org.orekit.orbits.FieldKeplerianOrbit;
44  import org.orekit.orbits.KeplerianOrbit;
45  import org.orekit.orbits.Orbit;
46  import org.orekit.orbits.OrbitType;
47  import org.orekit.orbits.PositionAngleType;
48  import org.orekit.propagation.FieldSpacecraftState;
49  import org.orekit.propagation.SpacecraftState;
50  import org.orekit.propagation.ToleranceProvider;
51  import org.orekit.propagation.numerical.FieldNumericalPropagator;
52  import org.orekit.propagation.numerical.NumericalPropagator;
53  import org.orekit.time.AbsoluteDate;
54  import org.orekit.time.DateComponents;
55  import org.orekit.time.FieldAbsoluteDate;
56  import org.orekit.time.TimeComponents;
57  import org.orekit.time.TimeScalesFactory;
58  import org.orekit.utils.Constants;
59  import org.orekit.utils.FieldPVCoordinates;
60  import org.orekit.utils.PVCoordinates;
61  
62  public class SingleBodyRelativeAttractionTest extends AbstractLegacyForceModelTest {
63  
64      @BeforeEach
65      public void setUp() {
66          Utils.setDataRoot("regular-data");
67      }
68  
69      @Override
70      protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel, final FieldSpacecraftState<DerivativeStructure> state) {
71  
72          try {
73              final AbsoluteDate                       date     = state.getDate().toAbsoluteDate();
74              final FieldVector3D<DerivativeStructure> position = state.getPVCoordinates().getPosition();
75              java.lang.reflect.Field bodyField = AbstractBodyAttraction.class.getDeclaredField("positionProvider");
76              bodyField.setAccessible(true);
77              CelestialBody body = (CelestialBody) bodyField.get(forceModel);
78              double gm = forceModel.
79                          getParameterDriver(body.getName() + SingleBodyRelativeAttraction.ATTRACTION_COEFFICIENT_SUFFIX).
80                          getValue(date);
81  
82              // compute bodies separation vectors and squared norm
83              final FieldPVCoordinates<DerivativeStructure> bodyPV = body.getPVCoordinates(state.getDate(), state.getFrame());
84              final FieldVector3D<DerivativeStructure> satToBody = position.subtract(bodyPV.getPosition()).negate();
85              final DerivativeStructure r2Sat = satToBody.getNormSq();
86  
87              // compute relative acceleration
88              final FieldVector3D<DerivativeStructure> satAcc =
89                      new FieldVector3D<>(r2Sat.divide(gm).reciprocal(), satToBody.normalize()).add(bodyPV.getAcceleration());
90              return satAcc;
91  
92  
93          } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
94              return null;
95          }
96  
97      }
98  
99      @Override
100     protected FieldVector3D<Gradient> accelerationDerivativesGradient(final ForceModel forceModel, final FieldSpacecraftState<Gradient> state) {
101 
102         try {
103             final AbsoluteDate                       date     = state.getDate().toAbsoluteDate();
104             final FieldVector3D<Gradient> position = state.getPVCoordinates().getPosition();
105             java.lang.reflect.Field bodyField = AbstractBodyAttraction.class.getDeclaredField("positionProvider");
106             bodyField.setAccessible(true);
107             CelestialBody body = (CelestialBody) bodyField.get(forceModel);
108             double gm = forceModel.
109                         getParameterDriver(body.getName() + SingleBodyRelativeAttraction.ATTRACTION_COEFFICIENT_SUFFIX).
110                         getValue(date);
111 
112             // compute bodies separation vectors and squared norm
113             final FieldPVCoordinates<Gradient> bodyPV = body.getPVCoordinates(state.getDate(), state.getFrame());
114             final FieldVector3D<Gradient> satToBody = position.subtract(bodyPV.getPosition()).negate();
115             final Gradient r2Sat = satToBody.getNormSq();
116 
117             // compute relative acceleration
118             final FieldVector3D<Gradient> satAcc =
119                     new FieldVector3D<>(r2Sat.divide(gm).reciprocal(), satToBody.normalize()).add(bodyPV.getAcceleration());
120             return satAcc;
121 
122 
123         } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
124             return null;
125         }
126 
127     }
128 
129     @Test
130     public void RealFieldTest() {
131         DSFactory factory = new DSFactory(6, 5);
132         DerivativeStructure a_0 = factory.variable(0, 7e7);
133         DerivativeStructure e_0 = factory.variable(1, 0.4);
134         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
135         DerivativeStructure R_0 = factory.variable(3, 0.7);
136         DerivativeStructure O_0 = factory.variable(4, 0.5);
137         DerivativeStructure n_0 = factory.variable(5, 0.1);
138         DerivativeStructure mu  = factory.constant(Constants.EIGEN5C_EARTH_MU);
139 
140         Field<DerivativeStructure> field = a_0.getField();
141 
142         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
143 
144         Frame EME = FramesFactory.getEME2000();
145 
146         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
147                                                                                  PositionAngleType.MEAN,
148                                                                                  EME,
149                                                                                  J2000,
150                                                                                  mu);
151 
152         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
153 
154         SpacecraftState iSR = initialState.toSpacecraftState();
155         OrbitType type = OrbitType.KEPLERIAN;
156         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
157 
158 
159         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
160                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
161         integrator.setInitialStepSize(60);
162         AdaptiveStepsizeIntegrator RIntegrator =
163                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
164         RIntegrator.setInitialStepSize(60);
165 
166         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
167         FNP.setOrbitType(type);
168         FNP.setInitialState(initialState);
169 
170         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
171         NP.setOrbitType(type);
172         NP.setInitialState(iSR);
173 
174         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(CelestialBodyFactory.getSun());
175 
176         FNP.addForceModel(forceModel);
177         NP.addForceModel(forceModel);
178 
179         // Do the test
180         checkRealFieldPropagation(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
181                                   1.0e-15, 5.0e-10, 3.0e-11, 3.0e-10,
182                                   1, false);
183     }
184 
185     @Test
186     public void RealFieldGradientTest() {
187         int freeParameters = 6;
188         Gradient a_0 = Gradient.variable(freeParameters, 0, 7e7);
189         Gradient e_0 = Gradient.variable(freeParameters, 1, 0.4);
190         Gradient i_0 = Gradient.variable(freeParameters, 2, 85 * FastMath.PI / 180);
191         Gradient R_0 = Gradient.variable(freeParameters, 3, 0.7);
192         Gradient O_0 = Gradient.variable(freeParameters, 4, 0.5);
193         Gradient n_0 = Gradient.variable(freeParameters, 5, 0.1);
194         Gradient mu  = Gradient.constant(freeParameters, Constants.EIGEN5C_EARTH_MU);
195 
196         Field<Gradient> field = a_0.getField();
197 
198         FieldAbsoluteDate<Gradient> J2000 = new FieldAbsoluteDate<>(field);
199 
200         Frame EME = FramesFactory.getEME2000();
201 
202         FieldKeplerianOrbit<Gradient> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
203                                                                       PositionAngleType.MEAN,
204                                                                       EME,
205                                                                       J2000,
206                                                                       mu);
207 
208         FieldSpacecraftState<Gradient> initialState = new FieldSpacecraftState<>(FKO);
209 
210         SpacecraftState iSR = initialState.toSpacecraftState();
211         OrbitType type = OrbitType.KEPLERIAN;
212         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
213 
214 
215         AdaptiveStepsizeFieldIntegrator<Gradient> integrator =
216                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
217         integrator.setInitialStepSize(60);
218         AdaptiveStepsizeIntegrator RIntegrator =
219                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
220         RIntegrator.setInitialStepSize(60);
221 
222         FieldNumericalPropagator<Gradient> FNP = new FieldNumericalPropagator<>(field, integrator);
223         FNP.setOrbitType(type);
224         FNP.setInitialState(initialState);
225 
226         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
227         NP.setOrbitType(type);
228         NP.setInitialState(iSR);
229 
230         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(CelestialBodyFactory.getSun());
231 
232         FNP.addForceModel(forceModel);
233         NP.addForceModel(forceModel);
234 
235         // Do the test
236         checkRealFieldPropagationGradient(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
237                                   1.0e-15, 1.3e-2, 2.9e-4, 1.4e-3,
238                                   1, false);
239     }
240 
241     @Test
242     public void RealFieldExpectErrorTest() {
243         DSFactory factory = new DSFactory(6, 5);
244         DerivativeStructure a_0 = factory.variable(0, 7e7);
245         DerivativeStructure e_0 = factory.variable(1, 0.4);
246         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
247         DerivativeStructure R_0 = factory.variable(3, 0.7);
248         DerivativeStructure O_0 = factory.variable(4, 0.5);
249         DerivativeStructure n_0 = factory.variable(5, 0.1);
250 
251         Field<DerivativeStructure> field = a_0.getField();
252         DerivativeStructure zero = field.getZero();
253 
254         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
255 
256         Frame EME = FramesFactory.getEME2000();
257 
258         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
259                                                                                  PositionAngleType.MEAN,
260                                                                                  EME,
261                                                                                  J2000,
262                                                                                  zero.add(Constants.EIGEN5C_EARTH_MU));
263 
264         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
265 
266         SpacecraftState iSR = initialState.toSpacecraftState();
267         OrbitType type = OrbitType.KEPLERIAN;
268         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(0.001).getTolerances(FKO.toOrbit(), type);
269 
270 
271         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
272                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
273         integrator.setInitialStepSize(60);
274         AdaptiveStepsizeIntegrator RIntegrator =
275                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
276         RIntegrator.setInitialStepSize(60);
277 
278         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
279         FNP.setOrbitType(type);
280         FNP.setInitialState(initialState);
281 
282         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
283         NP.setOrbitType(type);
284         NP.setInitialState(iSR);
285 
286         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(CelestialBodyFactory.getSun());
287 
288         FNP.addForceModel(forceModel);
289         //NOT ADDING THE FORCE MODEL TO THE NUMERICAL PROPAGATOR   NP.addForceModel(forceModel);
290 
291         FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
292         FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
293         SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
294         FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
295         PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
296 
297         Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getX() - finPVC_R.getPosition().getX()) < FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
298         Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getY() - finPVC_R.getPosition().getY()) < FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
299         Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getZ() - finPVC_R.getPosition().getZ()) < FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
300     }
301 
302     @Test
303     void testParameterDerivative() {
304 
305         final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
306         final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
307         final SpacecraftState state =
308                 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
309                                                        FramesFactory.getGCRF(),
310                                                        new AbsoluteDate(2003, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
311                                                        Constants.EIGEN5C_EARTH_MU));
312 
313         final CelestialBody moon = CelestialBodyFactory.getMoon();
314         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
315         Assertions.assertTrue(forceModel.dependsOnPositionOnly());
316         final String name = moon.getName() + SingleBodyRelativeAttraction.ATTRACTION_COEFFICIENT_SUFFIX;
317         checkParameterDerivative(state, forceModel, name, 1.0, 7.0e-15);
318 
319     }
320 
321     @Test
322     void testParameterDerivativeGradient() {
323 
324         final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
325         final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
326         final SpacecraftState state =
327                 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
328                                                        FramesFactory.getGCRF(),
329                                                        new AbsoluteDate(2003, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
330                                                        Constants.EIGEN5C_EARTH_MU));
331 
332         final CelestialBody moon = CelestialBodyFactory.getMoon();
333         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
334         Assertions.assertTrue(forceModel.dependsOnPositionOnly());
335         final String name = moon.getName() + SingleBodyRelativeAttraction.ATTRACTION_COEFFICIENT_SUFFIX;
336         checkParameterDerivativeGradient(state, forceModel, name, 1.0, 7.0e-15);
337 
338     }
339 
340     @Test
341     void testJacobianVs80Implementation() {
342         // initialization
343         AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
344                                              new TimeComponents(13, 59, 27.816),
345                                              TimeScalesFactory.getUTC());
346         double i     = FastMath.toRadians(98.7);
347         double omega = FastMath.toRadians(93.0);
348         double OMEGA = FastMath.toRadians(15.0 * 22.5);
349         Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
350                                          0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
351                                          Constants.EIGEN5C_EARTH_MU);
352         final CelestialBody moon = CelestialBodyFactory.getMoon();
353         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
354         checkStateJacobianVs80Implementation(new SpacecraftState(orbit), forceModel,
355                                              new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS),
356                                              5.0e-16, false);
357     }
358 
359     @Test
360     void testJacobianVs80ImplementationGradient() {
361         // initialization
362         AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
363                                              new TimeComponents(13, 59, 27.816),
364                                              TimeScalesFactory.getUTC());
365         double i     = FastMath.toRadians(98.7);
366         double omega = FastMath.toRadians(93.0);
367         double OMEGA = FastMath.toRadians(15.0 * 22.5);
368         Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
369                                          0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
370                                          Constants.EIGEN5C_EARTH_MU);
371         final CelestialBody moon = CelestialBodyFactory.getMoon();
372         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
373         checkStateJacobianVs80ImplementationGradient(new SpacecraftState(orbit), forceModel,
374                                              new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS),
375                                              1.0e-16, false);
376     }
377 
378     @Test
379     void testGlobalStateJacobian()
380         {
381 
382         // initialization
383         AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
384                                              new TimeComponents(13, 59, 27.816),
385                                              TimeScalesFactory.getUTC());
386         double i     = FastMath.toRadians(98.7);
387         double omega = FastMath.toRadians(93.0);
388         double OMEGA = FastMath.toRadians(15.0 * 22.5);
389         Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
390                                          0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
391                                          Constants.EIGEN5C_EARTH_MU);
392         OrbitType integrationType = OrbitType.CARTESIAN;
393         double[][] tolerances = ToleranceProvider.getDefaultToleranceProvider(0.01).getTolerances(orbit, integrationType);
394 
395         NumericalPropagator propagator =
396                 new NumericalPropagator(new DormandPrince853Integrator(1.0e-3, 120,
397                                                                        tolerances[0], tolerances[1]));
398         propagator.setOrbitType(integrationType);
399         final CelestialBody moon = CelestialBodyFactory.getMoon();
400         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
401         propagator.addForceModel(forceModel);
402         SpacecraftState state0 = new SpacecraftState(orbit);
403 
404         checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0),
405                            1e4, tolerances[0], 9.0e-9);
406 
407     }
408 
409 }