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}&lt;{@link DerivativeStructure}&gt;.
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 }