1   /* Contributed in the public domain.
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.AbstractIntegrator;
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.jupiter.api.Assertions;
32  import org.junit.jupiter.api.BeforeAll;
33  import org.junit.jupiter.api.Test;
34  import org.orekit.Utils;
35  import org.orekit.attitudes.LofOffset;
36  import org.orekit.forces.AbstractLegacyForceModelTest;
37  import org.orekit.forces.ForceModel;
38  import org.orekit.frames.Frame;
39  import org.orekit.frames.FramesFactory;
40  import org.orekit.frames.LOFType;
41  import org.orekit.orbits.CartesianOrbit;
42  import org.orekit.orbits.CircularOrbit;
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.FieldAbsoluteDate;
55  import org.orekit.utils.Constants;
56  import org.orekit.utils.FieldPVCoordinates;
57  import org.orekit.utils.PVCoordinates;
58  
59  /** Unit tests for {@link Relativity}. */
60  public class RelativityTest extends AbstractLegacyForceModelTest {
61  
62      /** speed of light */
63      private static final double c = Constants.SPEED_OF_LIGHT;
64      /** arbitrary date */
65      private static final AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
66      /** inertial frame */
67      private static final Frame frame = FramesFactory.getGCRF();
68  
69      @Override
70      protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel,
71                                                                           final FieldSpacecraftState<DerivativeStructure> state)
72          {
73          try {
74              final FieldVector3D<DerivativeStructure> position = state.getPVCoordinates().getPosition();
75              final FieldVector3D<DerivativeStructure> velocity = state.getPVCoordinates().getVelocity();
76              double gm = forceModel.
77                          getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT).
78                          getValue(state.getDate().toAbsoluteDate());
79              //radius
80              final DerivativeStructure r2 = position.getNormSq();
81              final DerivativeStructure r = r2.sqrt();
82              //speed squared
83              final DerivativeStructure s2 = velocity.getNormSq();
84              final double c2 = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT;
85              //eq. 3.146
86              return new FieldVector3D<>(r.reciprocal().multiply(4 * gm).subtract(s2),
87                                         position,
88                                         position.dotProduct(velocity).multiply(4),
89                                         velocity).scalarMultiply(r2.multiply(r).multiply(c2).reciprocal().multiply(gm));
90  
91          } catch (IllegalArgumentException | SecurityException e) {
92              return null;
93          }
94      }
95  
96      @Override
97      protected FieldVector3D<Gradient> accelerationDerivativesGradient(final ForceModel forceModel,
98                                                                        final FieldSpacecraftState<Gradient> state)
99          {
100         try {
101             final FieldVector3D<Gradient> position = state.getPVCoordinates().getPosition();
102             final FieldVector3D<Gradient> velocity = state.getPVCoordinates().getVelocity();
103             double gm = forceModel.
104                         getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT).
105                         getValue(state.getDate().toAbsoluteDate());
106             //radius
107             final Gradient r2 = position.getNormSq();
108             final Gradient r = r2.sqrt();
109             //speed squared
110             final Gradient s2 = velocity.getNormSq();
111             final double c2 = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT;
112             //eq. 3.146
113             return new FieldVector3D<>(r.reciprocal().multiply(4 * gm).subtract(s2),
114                                        position,
115                                        position.dotProduct(velocity).multiply(4),
116                                        velocity).scalarMultiply(r2.multiply(r).multiply(c2).reciprocal().multiply(gm));
117 
118         } catch (IllegalArgumentException | SecurityException e) {
119             return null;
120         }
121     }
122 
123     /** set orekit data */
124     @BeforeAll
125     public static void setUpBefore() {
126         Utils.setDataRoot("regular-data");
127     }
128 
129     /**
130      * check the acceleration from relativity
131      */
132     @Test
133     void testAcceleration() {
134         double gm = Constants.EIGEN5C_EARTH_MU;
135         Relativity relativity = new Relativity(gm);
136         Assertions.assertFalse(relativity.dependsOnPositionOnly());
137         final Vector3D p = new Vector3D(3777828.75000531, -5543949.549783845, 2563117.448578311);
138         final Vector3D v = new Vector3D(489.0060271721, -2849.9328929417, -6866.4671013153);
139         SpacecraftState s = new SpacecraftState(new CartesianOrbit(
140                 new PVCoordinates(p, v),
141                 frame,
142                 date,
143                 gm
144         ));
145 
146         //action
147         Vector3D acceleration = relativity.acceleration(s, relativity.getParameters(s.getDate()));
148 
149         //verify
150         //force is ~1e-8 so this give ~3 sig figs.
151         double tol = 2e-11;
152         Vector3D circularApproximation = p.normalize().scalarMultiply(
153                 gm / p.getNormSq() * 3 * v.getNormSq() / (c * c));
154         Assertions.assertEquals(
155                 0,
156                 acceleration.subtract(circularApproximation).getNorm(),
157                 tol);
158         //check derivatives
159         FieldSpacecraftState<DerivativeStructure> sDS = toDS(s, new LofOffset(s.getFrame(), LOFType.LVLH_CCSDS));
160         final Vector3D actualDerivatives = relativity
161                 .acceleration(sDS, relativity.getParameters(sDS.getDate().getField(), sDS.getDate()))
162                 .toVector3D();
163         Assertions.assertEquals(
164                 0,
165                 actualDerivatives.subtract(circularApproximation).getNorm(),
166                 tol);
167     }
168 
169     @Test
170     void testJacobianVs80Implementation() {
171         double gm = Constants.EIGEN5C_EARTH_MU;
172         Relativity relativity = new Relativity(gm);
173         final Vector3D p = new Vector3D(3777828.75000531, -5543949.549783845, 2563117.448578311);
174         final Vector3D v = new Vector3D(489.0060271721, -2849.9328929417, -6866.4671013153);
175         SpacecraftState s = new SpacecraftState(new CartesianOrbit(
176                 new PVCoordinates(p, v),
177                 frame,
178                 date,
179                 gm
180         ));
181 
182         checkStateJacobianVs80Implementation(s, relativity,
183                                              new LofOffset(s.getFrame(), LOFType.LVLH_CCSDS),
184                                              1.0e-50, false);
185     }
186 
187     @Test
188     void testJacobianVs80ImplementationGradient() {
189         double gm = Constants.EIGEN5C_EARTH_MU;
190         Relativity relativity = new Relativity(gm);
191         final Vector3D p = new Vector3D(3777828.75000531, -5543949.549783845, 2563117.448578311);
192         final Vector3D v = new Vector3D(489.0060271721, -2849.9328929417, -6866.4671013153);
193         SpacecraftState s = new SpacecraftState(new CartesianOrbit(
194                 new PVCoordinates(p, v),
195                 frame,
196                 date,
197                 gm
198         ));
199 
200         checkStateJacobianVs80ImplementationGradient(s, relativity,
201                                              new LofOffset(s.getFrame(), LOFType.LVLH_CCSDS),
202                                              1.0e-50, false);
203     }
204 
205     /**
206      * Check a nearly circular orbit.
207      */
208     @Test
209     void testAccelerationCircular() {
210         double gm = Constants.EIGEN5C_EARTH_MU;
211         double re = Constants.WGS84_EARTH_EQUATORIAL_RADIUS;
212         Relativity relativity = new Relativity(gm);
213         final CircularOrbit orbit = new CircularOrbit(
214                 re + 500e3, 0, 0, FastMath.toRadians(41.2), -1, 3, PositionAngleType.TRUE,
215                 frame,
216                 date,
217                 gm
218         );
219         SpacecraftState state = new SpacecraftState(orbit);
220 
221         //action
222         Vector3D acceleration = relativity.acceleration(state, relativity.getParameters(state.getDate()));
223 
224         //verify
225         //force is ~1e-8 so this give ~7 sig figs.
226         double tol = 2e-10;
227         PVCoordinates pv = state.getPVCoordinates();
228         Vector3D p = pv.getPosition();
229         Vector3D v = pv.getVelocity();
230         Vector3D circularApproximation = p.normalize().scalarMultiply(
231                 gm / p.getNormSq() * 3 * v.getNormSq() / (c * c));
232         Assertions.assertEquals(
233                 0,
234                 acceleration.subtract(circularApproximation).getNorm(),
235                 tol);
236         //check derivatives
237         FieldSpacecraftState<DerivativeStructure> sDS = toDS(state, new LofOffset(state.getFrame(), LOFType.LVLH_CCSDS));
238         FieldVector3D<DerivativeStructure> gradient =
239                 relativity.acceleration(sDS, relativity.getParameters(sDS.getDate().getField(), sDS.getDate()));
240         Assertions.assertEquals(
241                 0,
242                 gradient.toVector3D().subtract(circularApproximation).getNorm(),
243                 tol);
244         double r = p.getNorm();
245         double s = v.getNorm();
246         final double[] actualdx = gradient.getX().getAllDerivatives();
247         final double x = p.getX();
248         final double vx = v.getX();
249         double expectedDxDx = gm / (c * c * r * r * r * r * r) *
250                 (-13 * x * x * s * s + 3 * r * r * s * s + 4 * r * r * vx * vx);
251         Assertions.assertEquals(expectedDxDx, actualdx[1], 2);
252     }
253 
254     /**Testing if the propagation between the FieldPropagation and the propagation
255      * is equivalent.
256      * Also testing if propagating X+dX with the propagation is equivalent to
257      * propagation X with the FieldPropagation and then applying the taylor
258      * expansion of dX to the result.*/
259     @Test
260     public void RealFieldTest() {
261         DSFactory factory = new DSFactory(6, 5);
262         DerivativeStructure a_0 = factory.variable(0, 7e7);
263         DerivativeStructure e_0 = factory.variable(1, 0.4);
264         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
265         DerivativeStructure R_0 = factory.variable(3, 0.7);
266         DerivativeStructure O_0 = factory.variable(4, 0.5);
267         DerivativeStructure n_0 = factory.variable(5, 0.1);
268         DerivativeStructure mu  = factory.constant(Constants.EIGEN5C_EARTH_MU);
269 
270         Field<DerivativeStructure> field = a_0.getField();
271 
272         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
273 
274         Frame EME = FramesFactory.getEME2000();
275 
276         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
277                                                                                  PositionAngleType.MEAN,
278                                                                                  EME,
279                                                                                  J2000,
280                                                                                  mu);
281 
282         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
283 
284         SpacecraftState iSR = initialState.toSpacecraftState();
285         OrbitType type = OrbitType.KEPLERIAN;
286         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(0.001).getTolerances(FKO.toOrbit(), type);
287 
288 
289         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
290                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
291         integrator.setInitialStepSize(60);
292         AdaptiveStepsizeIntegrator RIntegrator =
293                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
294         RIntegrator.setInitialStepSize(60);
295 
296         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
297         FNP.setOrbitType(type);
298         FNP.setInitialState(initialState);
299 
300         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
301         NP.setOrbitType(type);
302         NP.setInitialState(iSR);
303 
304         final Relativity forceModel = new Relativity(Constants.EIGEN5C_EARTH_MU);
305 
306         FNP.addForceModel(forceModel);
307         NP.addForceModel(forceModel);
308 
309         // Do the test
310         checkRealFieldPropagation(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
311                                   1.0e-15, 5.0e-10, 3.0e-11, 3.0e-10,
312                                   1, false);
313     }
314 
315     /**Testing if the propagation between the FieldPropagation and the propagation
316      * is equivalent.
317      * Also testing if propagating X+dX with the propagation is equivalent to
318      * propagation X with the FieldPropagation and then applying the taylor
319      * expansion of dX to the result.*/
320     @Test
321     public void RealFieldGradientTest() {
322 
323         final int freeParameters = 6;
324         Gradient a_0 = Gradient.variable(freeParameters, 0, 7e7);
325         Gradient e_0 = Gradient.variable(freeParameters, 1, 0.4);
326         Gradient i_0 = Gradient.variable(freeParameters, 2, 85 * FastMath.PI / 180);
327         Gradient R_0 = Gradient.variable(freeParameters, 3, 0.7);
328         Gradient O_0 = Gradient.variable(freeParameters, 4, 0.5);
329         Gradient n_0 = Gradient.variable(freeParameters, 5, 0.1);
330         Gradient mu  = Gradient.constant(freeParameters, Constants.EIGEN5C_EARTH_MU);
331 
332         Field<Gradient> field = a_0.getField();
333 
334         FieldAbsoluteDate<Gradient> J2000 = new FieldAbsoluteDate<>(field);
335 
336         Frame EME = FramesFactory.getEME2000();
337 
338         FieldKeplerianOrbit<Gradient> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
339                                                                       PositionAngleType.MEAN,
340                                                                       EME,
341                                                                       J2000,
342                                                                       mu);
343 
344         FieldSpacecraftState<Gradient> initialState = new FieldSpacecraftState<>(FKO);
345 
346         SpacecraftState iSR = initialState.toSpacecraftState();
347         OrbitType type = OrbitType.KEPLERIAN;
348         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(0.001).getTolerances(FKO.toOrbit(), type);
349 
350 
351         AdaptiveStepsizeFieldIntegrator<Gradient> integrator =
352                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
353         integrator.setInitialStepSize(60);
354         AdaptiveStepsizeIntegrator RIntegrator =
355                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
356         RIntegrator.setInitialStepSize(60);
357 
358         FieldNumericalPropagator<Gradient> FNP = new FieldNumericalPropagator<>(field, integrator);
359         FNP.setOrbitType(type);
360         FNP.setInitialState(initialState);
361 
362         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
363         NP.setOrbitType(type);
364         NP.setInitialState(iSR);
365 
366         final Relativity forceModel = new Relativity(Constants.EIGEN5C_EARTH_MU);
367 
368         FNP.addForceModel(forceModel);
369         NP.addForceModel(forceModel);
370 
371         // Do the test
372         checkRealFieldPropagationGradient(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
373                                   1.0e-15, 1.3e-2, 2.9e-4, 4.4e-3,
374                                   1, false);
375     }
376 
377     /**Same test as the previous one but not adding the ForceModel to the NumericalPropagator
378         it is a test to validate the previous test.
379         (to test if the ForceModel it's actually
380         doing something in the Propagator and the FieldPropagator)*/
381     @Test
382     public void RealFieldExpectErrorTest() {
383         DSFactory factory = new DSFactory(6, 0);
384         DerivativeStructure a_0 = factory.variable(0, 7e7);
385         DerivativeStructure e_0 = factory.variable(1, 0.4);
386         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
387         DerivativeStructure R_0 = factory.variable(3, 0.7);
388         DerivativeStructure O_0 = factory.variable(4, 0.5);
389         DerivativeStructure n_0 = factory.variable(5, 0.1);
390 
391         Field<DerivativeStructure> field = a_0.getField();
392         DerivativeStructure zero = field.getZero();
393 
394         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
395 
396         Frame EME = FramesFactory.getEME2000();
397 
398         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
399                                                                                  PositionAngleType.MEAN,
400                                                                                  EME,
401                                                                                  J2000,
402                                                                                  zero.add(Constants.EIGEN5C_EARTH_MU));
403 
404         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
405 
406         SpacecraftState iSR = initialState.toSpacecraftState();
407 
408         OrbitType type = OrbitType.KEPLERIAN;
409         double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(0.001).getTolerances(FKO.toOrbit(), type);
410 
411 
412         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
413                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
414         integrator.setInitialStepSize(60);
415         AdaptiveStepsizeIntegrator RIntegrator =
416                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
417         RIntegrator.setInitialStepSize(60);
418 
419         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
420         FNP.setOrbitType(type);
421         FNP.setInitialState(initialState);
422 
423         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
424         NP.setOrbitType(type);
425         NP.setInitialState(iSR);
426 
427         final Relativity forceModel = new Relativity(Constants.EIGEN5C_EARTH_MU);
428 
429         FNP.addForceModel(forceModel);
430      //NOT ADDING THE FORCE MODEL TO THE NUMERICAL PROPAGATOR   NP.addForceModel(forceModel);
431 
432         FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
433         FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
434         SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
435         FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
436         PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
437 
438         Assertions.assertEquals(0,
439                             Vector3D.distance(finPVC_DS.toPVCoordinates().getPosition(), finPVC_R.getPosition()),
440                             8.0e-13 * finPVC_R.getPosition().getNorm());
441     }
442     /**
443      * check against example in Tapley, Schutz, and Born, p 65-66. They predict a
444      * progression of perigee of 11 arcsec/year. To get the same results we must set the
445      * propagation tolerances to 1e-5.
446      */
447     @Test
448     void testSmallEffectOnOrbit() {
449         //setup
450         final double gm = Constants.EIGEN5C_EARTH_MU;
451         Orbit orbit =
452                 new KeplerianOrbit(
453                         7500e3, 0.025, FastMath.toRadians(41.2), 0, 0, 0, PositionAngleType.TRUE,
454                         frame,
455                         date,
456                         gm
457                 );
458         double[][] tol = ToleranceProvider.getDefaultToleranceProvider(0.00001).getTolerances(orbit, OrbitType.CARTESIAN);
459         AbstractIntegrator integrator = new DormandPrince853Integrator(1, 3600, tol[0], tol[1]);
460         NumericalPropagator propagator = new NumericalPropagator(integrator);
461         propagator.setOrbitType(OrbitType.CARTESIAN);
462         propagator.addForceModel(new Relativity(gm));
463         propagator.setInitialState(new SpacecraftState(orbit));
464 
465         //action: propagate a period
466         AbsoluteDate end = orbit.getDate().shiftedBy(30 * Constants.JULIAN_DAY);
467         PVCoordinates actual = propagator.getPVCoordinates(end, frame);
468 
469         //verify
470         KeplerianOrbit endOrbit = new KeplerianOrbit(actual, frame, end, gm);
471         KeplerianOrbit startOrbit = new KeplerianOrbit(orbit);
472         double dp = endOrbit.getPerigeeArgument() - startOrbit.getPerigeeArgument();
473         double dtYears = end.durationFrom(orbit.getDate()) / Constants.JULIAN_YEAR;
474         double dpDeg = FastMath.toDegrees(dp);
475         //change in argument of perigee in arcseconds per year
476         double arcsecPerYear = dpDeg * 3600 / dtYears;
477         Assertions.assertEquals(11, arcsecPerYear, 0.5);
478     }
479 
480     /**
481      * check {@link Relativity#setParameter(String, double)}, and {@link
482      * Relativity#getParameter(String)}
483      */
484     @Test
485     void testGetSetGM() {
486         //setup
487         Relativity relativity = new Relativity(Constants.EIGEN5C_EARTH_MU);
488 
489         //actions + verify
490         Assertions.assertEquals(
491                 Constants.EIGEN5C_EARTH_MU,
492                 relativity.getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT).getValue(),
493                 0);
494         relativity.getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT).setValue(1);
495         Assertions.assertEquals(
496                 1,
497                 relativity.getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT).getValue(),
498                 0);
499     }
500 
501 }