1 /* Copyright 2002-2025 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.utils;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
22 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
25 import org.hipparchus.util.FastMath;
26 import org.orekit.errors.OrekitInternalError;
27 import org.orekit.time.AbstractFieldTimeInterpolator;
28 import org.orekit.time.FieldAbsoluteDate;
29
30 import java.util.List;
31
32 /**
33 * Class using Hermite interpolator to interpolate time stamped angular coordinates.
34 * <p>
35 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation points
36 * (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
37 * and numerical problems (including NaN appearing).
38 *
39 * @param <KK> type of the field element
40 *
41 * @author Vincent Cucchietti
42 * @author Luc Maisonobe
43 * @see FieldHermiteInterpolator
44 * @see TimeStampedFieldAngularCoordinates
45 */
46 public class TimeStampedFieldAngularCoordinatesHermiteInterpolator<KK extends CalculusFieldElement<KK>>
47 extends AbstractFieldTimeInterpolator<TimeStampedFieldAngularCoordinates<KK>, KK> {
48
49 /** Filter for derivatives from the sample to use in interpolation. */
50 private final AngularDerivativesFilter filter;
51
52 /**
53 * Constructor with :
54 * <ul>
55 * <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
56 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
57 * <li>Use of angular and first time derivative for attitude interpolation</li>
58 * </ul>
59 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
60 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
61 * phenomenon</a> and numerical problems (including NaN appearing).
62 */
63 public TimeStampedFieldAngularCoordinatesHermiteInterpolator() {
64 this(DEFAULT_INTERPOLATION_POINTS);
65 }
66
67 /**
68 * /** Constructor with :
69 * <ul>
70 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
71 * <li>Use of angular and first time derivative for attitude interpolation</li>
72 * </ul>
73 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
74 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
75 * phenomenon</a> and numerical problems (including NaN appearing).
76 *
77 * @param interpolationPoints number of interpolation points
78 */
79 public TimeStampedFieldAngularCoordinatesHermiteInterpolator(final int interpolationPoints) {
80 this(interpolationPoints, AngularDerivativesFilter.USE_RR);
81 }
82
83 /**
84 * Constructor with :
85 * <ul>
86 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
87 * </ul>
88 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
89 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
90 * phenomenon</a> and numerical problems (including NaN appearing).
91 *
92 * @param interpolationPoints number of interpolation points
93 * @param filter filter for derivatives from the sample to use in interpolation
94 */
95 public TimeStampedFieldAngularCoordinatesHermiteInterpolator(final int interpolationPoints,
96 final AngularDerivativesFilter filter) {
97 this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, filter);
98 }
99
100 /**
101 * Constructor.
102 * <p>
103 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
104 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
105 * phenomenon</a> and numerical problems (including NaN appearing).
106 *
107 * @param interpolationPoints number of interpolation points
108 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
109 * @param filter filter for derivatives from the sample to use in interpolation
110 */
111 public TimeStampedFieldAngularCoordinatesHermiteInterpolator(final int interpolationPoints,
112 final double extrapolationThreshold,
113 final AngularDerivativesFilter filter) {
114 super(interpolationPoints, extrapolationThreshold);
115 this.filter = filter;
116 }
117
118 /** Get filter for derivatives from the sample to use in interpolation.
119 * @return filter for derivatives from the sample to use in interpolation
120 */
121 public AngularDerivativesFilter getFilter() {
122 return filter;
123 }
124
125 /**
126 * {@inheritDoc}
127 * <p>
128 * The interpolated instance is created by polynomial Hermite interpolation on Rodrigues vector ensuring rotation rate
129 * remains the exact derivative of rotation.
130 * <p>
131 * This method is based on Sergei Tanygin's paper <a
132 * href="http://www.agi.com/resources/white-papers/attitude-interpolation">Attitude Interpolation</a>, changing the norm
133 * of the vector to match the modified Rodrigues vector as described in Malcolm D. Shuster's paper <a
134 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
135 * Survey of Attitude Representations</a>. This change avoids the singularity at π. There is still a singularity at 2π,
136 * which is handled by slightly offsetting all rotations when this singularity is detected. Another change is that the
137 * mean linear motion is first removed before interpolation and added back after interpolation. This allows to use
138 * interpolation even when the sample covers much more than one turn and even when sample points are separated by more
139 * than one turn.
140 * </p>
141 * <p>
142 * Note that even if first and second time derivatives (rotation rates and acceleration) from sample can be ignored, the
143 * interpolated instance always includes interpolated derivatives. This feature can be used explicitly to compute these
144 * derivatives when it would be too complex to compute them from an analytical formula: just compute a few sample points
145 * from the explicit formula and set the derivatives to zero in these sample points, then use interpolation to add
146 * derivatives consistent with the rotations.
147 */
148 @Override
149 protected TimeStampedFieldAngularCoordinates<KK> interpolate(final InterpolationData interpolationData) {
150
151 // Get interpolation date
152 final FieldAbsoluteDate<KK> interpolationDate = interpolationData.getInterpolationDate();
153
154 // Get sample
155 final List<TimeStampedFieldAngularCoordinates<KK>> sample = interpolationData.getNeighborList();
156
157 // set up safety elements for 2π singularity avoidance
158 final double epsilon = 2 * FastMath.PI / sample.size();
159 final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));
160
161 // set up a linear model canceling mean rotation rate
162 final Field<KK> field = interpolationData.getField();
163 final FieldVector3D<KK> meanRate;
164 FieldVector3D<KK> sum = FieldVector3D.getZero(field);
165 if (filter != AngularDerivativesFilter.USE_R) {
166 for (final TimeStampedFieldAngularCoordinates<KK> datedAC : sample) {
167 sum = sum.add(datedAC.getRotationRate());
168 }
169 meanRate = new FieldVector3D<>(1.0 / sample.size(), sum);
170 }
171 else {
172 TimeStampedFieldAngularCoordinates<KK> previous = null;
173 for (final TimeStampedFieldAngularCoordinates<KK> datedAC : sample) {
174 if (previous != null) {
175 sum = sum.add(TimeStampedFieldAngularCoordinates.estimateRate(previous.getRotation(),
176 datedAC.getRotation(),
177 datedAC.getDate()
178 .durationFrom(previous.getDate())));
179 }
180 previous = datedAC;
181 }
182 meanRate = new FieldVector3D<>(1.0 / (sample.size() - 1), sum);
183 }
184 TimeStampedFieldAngularCoordinates<KK> offset =
185 new TimeStampedFieldAngularCoordinates<>(interpolationDate, FieldRotation.getIdentity(field),
186 meanRate, FieldVector3D.getZero(field));
187
188 boolean restart = true;
189 for (int i = 0; restart && i < sample.size() + 2; ++i) {
190
191 // offset adaptation parameters
192 restart = false;
193
194 // set up an interpolator taking derivatives into account
195 final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();
196
197 // add sample points
198 final KK one = interpolationData.getOne();
199 double sign = 1.0;
200 FieldRotation<KK> previous = FieldRotation.getIdentity(field);
201
202 for (final TimeStampedFieldAngularCoordinates<KK> ac : sample) {
203
204 // remove linear offset from the current coordinates
205 final KK dt = ac.getDate().durationFrom(interpolationDate);
206 final TimeStampedFieldAngularCoordinates<KK> fixed = ac.subtractOffset(offset.shiftedBy(dt));
207
208 // make sure all interpolated points will be on the same branch
209 final double dot = one.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
210 fixed.getRotation().getQ1(), previous.getQ1(),
211 fixed.getRotation().getQ2(), previous.getQ2(),
212 fixed.getRotation().getQ3(), previous.getQ3()).getReal();
213 sign = FastMath.copySign(1.0, dot * sign);
214 previous = fixed.getRotation();
215
216 // check modified Rodrigues vector singularity
217 if (fixed.getRotation().getQ0().getReal() * sign < threshold) {
218 // the sample point is close to a modified Rodrigues vector singularity
219 // we need to change the linear offset model to avoid this
220 restart = true;
221 break;
222 }
223
224 final KK[][] rodrigues = fixed.getModifiedRodrigues(sign);
225 switch (filter) {
226 case USE_RRA:
227 // populate sample with rotation, rotation rate and acceleration data
228 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
229 break;
230 case USE_RR:
231 // populate sample with rotation and rotation rate data
232 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
233 break;
234 case USE_R:
235 // populate sample with rotation data only
236 interpolator.addSamplePoint(dt, rodrigues[0]);
237 break;
238 default:
239 // this should never happen
240 throw new OrekitInternalError(null);
241 }
242 }
243
244 if (restart) {
245 // interpolation failed, some intermediate rotation was too close to 2π
246 // we need to offset all rotations to avoid the singularity
247 offset = offset.addOffset(
248 new FieldAngularCoordinates<>(new FieldRotation<>(FieldVector3D.getPlusI(field),
249 one.newInstance(epsilon),
250 RotationConvention.VECTOR_OPERATOR),
251 FieldVector3D.getZero(field), FieldVector3D.getZero(field)));
252 } else {
253 // interpolation succeeded with the current offset
254 final KK zero = interpolationData.getZero();
255 final KK[][] p = interpolator.derivatives(zero, 2);
256 final FieldAngularCoordinates<KK> ac = FieldAngularCoordinates.createFromModifiedRodrigues(p);
257 return new TimeStampedFieldAngularCoordinates<>(offset.getDate(),
258 ac.getRotation(),
259 ac.getRotationRate(),
260 ac.getRotationAcceleration()).addOffset(offset);
261 }
262
263 }
264
265 // this should never happen
266 throw new OrekitInternalError(null);
267 }
268 }