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