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.maneuvers;
18  
19  
20  import java.util.List;
21  
22  import org.hamcrest.MatcherAssert;
23  import org.hipparchus.Field;
24  import org.hipparchus.analysis.differentiation.DSFactory;
25  import org.hipparchus.analysis.differentiation.DerivativeStructure;
26  import org.hipparchus.analysis.differentiation.Gradient;
27  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
28  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
29  import org.hipparchus.geometry.euclidean.threed.Rotation;
30  import org.hipparchus.geometry.euclidean.threed.Vector3D;
31  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
32  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
33  import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
34  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
35  import org.hipparchus.util.FastMath;
36  import org.hipparchus.util.MathUtils;
37  import org.junit.Assert;
38  import org.junit.Before;
39  import org.junit.Test;
40  import org.orekit.OrekitMatchers;
41  import org.orekit.Utils;
42  import org.orekit.attitudes.AttitudeProvider;
43  import org.orekit.attitudes.InertialProvider;
44  import org.orekit.attitudes.LofOffset;
45  import org.orekit.forces.AbstractLegacyForceModelTest;
46  import org.orekit.forces.ForceModel;
47  import org.orekit.frames.Frame;
48  import org.orekit.frames.FramesFactory;
49  import org.orekit.frames.LOFType;
50  import org.orekit.orbits.CartesianOrbit;
51  import org.orekit.orbits.CircularOrbit;
52  import org.orekit.orbits.FieldKeplerianOrbit;
53  import org.orekit.orbits.KeplerianOrbit;
54  import org.orekit.orbits.Orbit;
55  import org.orekit.orbits.OrbitType;
56  import org.orekit.orbits.PositionAngle;
57  import org.orekit.propagation.FieldSpacecraftState;
58  import org.orekit.propagation.SpacecraftState;
59  import org.orekit.propagation.events.EventDetector;
60  import org.orekit.propagation.numerical.FieldNumericalPropagator;
61  import org.orekit.propagation.numerical.NumericalPropagator;
62  import org.orekit.time.AbsoluteDate;
63  import org.orekit.time.DateComponents;
64  import org.orekit.time.FieldAbsoluteDate;
65  import org.orekit.time.TimeComponents;
66  import org.orekit.time.TimeScalesFactory;
67  import org.orekit.utils.Constants;
68  import org.orekit.utils.FieldPVCoordinates;
69  import org.orekit.utils.PVCoordinates;
70  import org.orekit.utils.ParameterDriver;
71  
72  public class ConstantThrustManeuverTest extends AbstractLegacyForceModelTest {
73  
74      // Body mu
75      private double mu;
76  
77      @Override
78      protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel,
79                                                                           final AbsoluteDate date, final  Frame frame,
80                                                                           final FieldVector3D<DerivativeStructure> position,
81                                                                           final FieldVector3D<DerivativeStructure> velocity,
82                                                                           final FieldRotation<DerivativeStructure> rotation,
83                                                                           final DerivativeStructure mass) {
84          try {
85              final boolean firing = ((ConstantThrustManeuver) forceModel).isFiring(date);
86              
87              final Vector3D thrustVector = ((ConstantThrustManeuver) forceModel).getThrustVector();
88              final double thrust = thrustVector.getNorm();
89              final Vector3D direction = thrustVector.normalize();
90              
91              if (firing) {
92                  return new FieldVector3D<>(mass.reciprocal().multiply(thrust), rotation.applyInverseTo(direction));
93              } else {
94                  // constant (and null) acceleration when not firing
95                  return FieldVector3D.getZero(mass.getField());
96              }
97          } catch (IllegalArgumentException | SecurityException e) {
98              Assert.fail(e.getLocalizedMessage());
99              return null;
100         }
101     }
102 
103     @Override
104     protected FieldVector3D<Gradient> accelerationDerivativesGradient(final ForceModel forceModel,
105                                                                       final AbsoluteDate date, final  Frame frame,
106                                                                       final FieldVector3D<Gradient> position,
107                                                                       final FieldVector3D<Gradient> velocity,
108                                                                       final FieldRotation<Gradient> rotation,
109                                                                       final Gradient mass) {
110         try {
111             final boolean firing = ((ConstantThrustManeuver) forceModel).isFiring(date);
112             
113             final Vector3D thrustVector = ((ConstantThrustManeuver) forceModel).getThrustVector();
114             final double thrust = thrustVector.getNorm();
115             final Vector3D direction = thrustVector.normalize();
116             
117             if (firing) {
118                 return new FieldVector3D<>(mass.reciprocal().multiply(thrust), rotation.applyInverseTo(direction));
119             } else {
120                 // constant (and null) acceleration when not firing
121                 return FieldVector3D.getZero(mass.getField());
122             }
123         } catch (IllegalArgumentException | SecurityException e) {
124             Assert.fail(e.getLocalizedMessage());
125             return null;
126         }
127     }
128 
129     private CircularOrbit dummyOrbit(AbsoluteDate date) {
130         return new CircularOrbit(new PVCoordinates(Vector3D.PLUS_I, Vector3D.PLUS_J),
131                                  FramesFactory.getEME2000(), date, mu);
132     }
133 
134     @Test
135     public void testJacobianVs80Implementation() {
136         final double isp = 318;
137         final double mass = 2500;
138         final double a = 24396159;
139         final double e = 0.72831215;
140         final double i = FastMath.toRadians(7);
141         final double omega = FastMath.toRadians(180);
142         final double OMEGA = FastMath.toRadians(261);
143         final double lv = 0;
144 
145         final double duration = 3653.99;
146         final double f = 420;
147  
148         final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
149                                                        new TimeComponents(23, 30, 00.000),
150                                                        TimeScalesFactory.getUTC());
151         final Orbit orbit =
152             new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
153                                FramesFactory.getEME2000(), initDate, mu);
154         final AttitudeProvider law = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
155        final SpacecraftState initialState =
156             new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
157 
158         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
159                                                        new TimeComponents(04, 15, 34.080),
160                                                        TimeScalesFactory.getUTC());
161         final ConstantThrustManeuver maneuver =
162             new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
163 
164         // before maneuver (Jacobian wrt. state is zero)
165         checkStateJacobianVs80Implementation(initialState, maneuver, law, 1.0e-50, false);
166 
167         // in maneuver
168         SpacecraftState startState = initialState.shiftedBy(fireDate.durationFrom(initDate));
169         maneuver.getEventsDetectors().findFirst().get().eventOccurred(startState, true);
170         SpacecraftState midState = startState.shiftedBy(duration / 2.0);
171         checkStateJacobianVs80Implementation(midState, maneuver, law, 1.0e-20, false);
172 
173     }
174 
175     @Test
176     public void testJacobianVs80ImplementationGradient() {
177         final double isp = 318;
178         final double mass = 2500;
179         final double a = 24396159;
180         final double e = 0.72831215;
181         final double i = FastMath.toRadians(7);
182         final double omega = FastMath.toRadians(180);
183         final double OMEGA = FastMath.toRadians(261);
184         final double lv = 0;
185 
186         final double duration = 3653.99;
187         final double f = 420;
188  
189         final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
190                                                        new TimeComponents(23, 30, 00.000),
191                                                        TimeScalesFactory.getUTC());
192         final Orbit orbit =
193             new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
194                                FramesFactory.getEME2000(), initDate, mu);
195         final AttitudeProvider law = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
196        final SpacecraftState initialState =
197             new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
198 
199         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
200                                                        new TimeComponents(04, 15, 34.080),
201                                                        TimeScalesFactory.getUTC());
202         final ConstantThrustManeuver maneuver =
203             new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
204 
205         // before maneuver (Jacobian wrt. state is zero)
206         checkStateJacobianVs80ImplementationGradient(initialState, maneuver, law, 1.0e-50, false);
207 
208         // in maneuver
209         SpacecraftState startState = initialState.shiftedBy(fireDate.durationFrom(initDate));
210         maneuver.getEventsDetectors().findFirst().get().eventOccurred(startState, true);
211         SpacecraftState midState = startState.shiftedBy(duration / 2.0);
212         checkStateJacobianVs80ImplementationGradient(midState, maneuver, law, 1.0e-20, false);
213 
214     }
215 
216     @Test
217     public void testPositiveDuration() {
218         AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 01, 01),
219                                              new TimeComponents(23, 30, 00.000),
220                                              TimeScalesFactory.getUTC());
221         ConstantThrustManeuver maneuver =
222             new ConstantThrustManeuver(date, 10.0, 400.0, 300.0, Vector3D.PLUS_K);
223         Assert.assertFalse(maneuver.dependsOnPositionOnly());
224         Assert.assertNull(maneuver.getAttitudeOverride());
225         Assert.assertEquals(0.0, Vector3D.distance(maneuver.getDirection(), Vector3D.PLUS_K), 1.0e-15);
226         Assert.assertEquals(10.0, maneuver.getDuration(), 1.0e-15);
227         Assert.assertEquals(0.0, date.durationFrom(maneuver.getStartDate()), 1.0e-15);
228         Assert.assertEquals(0.0, date.shiftedBy(10.0).durationFrom(maneuver.getEndDate()), 1.0e-15);
229         Assert.assertEquals("", maneuver.getName());
230         List<ParameterDriver> drivers = maneuver.getParametersDrivers();
231         Assert.assertEquals(6, drivers.size());
232         Assert.assertEquals("thrust", drivers.get(0).getName());
233         Assert.assertEquals("flow rate", drivers.get(1).getName());
234         EventDetector detector = maneuver.getEventsDetectors().findFirst().get();
235 
236         Orbit o1 = dummyOrbit(date.shiftedBy(- 1.0));
237         Assert.assertTrue(detector.g(new SpacecraftState(o1)) < 0);
238         Orbit o2 = dummyOrbit(date.shiftedBy(  1.0));
239         Assert.assertTrue(detector.g(new SpacecraftState(o2)) > 0);
240         Orbit o3 = dummyOrbit(date.shiftedBy(  9.0));
241         Assert.assertTrue(detector.g(new SpacecraftState(o3)) > 0);
242         Orbit o4 = dummyOrbit(date.shiftedBy( 11.0));
243         Assert.assertTrue(detector.g(new SpacecraftState(o4)) < 0);
244     }
245 
246     @Test
247     public void testNegativeDuration() {
248         AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 01, 01),
249                                              new TimeComponents(23, 30, 00.000),
250                                              TimeScalesFactory.getUTC());
251         ConstantThrustManeuver maneuver =
252             new ConstantThrustManeuver(date, -10.0, 400.0, 300.0, Vector3D.PLUS_K,
253                                        "1A-");
254         List<ParameterDriver> drivers = maneuver.getParametersDrivers();
255         Assert.assertEquals(6, drivers.size());
256         Assert.assertEquals("1A-thrust", drivers.get(0).getName());
257         Assert.assertEquals("1A-flow rate", drivers.get(1).getName());
258         EventDetector detector = maneuver.getEventsDetectors().findFirst().get();
259 
260         Orbit o1 = dummyOrbit(date.shiftedBy(-11.0));
261         Assert.assertTrue(detector.g(new SpacecraftState(o1)) < 0);
262         Orbit o2 = dummyOrbit(date.shiftedBy( -9.0));
263         Assert.assertTrue(detector.g(new SpacecraftState(o2)) > 0);
264         Orbit o3 = dummyOrbit(date.shiftedBy( -1.0));
265         Assert.assertTrue(detector.g(new SpacecraftState(o3)) > 0);
266         Orbit o4 = dummyOrbit(date.shiftedBy(  1.0));
267         Assert.assertTrue(detector.g(new SpacecraftState(o4)) < 0);
268     }
269 
270     @Test
271     public void testRoughBehaviour() {
272         final double isp = 318;
273         final double mass = 2500;
274         final double a = 24396159;
275         final double e = 0.72831215;
276         final double i = FastMath.toRadians(7);
277         final double omega = FastMath.toRadians(180);
278         final double OMEGA = FastMath.toRadians(261);
279         final double lv = 0;
280 
281         final double duration = 3653.99;
282         final double f = 420;
283         final double delta = FastMath.toRadians(-7.4978);
284         final double alpha = FastMath.toRadians(351);
285         final AttitudeProvider law = new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
286 
287         final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
288                                                        new TimeComponents(23, 30, 00.000),
289                                                        TimeScalesFactory.getUTC());
290         final Orbit orbit =
291             new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
292                                FramesFactory.getEME2000(), initDate, mu);
293         final SpacecraftState initialState =
294             new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
295 
296         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
297                                                        new TimeComponents(04, 15, 34.080),
298                                                        TimeScalesFactory.getUTC());
299         final ConstantThrustManeuver maneuver =
300             new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
301         Assert.assertEquals(f,   maneuver.getThrust(), 1.0e-10);
302         Assert.assertEquals(isp, maneuver.getISP(),    1.0e-10);
303 
304         double[] absTolerance = {
305             0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001
306         };
307         double[] relTolerance = {
308             1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7
309         };
310         AdaptiveStepsizeIntegrator integrator =
311             new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance);
312         integrator.setInitialStepSize(60);
313         final NumericalPropagator propagator = new NumericalPropagator(integrator);
314         propagator.setInitialState(initialState);
315         propagator.setAttitudeProvider(law);
316         propagator.addForceModel(maneuver);
317         final SpacecraftState finalorb = propagator.propagate(fireDate.shiftedBy(3800));
318 
319         final double massTolerance =
320                 FastMath.abs(maneuver.getFlowRate()) * maneuver.getEventsDetectors().findFirst().get().getThreshold();
321         Assert.assertEquals(2007.8824544261233, finalorb.getMass(), massTolerance);
322         Assert.assertEquals(2.6872, FastMath.toDegrees(MathUtils.normalizeAngle(finalorb.getI(), FastMath.PI)), 1e-4);
323         Assert.assertEquals(28970, finalorb.getA()/1000, 1);
324 
325     }
326 
327     /**Testing if the propagation between the FieldPropagation and the propagation
328      * is equivalent.
329      * Also testing if propagating X+dX with the propagation is equivalent to
330      * propagation X with the FieldPropagation and then applying the taylor
331      * expansion of dX to the result.*/
332     @Test
333     public void testRealField() {
334         DSFactory factory = new DSFactory(6, 5);
335         DerivativeStructure a_0 = factory.variable(0, 7e7);
336         DerivativeStructure e_0 = factory.variable(1, 0.4);
337         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
338         DerivativeStructure R_0 = factory.variable(3, 0.7);
339         DerivativeStructure O_0 = factory.variable(4, 0.5);
340         DerivativeStructure n_0 = factory.variable(5, 0.1);
341 
342         Field<DerivativeStructure> field = a_0.getField();
343         DerivativeStructure zero = field.getZero();
344 
345         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
346 
347         Frame EME = FramesFactory.getEME2000();
348 
349         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
350                                                                                  PositionAngle.MEAN,
351                                                                                  EME,
352                                                                                  J2000,
353                                                                                  zero.add(Constants.EIGEN5C_EARTH_MU));
354 
355         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
356 
357         SpacecraftState iSR = initialState.toSpacecraftState();
358 
359         final OrbitType type = OrbitType.KEPLERIAN;
360         double[][] tolerance = NumericalPropagator.tolerances(10.0, FKO.toOrbit(), type);
361 
362 
363         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
364                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
365         integrator.setInitialStepSize(60);
366         AdaptiveStepsizeIntegrator RIntegrator =
367                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
368         RIntegrator.setInitialStepSize(60);
369 
370         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
371         FNP.setOrbitType(type);
372         FNP.setInitialState(initialState);
373 
374         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
375         NP.setOrbitType(type);
376         NP.setInitialState(iSR);
377 
378         final ConstantThrustManeuver forceModel =  new ConstantThrustManeuver(J2000.toAbsoluteDate().shiftedBy(100), 100.0, 400.0, 300.0, Vector3D.PLUS_K);
379 
380         FNP.addForceModel(forceModel);
381         NP.addForceModel(forceModel);
382         
383         // Do the test
384         checkRealFieldPropagation(FKO, PositionAngle.MEAN, 1005., NP, FNP,
385                                   1.0e-15, 5.0e-10, 3.0e-11, 3.0e-10,
386                                   1, false);
387     }
388 
389     /**Testing if the propagation between the FieldPropagation and the propagation
390      * is equivalent.
391      * Also testing if propagating X+dX with the propagation is equivalent to
392      * propagation X with the FieldPropagation and then applying the taylor
393      * expansion of dX to the result.*/
394     @Test
395     public void testRealFieldGradient() {
396         int freeParameters = 6;
397         Gradient a_0 = Gradient.variable(freeParameters, 0, 7e7);
398         Gradient e_0 = Gradient.variable(freeParameters, 1, 0.4);
399         Gradient i_0 = Gradient.variable(freeParameters, 2, 85 * FastMath.PI / 180);
400         Gradient R_0 = Gradient.variable(freeParameters, 3, 0.7);
401         Gradient O_0 = Gradient.variable(freeParameters, 4, 0.5);
402         Gradient n_0 = Gradient.variable(freeParameters, 5, 0.1);
403 
404         Field<Gradient> field = a_0.getField();
405         Gradient zero = field.getZero();
406 
407         FieldAbsoluteDate<Gradient> J2000 = new FieldAbsoluteDate<>(field);
408 
409         Frame EME = FramesFactory.getEME2000();
410 
411         FieldKeplerianOrbit<Gradient> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
412                                                                       PositionAngle.MEAN,
413                                                                       EME,
414                                                                       J2000,
415                                                                       zero.add(Constants.EIGEN5C_EARTH_MU));
416 
417         FieldSpacecraftState<Gradient> initialState = new FieldSpacecraftState<>(FKO);
418 
419         SpacecraftState iSR = initialState.toSpacecraftState();
420 
421         final OrbitType type = OrbitType.KEPLERIAN;
422         double[][] tolerance = NumericalPropagator.tolerances(10.0, FKO.toOrbit(), type);
423 
424 
425         AdaptiveStepsizeFieldIntegrator<Gradient> integrator =
426                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
427         integrator.setInitialStepSize(60);
428         AdaptiveStepsizeIntegrator RIntegrator =
429                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
430         RIntegrator.setInitialStepSize(60);
431 
432         FieldNumericalPropagator<Gradient> FNP = new FieldNumericalPropagator<>(field, integrator);
433         FNP.setOrbitType(type);
434         FNP.setInitialState(initialState);
435 
436         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
437         NP.setOrbitType(type);
438         NP.setInitialState(iSR);
439 
440         final ConstantThrustManeuver forceModel =  new ConstantThrustManeuver(J2000.toAbsoluteDate().shiftedBy(100), 100.0, 400.0, 300.0, Vector3D.PLUS_K);
441 
442         FNP.addForceModel(forceModel);
443         NP.addForceModel(forceModel);
444         
445         // Do the test
446         checkRealFieldPropagationGradient(FKO, PositionAngle.MEAN, 1005., NP, FNP,
447                                   1.0e-15, 1.3e-02, 2.9e-04, 2.4e-3,
448                                   1, false);
449     }
450 
451     /**Same test as the previous one but not adding the ForceModel to the NumericalPropagator
452     it is a test to validate the previous test.
453     (to test if the ForceModel it's actually
454     doing something in the Propagator and the FieldPropagator)*/
455     @Test
456     public void RealFieldExpectErrorTest() {
457         DSFactory factory = new DSFactory(6, 0);
458         DerivativeStructure a_0 = factory.variable(0, 7e7);
459         DerivativeStructure e_0 = factory.variable(1, 0.4);
460         DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
461         DerivativeStructure R_0 = factory.variable(3, 0.7);
462         DerivativeStructure O_0 = factory.variable(4, 0.5);
463         DerivativeStructure n_0 = factory.variable(5, 0.1);
464 
465         Field<DerivativeStructure> field = a_0.getField();
466         DerivativeStructure zero = field.getZero();
467 
468         FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
469 
470         Frame EME = FramesFactory.getEME2000();
471 
472         FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
473                                                                                  PositionAngle.MEAN,
474                                                                                  EME,
475                                                                                  J2000,
476                                                                                  zero.add(Constants.EIGEN5C_EARTH_MU));
477 
478         FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
479 
480         SpacecraftState iSR = initialState.toSpacecraftState();
481 
482         final OrbitType type = OrbitType.KEPLERIAN;
483         double[][] tolerance = NumericalPropagator.tolerances(10.0, FKO.toOrbit(), type);
484 
485 
486         AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
487                         new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
488         integrator.setInitialStepSize(60);
489         AdaptiveStepsizeIntegrator RIntegrator =
490                         new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
491         RIntegrator.setInitialStepSize(60);
492 
493         FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
494         FNP.setOrbitType(type);
495         FNP.setInitialState(initialState);
496 
497         NumericalPropagator NP = new NumericalPropagator(RIntegrator);
498         NP.setInitialState(iSR);
499 
500         final ConstantThrustManeuver forceModel =  new ConstantThrustManeuver(J2000.toAbsoluteDate().shiftedBy(100), 100.0, 400.0, 300.0, Vector3D.PLUS_K);
501 
502         FNP.addForceModel(forceModel);
503      //NOT ADDING THE FORCE MODEL TO THE NUMERICAL PROPAGATOR   NP.addForceModel(forceModel);
504 
505         FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
506         FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
507         SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
508         FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
509         PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
510 
511         Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getX() - finPVC_R.getPosition().getX()) < FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
512         Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getY() - finPVC_R.getPosition().getY()) < FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
513         Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getZ() - finPVC_R.getPosition().getZ()) < FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
514     }
515 
516     @Test
517     public void testForwardAndBackward() {
518         final double isp = 318;
519         final double mass = 2500;
520         final double a = 24396159;
521         final double e = 0.72831215;
522         final double i = FastMath.toRadians(7);
523         final double omega = FastMath.toRadians(180);
524         final double OMEGA = FastMath.toRadians(261);
525         final double lv = 0;
526 
527         final double duration = 3653.99;
528         final double f = 420;
529         final double delta = FastMath.toRadians(-7.4978);
530         final double alpha = FastMath.toRadians(351);
531         final AttitudeProvider law = new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
532 
533         final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
534                                                        new TimeComponents(23, 30, 00.000),
535                                                        TimeScalesFactory.getUTC());
536         final Orbit orbit =
537             new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
538                                FramesFactory.getEME2000(), initDate, mu);
539         final SpacecraftState initialState =
540             new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
541 
542         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
543                                                        new TimeComponents(04, 15, 34.080),
544                                                        TimeScalesFactory.getUTC());
545         final ConstantThrustManeuver maneuver =
546             new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
547         Assert.assertEquals(f,   maneuver.getThrust(), 1.0e-10);
548         Assert.assertEquals(isp, maneuver.getISP(),    1.0e-10);
549 
550         final OrbitType orbitType = OrbitType.KEPLERIAN;
551         double[][] tol = NumericalPropagator.tolerances(1.0e-5, orbit, orbitType);
552         AdaptiveStepsizeIntegrator integrator1 =
553             new DormandPrince853Integrator(1.0e-5, 1000, tol[0], tol[1]);
554         integrator1.setInitialStepSize(60);
555         final NumericalPropagator propagator1 = new NumericalPropagator(integrator1);
556         propagator1.setOrbitType(orbitType);
557         propagator1.setInitialState(initialState);
558         propagator1.setAttitudeProvider(law);
559         propagator1.addForceModel(maneuver);
560         final SpacecraftState finalState = propagator1.propagate(fireDate.shiftedBy(3800));
561         Assert.assertFalse(maneuver.isFiring(fireDate.shiftedBy(-1.0e-6)));
562         Assert.assertTrue(maneuver.isFiring(fireDate.shiftedBy(+1.0e-6)));
563         Assert.assertTrue(maneuver.isFiring(fireDate.shiftedBy(0.5 * duration)));
564         Assert.assertTrue(maneuver.isFiring(fireDate.shiftedBy(duration - 1.0e-6)));
565         Assert.assertFalse(maneuver.isFiring(fireDate.shiftedBy(duration + 1.0e-6)));
566 
567         AdaptiveStepsizeIntegrator integrator2 =
568                         new DormandPrince853Integrator(1.0e-5, 1000, tol[0], tol[1]);
569         integrator2.setInitialStepSize(60);
570         final NumericalPropagator propagator2 = new NumericalPropagator(integrator2);
571         propagator2.setOrbitType(orbitType);
572         propagator2.setInitialState(finalState);
573         propagator2.setAttitudeProvider(law);
574         propagator2.addForceModel(maneuver);
575         final SpacecraftState recoveredState = propagator2.propagate(orbit.getDate());
576         final Vector3D refPosition = initialState.getPVCoordinates().getPosition();
577         final Vector3D recoveredPosition = recoveredState.getPVCoordinates().getPosition();
578         Assert.assertEquals(0.0, Vector3D.distance(refPosition, recoveredPosition), 1.1e-3);
579         Assert.assertEquals(initialState.getMass(), recoveredState.getMass(), 1.4e-8);
580         Assert.assertFalse(maneuver.isFiring(fireDate.shiftedBy(-1.0e-6)));
581         Assert.assertTrue(maneuver.isFiring(fireDate.shiftedBy(+1.0e-6)));
582         Assert.assertTrue(maneuver.isFiring(fireDate.shiftedBy(0.5 * duration)));
583         Assert.assertTrue(maneuver.isFiring(fireDate.shiftedBy(duration - 1.0e-6)));
584         Assert.assertFalse(maneuver.isFiring(fireDate.shiftedBy(duration + 1.0e-6)));
585 
586     }
587 
588     @Test
589     public void testParameterDerivative() {
590 
591         // pos-vel (from a ZOOM ephemeris reference)
592         final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
593         final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
594         final SpacecraftState state =
595                 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
596                                                        FramesFactory.getGCRF(),
597                                                        new AbsoluteDate(2005, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
598                                                        Constants.EIGEN5C_EARTH_MU));
599 
600         final ConstantThrustManeuver maneuver =
601                 new ConstantThrustManeuver(state.getDate().shiftedBy(-10), 100.0, 20.0, 350.0,
602                                            Vector3D.PLUS_I, "along-X-");
603         maneuver.init(state, state.getDate().shiftedBy(3600.0));
604 
605         checkParameterDerivative(state, maneuver, "along-X-thrust",    1.0e-3, 7.1e-14);
606         checkParameterDerivative(state, maneuver, "along-X-flow rate", 1.0e-3, 1.0e-15);
607 
608     }
609 
610     @Test
611     public void testParameterDerivativeGradient() {
612 
613         // pos-vel (from a ZOOM ephemeris reference)
614         final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
615         final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
616         final SpacecraftState state =
617                 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
618                                                        FramesFactory.getGCRF(),
619                                                        new AbsoluteDate(2005, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
620                                                        Constants.EIGEN5C_EARTH_MU));
621 
622         final ConstantThrustManeuver maneuver =
623                 new ConstantThrustManeuver(state.getDate().shiftedBy(-10), 100.0, 20.0, 350.0,
624                                            Vector3D.PLUS_I, "along-X-");
625         maneuver.init(state, state.getDate().shiftedBy(3600.0));
626 
627         checkParameterDerivativeGradient(state, maneuver, "along-X-thrust",    1.0e-3, 3.0e-13);
628         checkParameterDerivativeGradient(state, maneuver, "along-X-flow rate", 1.0e-3, 1.0e-15);
629 
630     }
631 
632     @Test
633     public void testInertialManeuver() {
634         final double isp = 318;
635         final double mass = 2500;
636         final double a = 24396159;
637         final double e = 0.72831215;
638         final double i = FastMath.toRadians(7);
639         final double omega = FastMath.toRadians(180);
640         final double OMEGA = FastMath.toRadians(261);
641         final double lv = 0;
642 
643         final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
644                                                        new TimeComponents(23, 30, 00.000),
645                                                        TimeScalesFactory.getUTC());
646         final Orbit orbit =
647             new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
648                                FramesFactory.getEME2000(), initDate, mu);
649 
650         final double duration = 3653.99;
651         final double f = 420;
652         final double delta = FastMath.toRadians(-7.4978);
653         final double alpha = FastMath.toRadians(351);
654         final AttitudeProvider inertialLaw = new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
655         final AttitudeProvider lofLaw = new LofOffset(orbit.getFrame(), LOFType.VNC);
656 
657         final SpacecraftState initialState =
658             new SpacecraftState(orbit, inertialLaw.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
659 
660         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
661                                                        new TimeComponents(04, 15, 34.080),
662                                                        TimeScalesFactory.getUTC());
663         final ConstantThrustManeuver maneuverWithoutOverride =
664             new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
665         Assert.assertEquals(f,   maneuverWithoutOverride.getThrust(), 1.0e-10);
666         Assert.assertEquals(isp, maneuverWithoutOverride.getISP(),    1.0e-10);
667 
668         // reference propagation:
669         // propagator already uses inertial law
670         // maneuver does not need to override it to get an inertial maneuver
671         double[][] tol = NumericalPropagator.tolerances(1.0, orbit, OrbitType.KEPLERIAN);
672         AdaptiveStepsizeIntegrator integrator1 =
673             new DormandPrince853Integrator(0.001, 1000, tol[0], tol[1]);
674         integrator1.setInitialStepSize(60);
675         final NumericalPropagator propagator1 = new NumericalPropagator(integrator1);
676         propagator1.setInitialState(initialState);
677         propagator1.setAttitudeProvider(inertialLaw);
678         propagator1.addForceModel(maneuverWithoutOverride);
679         final SpacecraftState finalState1 = propagator1.propagate(fireDate.shiftedBy(3800));
680 
681 
682         // test propagation:
683         // propagator uses a LOF-aligned law
684         // maneuver needs to override it to get an inertial maneuver
685         final ConstantThrustManeuver maneuverWithOverride =
686                         new ConstantThrustManeuver(fireDate, duration, f, isp,
687                                                    inertialLaw, Vector3D.PLUS_I);
688         Assert.assertEquals(f,   maneuverWithoutOverride.getThrust(), 1.0e-10);
689         Assert.assertEquals(isp, maneuverWithoutOverride.getISP(),    1.0e-10);
690 
691         AdaptiveStepsizeIntegrator integrator2 =
692                         new DormandPrince853Integrator(0.001, 1000, tol[0], tol[1]);
693         integrator2.setInitialStepSize(60);
694         final NumericalPropagator propagator2 = new NumericalPropagator(integrator2);
695         propagator2.setInitialState(initialState);
696         propagator2.setAttitudeProvider(lofLaw);
697         propagator2.addForceModel(maneuverWithOverride);
698         final SpacecraftState finalState2 = propagator2.propagate(finalState1.getDate());
699         MatcherAssert.assertThat(finalState2.getPVCoordinates(),
700                                  OrekitMatchers.pvCloseTo(finalState1.getPVCoordinates(),
701                                                           1.0e-10));
702         Assert.assertFalse(maneuverWithoutOverride.isFiring(fireDate.shiftedBy(-1.0e-6)));
703         Assert.assertTrue(maneuverWithoutOverride.isFiring(fireDate.shiftedBy(+1.0e-6)));
704         Assert.assertTrue(maneuverWithoutOverride.isFiring(fireDate.shiftedBy(0.5 * duration)));
705         Assert.assertTrue(maneuverWithoutOverride.isFiring(fireDate.shiftedBy(duration - 1.0e-6)));
706         Assert.assertFalse(maneuverWithoutOverride.isFiring(fireDate.shiftedBy(duration + 1.0e-6)));
707 
708         // intentionally wrong propagation, that will produce a very different state
709         // propagator uses LOF attitude,
710         // maneuver forget to override it, so maneuver will be LOF-aligned in this case
711         AdaptiveStepsizeIntegrator integrator3 =
712                         new DormandPrince853Integrator(0.001, 1000, tol[0], tol[1]);
713         integrator3.setInitialStepSize(60);
714         final NumericalPropagator propagator3 = new NumericalPropagator(integrator3);
715         propagator3.setInitialState(initialState);
716         propagator3.setAttitudeProvider(lofLaw);
717         propagator3.addForceModel(maneuverWithoutOverride);
718         final SpacecraftState finalState3 = propagator3.propagate(finalState1.getDate());
719         Assert.assertEquals(345859.0,
720                            Vector3D.distance(finalState1.getPVCoordinates().getPosition(),
721                                              finalState3.getPVCoordinates().getPosition()),
722                            1.0);
723     }
724 
725     @Test
726     public void testStopInMiddle() {
727         final double isp = 318;
728         final double mass = 2500;
729         final double a = 24396159;
730         final double e = 0.72831215;
731         final double i = FastMath.toRadians(7);
732         final double omega = FastMath.toRadians(180);
733         final double OMEGA = FastMath.toRadians(261);
734         final double lv = 0;
735 
736         final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
737                                                        new TimeComponents(23, 30, 00.000),
738                                                        TimeScalesFactory.getUTC());
739         final Orbit orbit =
740             new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
741                                FramesFactory.getEME2000(), initDate, mu);
742 
743         final double duration = 3653.99;
744         final double f = 420;
745         final double delta = FastMath.toRadians(-7.4978);
746         final double alpha = FastMath.toRadians(351);
747         final AttitudeProvider inertialLaw = new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
748 
749         final SpacecraftState initialState =
750             new SpacecraftState(orbit, inertialLaw.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
751 
752         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
753                                                        new TimeComponents(04, 15, 34.080),
754                                                        TimeScalesFactory.getUTC());
755         final ConstantThrustManeuver maneuver =
756             new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
757         Assert.assertEquals(f,   maneuver.getThrust(), 1.0e-10);
758         Assert.assertEquals(isp, maneuver.getISP(),    1.0e-10);
759 
760         // before events have been encountered, the maneuver is not yet allowed to generate non zero acceleration
761         for (double dt = 0 ; dt < fireDate.durationFrom(initDate) + duration + 100; dt += 0.1) {
762             Assert.assertFalse(maneuver.isFiring(initDate.shiftedBy(dt)));
763             Assert.assertEquals(0.0,
764                                 maneuver.acceleration(initialState.shiftedBy(dt), maneuver.getParameters()).getNorm(),
765                                 1.0e-15);
766         }
767 
768         // reference propagation:
769         // propagator already uses inertial law
770         // maneuver does not need to override it to get an inertial maneuver
771         double[][] tol = NumericalPropagator.tolerances(1.0, orbit, OrbitType.KEPLERIAN);
772         AdaptiveStepsizeIntegrator integrator1 =
773             new DormandPrince853Integrator(0.001, 1000, tol[0], tol[1]);
774         integrator1.setInitialStepSize(60);
775         final NumericalPropagator propagator1 = new NumericalPropagator(integrator1);
776         propagator1.setInitialState(initialState);
777         propagator1.setAttitudeProvider(inertialLaw);
778         propagator1.addForceModel(maneuver);
779         final SpacecraftState middleState = propagator1.propagate(fireDate.shiftedBy(0.5 * duration));
780 
781         Assert.assertFalse(maneuver.isFiring(initialState));
782         Assert.assertEquals(0.0,
783                             maneuver.acceleration(initialState, maneuver.getParameters()).getNorm(),
784                             1.0e-15);
785         Assert.assertTrue(maneuver.isFiring(middleState));
786         Assert.assertEquals(0.186340263,
787                             maneuver.acceleration(middleState, maneuver.getParameters()).getNorm(),
788                             1.0e-9);
789         Assert.assertTrue(maneuver.isFiring(middleState.shiftedBy(3 * duration)));
790         Assert.assertEquals(0.186340263,
791                             maneuver.acceleration(middleState.shiftedBy(3 * duration), maneuver.getParameters()).getNorm(),
792                             1.0e-9);
793         
794 
795     }
796 
797     @Before
798     public void setUp() {
799         Utils.setDataRoot("regular-data");
800 
801         // Body mu
802         mu = 3.9860047e14;
803 
804     }
805 
806 }