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             Gradient gmGrad = state.getDate().getField().getZero().add(gm);
112 
113             // compute bodies separation vectors and squared norm
114             final FieldPVCoordinates<Gradient> bodyPV = body.getPVCoordinates(state.getDate(), state.getFrame());
115             final FieldVector3D<Gradient> satToBody = position.subtract(bodyPV.getPosition()).negate();
116             final Gradient r2Sat = satToBody.getNormSq();
117 
118             // compute relative acceleration
119             final FieldVector3D<Gradient> satAcc =
120                     new FieldVector3D<>(gmGrad.divide(r2Sat), satToBody.normalize()).add(bodyPV.getAcceleration());
121             return satAcc;
122 
123 
124         } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
125             return null;
126         }
127 
128     }
129 
130     @Test
131     public void RealFieldTest() {
132         DSFactory factory = new DSFactory(6, 5);
133         DerivativeStructure a_0 = factory.variable(0, 7e7);
134         DerivativeStructure e_0 = factory.variable(1, 0.4);
135         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
136         DerivativeStructure R_0 = factory.variable(3, 0.7);
137         DerivativeStructure O_0 = factory.variable(4, 0.5);
138         DerivativeStructure n_0 = factory.variable(5, 0.1);
139         DerivativeStructure mu  = factory.constant(Constants.EIGEN5C_EARTH_MU);
140 
141         Field<DerivativeStructure> field = a_0.getField();
142 
143         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
144 
145         Frame EME = FramesFactory.getEME2000();
146 
147         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
148                                                                                  PositionAngleType.MEAN,
149                                                                                  EME,
150                                                                                  J2000,
151                                                                                  mu);
152 
153         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
154 
155         SpacecraftState iSR = initialState.toSpacecraftState();
156         OrbitType type = OrbitType.KEPLERIAN;
157         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
158 
159 
160         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
161                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
162         integrator.setInitialStepSize(60);
163         AdaptiveStepsizeIntegrator RIntegrator =
164                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
165         RIntegrator.setInitialStepSize(60);
166 
167         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
168         FNP.setOrbitType(type);
169         FNP.setInitialState(initialState);
170 
171         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
172         NP.setOrbitType(type);
173         NP.setInitialState(iSR);
174 
175         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(CelestialBodyFactory.getSun());
176 
177         FNP.addForceModel(forceModel);
178         NP.addForceModel(forceModel);
179 
180         // Do the test
181         checkRealFieldPropagation(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
182                                   1.0e-15, 5.0e-10, 3.0e-11, 3.0e-10,
183                                   1, false);
184     }
185 
186     @Test
187     public void RealFieldGradientTest() {
188         int freeParameters = 6;
189         Gradient a_0 = Gradient.variable(freeParameters, 0, 7e7);
190         Gradient e_0 = Gradient.variable(freeParameters, 1, 0.4);
191         Gradient i_0 = Gradient.variable(freeParameters, 2, 85 * FastMath.PI / 180);
192         Gradient R_0 = Gradient.variable(freeParameters, 3, 0.7);
193         Gradient O_0 = Gradient.variable(freeParameters, 4, 0.5);
194         Gradient n_0 = Gradient.variable(freeParameters, 5, 0.1);
195         Gradient mu  = Gradient.constant(freeParameters, Constants.EIGEN5C_EARTH_MU);
196 
197         Field<Gradient> field = a_0.getField();
198 
199         FieldAbsoluteDate<Gradient> J2000 = new FieldAbsoluteDate<>(field);
200 
201         Frame EME = FramesFactory.getEME2000();
202 
203         FieldKeplerianOrbit<Gradient> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
204                                                                       PositionAngleType.MEAN,
205                                                                       EME,
206                                                                       J2000,
207                                                                       mu);
208 
209         FieldSpacecraftState<Gradient> initialState = new FieldSpacecraftState<>(FKO);
210 
211         SpacecraftState iSR = initialState.toSpacecraftState();
212         OrbitType type = OrbitType.KEPLERIAN;
213         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
214 
215 
216         AdaptiveStepsizeFieldIntegrator<Gradient> integrator =
217                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
218         integrator.setInitialStepSize(60);
219         AdaptiveStepsizeIntegrator RIntegrator =
220                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
221         RIntegrator.setInitialStepSize(60);
222 
223         FieldNumericalPropagator<Gradient> FNP = new FieldNumericalPropagator<>(field, integrator);
224         FNP.setOrbitType(type);
225         FNP.setInitialState(initialState);
226 
227         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
228         NP.setOrbitType(type);
229         NP.setInitialState(iSR);
230 
231         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(CelestialBodyFactory.getSun());
232 
233         FNP.addForceModel(forceModel);
234         NP.addForceModel(forceModel);
235 
236         // Do the test
237         checkRealFieldPropagationGradient(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
238                                   1.0e-15, 1.3e-2, 2.9e-4, 1.4e-3,
239                                   1, false);
240     }
241 
242     @Test
243     public void RealFieldExpectErrorTest() {
244         DSFactory factory = new DSFactory(6, 5);
245         DerivativeStructure a_0 = factory.variable(0, 7e7);
246         DerivativeStructure e_0 = factory.variable(1, 0.4);
247         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
248         DerivativeStructure R_0 = factory.variable(3, 0.7);
249         DerivativeStructure O_0 = factory.variable(4, 0.5);
250         DerivativeStructure n_0 = factory.variable(5, 0.1);
251 
252         Field<DerivativeStructure> field = a_0.getField();
253         DerivativeStructure zero = field.getZero();
254 
255         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
256 
257         Frame EME = FramesFactory.getEME2000();
258 
259         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
260                                                                                  PositionAngleType.MEAN,
261                                                                                  EME,
262                                                                                  J2000,
263                                                                                  zero.add(Constants.EIGEN5C_EARTH_MU));
264 
265         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
266 
267         SpacecraftState iSR = initialState.toSpacecraftState();
268         OrbitType type = OrbitType.KEPLERIAN;
269         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(0.001).getTolerances(FKO.toOrbit(), type);
270 
271 
272         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
273                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
274         integrator.setInitialStepSize(60);
275         AdaptiveStepsizeIntegrator RIntegrator =
276                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
277         RIntegrator.setInitialStepSize(60);
278 
279         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
280         FNP.setOrbitType(type);
281         FNP.setInitialState(initialState);
282 
283         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
284         NP.setOrbitType(type);
285         NP.setInitialState(iSR);
286 
287         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(CelestialBodyFactory.getSun());
288 
289         FNP.addForceModel(forceModel);
290         //NOT ADDING THE FORCE MODEL TO THE NUMERICAL PROPAGATOR   NP.addForceModel(forceModel);
291 
292         FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
293         FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
294         SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
295         FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
296         PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
297 
298         Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getX() - finPVC_R.getPosition().getX()) < FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
299         Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getY() - finPVC_R.getPosition().getY()) < FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
300         Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getZ() - finPVC_R.getPosition().getZ()) < FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
301     }
302 
303     @Test
304     void testParameterDerivative() {
305 
306         final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
307         final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
308         final SpacecraftState state =
309                 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
310                                                        FramesFactory.getGCRF(),
311                                                        new AbsoluteDate(2003, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
312                                                        Constants.EIGEN5C_EARTH_MU));
313 
314         final CelestialBody moon = CelestialBodyFactory.getMoon();
315         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
316         Assertions.assertTrue(forceModel.dependsOnPositionOnly());
317         final String name = moon.getName() + SingleBodyRelativeAttraction.ATTRACTION_COEFFICIENT_SUFFIX;
318         checkParameterDerivative(state, forceModel, name, 1.0, 7.0e-15);
319 
320     }
321 
322     @Test
323     void testParameterDerivativeGradient() {
324 
325         final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
326         final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
327         final SpacecraftState state =
328                 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
329                                                        FramesFactory.getGCRF(),
330                                                        new AbsoluteDate(2003, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
331                                                        Constants.EIGEN5C_EARTH_MU));
332 
333         final CelestialBody moon = CelestialBodyFactory.getMoon();
334         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
335         Assertions.assertTrue(forceModel.dependsOnPositionOnly());
336         final String name = moon.getName() + SingleBodyRelativeAttraction.ATTRACTION_COEFFICIENT_SUFFIX;
337         checkParameterDerivativeGradient(state, forceModel, name, 1.0, 7.0e-15);
338 
339     }
340 
341     @Test
342     void testJacobianVs80Implementation() {
343         // initialization
344         AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
345                                              new TimeComponents(13, 59, 27.816),
346                                              TimeScalesFactory.getUTC());
347         double i     = FastMath.toRadians(98.7);
348         double omega = FastMath.toRadians(93.0);
349         double OMEGA = FastMath.toRadians(15.0 * 22.5);
350         Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
351                                          0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
352                                          Constants.EIGEN5C_EARTH_MU);
353         final CelestialBody moon = CelestialBodyFactory.getMoon();
354         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
355         checkStateJacobianVs80Implementation(new SpacecraftState(orbit), forceModel,
356                                              new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS),
357                                              5.0e-16, false);
358     }
359 
360     @Test
361     void testJacobianVs80ImplementationGradient() {
362         // initialization
363         AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
364                                              new TimeComponents(13, 59, 27.816),
365                                              TimeScalesFactory.getUTC());
366         double i     = FastMath.toRadians(98.7);
367         double omega = FastMath.toRadians(93.0);
368         double OMEGA = FastMath.toRadians(15.0 * 22.5);
369         Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
370                                          0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
371                                          Constants.EIGEN5C_EARTH_MU);
372         final CelestialBody moon = CelestialBodyFactory.getMoon();
373         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
374         checkStateJacobianVs80ImplementationGradient(new SpacecraftState(orbit), forceModel,
375                                              new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS),
376                                              1.0e-16, false);
377     }
378 
379     @Test
380     void testGlobalStateJacobian()
381         {
382 
383         // initialization
384         AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
385                                              new TimeComponents(13, 59, 27.816),
386                                              TimeScalesFactory.getUTC());
387         double i     = FastMath.toRadians(98.7);
388         double omega = FastMath.toRadians(93.0);
389         double OMEGA = FastMath.toRadians(15.0 * 22.5);
390         Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
391                                          0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
392                                          Constants.EIGEN5C_EARTH_MU);
393         OrbitType integrationType = OrbitType.CARTESIAN;
394         double[][] tolerances = ToleranceProvider.getDefaultToleranceProvider(0.01).getTolerances(orbit, integrationType);
395 
396         NumericalPropagator propagator =
397                 new NumericalPropagator(new DormandPrince853Integrator(1.0e-3, 120,
398                                                                        tolerances[0], tolerances[1]));
399         propagator.setOrbitType(integrationType);
400         final CelestialBody moon = CelestialBodyFactory.getMoon();
401         final SingleBodyRelativeAttraction forceModel = new SingleBodyRelativeAttraction(moon);
402         propagator.addForceModel(forceModel);
403         SpacecraftState state0 = new SpacecraftState(orbit);
404 
405         checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0),
406                            1e4, tolerances[0], 9.0e-9);
407 
408     }
409 
410 }