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 java.util.ArrayList;
20  import java.util.List;
21  
22  import org.hipparchus.geometry.euclidean.threed.Vector3D;
23  import org.hipparchus.ode.ODEIntegrator;
24  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
25  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
26  import org.junit.jupiter.api.Assertions;
27  import org.junit.jupiter.api.BeforeEach;
28  import org.junit.jupiter.api.Test;
29  import org.orekit.Utils;
30  import org.orekit.bodies.CR3BPFactory;
31  import org.orekit.bodies.CR3BPSystem;
32  import org.orekit.errors.OrekitException;
33  import org.orekit.frames.Frame;
34  import org.orekit.orbits.CR3BPDifferentialCorrection;
35  import org.orekit.orbits.HaloOrbit;
36  import org.orekit.orbits.LibrationOrbitFamily;
37  import org.orekit.orbits.LibrationOrbitType;
38  import org.orekit.orbits.RichardsonExpansion;
39  import org.orekit.propagation.SpacecraftState;
40  import org.orekit.propagation.numerical.NumericalPropagator;
41  import org.orekit.time.AbsoluteDate;
42  import org.orekit.time.TimeScalesFactory;
43  import org.orekit.utils.AbsolutePVCoordinates;
44  import org.orekit.utils.LagrangianPoints;
45  import org.orekit.utils.PVCoordinates;
46  
47  public class CR3BPMultipleShooterTest {
48  
49      @Test
50      public void testCannotSetEpochFreedom() {
51          Assertions.assertThrows(OrekitException.class, () -> {
52              final CR3BPSystem syst = CR3BPFactory.getEarthMoonCR3BP();
53              final AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
54              final HaloOrbit h1 = new HaloOrbit(new RichardsonExpansion(syst, LagrangianPoints.L1), 8E6, LibrationOrbitFamily.NORTHERN);
55              final int narcs = 1;
56              final List<STMEquations> cr3bpAdditionalEquations = new ArrayList<>(narcs);
57              cr3bpAdditionalEquations.add(new STMEquations(syst));
58              final PVCoordinates firstGuess1 = h1.getInitialPV();
59              List<SpacecraftState> firstGuessList = new ArrayList<>(narcs + 1);
60              firstGuessList.add(new SpacecraftState(new AbsolutePVCoordinates(syst.getRotatingFrame(),
61                                                                               date,
62                                                                               firstGuess1)));
63  
64              new CR3BPMultipleShooter(firstGuessList, new ArrayList<>(), cr3bpAdditionalEquations, 1E-8, 20).setEpochFreedom(1, false);
65          });
66      }
67  
68      @Test
69      public void testCannotSetScaleLength() {
70          Assertions.assertThrows(OrekitException.class, () -> {
71              final CR3BPSystem syst = CR3BPFactory.getEarthMoonCR3BP();
72              final AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
73              final HaloOrbit h1 = new HaloOrbit(new RichardsonExpansion(syst, LagrangianPoints.L1), 8E6, LibrationOrbitFamily.NORTHERN);
74              final int narcs = 1;
75              final List<STMEquations> cr3bpAdditionalEquations = new ArrayList<>(narcs);
76              cr3bpAdditionalEquations.add(new STMEquations(syst));
77              final PVCoordinates firstGuess1 = h1.getInitialPV();
78              List<SpacecraftState> firstGuessList = new ArrayList<>(narcs + 1);
79              firstGuessList.add(new SpacecraftState(new AbsolutePVCoordinates(syst.getRotatingFrame(),
80                                                                               date,
81                                                                               firstGuess1)));
82  
83              new CR3BPMultipleShooter(firstGuessList, new ArrayList<>(), cr3bpAdditionalEquations, 1E-8, 20).setScaleLength(1);
84          });
85      }
86  
87      @Test
88      public void testCannotSetScaleTime() {
89          Assertions.assertThrows(OrekitException.class, () -> {
90              final CR3BPSystem syst = CR3BPFactory.getEarthMoonCR3BP();
91              final AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
92              final HaloOrbit h1 = new HaloOrbit(new RichardsonExpansion(syst, LagrangianPoints.L1), 8E6, LibrationOrbitFamily.NORTHERN);
93              final int narcs = 1;
94              final List<STMEquations> cr3bpAdditionalEquations = new ArrayList<>(narcs);
95              cr3bpAdditionalEquations.add(new STMEquations(syst));
96              final PVCoordinates firstGuess1 = h1.getInitialPV();
97              List<SpacecraftState> firstGuessList = new ArrayList<>(narcs + 1);
98              firstGuessList.add(new SpacecraftState(new AbsolutePVCoordinates(syst.getRotatingFrame(),
99                                                                               date,
100                                                                              firstGuess1)));
101 
102             new CR3BPMultipleShooter(firstGuessList, new ArrayList<>(), cr3bpAdditionalEquations, 1E-8, 20).setScaleTime(1);
103         });
104     }
105 
106     @Test
107     public void testHaloOrbit() {
108 
109         final CR3BPSystem syst = CR3BPFactory.getEarthMoonCR3BP();
110         final AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
111         final HaloOrbit h1 = new HaloOrbit(new RichardsonExpansion(syst, LagrangianPoints.L1), 8E6, LibrationOrbitFamily.NORTHERN);
112 
113         // Adaptive stepsize boundaries
114         final double minStep = 1E-12;
115         final double maxstep = 0.001;
116 
117         // Integrator tolerances
118         final double positionTolerance = 1E-5;
119         final double velocityTolerance = 1E-5;
120         final double massTolerance = 1.0e-6;
121         final double[] vecAbsoluteTolerances = {positionTolerance, positionTolerance, positionTolerance, velocityTolerance, velocityTolerance, velocityTolerance, massTolerance };
122         final double[] vecRelativeTolerances =
123                         new double[vecAbsoluteTolerances.length];
124 
125         // Integrator definition
126         final AdaptiveStepsizeIntegrator integrator =
127                         new DormandPrince853Integrator(minStep, maxstep,
128                                                        vecAbsoluteTolerances,
129                                                        vecRelativeTolerances);
130         final int narcs = 1;
131         final List<STMEquations> cr3bpAdditionalEquations = new ArrayList<>(narcs);
132         cr3bpAdditionalEquations.add(new STMEquations(syst));
133 
134         // Propagator definition for CR3BP
135         final List<NumericalPropagator> propagatorList = new ArrayList<>(narcs);
136         final NumericalPropagator propagator = new NumericalPropagator(integrator);
137         propagator.setOrbitType(null);
138         propagator.setIgnoreCentralAttraction(true);
139         propagator.addForceModel(new CR3BPForceModel(syst));
140 
141         // Add new set of additional equations to the propagator
142         propagator.addAdditionalDerivativesProvider(cr3bpAdditionalEquations.get(0));
143 
144         propagatorList.add(propagator);
145 
146         // First guess trajectory definition
147 
148         final PVCoordinates firstGuess1 = h1.getInitialPV();
149         final PVCoordinates firstGuess2 = new PVCoordinates(firstGuess1.getPosition().add(new Vector3D(0.1,0,0)), firstGuess1.getVelocity().negate());
150         final double arcDuration = h1.getOrbitalPeriod()/2;
151 
152 
153         List<SpacecraftState> firstGuessList = new ArrayList<>(narcs + 1);
154         firstGuessList.add(new SpacecraftState(new AbsolutePVCoordinates(syst.getRotatingFrame(),
155                                                                          date,
156                                                                          firstGuess1)));
157         firstGuessList.add(new SpacecraftState(new AbsolutePVCoordinates(syst.getRotatingFrame(),
158                                                                          date.shiftedBy(arcDuration),
159                                                                          firstGuess2)));
160 
161         // Multiple Shooting definition
162         final CR3BPMultipleShooter multipleShooting = new CR3BPMultipleShooter(firstGuessList, propagatorList, cr3bpAdditionalEquations, 1E-8, 20);
163         multipleShooting.setPatchPointComponentFreedom(0, 1, false);
164         multipleShooting.setPatchPointComponentFreedom(0, 2, false); // Halo corrector is Z-fix
165         multipleShooting.setPatchPointComponentFreedom(0, 3, false);
166         multipleShooting.setPatchPointComponentFreedom(0, 5, false);
167         multipleShooting.setPatchPointComponentFreedom(1, 1, false);
168         multipleShooting.setPatchPointComponentFreedom(1, 3, false);
169         multipleShooting.setPatchPointComponentFreedom(1, 5, false);
170 
171         // Differential correction
172         h1.applyDifferentialCorrection();
173         final PVCoordinates initialPVDC = h1.getInitialPV();
174         final double periodDC = h1.getOrbitalPeriod();
175 
176         // Multiple shooting computation
177         List<SpacecraftState> result = multipleShooting.compute();
178         final AbsolutePVCoordinates initialPVMS = result.get(0).getAbsPVA();
179         final double periodMS = 2 * result.get(1).getDate().durationFrom(result.get(0).getDate());
180 
181         Assertions.assertEquals(0.0, initialPVDC.getPosition().getY(), 1E-15);
182         Assertions.assertEquals(0.0, initialPVDC.getVelocity().getX(), 1E-15);
183         Assertions.assertEquals(0.0, initialPVDC.getVelocity().getZ(), 1E-15);
184 
185         Assertions.assertEquals(0.0, initialPVMS.getPosition().getY(), 1E-15);
186         Assertions.assertEquals(0.0, initialPVMS.getVelocity().getX(), 1E-15);
187         Assertions.assertEquals(0.0, initialPVMS.getVelocity().getZ(), 1E-15);
188 
189         Assertions.assertEquals(initialPVDC.getPosition().getX(), initialPVMS.getPosition().getX(), 1E-9);
190         Assertions.assertEquals(initialPVDC.getPosition().getZ(), initialPVMS.getPosition().getZ(), 1E-15);
191         Assertions.assertEquals(initialPVDC.getVelocity().getY(), initialPVMS.getVelocity().getY(), 1E-7);
192 
193         Assertions.assertEquals(periodDC, periodMS, 7E-9);
194     }
195 
196     @Test
197     public void testClosedOrbit() {
198 
199         // Earth-Moon system and L2 southern Halo
200         final CR3BPSystem earthMoon = CR3BPFactory.getEarthMoonCR3BP();
201         final HaloOrbit halo        = new HaloOrbit(new RichardsonExpansion(earthMoon, LagrangianPoints.L2), 30e6, LibrationOrbitFamily.SOUTHERN);
202         halo.applyDifferentialCorrection();
203         final double periodGuess    = halo.getOrbitalPeriod();
204 
205         // reference frame and date
206         final Frame frame           = earthMoon.getRotatingFrame();
207         final AbsoluteDate date     = AbsoluteDate.J2000_EPOCH;
208 
209         // propagators and additional equations
210         final int nArcs = 2;
211         final List<NumericalPropagator> propagators = new ArrayList<>(nArcs);
212         final List<STMEquations> stmEquations       = new ArrayList<>(nArcs);
213         for (int i = 0; i < nArcs; i++) {
214             final ODEIntegrator integ      = new DormandPrince853Integrator(1e-16, 1e16, 1e-14, 3e-14);
215             final NumericalPropagator prop = new NumericalPropagator(integ);
216             prop.setOrbitType(null);
217             prop.setIgnoreCentralAttraction(true);
218             prop.addForceModel(new CR3BPForceModel(earthMoon));
219             propagators.add(prop);
220             stmEquations.add(new STMEquations(earthMoon));
221             propagators.get(i).addAdditionalDerivativesProvider(stmEquations.get(i));
222         }
223 
224         // initial guess
225         final List<SpacecraftState> initialGuess = new ArrayList<>(nArcs + 1);
226         initialGuess.add(new SpacecraftState(new AbsolutePVCoordinates(frame, date, halo.getInitialPV())));
227 
228         final ODEIntegrator integ            = new DormandPrince853Integrator(1e-16, 1e16, 1e-6, 3e-6);
229         final NumericalPropagator propagator = new NumericalPropagator(integ);
230         propagator.setOrbitType(null);
231         propagator.setIgnoreCentralAttraction(true);
232         propagator.addForceModel(new CR3BPForceModel(earthMoon));
233         propagator.setInitialState(initialGuess.get(0));
234         initialGuess.add(propagator.propagate(date.shiftedBy(periodGuess / 2.0)));
235 
236         propagator.setInitialState(initialGuess.get(0));
237         initialGuess.add(propagator.propagate(date.shiftedBy(periodGuess)));
238 
239         // shooting
240         final CR3BPMultipleShooter shooter = new CR3BPMultipleShooter(initialGuess, propagators, stmEquations, 1e-13, 20);
241         shooter.setClosedOrbitConstraint(true);
242         shooter.setPatchPointComponentFreedom(0, 1, false);
243         shooter.setPatchPointComponentFreedom(0, 3, false);
244         shooter.setPatchPointComponentFreedom(0, 5, false);
245 
246         final List<SpacecraftState> corrStates = shooter.compute();
247 
248         final PVCoordinates pv0 = corrStates.get(0).getPVCoordinates();
249         final PVCoordinates pv2 = corrStates.get(nArcs).getPVCoordinates();
250         Assertions.assertEquals(pv0.getPosition().getX(), pv2.getPosition().getX(), 1e-13);
251         Assertions.assertEquals(pv0.getPosition().getY(), pv2.getPosition().getY(), 1e-13);
252         Assertions.assertEquals(pv0.getPosition().getZ(), pv2.getPosition().getZ(), 1e-13);
253         Assertions.assertEquals(pv0.getVelocity().getX(), pv2.getVelocity().getX(), 1e-13);
254         Assertions.assertEquals(pv0.getVelocity().getY(), pv2.getVelocity().getY(), 1e-13);
255         Assertions.assertEquals(pv0.getVelocity().getZ(), pv2.getVelocity().getZ(), 1e-13);
256         Assertions.assertEquals(0.0, pv0.getPosition().getY(), 1e-16);
257         Assertions.assertEquals(0.0, pv2.getVelocity().getX(), 1e-16);
258         Assertions.assertEquals(0.0, pv2.getVelocity().getZ(), 1e-16);
259 
260     }
261 
262     // Non regression test
263     @Test
264     public void testWithConstraint() {
265 
266         // Earth-Moon system and L2 southern Halo
267         final CR3BPSystem earthMoon = CR3BPFactory.getEarthMoonCR3BP();
268         final HaloOrbit halo        = new HaloOrbit(new RichardsonExpansion(earthMoon, LagrangianPoints.L2), 30e6, LibrationOrbitFamily.SOUTHERN);
269         halo.applyDifferentialCorrection();
270         final double periodGuess    = halo.getOrbitalPeriod();
271 
272         // reference frame and date
273         final Frame frame           = earthMoon.getRotatingFrame();
274         final AbsoluteDate date     = AbsoluteDate.J2000_EPOCH;
275 
276         // propagators and additional equations
277         final int nArcs = 2;
278         final List<NumericalPropagator> propagators = new ArrayList<>(nArcs);
279         final List<STMEquations> stmEquations       = new ArrayList<>(nArcs);
280         for (int i = 0; i < nArcs; i++) {
281             final ODEIntegrator integ      = new DormandPrince853Integrator(1e-16, 1e16, 1e-14, 3e-14);
282             final NumericalPropagator prop = new NumericalPropagator(integ);
283             prop.setOrbitType(null);
284             prop.setIgnoreCentralAttraction(true);
285             prop.addForceModel(new CR3BPForceModel(earthMoon));
286             propagators.add(prop);
287             stmEquations.add(new STMEquations(earthMoon));
288             propagators.get(i).addAdditionalDerivativesProvider(stmEquations.get(i));
289         }
290 
291         // initial guess
292         final List<SpacecraftState> initialGuess = new ArrayList<>(nArcs + 1);
293         initialGuess.add(new SpacecraftState(new AbsolutePVCoordinates(frame, date, halo.getInitialPV())));
294 
295         final ODEIntegrator integ            = new DormandPrince853Integrator(1e-16, 1e16, 1e-6, 3e-6);
296         final NumericalPropagator propagator = new NumericalPropagator(integ);
297         propagator.setOrbitType(null);
298         propagator.setIgnoreCentralAttraction(true);
299         propagator.addForceModel(new CR3BPForceModel(earthMoon));
300         propagator.setInitialState(initialGuess.get(0));
301         initialGuess.add(propagator.propagate(date.shiftedBy(periodGuess / 2.0)));
302 
303         propagator.setInitialState(initialGuess.get(0));
304         initialGuess.add(propagator.propagate(date.shiftedBy(periodGuess)));
305 
306         // shooting
307         final CR3BPMultipleShooter shooter = new CR3BPMultipleShooter(initialGuess, propagators, stmEquations, 1e-13, 20);
308         shooter.setClosedOrbitConstraint(true);
309         shooter.setPatchPointComponentFreedom(0, 1, false);
310         shooter.setPatchPointComponentFreedom(0, 3, false);
311         shooter.setPatchPointComponentFreedom(0, 5, false);
312         shooter.addConstraint(1, 1, 1.0);
313 
314         final List<SpacecraftState> corrStates = shooter.compute();
315 
316         final PVCoordinates pv0 = corrStates.get(0).getPVCoordinates();
317         final PVCoordinates pv2 = corrStates.get(2).getPVCoordinates();
318         Assertions.assertEquals(pv0.getPosition().getX(), pv2.getPosition().getX(), 1e-4);
319         Assertions.assertEquals(pv0.getPosition().getY(), pv2.getPosition().getY(), 1e-4);
320         Assertions.assertEquals(pv0.getPosition().getZ(), pv2.getPosition().getZ(), 1e-4);
321         Assertions.assertEquals(pv0.getVelocity().getX(), pv2.getVelocity().getX(), 1.1e-5);
322         Assertions.assertEquals(pv0.getVelocity().getY(), pv2.getVelocity().getY(), 1.1e-5);
323         Assertions.assertEquals(pv0.getVelocity().getZ(), pv2.getVelocity().getZ(), 1.1e-5);
324 
325     }
326 
327     @Test
328     public void testLagrangianError() {
329         Assertions.assertThrows(OrekitException.class, () -> {
330             CR3BPSystem syst = CR3BPFactory.getEarthMoonCR3BP();
331             final HaloOrbit h = new HaloOrbit(new RichardsonExpansion(syst, LagrangianPoints.L3), 8E6, LibrationOrbitFamily.NORTHERN);
332             h.getClass();
333         });
334     }
335 
336     @Test
337     public void testDifferentialCorrectionError() {
338         Assertions.assertThrows(OrekitException.class, () -> {
339             CR3BPSystem syst = CR3BPFactory.getEarthMoonCR3BP();
340 
341             final double orbitalPeriod = 1;
342 
343             final PVCoordinates firstGuess = new PVCoordinates(new Vector3D(0.0, 1.0, 2.0), new Vector3D(3.0, 4.0, 5.0));
344 
345             final PVCoordinates initialConditions =
346                     new CR3BPDifferentialCorrection(firstGuess, syst, orbitalPeriod).compute(LibrationOrbitType.HALO);
347             initialConditions.toString();
348         });
349     }
350 
351     @Test
352     public void testSTMError() {
353         Assertions.assertThrows(OrekitException.class, () -> {
354             // Time settings
355             final AbsoluteDate initialDate =
356                     new AbsoluteDate(1996, 6, 25, 0, 0, 00.000,
357                             TimeScalesFactory.getUTC());
358             CR3BPSystem syst = CR3BPFactory.getEarthMoonCR3BP();
359 
360             final Frame Frame = syst.getRotatingFrame();
361 
362             // Define a Northern Halo orbit around Earth-Moon L1 with a Z-amplitude
363             // of 8 000 km
364             HaloOrbit h = new HaloOrbit(new RichardsonExpansion(syst, LagrangianPoints.L1), 8E6, LibrationOrbitFamily.SOUTHERN);
365 
366             final PVCoordinates pv = new PVCoordinates(new Vector3D(0.0, 1.0, 2.0), new Vector3D(3.0, 4.0, 5.0));
367 
368             final AbsolutePVCoordinates initialAbsPV =
369                     new AbsolutePVCoordinates(Frame, initialDate, pv);
370 
371             // Creating the initial spacecraftstate that will be given to the
372             // propagator
373             final SpacecraftState s = new SpacecraftState(initialAbsPV);
374 
375 
376             final PVCoordinates manifold = h.getManifolds(s, true);
377             manifold.getMomentum();
378         });
379     }
380 
381     @BeforeEach
382     public void setUp() {
383         Utils.setDataRoot("cr3bp:regular-data");
384     }
385 }