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