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