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.triggers;
18  
19  
20  import java.util.List;
21  import java.util.stream.Collectors;
22  
23  import org.hipparchus.CalculusFieldElement;
24  import org.hipparchus.Field;
25  import org.hipparchus.geometry.euclidean.threed.Rotation;
26  import org.hipparchus.geometry.euclidean.threed.Vector3D;
27  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
28  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
29  import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
30  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
31  import org.hipparchus.util.Decimal64Field;
32  import org.hipparchus.util.FastMath;
33  import org.hipparchus.util.MathUtils;
34  import org.junit.Assert;
35  import org.junit.Before;
36  import org.junit.Test;
37  import org.orekit.Utils;
38  import org.orekit.attitudes.AttitudeProvider;
39  import org.orekit.attitudes.InertialProvider;
40  import org.orekit.forces.maneuvers.Maneuver;
41  import org.orekit.forces.maneuvers.propulsion.BasicConstantThrustPropulsionModel;
42  import org.orekit.forces.maneuvers.trigger.AbstractManeuverTriggers;
43  import org.orekit.forces.maneuvers.trigger.FieldManeuverTriggersResetter;
44  import org.orekit.forces.maneuvers.trigger.ManeuverTriggersResetter;
45  import org.orekit.frames.FramesFactory;
46  import org.orekit.orbits.FieldKeplerianOrbit;
47  import org.orekit.orbits.FieldOrbit;
48  import org.orekit.orbits.KeplerianOrbit;
49  import org.orekit.orbits.Orbit;
50  import org.orekit.orbits.OrbitType;
51  import org.orekit.orbits.PositionAngle;
52  import org.orekit.propagation.FieldSpacecraftState;
53  import org.orekit.propagation.SpacecraftState;
54  import org.orekit.propagation.events.EventDetector;
55  import org.orekit.propagation.events.FieldEventDetector;
56  import org.orekit.propagation.numerical.FieldNumericalPropagator;
57  import org.orekit.propagation.numerical.NumericalPropagator;
58  import org.orekit.time.AbsoluteDate;
59  import org.orekit.time.DateComponents;
60  import org.orekit.time.FieldAbsoluteDate;
61  import org.orekit.time.TimeComponents;
62  import org.orekit.time.TimeScalesFactory;
63  import org.orekit.utils.Constants;
64  
65  public abstract class AbstractManeuverTriggersTest<T extends AbstractManeuverTriggers> {
66  
67      /** trigger dates. */
68      private AbsoluteDate triggerStart;
69      private AbsoluteDate triggerStop;
70  
71      protected abstract T createTrigger(AbsoluteDate start, AbsoluteDate stop);
72  
73      private T configureTrigger(final AbsoluteDate start, final AbsoluteDate stop) {
74          T trigger = createTrigger(start, stop);
75          // set up separate detectors, to check lists of observers are handled properly
76          trigger.addResetter(new Resetter());
77          trigger.addResetter(new Resetter());
78          return trigger;
79      }
80  
81      private <S extends CalculusFieldElement<S>> T configureTrigger(final Field<S> field,
82                                                                     final AbsoluteDate start, final AbsoluteDate stop) {
83          T trigger = createTrigger(start, stop);
84          // set up separate detectors, to check lists of observers are handled properly
85          trigger.addResetter(field, new FieldResetter<>());
86          trigger.addResetter(field, new FieldResetter<>());
87          return trigger;
88      }
89  
90      private class Resetter implements ManeuverTriggersResetter {
91          public void maneuverTriggered(SpacecraftState state, boolean start) {
92              if (start)  {
93                  triggerStart = state.getDate();
94              } else {
95                  triggerStop  = state.getDate();
96              }
97          }
98          public SpacecraftState resetState(SpacecraftState state) {
99              return state;
100         }
101     }
102     private class FieldResetter<S extends CalculusFieldElement<S>> implements FieldManeuverTriggersResetter<S> {
103         public void maneuverTriggered(FieldSpacecraftState<S> state, boolean start) {
104             if (start)  {
105                 triggerStart = state.getDate().toAbsoluteDate();
106             } else {
107                 triggerStop = state.getDate().toAbsoluteDate();
108             }
109         }
110         public FieldSpacecraftState<S> resetState(FieldSpacecraftState<S> state) {
111             return state;
112         }
113     }
114     @Before
115     public void setUp() {
116         Utils.setDataRoot("regular-data");
117         triggerStart = null;
118         triggerStop  = null;
119     }
120 
121     @Test
122     public void testRoughBehaviour() {
123 
124         final AttitudeProvider attitudeProvider = buildAttitudeProvider();
125         final SpacecraftState initialState = buildInitialState(attitudeProvider);
126 
127         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
128                                                        new TimeComponents(04, 15, 34.080),
129                                                        TimeScalesFactory.getUTC());
130         final double isp = 318;
131         final double duration = 3653.99;
132         final double f = 420;
133         final Maneuver maneuver = new Maneuver(null,
134                                                configureTrigger(fireDate, fireDate.shiftedBy(duration)),
135                                                new BasicConstantThrustPropulsionModel(f, isp, Vector3D.PLUS_I, "ABM"));
136         Assert.assertEquals(f,   ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getThrust(), 1.0e-10);
137         Assert.assertEquals(isp, ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getIsp(),    1.0e-10);
138 
139         final SpacecraftState finalorb = buildPropagator(attitudeProvider, initialState, maneuver).
140                                          propagate(fireDate.shiftedBy(3800));
141 
142         final double flowRate = ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getFlowRate();
143         final double massTolerance = FastMath.abs(flowRate) * maneuver.getEventsDetectors().findFirst().get().getThreshold();
144         Assert.assertEquals(2007.8824544261233, finalorb.getMass(), massTolerance);
145         Assert.assertEquals(2.6872, FastMath.toDegrees(MathUtils.normalizeAngle(finalorb.getI(), FastMath.PI)), 1e-4);
146         Assert.assertEquals(28970, finalorb.getA()/1000, 1);
147 
148         final List<EventDetector> list1 = maneuver.getManeuverTriggers().getEventsDetectors().collect(Collectors.toList());
149         final List<EventDetector> list2 = maneuver.getManeuverTriggers().getEventsDetectors().collect(Collectors.toList());
150         Assert.assertEquals(list1.size(), list2.size());
151         for (int i = 0; i < list1.size(); ++i ) {
152             Assert.assertSame(list1.get(i), list2.get(i));
153         }
154 
155         Assert.assertEquals(0.0,      triggerStart.durationFrom(fireDate), 1.0e-10);
156         Assert.assertEquals(duration, triggerStop.durationFrom(fireDate),  1.0e-10);
157 
158     }
159 
160     @Test
161     public void testBackward() {
162 
163         final AttitudeProvider attitudeProvider = buildAttitudeProvider();
164         final SpacecraftState initialState = buildInitialState(attitudeProvider);
165 
166         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
167                                                        new TimeComponents(04, 15, 34.080),
168                                                        TimeScalesFactory.getUTC());
169         final double isp = 318;
170         final double duration = 30;
171         final double f = 420;
172         final Maneuver maneuver = new Maneuver(null,
173                                                configureTrigger(fireDate, fireDate.shiftedBy(duration)),
174                                                new BasicConstantThrustPropulsionModel(f, isp, Vector3D.PLUS_I, "ABM"));
175         Assert.assertEquals(f,   ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getThrust(), 1.0e-10);
176         Assert.assertEquals(isp, ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getIsp(),    1.0e-10);
177 
178         final SpacecraftState finalorb = buildPropagator(attitudeProvider,
179                                                          initialState.shiftedBy(initialState.getKeplerianPeriod()),
180                                                          maneuver).
181                                          propagate(fireDate.shiftedBy(-10));
182 
183         Assert.assertEquals(2504.040, finalorb.getMass(), 1.0e-3);
184 
185         Assert.assertEquals(0.0,      triggerStart.durationFrom(fireDate), 1.0e-10);
186         Assert.assertEquals(duration, triggerStop.durationFrom(fireDate),  1.0e-10);
187 
188     }
189 
190     @Test
191     public void testRoughBehaviourField() {
192         doTestRoughBehaviourField(Decimal64Field.getInstance());
193     }
194 
195     private <S extends CalculusFieldElement<S>> void doTestRoughBehaviourField(Field<S> field) {
196         final AttitudeProvider attitudeProvider = buildAttitudeProvider();
197         final FieldSpacecraftState<S> initialState = buildInitialState(field, attitudeProvider);
198 
199 
200         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
201                                                        new TimeComponents(04, 15, 34.080),
202                                                        TimeScalesFactory.getUTC());
203         final double isp = 318;
204         final double duration = 3653.99;
205         final double f = 420;
206         final Maneuver maneuver = new Maneuver(null,
207                                                configureTrigger(field, fireDate, fireDate.shiftedBy(duration)),
208                                                new BasicConstantThrustPropulsionModel(f, isp, Vector3D.PLUS_I, "ABM"));
209         Assert.assertEquals(f,   ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getThrust(), 1.0e-10);
210         Assert.assertEquals(isp, ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getIsp(),    1.0e-10);
211 
212         final FieldSpacecraftState<S> finalorb = buildPropagator(field, attitudeProvider, initialState, maneuver).
213                                                  propagate(new FieldAbsoluteDate<>(field, fireDate).shiftedBy(3800));
214 
215         final double flowRate = ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getFlowRate();
216         final double massTolerance = FastMath.abs(flowRate) * maneuver.getEventsDetectors().findFirst().get().getThreshold();
217         Assert.assertEquals(2007.8824544261233, finalorb.getMass().getReal(), massTolerance);
218         Assert.assertEquals(2.6872, FastMath.toDegrees(MathUtils.normalizeAngle(finalorb.getI(), field.getZero().newInstance(FastMath.PI))).getReal(), 1e-4);
219         Assert.assertEquals(28970, finalorb.getA().divide(1000).getReal(), 1);
220 
221         final List<FieldEventDetector<?>> list1 = maneuver.getManeuverTriggers().getFieldEventsDetectors(field).collect(Collectors.toList());
222         final List<FieldEventDetector<?>> list2 = maneuver.getManeuverTriggers().getFieldEventsDetectors(field).collect(Collectors.toList());
223         Assert.assertEquals(list1.size(), list2.size());
224         for (int i = 0; i < list1.size(); ++i ) {
225             Assert.assertSame(list1.get(i), list2.get(i));
226         }
227 
228         Assert.assertEquals(0.0,      triggerStart.durationFrom(fireDate), 1.0e-10);
229         Assert.assertEquals(duration, triggerStop.durationFrom(fireDate),  1.0e-10);
230 
231     }
232 
233     @Test
234     public void testBackwardField() {
235         doTestBackwardField(Decimal64Field.getInstance());
236     }
237 
238     private <S extends CalculusFieldElement<S>> void doTestBackwardField(Field<S> field) {
239         final AttitudeProvider attitudeProvider = buildAttitudeProvider();
240         final FieldSpacecraftState<S> initialState = buildInitialState(field, attitudeProvider);
241 
242 
243         final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
244                                                        new TimeComponents(04, 15, 34.080),
245                                                        TimeScalesFactory.getUTC());
246         final double isp = 318;
247         final double duration = 30;
248         final double f = 420;
249         final Maneuver maneuver = new Maneuver(null,
250                                                configureTrigger(field, fireDate, fireDate.shiftedBy(duration)),
251                                                new BasicConstantThrustPropulsionModel(f, isp, Vector3D.PLUS_I, "ABM"));
252         Assert.assertEquals(f,   ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getThrust(), 1.0e-10);
253         Assert.assertEquals(isp, ((BasicConstantThrustPropulsionModel) maneuver.getPropulsionModel()).getIsp(),    1.0e-10);
254 
255         final FieldSpacecraftState<S> finalorb = buildPropagator(field, attitudeProvider,
256                                                                  initialState.shiftedBy(initialState.getKeplerianPeriod()),
257                                                                  maneuver).
258                                                  propagate(new FieldAbsoluteDate<>(field, fireDate).shiftedBy(-10));
259 
260         Assert.assertEquals(2504.040, finalorb.getMass().getReal(), 1.0e-3);
261 
262         Assert.assertEquals(0.0,      triggerStart.durationFrom(fireDate), 1.0e-10);
263         Assert.assertEquals(duration, triggerStop.durationFrom(fireDate),  1.0e-10);
264 
265     }
266 
267     private AttitudeProvider buildAttitudeProvider() {
268         final double delta = FastMath.toRadians(-7.4978);
269         final double alpha = FastMath.toRadians(351);
270         return new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
271     }
272 
273     private SpacecraftState buildInitialState(final AttitudeProvider attitudeProvider) {
274         final double mass = 2500;
275         final double a = 24396159;
276         final double e = 0.72831215;
277         final double i = FastMath.toRadians(7);
278         final double omega = FastMath.toRadians(180);
279         final double OMEGA = FastMath.toRadians(261);
280         final double lv = 0;
281 
282         final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
283                                                        new TimeComponents(23, 30, 00.000),
284                                                        TimeScalesFactory.getUTC());
285         final Orbit orbit =
286             new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
287                                FramesFactory.getEME2000(), initDate, Constants.EIGEN5C_EARTH_MU);
288         return new SpacecraftState(orbit, attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
289     }
290 
291     private <S extends CalculusFieldElement<S>> FieldSpacecraftState<S> buildInitialState(final Field<S> field,
292                                                                        final AttitudeProvider attitudeProvider) {
293         final S zero = field.getZero();
294         final S mass = zero.newInstance(2500);
295         final S a = zero.newInstance(24396159);
296         final S e = zero.newInstance(0.72831215);
297         final S i = FastMath.toRadians(zero.newInstance(7));
298         final S omega = FastMath.toRadians(zero.newInstance(180));
299         final S OMEGA = FastMath.toRadians(zero.newInstance(261));
300         final S lv = zero.newInstance(0);
301 
302         final FieldAbsoluteDate<S> initDate = new FieldAbsoluteDate<>(field,
303                         new DateComponents(2004, 01, 01),
304                         new TimeComponents(23, 30, 00.000),
305                         TimeScalesFactory.getUTC());
306         final FieldOrbit<S> orbit =
307                         new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
308                                         FramesFactory.getEME2000(), initDate, zero.newInstance(Constants.EIGEN5C_EARTH_MU));
309         return new FieldSpacecraftState<>(orbit, attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
310     }
311 
312     private NumericalPropagator buildPropagator(final AttitudeProvider attitudeProvider, final SpacecraftState initialState,
313                                                 final Maneuver maneuver) {
314         OrbitType orbitType = OrbitType.EQUINOCTIAL;
315         double[][] tol = NumericalPropagator.tolerances(1.0e-3, initialState.getOrbit(), orbitType);
316         AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 1000, tol[0], tol[1]);
317         integrator.setInitialStepSize(60);
318         final NumericalPropagator propagator = new NumericalPropagator(integrator);
319         propagator.setOrbitType(orbitType);
320         propagator.setInitialState(initialState);
321         propagator.setAttitudeProvider(attitudeProvider);
322         propagator.addForceModel(maneuver);
323         return propagator;
324     }
325 
326     private <S extends CalculusFieldElement<S>> FieldNumericalPropagator<S> buildPropagator(final Field<S> field,
327                                                                                             final AttitudeProvider attitudeProvider,
328                                                                                             final FieldSpacecraftState<S> initialState,
329                                                                                             final Maneuver maneuver) {
330         OrbitType orbitType = OrbitType.EQUINOCTIAL;
331         double[][] tol = FieldNumericalPropagator.tolerances(field.getZero().newInstance(1.0e-3), initialState.getOrbit(), orbitType);
332         AdaptiveStepsizeFieldIntegrator<S> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 1000, tol[0], tol[1]);
333         integrator.setInitialStepSize(60);
334         final FieldNumericalPropagator<S> propagator = new FieldNumericalPropagator<>(field, integrator);
335         propagator.setOrbitType(orbitType);
336         propagator.setInitialState(initialState);
337         propagator.setAttitudeProvider(attitudeProvider);
338         propagator.addForceModel(maneuver);
339         return propagator;
340     }
341 
342 }