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