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