1 /* Copyright 2002-2013 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.io.Serializable;
20 import java.util.Collection;
21
22 import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
23 import org.apache.commons.math3.analysis.interpolation.HermiteInterpolator;
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.apache.commons.math3.util.Pair;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.time.TimeShiftable;
32
33 /** Simple container for rotation/rotation rate pairs.
34 * <p>
35 * The state can be slightly shifted to close dates. This shift is based on
36 * a simple linear model. It is <em>not</em> intended as a replacement for
37 * proper attitude propagation but should be sufficient for either small
38 * time shifts or coarse accuracy.
39 * </p>
40 * <p>
41 * This class is the angular counterpart to {@link PVCoordinates}.
42 * </p>
43 * <p>Instances of this class are guaranteed to be immutable.</p>
44 * @author Luc Maisonobe
45 */
46 public class AngularCoordinates implements TimeShiftable<AngularCoordinates>, Serializable {
47
48 /** Fixed orientation parallel with reference frame (identity rotation and zero rotation rate). */
49 public static final AngularCoordinates IDENTITY =
50 new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO);
51
52 /** Serializable UID. */
53 private static final long serialVersionUID = 3750363056414336775L;
54
55 /** Rotation. */
56 private final Rotation rotation;
57
58 /** Rotation rate. */
59 private final Vector3D rotationRate;
60
61 /** Simple constructor.
62 * <p> Sets the Coordinates to default : Identity (0 0 0).</p>
63 */
64 public AngularCoordinates() {
65 rotation = Rotation.IDENTITY;
66 rotationRate = Vector3D.ZERO;
67 }
68
69 /** Builds a rotation/rotation rate pair.
70 * @param rotation rotation
71 * @param rotationRate rotation rate (rad/s)
72 */
73 public AngularCoordinates(final Rotation rotation, final Vector3D rotationRate) {
74 this.rotation = rotation;
75 this.rotationRate = rotationRate;
76 }
77
78 /** Estimate rotation rate between two orientations.
79 * <p>Estimation is based on a simple fixed rate rotation
80 * during the time interval between the two orientations.</p>
81 * @param start start orientation
82 * @param end end orientation
83 * @param dt time elapsed between the dates of the two orientations
84 * @return rotation rate allowing to go from start to end orientations
85 */
86 public static Vector3D estimateRate(final Rotation start, final Rotation end, final double dt) {
87 final Rotation evolution = start.applyTo(end.revert());
88 return new Vector3D(evolution.getAngle() / dt, evolution.getAxis());
89 }
90
91 /** Revert a rotation/rotation rate pair.
92 * Build a pair which reverse the effect of another pair.
93 * @return a new pair whose effect is the reverse of the effect
94 * of the instance
95 */
96 public AngularCoordinates revert() {
97 return new AngularCoordinates(rotation.revert(), rotation.applyInverseTo(rotationRate.negate()));
98 }
99
100 /** Get a time-shifted state.
101 * <p>
102 * The state can be slightly shifted to close dates. This shift is based on
103 * a simple linear model. It is <em>not</em> intended as a replacement for
104 * proper attitude propagation but should be sufficient for either small
105 * time shifts or coarse accuracy.
106 * </p>
107 * @param dt time shift in seconds
108 * @return a new state, shifted with respect to the instance (which is immutable)
109 */
110 public AngularCoordinates shiftedBy(final double dt) {
111 final double rate = rotationRate.getNorm();
112 if (rate == 0.0) {
113 // special case for fixed rotations
114 return this;
115 }
116
117 // BEWARE: there is really a minus sign here, because if
118 // the target frame rotates in one direction, the vectors in the origin
119 // frame seem to rotate in the opposite direction
120 final Rotation evolution = new Rotation(rotationRate, -rate * dt);
121
122 return new AngularCoordinates(evolution.applyTo(rotation), rotationRate);
123
124 }
125
126 /** Get the rotation.
127 * @return the rotation.
128 */
129 public Rotation getRotation() {
130 return rotation;
131 }
132
133 /** Get the rotation rate.
134 * @return the rotation rate vector (rad/s).
135 */
136 public Vector3D getRotationRate() {
137 return rotationRate;
138 }
139
140 /** Add an offset from the instance.
141 * <p>
142 * We consider here that the offset rotation is applied first and the
143 * instance is applied afterward. Note that angular coordinates do <em>not</em>
144 * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
145 * b.addOffset(a)} lead to <em>different</em> results in most cases.
146 * </p>
147 * <p>
148 * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
149 * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
150 * so that round trip applications are possible. This means that both {@code
151 * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
152 * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
153 * </p>
154 * @param offset offset to subtract
155 * @return new instance, with offset subtracted
156 * @see #subtractOffset(AngularCoordinates)
157 */
158 public AngularCoordinates addOffset(final AngularCoordinates offset) {
159 return new AngularCoordinates(rotation.applyTo(offset.rotation),
160 rotationRate.add(rotation.applyTo(offset.rotationRate)));
161 }
162
163 /** Subtract 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.subtractOffset(b)} and {@code
168 * b.subtractOffset(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 #addOffset(AngularCoordinates)
180 */
181 public AngularCoordinates subtractOffset(final AngularCoordinates offset) {
182 return addOffset(offset.revert());
183 }
184
185 /** Interpolate angular coordinates.
186 * <p>
187 * The interpolated instance is created by polynomial Hermite interpolation
188 * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
189 * </p>
190 * <p>
191 * This method is based on Sergei Tanygin's paper <a
192 * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
193 * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
194 * vector as described in Malcolm D. Shuster's paper <a
195 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
196 * Survey of Attitude Representations</a>. This change avoids the singularity at π.
197 * There is still a singularity at 2π, which is handled by slightly offsetting all rotations
198 * when this singularity is detected.
199 * </p>
200 * <p>
201 * Note that even if first time derivatives (rotation rates)
202 * from sample can be ignored, the interpolated instance always includes
203 * interpolated derivatives. This feature can be used explicitly to
204 * compute these derivatives when it would be too complex to compute them
205 * from an analytical formula: just compute a few sample points from the
206 * explicit formula and set the derivatives to zero in these sample points,
207 * then use interpolation to add derivatives consistent with the rotations.
208 * </p>
209 * @param date interpolation date
210 * @param useRotationRates if true, use sample points rotation rates,
211 * otherwise ignore them and use only rotations
212 * @param sample sample points on which interpolation should be done
213 * @return a new position-velocity, interpolated at specified date
214 */
215 public static AngularCoordinates interpolate(final AbsoluteDate date, final boolean useRotationRates,
216 final Collection<Pair<AbsoluteDate, AngularCoordinates>> sample) {
217
218 // set up safety elements for 2PI singularity avoidance
219 final double epsilon = 2 * FastMath.PI / sample.size();
220 final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));
221
222 // set up a linear offset model canceling mean rotation rate
223 final Vector3D meanRate;
224 if (useRotationRates) {
225 Vector3D sum = Vector3D.ZERO;
226 for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
227 sum = sum.add(datedAC.getValue().getRotationRate());
228 }
229 meanRate = new Vector3D(1.0 / sample.size(), sum);
230 } else {
231 Vector3D sum = Vector3D.ZERO;
232 Pair<AbsoluteDate, AngularCoordinates> previous = null;
233 for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
234 if (previous != null) {
235 sum = sum.add(estimateRate(previous.getValue().getRotation(),
236 datedAC.getValue().getRotation(),
237 datedAC.getKey().durationFrom(previous.getKey().getDate())));
238 }
239 previous = datedAC;
240 }
241 meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
242 }
243 AngularCoordinates offset = new AngularCoordinates(Rotation.IDENTITY, meanRate);
244
245 boolean restart = true;
246 for (int i = 0; restart && i < sample.size() + 2; ++i) {
247
248 // offset adaptation parameters
249 restart = false;
250
251 // set up an interpolator taking derivatives into account
252 final HermiteInterpolator interpolator = new HermiteInterpolator();
253
254 // add sample points
255 if (useRotationRates) {
256 // populate sample with rotation and rotation rate data
257 for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
258 final double[] rodrigues = getModifiedRodrigues(datedAC.getKey(), datedAC.getValue(),
259 date, offset, threshold);
260 if (rodrigues == null) {
261 // the sample point is close to a modified Rodrigues vector singularity
262 // we need to change the linear offset model to avoid this
263 restart = true;
264 break;
265 }
266 interpolator.addSamplePoint(datedAC.getKey().getDate().durationFrom(date),
267 new double[] {
268 rodrigues[0],
269 rodrigues[1],
270 rodrigues[2]
271 },
272 new double[] {
273 rodrigues[3],
274 rodrigues[4],
275 rodrigues[5]
276 });
277 }
278 } else {
279 // populate sample with rotation data only, ignoring rotation rate
280 for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
281 final double[] rodrigues = getModifiedRodrigues(datedAC.getKey(), datedAC.getValue(),
282 date, offset, threshold);
283 if (rodrigues == null) {
284 // the sample point is close to a modified Rodrigues vector singularity
285 // we need to change the linear offset model to avoid this
286 restart = true;
287 break;
288 }
289 interpolator.addSamplePoint(datedAC.getKey().getDate().durationFrom(date),
290 new double[] {
291 rodrigues[0],
292 rodrigues[1],
293 rodrigues[2]
294 });
295 }
296 }
297
298 if (restart) {
299 // interpolation failed, some intermediate rotation was too close to 2PI
300 // we need to offset all rotations to avoid the singularity
301 offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I, epsilon),
302 Vector3D.ZERO));
303 } else {
304 // interpolation succeeded with the current offset
305 final DerivativeStructure zero = new DerivativeStructure(1, 1, 0, 0.0);
306 final DerivativeStructure[] p = interpolator.value(zero);
307 return createFromModifiedRodrigues(new double[] {
308 p[0].getValue(), p[1].getValue(), p[2].getValue(),
309 p[0].getPartialDerivative(1), p[1].getPartialDerivative(1), p[2].getPartialDerivative(1)
310 }, offset);
311 }
312
313 }
314
315 // this should never happen
316 throw OrekitException.createInternalError(null);
317
318 }
319
320 /** Convert rotation and rate to modified Rodrigues vector and derivative.
321 * <p>
322 * The modified Rodrigues vector is tan(θ/4) u where θ and u are the
323 * rotation angle and axis respectively.
324 * </p>
325 * @param date date of the angular coordinates
326 * @param ac coordinates to convert
327 * @param offsetDate date of the linear offset model to remove
328 * @param offset linear offset model to remove
329 * @param threshold threshold for rotations too close to 2π
330 * @return modified Rodrigues vector and derivative, or null if rotation is too close to 2π
331 */
332 private static double[] getModifiedRodrigues(final AbsoluteDate date, final AngularCoordinates ac,
333 final AbsoluteDate offsetDate, final AngularCoordinates offset,
334 final double threshold) {
335
336 // remove linear offset from the current coordinates
337 final double dt = date.durationFrom(offsetDate);
338 final AngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));
339
340 // check modified Rodrigues vector singularity
341 double q0 = fixed.getRotation().getQ0();
342 double q1 = fixed.getRotation().getQ1();
343 double q2 = fixed.getRotation().getQ2();
344 double q3 = fixed.getRotation().getQ3();
345 if (q0 < threshold && FastMath.abs(dt) * fixed.getRotationRate().getNorm() > 1.0e-3) {
346 // this is an intermediate point that happens to be 2PI away from reference
347 // we need to change the linear offset model to avoid this point
348 return null;
349 }
350
351 // make sure all interpolated points will be on the same branch
352 if (q0 < 0) {
353 q0 = -q0;
354 q1 = -q1;
355 q2 = -q2;
356 q3 = -q3;
357 }
358
359 final double x = fixed.getRotationRate().getX();
360 final double y = fixed.getRotationRate().getY();
361 final double z = fixed.getRotationRate().getZ();
362
363 // derivatives of the quaternion
364 final double q0Dot = -0.5 * MathArrays.linearCombination(q1, x, q2, y, q3, z);
365 final double q1Dot = 0.5 * MathArrays.linearCombination(q0, x, q2, z, -q3, y);
366 final double q2Dot = 0.5 * MathArrays.linearCombination(q0, y, q3, x, -q1, z);
367 final double q3Dot = 0.5 * MathArrays.linearCombination(q0, z, q1, y, -q2, x);
368
369 final double inv = 1.0 / (1.0 + q0);
370 return new double[] {
371 inv * q1,
372 inv * q2,
373 inv * q3,
374 inv * (q1Dot - inv * q1 * q0Dot),
375 inv * (q2Dot - inv * q2 * q0Dot),
376 inv * (q3Dot - inv * q3 * q0Dot)
377 };
378
379 }
380
381 /** Convert a modified Rodrigues vector and derivative to angular coordinates.
382 * @param r modified Rodrigues vector (with first derivatives)
383 * @param offset linear offset model to add (its date must be consistent with the modified Rodrigues vector)
384 * @return angular coordinates
385 */
386 private static AngularCoordinates createFromModifiedRodrigues(final double[] r,
387 final AngularCoordinates offset) {
388
389 // rotation
390 final double rSquared = r[0] * r[0] + r[1] * r[1] + r[2] * r[2];
391 final double inv = 1.0 / (1 + rSquared);
392 final double ratio = inv * (1 - rSquared);
393 final Rotation rotation =
394 new Rotation(ratio, 2 * inv * r[0], 2 * inv * r[1], 2 * inv * r[2], false);
395
396 // rotation rate
397 final Vector3D p = new Vector3D(r[0], r[1], r[2]);
398 final Vector3D pDot = new Vector3D(r[3], r[4], r[5]);
399 final Vector3D rate = new Vector3D( 4 * ratio * inv, pDot,
400 -8 * inv * inv, p.crossProduct(pDot),
401 8 * inv * inv * p.dotProduct(pDot), p);
402
403 return new AngularCoordinates(rotation, rate).addOffset(offset);
404
405 }
406
407 }