1 /* Copyright 2002-2024 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.events;
18
19 import java.util.function.Function;
20
21 import org.hipparchus.analysis.UnivariateFunction;
22 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
23 import org.hipparchus.util.FastMath;
24 import org.hipparchus.util.MathUtils;
25 import org.orekit.errors.OrekitIllegalArgumentException;
26 import org.orekit.errors.OrekitMessages;
27 import org.orekit.orbits.CircularOrbit;
28 import org.orekit.orbits.EquinoctialOrbit;
29 import org.orekit.orbits.KeplerianOrbit;
30 import org.orekit.orbits.Orbit;
31 import org.orekit.orbits.OrbitType;
32 import org.orekit.orbits.PositionAngleType;
33 import org.orekit.propagation.SpacecraftState;
34 import org.orekit.propagation.events.handlers.EventHandler;
35 import org.orekit.propagation.events.handlers.StopOnEvent;
36 import org.orekit.time.AbsoluteDate;
37 import org.orekit.utils.TimeSpanMap;
38
39 /** Detector for in-orbit position angle.
40 * <p>
41 * The detector is based on anomaly for {@link OrbitType#KEPLERIAN Keplerian}
42 * orbits, latitude argument for {@link OrbitType#CIRCULAR circular} orbits,
43 * or longitude argument for {@link OrbitType#EQUINOCTIAL equinoctial} orbits.
44 * It does not support {@link OrbitType#CARTESIAN Cartesian} orbits. The
45 * angles can be either {@link PositionAngleType#TRUE true}, {@link PositionAngleType#MEAN
46 * mean} or {@link PositionAngleType#ECCENTRIC eccentric} angles.
47 * </p>
48 * @author Luc Maisonobe
49 * @since 7.1
50 */
51 public class PositionAngleDetector extends AbstractDetector<PositionAngleDetector> {
52
53 /** Orbit type defining the angle type. */
54 private final OrbitType orbitType;
55
56 /** Type of position angle. */
57 private final PositionAngleType positionAngleType;
58
59 /** Fixed angle to be crossed. */
60 private final double angle;
61
62 /** Position angle extraction function. */
63 private final Function<Orbit, Double> positionAngleExtractor;
64
65 /** Estimators for the offset angle, taking care of 2π wrapping and g function continuity. */
66 private TimeSpanMap<OffsetEstimator> offsetEstimators;
67
68 /** Build a new detector.
69 * <p>The new instance uses default values for maximal checking interval
70 * ({@link #DEFAULT_MAXCHECK}) and convergence threshold ({@link
71 * #DEFAULT_THRESHOLD}).</p>
72 * @param orbitType orbit type defining the angle type
73 * @param positionAngleType type of position angle
74 * @param angle fixed angle to be crossed
75 * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
76 */
77 public PositionAngleDetector(final OrbitType orbitType, final PositionAngleType positionAngleType,
78 final double angle)
79 throws OrekitIllegalArgumentException {
80 this(DEFAULT_MAXCHECK, DEFAULT_THRESHOLD, orbitType, positionAngleType, angle);
81 }
82
83 /** Build a detector.
84 * <p> This instance uses by default the {@link StopOnEvent} handler </p>
85 * @param maxCheck maximal checking interval (s)
86 * @param threshold convergence threshold (s)
87 * @param orbitType orbit type defining the angle type
88 * @param positionAngleType type of position angle
89 * @param angle fixed angle to be crossed
90 * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
91 */
92 public PositionAngleDetector(final double maxCheck, final double threshold,
93 final OrbitType orbitType, final PositionAngleType positionAngleType,
94 final double angle)
95 throws OrekitIllegalArgumentException {
96 this(AdaptableInterval.of(maxCheck), threshold, DEFAULT_MAX_ITER, new StopOnEvent(),
97 orbitType, positionAngleType, angle);
98 }
99
100 /** Protected constructor with full parameters.
101 * <p>
102 * This constructor is not public as users are expected to use the builder
103 * API with the various {@code withXxx()} methods to set up the instance
104 * in a readable manner without using a huge amount of parameters.
105 * </p>
106 * @param maxCheck maximum checking interval
107 * @param threshold convergence threshold (s)
108 * @param maxIter maximum number of iterations in the event time search
109 * @param handler event handler to call at event occurrences
110 * @param orbitType orbit type defining the angle type
111 * @param positionAngleType type of position angle
112 * @param angle fixed angle to be crossed
113 * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
114 */
115 protected PositionAngleDetector(final AdaptableInterval maxCheck, final double threshold,
116 final int maxIter, final EventHandler handler,
117 final OrbitType orbitType, final PositionAngleType positionAngleType,
118 final double angle)
119 throws OrekitIllegalArgumentException {
120
121 super(maxCheck, threshold, maxIter, handler);
122
123 this.orbitType = orbitType;
124 this.positionAngleType = positionAngleType;
125 this.angle = angle;
126 this.offsetEstimators = null;
127
128 switch (orbitType) {
129 case KEPLERIAN:
130 positionAngleExtractor = o -> ((KeplerianOrbit) orbitType.convertType(o)).getAnomaly(positionAngleType);
131 break;
132 case CIRCULAR:
133 positionAngleExtractor = o -> ((CircularOrbit) orbitType.convertType(o)).getAlpha(positionAngleType);
134 break;
135 case EQUINOCTIAL:
136 positionAngleExtractor = o -> ((EquinoctialOrbit) orbitType.convertType(o)).getL(positionAngleType);
137 break;
138 default:
139 final String sep = ", ";
140 throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_TYPE_NOT_ALLOWED,
141 orbitType,
142 OrbitType.KEPLERIAN + sep +
143 OrbitType.CIRCULAR + sep +
144 OrbitType.EQUINOCTIAL);
145 }
146
147 }
148
149 /** {@inheritDoc} */
150 @Override
151 protected PositionAngleDetector create(final AdaptableInterval newMaxCheck, final double newThreshold,
152 final int newMaxIter,
153 final EventHandler newHandler) {
154 return new PositionAngleDetector(newMaxCheck, newThreshold, newMaxIter, newHandler,
155 orbitType, positionAngleType, angle);
156 }
157
158 /** Get the orbit type defining the angle type.
159 * @return orbit type defining the angle type
160 */
161 public OrbitType getOrbitType() {
162 return orbitType;
163 }
164
165 /** Get the type of position angle.
166 * @return type of position angle
167 */
168 public PositionAngleType getPositionAngleType() {
169 return positionAngleType;
170 }
171
172 /** Get the fixed angle to be crossed (radians).
173 * @return fixed angle to be crossed (radians)
174 */
175 public double getAngle() {
176 return angle;
177 }
178
179 /** {@inheritDoc} */
180 public void init(final SpacecraftState s0, final AbsoluteDate t) {
181 super.init(s0, t);
182 offsetEstimators = new TimeSpanMap<>(new OffsetEstimator(s0.getOrbit(), +1.0));
183 }
184
185 /** Compute the value of the detection function.
186 * <p>
187 * The value is the angle difference between the spacecraft and the fixed
188 * angle to be crossed, with some sign tweaks to ensure continuity.
189 * These tweaks imply the {@code increasing} flag in events detection becomes
190 * irrelevant here! As an example, the angle always increase in a Keplerian
191 * orbit, but this g function will increase and decrease so it
192 * will cross the zero value once per orbit, in increasing and decreasing
193 * directions on alternate orbits..
194 * </p>
195 * @param s the current state information: date, kinematics, attitude
196 * @return angle difference between the spacecraft and the fixed
197 * angle, with some sign tweaks to ensure continuity
198 */
199 public double g(final SpacecraftState s) {
200
201 final Orbit orbit = s.getOrbit();
202
203 // angle difference
204 OffsetEstimator estimator = offsetEstimators.get(s.getDate());
205 double delta = estimator.delta(orbit);
206
207 // we use a value greater than π for handover in order to avoid
208 // several switches to be estimated as the calling propagator
209 // and Orbit.shiftedBy have different accuracy. It is sufficient
210 // to have a handover roughly opposite to the detected position angle
211 while (FastMath.abs(delta) >= 3.5) {
212 // we are too far away from the current estimator, we need to set up a new one
213 // ensuring that we do have a crossing event in the current orbit
214 // and we ensure sign continuity with the current estimator
215
216 // find when the previous estimator becomes invalid
217 final AbsoluteDate handover = estimator.dateForOffset(FastMath.copySign(FastMath.PI, delta), orbit);
218
219 // perform handover to a new estimator at this date
220 estimator = new OffsetEstimator(orbit, delta);
221 delta = estimator.delta(orbit);
222 if (isForward()) {
223 offsetEstimators.addValidAfter(estimator, handover.getDate(), false);
224 } else {
225 offsetEstimators.addValidBefore(estimator, handover.getDate(), false);
226 }
227
228 }
229
230 return delta;
231
232 }
233
234 /** Local class for estimating offset angle, handling 2π wrap-up and sign continuity. */
235 private class OffsetEstimator {
236
237 /** Target angle. */
238 private final double target;
239
240 /** Sign correction to offset. */
241 private final double sign;
242
243 /** Reference angle. */
244 private final double r0;
245
246 /** Slope of the linearized model. */
247 private final double r1;
248
249 /** Reference date. */
250 private final AbsoluteDate t0;
251
252 /** Simple constructor.
253 * @param orbit current orbit
254 * @param currentSign desired sign of the offset at current orbit time (magnitude is ignored)
255 */
256 OffsetEstimator(final Orbit orbit, final double currentSign) {
257 r0 = positionAngleExtractor.apply(orbit);
258 target = MathUtils.normalizeAngle(angle, r0);
259 sign = FastMath.copySign(1.0, (r0 - target) * currentSign);
260 r1 = orbit.getKeplerianMeanMotion();
261 t0 = orbit.getDate();
262 }
263
264 /** Compute offset from reference angle.
265 * @param orbit current orbit
266 * @return offset between current angle and reference angle
267 */
268 public double delta(final Orbit orbit) {
269 final double rawAngle = positionAngleExtractor.apply(orbit);
270 final double linearReference = r0 + r1 * orbit.getDate().durationFrom(t0);
271 final double linearizedAngle = MathUtils.normalizeAngle(rawAngle, linearReference);
272 return sign * (linearizedAngle - target);
273 }
274
275 /** Find date at which offset reaches specified value.
276 * <p>
277 * This computation is an approximation because it relies on
278 * {@link Orbit#shiftedBy(double)} only.
279 * </p>
280 * @param offset target value for offset angle
281 * @param orbit current orbit
282 * @return approximate date at which offset reached specified value
283 */
284 public AbsoluteDate dateForOffset(final double offset, final Orbit orbit) {
285
286 // bracket the search
287 final double period = orbit.getKeplerianPeriod();
288 final double delta0 = delta(orbit);
289 final double searchInf;
290 final double searchSup;
291 if ((delta0 - offset) * sign >= 0) {
292 // the date is before current orbit
293 searchInf = -period;
294 searchSup = 0;
295 } else {
296 // the date is after current orbit
297 searchInf = 0;
298 searchSup = +period;
299 }
300
301 // find the date as an offset from current orbit
302 final BracketingNthOrderBrentSolver solver = new BracketingNthOrderBrentSolver(getThreshold(), 5);
303 final UnivariateFunction f = dt -> delta(orbit.shiftedBy(dt)) - offset;
304 final double root = solver.solve(getMaxIterationCount(), f, searchInf, searchSup);
305
306 return orbit.getDate().shiftedBy(root);
307
308 }
309
310 }
311
312 }