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.propagation.numerical.cr3bp;
18  
19  import org.hipparchus.Field;
20  import org.hipparchus.analysis.differentiation.DSFactory;
21  import org.hipparchus.analysis.differentiation.DerivativeStructure;
22  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23  import org.hipparchus.geometry.euclidean.threed.Vector3D;
24  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
25  import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaFieldIntegrator;
26  import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator;
27  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
28  import org.hipparchus.random.GaussianRandomGenerator;
29  import org.hipparchus.random.RandomGenerator;
30  import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
31  import org.hipparchus.random.Well19937a;
32  import org.hipparchus.util.FastMath;
33  import org.junit.jupiter.api.Assertions;
34  import org.junit.jupiter.api.BeforeEach;
35  import org.junit.jupiter.api.Test;
36  import org.orekit.Utils;
37  import org.orekit.bodies.CR3BPFactory;
38  import org.orekit.bodies.CR3BPSystem;
39  import org.orekit.frames.Frame;
40  import org.orekit.frames.FramesFactory;
41  import org.orekit.propagation.FieldSpacecraftState;
42  import org.orekit.propagation.SpacecraftState;
43  import org.orekit.propagation.numerical.FieldNumericalPropagator;
44  import org.orekit.propagation.numerical.NumericalPropagator;
45  import org.orekit.time.AbsoluteDate;
46  import org.orekit.time.FieldAbsoluteDate;
47  import org.orekit.time.TimeScalesFactory;
48  import org.orekit.utils.AbsolutePVCoordinates;
49  import org.orekit.utils.FieldAbsolutePVCoordinates;
50  import org.orekit.utils.FieldPVCoordinates;
51  import org.orekit.utils.PVCoordinates;
52  
53  public class CR3BPForceModelTest {
54  
55      private CR3BPSystem syst;
56  
57      @Test
58      public void testModel() {
59                  
60          final double mu = new CR3BPForceModel(syst).getParameters(new AbsoluteDate())[0];
61          Assertions.assertEquals(0.0121, mu, 1E-3);
62          
63       // Time settings
64          final AbsoluteDate initialDate =
65              new AbsoluteDate(1996, 06, 25, 0, 0, 00.000,
66                               TimeScalesFactory.getUTC());
67  
68          final PVCoordinates initialConditions =
69              new PVCoordinates(new Vector3D(0.8, 0.2, 0.0),
70                                new Vector3D(0.0, 0.0, 0.1));
71          //final Frame Frame = syst.getRotatingFrame();
72          final Frame Frame = FramesFactory.getGCRF();
73          final AbsolutePVCoordinates initialAbsPV =
74              new AbsolutePVCoordinates(Frame, initialDate, initialConditions);
75  
76          // Creating the initial spacecraftstate that will be given to the
77          // propagator
78          final SpacecraftState initialState = new SpacecraftState(initialAbsPV);
79  
80          // Integration parameters
81          // These parameters are used for the Dormand-Prince integrator, a
82          // variable step integrator,
83          // these limits prevent the integrator to spend too much time when the
84          // equations are too stiff,
85          // as well as the reverse situation.
86          final double minStep = 0.00001;
87          final double maxstep = 3600.0;
88          final double integrationTime = 5;
89  
90          // tolerances for integrators
91          // Used by the integrator to estimate its variable integration step
92          final double positionTolerance = 0.01;
93          final double velocityTolerance = 0.01;
94          final double massTolerance = 1.0e-6;
95          final double[] vecAbsoluteTolerances =
96              {
97                  positionTolerance, positionTolerance, positionTolerance,
98                  velocityTolerance, velocityTolerance, velocityTolerance,
99                  massTolerance
100             };
101         final double[] vecRelativeTolerances =
102             new double[vecAbsoluteTolerances.length];
103 
104         // Defining the numerical integrator that will be used by the propagator
105         AdaptiveStepsizeIntegrator integrator =
106             new DormandPrince853Integrator(minStep, maxstep,
107                                            vecAbsoluteTolerances,
108                                            vecRelativeTolerances);
109 
110         NumericalPropagator propagator = new NumericalPropagator(integrator);
111         propagator.setOrbitType(null);
112         propagator.setIgnoreCentralAttraction(true);
113         propagator.addForceModel(new CR3BPForceModel(syst));
114         propagator.setInitialState(initialState);
115         propagator.clearStepHandlers();
116         final SpacecraftState finalState = propagator.propagate(initialDate.shiftedBy(integrationTime));
117 
118         Assertions.assertNotEquals(initialState.getPosition().getX(), finalState.getPosition().getX(), 1E-2);
119     }
120 
121     /**Testing if the propagation between the FieldPropagation and the propagation
122      * is equivalent.
123      * Also testing if propagating X+dX with the propagation is equivalent to
124      * propagation X with the FieldPropagation and then applying the taylor
125      * expansion of dX to the result.*/
126     @Test
127     public void RealFieldTest() {
128         DSFactory factory = new DSFactory(6, 5);
129         DerivativeStructure fpx = factory.variable(0, 0.8);
130         DerivativeStructure fpy = factory.variable(1, 0.2);
131         DerivativeStructure fpz = factory.variable(2, 0.0);
132         DerivativeStructure fvx = factory.variable(3, 0.0);
133         DerivativeStructure fvy = factory.variable(4, 0.0);
134         DerivativeStructure fvz = factory.variable(5, 0.1);
135 
136         final FieldPVCoordinates<DerivativeStructure> initialConditions =
137                         new FieldPVCoordinates<>(new FieldVector3D<>(fpx, fpy, fpz),
138                                           new FieldVector3D<>(fvx, fvy, fvz));
139 
140         Field<DerivativeStructure> field = fpx.getField();
141         DerivativeStructure zero = field.getZero();
142         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
143 
144         //final Frame frame = syst.getRotatingFrame();
145         final Frame frame = FramesFactory.getGCRF();
146 
147         // PVCoordinates linked to a Frame and a Date
148         final FieldAbsolutePVCoordinates<DerivativeStructure> initialAbsPV =
149             new FieldAbsolutePVCoordinates<>(frame, J2000, initialConditions);
150 
151 
152         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(initialAbsPV);
153 
154         SpacecraftState iSR = initialState.toSpacecraftState();
155 
156         ClassicalRungeKuttaFieldIntegrator<DerivativeStructure> integrator = new ClassicalRungeKuttaFieldIntegrator<>(field, zero.add(1.0));
157 
158         ClassicalRungeKuttaIntegrator RIntegrator = new ClassicalRungeKuttaIntegrator(1.0);
159 
160         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
161         FNP.setOrbitType(null);
162         FNP.setIgnoreCentralAttraction(true);
163         FNP.setInitialState(initialState);
164 
165         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
166         NP.setOrbitType(null);
167         NP.setIgnoreCentralAttraction(true);
168         NP.setInitialState(iSR);
169 
170         final CR3BPForceModel forceModel = new CR3BPForceModel(syst);
171 
172         FNP.addForceModel(forceModel);
173         NP.addForceModel(forceModel);
174 
175 
176         FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1.);
177         FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
178         SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
179         FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
180         PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
181 
182         Assertions.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getX(), finPVC_R.getPosition().getX(), FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
183         Assertions.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getY(), finPVC_R.getPosition().getY(), FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
184         Assertions.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getZ(), finPVC_R.getPosition().getZ(), FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
185         Assertions.assertTrue(forceModel.dependsOnPositionOnly());
186         long number = 23091991;
187         RandomGenerator RG = new Well19937a(number);
188         GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG);
189         UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 },
190                                                                                        new double[] {0.001, 0.001, 0.001, 0.001, 0.001, 0.001},
191                                                                                        NGG);
192         double px_R = fpx.getReal();
193         double py_R = fpy.getReal();
194         double pz_R = fpz.getReal();
195         double vx_R = fvx.getReal();
196         double vy_R = fvy.getReal();
197         double vz_R = fvz.getReal();
198         double maxP = 0;
199         double maxV = 0;
200         double maxA = 0;
201         for (int ii = 0; ii < 1; ii++){
202             double[] rand_next = URVG.nextVector();
203             double px_shift = px_R + rand_next[0];
204             double py_shift = py_R + rand_next[1];
205             double pz_shift = pz_R + rand_next[2];
206             double vx_shift = vx_R + rand_next[3];
207             double vy_shift = vy_R + rand_next[4];
208             double vz_shift = vz_R + rand_next[5];
209 
210             final PVCoordinates shiftedConditions =
211                             new PVCoordinates(new Vector3D(px_shift, py_shift, pz_shift),
212                                               new Vector3D(vx_shift, vy_shift, vz_shift));
213             // PVCoordinates linked to a Frame and a Date
214             final AbsolutePVCoordinates shiftedAbsPV =
215                 new AbsolutePVCoordinates(frame, J2000.toAbsoluteDate(), shiftedConditions);
216 
217             SpacecraftState shift_iSR = new SpacecraftState(shiftedAbsPV);
218 
219 
220 
221             NumericalPropagator shift_NP = new NumericalPropagator(RIntegrator);
222 
223             shift_NP.setInitialState(shift_iSR);
224 
225             shift_NP.setOrbitType(null);
226             shift_NP.setIgnoreCentralAttraction(true);
227             shift_NP.addForceModel(forceModel);
228 
229 
230             SpacecraftState finalState_shift = shift_NP.propagate(target.toAbsoluteDate());
231 
232 
233             PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates();
234 
235             //position check
236             FieldVector3D<DerivativeStructure> pos_DS = finPVC_DS.getPosition();
237             double x_DS = pos_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
238             double y_DS = pos_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
239             double z_DS = pos_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
240             double x = finPVC_shift.getPosition().getX();
241             double y = finPVC_shift.getPosition().getY();
242             double z = finPVC_shift.getPosition().getZ();
243             maxP = FastMath.max(maxP, FastMath.abs((x_DS - x) / (x - pos_DS.getX().getReal())));
244             maxP = FastMath.max(maxP, FastMath.abs((y_DS - y) / (y - pos_DS.getY().getReal())));
245             maxP = FastMath.max(maxP, FastMath.abs((z_DS - z) / (z - pos_DS.getZ().getReal())));
246 
247             // velocity check
248             FieldVector3D<DerivativeStructure> vel_DS = finPVC_DS.getVelocity();
249             double vx_DS = vel_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
250             double vy_DS = vel_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
251             double vz_DS = vel_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
252             double vx = finPVC_shift.getVelocity().getX();
253             double vy = finPVC_shift.getVelocity().getY();
254             double vz = finPVC_shift.getVelocity().getZ();
255             maxV = FastMath.max(maxV, FastMath.abs((vx_DS - vx) / vx));
256             maxV = FastMath.max(maxV, FastMath.abs((vy_DS - vy) / vy));
257             maxV = FastMath.max(maxV, FastMath.abs((vz_DS - vz) / vz));
258 
259             // acceleration check
260             FieldVector3D<DerivativeStructure> acc_DS = finPVC_DS.getAcceleration();
261             double ax_DS = acc_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
262             double ay_DS = acc_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
263             double az_DS = acc_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
264             double ax = finPVC_shift.getAcceleration().getX();
265             double ay = finPVC_shift.getAcceleration().getY();
266             double az = finPVC_shift.getAcceleration().getZ();
267             if (ax != 0 || ay !=0 || az != 0) {
268                 maxA = FastMath.max(maxA, FastMath.abs((ax_DS - ax) / ax));
269                 maxA = FastMath.max(maxA, FastMath.abs((ay_DS - ay) / ay));
270                 maxA = FastMath.max(maxA, FastMath.abs((az_DS - az) / az));
271             } else {
272                 maxA = 0;
273             }
274         }
275         Assertions.assertEquals(0, maxP, 4.2e-11);
276         Assertions.assertEquals(0, maxV, 1.4e-12);
277         Assertions.assertEquals(0, maxA, 8.5e-12);
278     }
279 
280 
281 
282     @BeforeEach
283     public void setUp() {
284         Utils.setDataRoot("regular-data");
285 
286         this.syst = CR3BPFactory.getEarthMoonCR3BP();
287     }
288 }