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.DerivativeStructure;
21  import org.hipparchus.analysis.differentiation.Gradient;
22  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
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.Assert;
31  import org.junit.Before;
32  import org.junit.Test;
33  import org.orekit.Utils;
34  import org.orekit.attitudes.LofOffset;
35  import org.orekit.forces.AbstractLegacyForceModelTest;
36  import org.orekit.forces.ForceModel;
37  import org.orekit.frames.Frame;
38  import org.orekit.frames.FramesFactory;
39  import org.orekit.frames.LOFType;
40  import org.orekit.frames.Transform;
41  import org.orekit.orbits.CartesianOrbit;
42  import org.orekit.orbits.FieldKeplerianOrbit;
43  import org.orekit.orbits.KeplerianOrbit;
44  import org.orekit.orbits.Orbit;
45  import org.orekit.orbits.OrbitType;
46  import org.orekit.orbits.PositionAngle;
47  import org.orekit.propagation.FieldSpacecraftState;
48  import org.orekit.propagation.SpacecraftState;
49  import org.orekit.propagation.numerical.FieldNumericalPropagator;
50  import org.orekit.propagation.numerical.NumericalPropagator;
51  import org.orekit.time.AbsoluteDate;
52  import org.orekit.time.DateComponents;
53  import org.orekit.time.FieldAbsoluteDate;
54  import org.orekit.time.TimeComponents;
55  import org.orekit.time.TimeScalesFactory;
56  import org.orekit.utils.Constants;
57  import org.orekit.utils.IERSConventions;
58  import org.orekit.utils.PVCoordinates;
59  
60  public class LenseThirringRelativityTest 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>
71          accelerationDerivatives(ForceModel forceModel, AbsoluteDate date,
72                                  Frame frame,
73                                  FieldVector3D<DerivativeStructure> position,
74                                  FieldVector3D<DerivativeStructure> velocity,
75                                  FieldRotation<DerivativeStructure> rotation,
76                                  DerivativeStructure mass) {
77          try {
78              java.lang.reflect.Field bodyFrameField = LenseThirringRelativity.class.getDeclaredField("bodyFrame");
79              bodyFrameField.setAccessible(true);
80              Frame bodyFrame = (Frame) bodyFrameField.get(forceModel);
81  
82              double gm = forceModel.getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT).getValue();
83              // Radius
84              final DerivativeStructure r  = position.getNorm();
85              final DerivativeStructure r2 = r.multiply(r);
86  
87              // Earth’s angular momentum per unit mass
88              final Transform t = bodyFrame.getTransformTo(frame, date);
89              final Vector3D  j = t.transformVector(Vector3D.PLUS_K).scalarMultiply(9.8e8);
90  
91              return new FieldVector3D<>(position.dotProduct(j).multiply(3.0).divide(r2),
92                                         position.crossProduct(velocity),
93                                         r.getField().getOne(),
94                                         velocity.crossProduct(j))
95                                         .scalarMultiply(r2.multiply(r).multiply(c * c).reciprocal().multiply(2.0 * gm));
96          } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
97              return null;
98          }
99      }
100 
101     @Override
102     protected FieldVector3D<Gradient>
103         accelerationDerivativesGradient(ForceModel forceModel,
104                                         AbsoluteDate date, Frame frame,
105                                         FieldVector3D<Gradient> position,
106                                         FieldVector3D<Gradient> velocity,
107                                         FieldRotation<Gradient> rotation,
108                                         Gradient mass) {
109         try {
110             java.lang.reflect.Field bodyFrameField = LenseThirringRelativity.class.getDeclaredField("bodyFrame");
111             bodyFrameField.setAccessible(true);
112             Frame bodyFrame = (Frame) bodyFrameField.get(forceModel);
113 
114             // Useful constant
115             final double c2 = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT;
116             double gm = forceModel.getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT).getValue();
117             // Radius
118             final Gradient r  = position.getNorm();
119             final Gradient r2 = r.multiply(r);
120 
121             // Earth’s angular momentum per unit mass
122             final Transform t = bodyFrame.getTransformTo(frame, date);
123             final Vector3D  j = t.transformVector(Vector3D.PLUS_K).scalarMultiply(9.8e8);
124 
125             return new FieldVector3D<>(position.dotProduct(j).multiply(3.0).divide(r2),
126                                        position.crossProduct(velocity),
127                                        r.getField().getOne(),
128                                        velocity.crossProduct(j))
129                                        .scalarMultiply(r2.multiply(r).multiply(c2).reciprocal().multiply(2.0 * gm));
130         } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
131             return null;
132         }
133     }
134 
135     @Test
136     public void testJacobianVs80Implementation() {
137         double gm = Constants.EIGEN5C_EARTH_MU;
138         LenseThirringRelativity relativity = new LenseThirringRelativity(gm, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
139         final Vector3D p = new Vector3D(3777828.75000531, -5543949.549783845, 2563117.448578311);
140         final Vector3D v = new Vector3D(489.0060271721, -2849.9328929417, -6866.4671013153);
141         SpacecraftState s = new SpacecraftState(new CartesianOrbit(
142                 new PVCoordinates(p, v),
143                 frame,
144                 date,
145                 gm
146         ));
147 
148         checkStateJacobianVs80Implementation(s, relativity,
149                                              new LofOffset(s.getFrame(), LOFType.LVLH_CCSDS),
150                                              1.0e-50, false);
151     }
152 
153     @Test
154     public void testJacobianVs80ImplementationGradient() {
155         double gm = Constants.EIGEN5C_EARTH_MU;
156         LenseThirringRelativity relativity = new LenseThirringRelativity(gm, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
157         final Vector3D p = new Vector3D(3777828.75000531, -5543949.549783845, 2563117.448578311);
158         final Vector3D v = new Vector3D(489.0060271721, -2849.9328929417, -6866.4671013153);
159         SpacecraftState s = new SpacecraftState(new CartesianOrbit(
160                 new PVCoordinates(p, v),
161                 frame,
162                 date,
163                 gm
164         ));
165 
166         checkStateJacobianVs80ImplementationGradient(s, relativity,
167                                              new LofOffset(s.getFrame(), LOFType.LVLH_CCSDS),
168                                              1.0e-15, false);
169     }
170 
171     /**Testing if the propagation between the FieldPropagation and the propagation
172      * is equivalent.
173      * Also testing if propagating X+dX with the propagation is equivalent to
174      * propagation X with the FieldPropagation and then applying the taylor
175      * expansion of dX to the result.*/
176     @Test
177     public void RealFieldGradientTest() {
178 
179         final int freeParameters = 6;
180         Gradient a_0 = Gradient.variable(freeParameters, 0, 7e7);
181         Gradient e_0 = Gradient.variable(freeParameters, 1, 0.4);
182         Gradient i_0 = Gradient.variable(freeParameters, 2, 85 * FastMath.PI / 180);
183         Gradient R_0 = Gradient.variable(freeParameters, 3, 0.7);
184         Gradient O_0 = Gradient.variable(freeParameters, 4, 0.5);
185         Gradient n_0 = Gradient.variable(freeParameters, 5, 0.1);
186         Gradient mu  = Gradient.constant(freeParameters, Constants.EIGEN5C_EARTH_MU);
187 
188         Field<Gradient> field = a_0.getField();
189 
190         FieldAbsoluteDate<Gradient> J2000 = new FieldAbsoluteDate<>(field);
191 
192         Frame EME = FramesFactory.getEME2000();
193 
194         FieldKeplerianOrbit<Gradient> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
195                                                                       PositionAngle.MEAN,
196                                                                       EME,
197                                                                       J2000,
198                                                                       mu);
199 
200         FieldSpacecraftState<Gradient> initialState = new FieldSpacecraftState<>(FKO);
201 
202         SpacecraftState iSR = initialState.toSpacecraftState();
203         OrbitType type = OrbitType.KEPLERIAN;
204         double[][] tolerance = NumericalPropagator.tolerances(0.001, FKO.toOrbit(), type);
205 
206 
207         AdaptiveStepsizeFieldIntegrator<Gradient> integrator =
208                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
209         integrator.setInitialStepSize(60);
210         AdaptiveStepsizeIntegrator RIntegrator =
211                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
212         RIntegrator.setInitialStepSize(60);
213 
214         FieldNumericalPropagator<Gradient> FNP = new FieldNumericalPropagator<>(field, integrator);
215         FNP.setOrbitType(type);
216         FNP.setInitialState(initialState);
217 
218         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
219         NP.setOrbitType(type);
220         NP.setInitialState(iSR);
221 
222         LenseThirringRelativity relativity = new LenseThirringRelativity(Constants.EIGEN5C_EARTH_MU, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
223 
224         FNP.addForceModel(relativity);
225         NP.addForceModel(relativity);
226         
227         // Do the test
228         checkRealFieldPropagationGradient(FKO, PositionAngle.MEAN, 1005., NP, FNP,
229                                   1.0e-15, 1.3e-2, 2.9e-4, 1.4e-3,
230                                   1, false);
231     }
232 
233     @Test
234     public void testParameterDerivativeGradient() {
235 
236         final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
237         final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
238         final SpacecraftState state =
239                 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
240                                                        FramesFactory.getGCRF(),
241                                                        new AbsoluteDate(2003, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
242                                                        Constants.EIGEN5C_EARTH_MU));
243 
244         LenseThirringRelativity relativity = new LenseThirringRelativity(Constants.EIGEN5C_EARTH_MU, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
245         Assert.assertFalse(relativity.dependsOnPositionOnly());
246         final String name = NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT;
247         checkParameterDerivativeGradient(state, relativity, name, 1.0, 1.0e-15);
248 
249     }
250 
251     @Test
252     public void testGlobalStateJacobian()
253         {
254 
255         // initialization
256         AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
257                                              new TimeComponents(13, 59, 27.816),
258                                              TimeScalesFactory.getUTC());
259         double i     = FastMath.toRadians(98.7);
260         double omega = FastMath.toRadians(93.0);
261         double OMEGA = FastMath.toRadians(15.0 * 22.5);
262         Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
263                                          0, PositionAngle.MEAN, FramesFactory.getEME2000(), date,
264                                          Constants.EIGEN5C_EARTH_MU);
265         OrbitType integrationType = OrbitType.CARTESIAN;
266         double[][] tolerances = NumericalPropagator.tolerances(0.01, orbit, integrationType);
267 
268         NumericalPropagator propagator =
269                 new NumericalPropagator(new DormandPrince853Integrator(1.0e-3, 120,
270                                                                        tolerances[0], tolerances[1]));
271         propagator.setOrbitType(integrationType);
272         LenseThirringRelativity relativity = new LenseThirringRelativity(Constants.EIGEN5C_EARTH_MU, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
273         propagator.addForceModel(relativity);
274         SpacecraftState state0 = new SpacecraftState(orbit);
275 
276         checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0),
277                            1e4, tolerances[0], 2.5e-9);
278 
279     }
280 
281     @Before
282     public void setUp() {
283         Utils.setDataRoot("regular-data");
284     }
285 
286 }