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.utils;
18
19 import java.util.Collection;
20
21 import org.hipparchus.Field;
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.analysis.differentiation.FieldDerivative;
24 import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
25 import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
26 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
29 import org.hipparchus.util.FastMath;
30 import org.orekit.errors.OrekitException;
31 import org.orekit.errors.OrekitInternalError;
32 import org.orekit.errors.OrekitMessages;
33 import org.orekit.time.AbsoluteDate;
34 import org.orekit.time.FieldAbsoluteDate;
35 import org.orekit.time.FieldTimeStamped;
36 import org.orekit.time.TimeStamped;
37
38 /** {@link TimeStamped time-stamped} version of {@link FieldAngularCoordinates}.
39 * <p>Instances of this class are guaranteed to be immutable.</p>
40 * @param <T> the type of the field elements
41 * @author Luc Maisonobe
42 * @since 7.0
43 */
44 public class TimeStampedFieldAngularCoordinates<T extends CalculusFieldElement<T>>
45 extends FieldAngularCoordinates<T> implements FieldTimeStamped<T> {
46
47 /** The date. */
48 private final FieldAbsoluteDate<T> date;
49
50 /** Build the rotation that transforms a pair of pv coordinates into another pair.
51
52 * <p><em>WARNING</em>! This method requires much more stringent assumptions on
53 * its parameters than the similar {@link org.hipparchus.geometry.euclidean.threed.Rotation#Rotation(
54 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D,
55 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D)
56 * constructor} from the {@link org.hipparchus.geometry.euclidean.threed.Rotation Rotation} class.
57 * As far as the Rotation constructor is concerned, the {@code v₂} vector from
58 * the second pair can be slightly misaligned. The Rotation constructor will
59 * compensate for this misalignment and create a rotation that ensure {@code
60 * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
61 * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
62 * preserved, this constructor works <em>only</em> if the two pairs are fully
63 * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
64 * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
65 * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>
66
67 * @param date coordinates date
68 * @param u1 first vector of the origin pair
69 * @param u2 second vector of the origin pair
70 * @param v1 desired image of u1 by the rotation
71 * @param v2 desired image of u2 by the rotation
72 * @param tolerance relative tolerance factor used to check singularities
73 */
74 public TimeStampedFieldAngularCoordinates (final AbsoluteDate date,
75 final FieldPVCoordinates<T> u1, final FieldPVCoordinates<T> u2,
76 final FieldPVCoordinates<T> v1, final FieldPVCoordinates<T> v2,
77 final double tolerance) {
78 this(new FieldAbsoluteDate<>(u1.getPosition().getX().getField(), date),
79 u1, u2, v1, v2, tolerance);
80 }
81
82 /** Build the rotation that transforms a pair of pv coordinates into another pair.
83
84 * <p><em>WARNING</em>! This method requires much more stringent assumptions on
85 * its parameters than the similar {@link org.hipparchus.geometry.euclidean.threed.Rotation#Rotation(
86 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D,
87 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D)
88 * constructor} from the {@link org.hipparchus.geometry.euclidean.threed.Rotation Rotation} class.
89 * As far as the Rotation constructor is concerned, the {@code v₂} vector from
90 * the second pair can be slightly misaligned. The Rotation constructor will
91 * compensate for this misalignment and create a rotation that ensure {@code
92 * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
93 * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
94 * preserved, this constructor works <em>only</em> if the two pairs are fully
95 * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
96 * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
97 * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>
98
99 * @param date coordinates date
100 * @param u1 first vector of the origin pair
101 * @param u2 second vector of the origin pair
102 * @param v1 desired image of u1 by the rotation
103 * @param v2 desired image of u2 by the rotation
104 * @param tolerance relative tolerance factor used to check singularities
105 */
106 public TimeStampedFieldAngularCoordinates (final FieldAbsoluteDate<T> date,
107 final FieldPVCoordinates<T> u1, final FieldPVCoordinates<T> u2,
108 final FieldPVCoordinates<T> v1, final FieldPVCoordinates<T> v2,
109 final double tolerance) {
110 super(u1, u2, v1, v2, tolerance);
111 this.date = date;
112 }
113
114 /** Builds a rotation/rotation rate pair.
115 * @param date coordinates date
116 * @param rotation rotation
117 * @param rotationRate rotation rate Ω (rad/s)
118 * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²)
119 */
120 public TimeStampedFieldAngularCoordinates(final AbsoluteDate date,
121 final FieldRotation<T> rotation,
122 final FieldVector3D<T> rotationRate,
123 final FieldVector3D<T> rotationAcceleration) {
124 this(new FieldAbsoluteDate<>(rotation.getQ0().getField(), date),
125 rotation, rotationRate, rotationAcceleration);
126 }
127
128 /** Builds a rotation/rotation rate pair.
129 * @param date coordinates date
130 * @param rotation rotation
131 * @param rotationRate rotation rate Ω (rad/s)
132 * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²)
133 */
134 public TimeStampedFieldAngularCoordinates(final FieldAbsoluteDate<T> date,
135 final FieldRotation<T> rotation,
136 final FieldVector3D<T> rotationRate,
137 final FieldVector3D<T> rotationAcceleration) {
138 super(rotation, rotationRate, rotationAcceleration);
139 this.date = date;
140 }
141
142 /** Builds an instance for a regular {@link TimeStampedAngularCoordinates}.
143 * @param field fields to which the elements belong
144 * @param ac coordinates to convert
145 * @since 9.0
146 */
147 public TimeStampedFieldAngularCoordinates(final Field<T> field,
148 final TimeStampedAngularCoordinates ac) {
149 this(new FieldAbsoluteDate<>(field, ac.getDate()),
150 new FieldRotation<>(field, ac.getRotation()),
151 new FieldVector3D<>(field, ac.getRotationRate()),
152 new FieldVector3D<>(field, ac.getRotationAcceleration()));
153 }
154
155 /** Builds a TimeStampedFieldAngularCoordinates from a {@link FieldRotation}<{@link FieldDerivativeStructure}>.
156 * <p>
157 * The rotation components must have time as their only derivation parameter and
158 * have consistent derivation orders.
159 * </p>
160 * @param date coordinates date
161 * @param r rotation with time-derivatives embedded within the coordinates
162 * @param <U> type of the derivative
163 * @since 9.2
164 */
165 public <U extends FieldDerivative<T, U>> TimeStampedFieldAngularCoordinates(final FieldAbsoluteDate<T> date,
166 final FieldRotation<U> r) {
167 super(r);
168 this.date = date;
169 }
170
171 /** Revert a rotation/rotation rate pair.
172 * Build a pair which reverse the effect of another pair.
173 * @return a new pair whose effect is the reverse of the effect
174 * of the instance
175 */
176 public TimeStampedFieldAngularCoordinates<T> revert() {
177 return new TimeStampedFieldAngularCoordinates<>(date,
178 getRotation().revert(),
179 getRotation().applyInverseTo(getRotationRate().negate()),
180 getRotation().applyInverseTo(getRotationAcceleration().negate()));
181 }
182
183 /** {@inheritDoc} */
184 @Override
185 public FieldAbsoluteDate<T> getDate() {
186 return date;
187 }
188
189 /** Get a time-shifted state.
190 * <p>
191 * The state can be slightly shifted to close dates. This shift is based on
192 * a simple linear model. It is <em>not</em> intended as a replacement for
193 * proper attitude propagation but should be sufficient for either small
194 * time shifts or coarse accuracy.
195 * </p>
196 * @param dt time shift in seconds
197 * @return a new state, shifted with respect to the instance (which is immutable)
198 */
199 public TimeStampedFieldAngularCoordinates<T> shiftedBy(final double dt) {
200 return shiftedBy(getDate().getField().getZero().add(dt));
201 }
202
203 /** Get a time-shifted state.
204 * <p>
205 * The state can be slightly shifted to close dates. This shift is based on
206 * a simple linear model. It is <em>not</em> intended as a replacement for
207 * proper attitude propagation but should be sufficient for either small
208 * time shifts or coarse accuracy.
209 * </p>
210 * @param dt time shift in seconds
211 * @return a new state, shifted with respect to the instance (which is immutable)
212 */
213 public TimeStampedFieldAngularCoordinates<T> shiftedBy(final T dt) {
214 final FieldAngularCoordinates<T> sac = super.shiftedBy(dt);
215 return new TimeStampedFieldAngularCoordinates<>(date.shiftedBy(dt),
216 sac.getRotation(), sac.getRotationRate(), sac.getRotationAcceleration());
217
218 }
219
220 /** Add an offset from the instance.
221 * <p>
222 * We consider here that the offset rotation is applied first and the
223 * instance is applied afterward. Note that angular coordinates do <em>not</em>
224 * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
225 * b.addOffset(a)} lead to <em>different</em> results in most cases.
226 * </p>
227 * <p>
228 * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
229 * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
230 * so that round trip applications are possible. This means that both {@code
231 * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
232 * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
233 * </p>
234 * @param offset offset to subtract
235 * @return new instance, with offset subtracted
236 * @see #subtractOffset(FieldAngularCoordinates)
237 */
238 public TimeStampedFieldAngularCoordinates<T> addOffset(final FieldAngularCoordinates<T> offset) {
239 final FieldVector3D<T> rOmega = getRotation().applyTo(offset.getRotationRate());
240 final FieldVector3D<T> rOmegaDot = getRotation().applyTo(offset.getRotationAcceleration());
241 return new TimeStampedFieldAngularCoordinates<>(date,
242 getRotation().compose(offset.getRotation(), RotationConvention.VECTOR_OPERATOR),
243 getRotationRate().add(rOmega),
244 new FieldVector3D<>( 1.0, getRotationAcceleration(),
245 1.0, rOmegaDot,
246 -1.0, FieldVector3D.crossProduct(getRotationRate(), rOmega)));
247 }
248
249 /** Subtract an offset from the instance.
250 * <p>
251 * We consider here that the offset Rotation is applied first and the
252 * instance is applied afterward. Note that angular coordinates do <em>not</em>
253 * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
254 * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
255 * </p>
256 * <p>
257 * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
258 * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
259 * so that round trip applications are possible. This means that both {@code
260 * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
261 * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
262 * </p>
263 * @param offset offset to subtract
264 * @return new instance, with offset subtracted
265 * @see #addOffset(FieldAngularCoordinates)
266 */
267 public TimeStampedFieldAngularCoordinates<T> subtractOffset(final FieldAngularCoordinates<T> offset) {
268 return addOffset(offset.revert());
269 }
270
271 /** Interpolate angular coordinates.
272 * <p>
273 * The interpolated instance is created by polynomial Hermite interpolation
274 * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
275 * </p>
276 * <p>
277 * This method is based on Sergei Tanygin's paper <a
278 * href="http://www.agi.com/resources/white-papers/attitude-interpolation">Attitude
279 * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
280 * vector as described in Malcolm D. Shuster's paper <a
281 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
282 * Survey of Attitude Representations</a>. This change avoids the singularity at π.
283 * There is still a singularity at 2π, which is handled by slightly offsetting all rotations
284 * when this singularity is detected.
285 * </p>
286 * <p>
287 * Note that even if first time derivatives (rotation rates)
288 * from sample can be ignored, the interpolated instance always includes
289 * interpolated derivatives. This feature can be used explicitly to
290 * compute these derivatives when it would be too complex to compute them
291 * from an analytical formula: just compute a few sample points from the
292 * explicit formula and set the derivatives to zero in these sample points,
293 * then use interpolation to add derivatives consistent with the rotations.
294 * </p>
295 * @param date interpolation date
296 * @param filter filter for derivatives from the sample to use in interpolation
297 * @param sample sample points on which interpolation should be done
298 * @param <T> the type of the field elements
299 * @return a new position-velocity, interpolated at specified date
300 */
301 public static <T extends CalculusFieldElement<T>>
302 TimeStampedFieldAngularCoordinates<T> interpolate(final AbsoluteDate date,
303 final AngularDerivativesFilter filter,
304 final Collection<TimeStampedFieldAngularCoordinates<T>> sample) {
305 return interpolate(new FieldAbsoluteDate<>(sample.iterator().next().getRotation().getQ0().getField(), date),
306 filter, sample);
307 }
308
309 /** Interpolate angular coordinates.
310 * <p>
311 * The interpolated instance is created by polynomial Hermite interpolation
312 * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
313 * </p>
314 * <p>
315 * This method is based on Sergei Tanygin's paper <a
316 * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
317 * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
318 * vector as described in Malcolm D. Shuster's paper <a
319 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
320 * Survey of Attitude Representations</a>. This change avoids the singularity at π.
321 * There is still a singularity at 2π, which is handled by slightly offsetting all rotations
322 * when this singularity is detected.
323 * </p>
324 * <p>
325 * Note that even if first time derivatives (rotation rates)
326 * from sample can be ignored, the interpolated instance always includes
327 * interpolated derivatives. This feature can be used explicitly to
328 * compute these derivatives when it would be too complex to compute them
329 * from an analytical formula: just compute a few sample points from the
330 * explicit formula and set the derivatives to zero in these sample points,
331 * then use interpolation to add derivatives consistent with the rotations.
332 * </p>
333 * @param date interpolation date
334 * @param filter filter for derivatives from the sample to use in interpolation
335 * @param sample sample points on which interpolation should be done
336 * @param <T> the type of the field elements
337 * @return a new position-velocity, interpolated at specified date
338 */
339 public static <T extends CalculusFieldElement<T>>
340 TimeStampedFieldAngularCoordinates<T> interpolate(final FieldAbsoluteDate<T> date,
341 final AngularDerivativesFilter filter,
342 final Collection<TimeStampedFieldAngularCoordinates<T>> sample) {
343
344 // get field properties
345 final Field<T> field = sample.iterator().next().getRotation().getQ0().getField();
346
347 // set up safety elements for 2π singularity avoidance
348 final double epsilon = 2 * FastMath.PI / sample.size();
349 final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));
350
351 // set up a linear model canceling mean rotation rate
352 final FieldVector3D<T> meanRate;
353 if (filter != AngularDerivativesFilter.USE_R) {
354 FieldVector3D<T> sum = FieldVector3D.getZero(field);
355 for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) {
356 sum = sum.add(datedAC.getRotationRate());
357 }
358 meanRate = new FieldVector3D<>(1.0 / sample.size(), sum);
359 } else {
360 if (sample.size() < 2) {
361 throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION,
362 sample.size());
363 }
364 FieldVector3D<T> sum = FieldVector3D.getZero(field);
365 TimeStampedFieldAngularCoordinates<T> previous = null;
366 for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) {
367 if (previous != null) {
368 sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(),
369 datedAC.date.durationFrom(previous.getDate())));
370 }
371 previous = datedAC;
372 }
373 meanRate = new FieldVector3D<>(1.0 / (sample.size() - 1), sum);
374 }
375 TimeStampedFieldAngularCoordinates<T> offset =
376 new TimeStampedFieldAngularCoordinates<>(date, FieldRotation.getIdentity(field),
377 meanRate, FieldVector3D.getZero(field));
378
379 boolean restart = true;
380 for (int i = 0; restart && i < sample.size() + 2; ++i) {
381
382 // offset adaptation parameters
383 restart = false;
384
385 // set up an interpolator taking derivatives into account
386 final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<>();
387
388 // add sample points
389 double sign = +1.0;
390 FieldRotation<T> previous = FieldRotation.getIdentity(field);
391
392 for (final TimeStampedFieldAngularCoordinates<T> ac : sample) {
393
394 // remove linear offset from the current coordinates
395 final T dt = ac.date.durationFrom(date);
396 final TimeStampedFieldAngularCoordinates<T> fixed = ac.subtractOffset(offset.shiftedBy(dt));
397
398 // make sure all interpolated points will be on the same branch
399 final T dot = dt.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
400 fixed.getRotation().getQ1(), previous.getQ1(),
401 fixed.getRotation().getQ2(), previous.getQ2(),
402 fixed.getRotation().getQ3(), previous.getQ3());
403 sign = FastMath.copySign(1.0, dot.getReal() * sign);
404 previous = fixed.getRotation();
405
406 // check modified Rodrigues vector singularity
407 if (fixed.getRotation().getQ0().getReal() * sign < threshold) {
408 // the sample point is close to a modified Rodrigues vector singularity
409 // we need to change the linear offset model to avoid this
410 restart = true;
411 break;
412 }
413
414 final T[][] rodrigues = fixed.getModifiedRodrigues(sign);
415 switch (filter) {
416 case USE_RRA:
417 // populate sample with rotation, rotation rate and acceleration data
418 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
419 break;
420 case USE_RR:
421 // populate sample with rotation and rotation rate data
422 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
423 break;
424 case USE_R:
425 // populate sample with rotation data only
426 interpolator.addSamplePoint(dt, rodrigues[0]);
427 break;
428 default :
429 // this should never happen
430 throw new OrekitInternalError(null);
431 }
432 }
433
434 if (restart) {
435 // interpolation failed, some intermediate rotation was too close to 2π
436 // we need to offset all rotations to avoid the singularity
437 offset = offset.addOffset(new FieldAngularCoordinates<>(new FieldRotation<>(FieldVector3D.getPlusI(field),
438 field.getZero().add(epsilon),
439 RotationConvention.VECTOR_OPERATOR),
440 FieldVector3D.getZero(field),
441 FieldVector3D.getZero(field)));
442 } else {
443 // interpolation succeeded with the current offset
444 final T[][] p = interpolator.derivatives(field.getZero(), 2);
445 final FieldAngularCoordinates<T> ac = createFromModifiedRodrigues(p);
446 return new TimeStampedFieldAngularCoordinates<>(offset.getDate(),
447 ac.getRotation(),
448 ac.getRotationRate(),
449 ac.getRotationAcceleration()).addOffset(offset);
450 }
451
452 }
453
454 // this should never happen
455 throw new OrekitInternalError(null);
456
457 }
458
459 }