1 /* Copyright 2002-2026 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.control.relative;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.Derivative;
22 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
23 import org.hipparchus.analysis.solvers.NewtonRaphsonSolver;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.util.FastMath;
26 import org.orekit.propagation.relative.clohessywiltshire.FieldClohessyWiltshireEquations;
27 import org.orekit.propagation.relative.clohessywiltshire.FieldClohessyWiltshireMatrices;
28 import org.orekit.time.FieldAbsoluteDate;
29 import org.orekit.utils.FieldPVCoordinates;
30 import org.orekit.utils.TimeStampedFieldPVCoordinates;
31
32 import java.util.ArrayList;
33 import java.util.List;
34
35 /**
36 * Class to store and compute the waypoints representing a teardrop maneuver sequence.
37 * <p>
38 * Note: The analytical solution of the Teardrop maneuver sequence is valid only using the Clohessy-Wiltshire equations
39 * (circular orbit).
40 * </p>
41 *
42 * @param <T> Any scalar field
43 * @author Romain Cuvillon
44 * @since 14.0
45 */
46 public class FieldTeardropCircularWaypointCalculator<T extends CalculusFieldElement<T>> {
47
48 /**
49 * Number of orbits to consider. Must be ≥ 1.
50 */
51 private final int numberOfOrbits;
52
53 /**
54 * Mean motion of the target orbit.
55 */
56 private final T targetMeanMotion;
57
58 /**
59 * Turn-around distance of the teardrop orbit. This is the "round" end of the orbit. Note that this distance is
60 * signed : negative means below the target spacecraft (in between the planet and the target), while positive means
61 * above the target (target is in between the chaser and the planet).
62 */
63 private final T turnAroundDistance;
64
65 /**
66 * Maneuver distance of the teardrop orbit. This is the "pointy" end of the orbit. Note that this distance is signed
67 * : negative means below the target spacecraft (in between the planet and the target), while positive means above
68 * the target (target is in between the chaser and the planet).
69 */
70 private final T maneuverDistance;
71
72 /**
73 * Creates a new teardrop relative orbit calculator.
74 *
75 * @param targetMeanMotion Target spacecraft's orbital mean motion, in rad/s.
76 * @param turnAroundDistance Turn-around distance. This is the "round" end of the orbit. Note that this distance is
77 * signed : negative means below the target spacecraft (in between the planet and the
78 * target), while positive means above the target (target is in between the chaser and the
79 * planet).
80 * @param maneuverDistance Maneuver distance of the teardrop orbit. This is the "pointy" end of the orbit. Note
81 * that this distance is signed : negative means below the target spacecraft (in between
82 * the planet and the target), while positive means above the target (target is in between
83 * the chaser and the planet).
84 * @param numberOfOrbits Number of teardrop orbits to perform. Must be ≥ 1.
85 */
86 public FieldTeardropCircularWaypointCalculator(final T targetMeanMotion, final T turnAroundDistance,
87 final T maneuverDistance, final int numberOfOrbits) {
88 this.targetMeanMotion = targetMeanMotion;
89 this.turnAroundDistance = turnAroundDistance;
90 this.maneuverDistance = maneuverDistance;
91 this.numberOfOrbits = FastMath.max(1, numberOfOrbits); // Ensure that the number of orbits is ≥ 1.
92 }
93
94 /**
95 * Computes the waypoints of the teardrop relative orbit in QSW Local Orbital Frame to use them with
96 * Clohessy-Wiltshire maneuvers.
97 * <p>The injection point is the turn-around point of the teardrop (the round end).</p>
98 * <p>All maneuvers happen at the pointy end of the teardrop.</p>
99 *
100 * @param injectionDate Date of the first waypoint, which corresponds to the injection point of the teardrop orbit.
101 * @return List of waypoints in time. Date, position, and velocity are non-zero.
102 */
103 public List<TimeStampedFieldPVCoordinates<T>> computeTearDropWaypoints(final FieldAbsoluteDate<T> injectionDate) {
104 final Field<T> field = injectionDate.getField();
105 // Define start and end points of the first arc (from turn-around to maneuver point)
106 final FieldVector3D<T> posTurnAround =
107 new FieldVector3D<>(turnAroundDistance, field.getZero(), field.getZero());
108
109 // Compute the relative orbit's period
110 final T halfRelativePeriod = computeRelativeOrbitalPeriod().divide(2.0);
111
112 // Compute the norm of Y-axis aligned initial velocity v0 to reach the desired target X value at a given time using the Clohessy-Wiltshire equations
113 final T v0Xtgt = targetMeanMotion.multiply(maneuverDistance.add(
114 turnAroundDistance.multiply(3).multiply(targetMeanMotion.multiply(halfRelativePeriod).cos()))
115 .subtract(turnAroundDistance.multiply(4)))
116 .divide(targetMeanMotion.multiply(halfRelativePeriod).cos().subtract(1)
117 .multiply(2));
118
119 final TimeStampedFieldPVCoordinates<T> pvtInjection = new TimeStampedFieldPVCoordinates<>(injectionDate,
120 new FieldPVCoordinates<>(
121 posTurnAround,
122 new FieldVector3D<>(
123 field.getZero(),
124 v0Xtgt.negate(),
125 field.getZero())));
126
127 // Propagate chaser motion using Clohessy-Wiltshire equations and initial conditions for t = halfRelativePeriod
128 final FieldClohessyWiltshireMatrices<T> cwMatrices =
129 (new FieldClohessyWiltshireEquations<T>()).computeMatrices(halfRelativePeriod,
130 targetMeanMotion);
131 final TimeStampedFieldPVCoordinates<T> pvtBeforeMan = cwMatrices.transform(pvtInjection);
132
133 // Compute the PVT at maneuver point after the maneuver : same velocity along the Y axis, but reversed velocity along the X axis. The Z velocity is zero for a circular Keplerian orbit (Clohessy-Wiltshire theory).
134 final TimeStampedFieldPVCoordinates<T> pvtAfterMan = new TimeStampedFieldPVCoordinates<>(pvtBeforeMan.getDate(),
135 new FieldPVCoordinates<>(
136 pvtBeforeMan.getPosition(),
137 new FieldVector3D<>(
138 pvtBeforeMan.getVelocity()
139 .getX()
140 .negate(),
141 pvtBeforeMan.getVelocity()
142 .getY(),
143 pvtBeforeMan.getVelocity()
144 .getZ())));
145
146 // Generate waypoints for each maneuver with the correct post-maneuver velocity
147 final List<TimeStampedFieldPVCoordinates<T>> waypoints = new ArrayList<>();
148 waypoints.add(pvtInjection);
149
150 // Add one waypoint to ensure that the correct number of iterations is performed. One iteration = from a maneuver to the next maneuver.
151 for (int orbitNumber = 0; orbitNumber < numberOfOrbits + 1; orbitNumber++) {
152 waypoints.add(new TimeStampedFieldPVCoordinates<>(
153 pvtInjection.getDate().shiftedBy(halfRelativePeriod.multiply((2 * orbitNumber) + 1)),
154 new FieldPVCoordinates<>(pvtAfterMan.getPosition(), pvtAfterMan.getVelocity())));
155 }
156
157 return waypoints;
158 }
159
160 /**
161 * Computes the relative orbit's period. Depends on the target's orbital pulsation and the geometry of the teardrop
162 * relative orbit.
163 *
164 * @return Period of the relative orbit, in seconds.
165 */
166 public T computeRelativeOrbitalPeriod() {
167 // Solve the Clohessy-Wiltshire equations to acquire the value of the time so that the Y coordinate is zero while the X coordinate is maneuverDistance
168 // The function looks like a tangent function with f(0) -> -∞ and f(orbital period) -> +∞.
169 // It happens that it has exactly one root in t ∈ ]0 ; orbital period[, and a mirrored root in t ∈ ]-orbital period ; 0[.
170 // If the solver jumps to the negative side, the root can still be used.
171 return targetMeanMotion.getField().getOne().multiply(2.0 * FastMath.abs((new NewtonRaphsonSolver()).solve(1000,
172 new yEquation(targetMeanMotion,
173 maneuverDistance,
174 turnAroundDistance),
175 1e-12,
176 getTargetKeplerianPeriod().getReal(),
177 1)));
178 }
179
180 /**
181 * Equation to solve to find the relative orbital period.
182 * <p>
183 * Here the "value(double)" method may lose the "fielded" part of the computation. This is done so as Hipparchus
184 * does not have fielded version of UnivariateDifferentiableFunction.
185 * </p>
186 */
187 private class yEquation implements UnivariateDifferentiableFunction {
188
189 /**
190 * targetMeanMotion to compute tearDrop relative Orbital Period.
191 */
192 private final T targetMeanMotion;
193
194 /**
195 * maneuverDistance to compute tearDrop relative Orbital Period.
196 */
197 private final T maneuverDistance;
198
199 /**
200 * turnAroundDistance to compute tearDrop relative Orbital Period.
201 */
202 private final T turnAroundDistance;
203
204 private yEquation(final T targetMeanMotion, final T maneuverDistance, final T turnAroundDistance) {
205 this.targetMeanMotion = targetMeanMotion;
206 this.maneuverDistance = maneuverDistance;
207 this.turnAroundDistance = turnAroundDistance;
208 }
209
210 public double value(final double t) {
211 final double maneuverDistanceReal = maneuverDistance.getReal();
212 final double targetMeanMotionReal = targetMeanMotion.getReal();
213 final double turnAroundDistanceReal = turnAroundDistance.getReal();
214 return (3 * maneuverDistanceReal * targetMeanMotionReal * t -
215 3 * targetMeanMotionReal * t * turnAroundDistanceReal * FastMath.cos(targetMeanMotionReal * t) -
216 4 * (maneuverDistanceReal - turnAroundDistanceReal) * FastMath.sin(targetMeanMotionReal * t)) /
217 (2. * (-1 + FastMath.cos(targetMeanMotionReal * t)));
218 }
219
220 public <M extends Derivative<M>> M value(final M t) {
221 final double maneuverDistanceReal = maneuverDistance.getReal();
222 final double targetMeanMotionReal = targetMeanMotion.getReal();
223 final double turnAroundDistanceReal = turnAroundDistance.getReal();
224 return (t.multiply(3 * maneuverDistanceReal * targetMeanMotionReal)
225 .subtract(t.multiply(3 * targetMeanMotionReal * turnAroundDistanceReal)
226 .multiply(t.multiply(targetMeanMotionReal).cos()))
227 .subtract(t.multiply(targetMeanMotionReal).sin()
228 .multiply(4 * (maneuverDistanceReal - turnAroundDistanceReal)))).divide(
229 t.multiply(targetMeanMotionReal).cos().add(-1).multiply(2));
230
231 }
232 }
233
234 /**
235 * Computes and returns the target's Keplerian period, in seconds.
236 *
237 * @return The target's Keplerian period, in seconds.
238 */
239 private T getTargetKeplerianPeriod() {
240 final T pi = targetMeanMotion.getPi();
241 return pi.multiply(2).divide(targetMeanMotion);
242 }
243 }