1   /* Copyright 2002-2020 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.io.Serializable;
20  
21  import org.hipparchus.RealFieldElement;
22  import org.hipparchus.analysis.differentiation.DSFactory;
23  import org.hipparchus.analysis.differentiation.DerivativeStructure;
24  import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
25  import org.hipparchus.exception.LocalizedCoreFormats;
26  import org.hipparchus.exception.MathIllegalArgumentException;
27  import org.hipparchus.exception.MathRuntimeException;
28  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
29  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
30  import org.hipparchus.geometry.euclidean.threed.Rotation;
31  import org.hipparchus.geometry.euclidean.threed.RotationConvention;
32  import org.hipparchus.geometry.euclidean.threed.Vector3D;
33  import org.hipparchus.linear.DecompositionSolver;
34  import org.hipparchus.linear.MatrixUtils;
35  import org.hipparchus.linear.QRDecomposition;
36  import org.hipparchus.linear.RealMatrix;
37  import org.hipparchus.linear.RealVector;
38  import org.hipparchus.util.FastMath;
39  import org.hipparchus.util.MathArrays;
40  import org.orekit.errors.OrekitException;
41  import org.orekit.errors.OrekitMessages;
42  import org.orekit.time.TimeShiftable;
43  
44  /** Simple container for rotation/rotation rate/rotation acceleration triplets.
45   * <p>
46   * The state can be slightly shifted to close dates. This shift is based on
47   * an approximate solution of the fixed acceleration motion. It is <em>not</em>
48   * intended as a replacement for proper attitude propagation but should be
49   * sufficient for either small time shifts or coarse accuracy.
50   * </p>
51   * <p>
52   * This class is the angular counterpart to {@link PVCoordinates}.
53   * </p>
54   * <p>Instances of this class are guaranteed to be immutable.</p>
55   * @author Luc Maisonobe
56   */
57  public class AngularCoordinates implements TimeShiftable<AngularCoordinates>, Serializable {
58  
59      /** Fixed orientation parallel with reference frame
60       * (identity rotation, zero rotation rate and acceleration).
61       */
62      public static final AngularCoordinates IDENTITY =
63              new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
64  
65      /** Serializable UID. */
66      private static final long serialVersionUID = 20140414L;
67  
68      /** Rotation. */
69      private final Rotation rotation;
70  
71      /** Rotation rate. */
72      private final Vector3D rotationRate;
73  
74      /** Rotation acceleration. */
75      private final Vector3D rotationAcceleration;
76  
77      /** Simple constructor.
78       * <p> Sets the Coordinates to default : Identity, Ω = (0 0 0), dΩ/dt = (0 0 0).</p>
79       */
80      public AngularCoordinates() {
81          this(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
82      }
83  
84      /** Builds a rotation/rotation rate pair.
85       * @param rotation rotation
86       * @param rotationRate rotation rate Ω (rad/s)
87       */
88      public AngularCoordinates(final Rotation rotation, final Vector3D rotationRate) {
89          this(rotation, rotationRate, Vector3D.ZERO);
90      }
91  
92      /** Builds a rotation/rotation rate/rotation acceleration triplet.
93       * @param rotation rotation
94       * @param rotationRate rotation rate Ω (rad/s)
95       * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²)
96       */
97      public AngularCoordinates(final Rotation rotation,
98                                final Vector3D rotationRate, final Vector3D rotationAcceleration) {
99          this.rotation             = rotation;
100         this.rotationRate         = rotationRate;
101         this.rotationAcceleration = rotationAcceleration;
102     }
103 
104     /** Build the rotation that transforms a pair of pv coordinates into another one.
105 
106      * <p><em>WARNING</em>! This method requires much more stringent assumptions on
107      * its parameters than the similar {@link Rotation#Rotation(Vector3D, Vector3D,
108      * Vector3D, Vector3D) constructor} from the {@link Rotation Rotation} class.
109      * As far as the Rotation constructor is concerned, the {@code v₂} vector from
110      * the second pair can be slightly misaligned. The Rotation constructor will
111      * compensate for this misalignment and create a rotation that ensure {@code
112      * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
113      * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
114      * preserved, this constructor works <em>only</em> if the two pairs are fully
115      * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
116      * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
117      * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>
118      * @param u1 first vector of the origin pair
119      * @param u2 second vector of the origin pair
120      * @param v1 desired image of u1 by the rotation
121      * @param v2 desired image of u2 by the rotation
122      * @param tolerance relative tolerance factor used to check singularities
123      */
124     public AngularCoordinates(final PVCoordinates#PVCoordinates">PVCoordinates u1, final PVCoordinates u2,
125                               final PVCoordinates#PVCoordinates">PVCoordinates v1, final PVCoordinates v2,
126                               final double tolerance) {
127 
128         try {
129             // find the initial fixed rotation
130             rotation = new Rotation(u1.getPosition(), u2.getPosition(),
131                                     v1.getPosition(), v2.getPosition());
132 
133             // find rotation rate Ω such that
134             //  Ω ⨯ v₁ = r(dot(u₁)) - dot(v₁)
135             //  Ω ⨯ v₂ = r(dot(u₂)) - dot(v₂)
136             final Vector3D ru1Dot = rotation.applyTo(u1.getVelocity());
137             final Vector3D ru2Dot = rotation.applyTo(u2.getVelocity());
138             rotationRate = inverseCrossProducts(v1.getPosition(), ru1Dot.subtract(v1.getVelocity()),
139                                                 v2.getPosition(), ru2Dot.subtract(v2.getVelocity()),
140                                                 tolerance);
141 
142             // find rotation acceleration dot(Ω) such that
143             // dot(Ω) ⨯ v₁ = r(dotdot(u₁)) - 2 Ω ⨯ dot(v₁) - Ω ⨯  (Ω ⨯ v₁) - dotdot(v₁)
144             // dot(Ω) ⨯ v₂ = r(dotdot(u₂)) - 2 Ω ⨯ dot(v₂) - Ω ⨯  (Ω ⨯ v₂) - dotdot(v₂)
145             final Vector3D ru1DotDot = rotation.applyTo(u1.getAcceleration());
146             final Vector3D oDotv1    = Vector3D.crossProduct(rotationRate, v1.getVelocity());
147             final Vector3D oov1      = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v1.getPosition()));
148             final Vector3D c1        = new Vector3D(1, ru1DotDot, -2, oDotv1, -1, oov1, -1, v1.getAcceleration());
149             final Vector3D ru2DotDot = rotation.applyTo(u2.getAcceleration());
150             final Vector3D oDotv2    = Vector3D.crossProduct(rotationRate, v2.getVelocity());
151             final Vector3D oov2      = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v2.getPosition()));
152             final Vector3D c2        = new Vector3D(1, ru2DotDot, -2, oDotv2, -1, oov2, -1, v2.getAcceleration());
153             rotationAcceleration     = inverseCrossProducts(v1.getPosition(), c1, v2.getPosition(), c2, tolerance);
154 
155         } catch (MathRuntimeException mrte) {
156             throw new OrekitException(mrte);
157         }
158 
159     }
160 
161     /** Build one of the rotations that transform one pv coordinates into another one.
162 
163      * <p>Except for a possible scale factor, if the instance were
164      * applied to the vector u it will produce the vector v. There is an
165      * infinite number of such rotations, this constructor choose the
166      * one with the smallest associated angle (i.e. the one whose axis
167      * is orthogonal to the (u, v) plane). If u and v are collinear, an
168      * arbitrary rotation axis is chosen.</p>
169 
170      * @param u origin vector
171      * @param v desired image of u by the rotation
172      */
173     public AngularCoordinates(final PVCoordinatesl#PVCoordinates">PVCoordinates u, final PVCoordinates v) {
174         this(new FieldRotation<>(u.toDerivativeStructureVector(2),
175                                  v.toDerivativeStructureVector(2)));
176     }
177 
178     /** Builds a AngularCoordinates from  a {@link FieldRotation}&lt;{@link DerivativeStructure}&gt;.
179      * <p>
180      * The rotation components must have time as their only derivation parameter and
181      * have consistent derivation orders.
182      * </p>
183      * @param r rotation with time-derivatives embedded within the coordinates
184      */
185     public AngularCoordinates(final FieldRotation<DerivativeStructure> r) {
186 
187         final double q0       = r.getQ0().getReal();
188         final double q1       = r.getQ1().getReal();
189         final double q2       = r.getQ2().getReal();
190         final double q3       = r.getQ3().getReal();
191 
192         rotation     = new Rotation(q0, q1, q2, q3, false);
193         if (r.getQ0().getOrder() >= 1) {
194             final double q0Dot    = r.getQ0().getPartialDerivative(1);
195             final double q1Dot    = r.getQ1().getPartialDerivative(1);
196             final double q2Dot    = r.getQ2().getPartialDerivative(1);
197             final double q3Dot    = r.getQ3().getPartialDerivative(1);
198             rotationRate =
199                     new Vector3D(2 * MathArrays.linearCombination(-q1, q0Dot,  q0, q1Dot,  q3, q2Dot, -q2, q3Dot),
200                                  2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot,  q0, q2Dot,  q1, q3Dot),
201                                  2 * MathArrays.linearCombination(-q3, q0Dot,  q2, q1Dot, -q1, q2Dot,  q0, q3Dot));
202             if (r.getQ0().getOrder() >= 2) {
203                 final double q0DotDot = r.getQ0().getPartialDerivative(2);
204                 final double q1DotDot = r.getQ1().getPartialDerivative(2);
205                 final double q2DotDot = r.getQ2().getPartialDerivative(2);
206                 final double q3DotDot = r.getQ3().getPartialDerivative(2);
207                 rotationAcceleration =
208                         new Vector3D(2 * MathArrays.linearCombination(-q1, q0DotDot,  q0, q1DotDot,  q3, q2DotDot, -q2, q3DotDot),
209                                      2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot,  q0, q2DotDot,  q1, q3DotDot),
210                                      2 * MathArrays.linearCombination(-q3, q0DotDot,  q2, q1DotDot, -q1, q2DotDot,  q0, q3DotDot));
211             } else {
212                 rotationAcceleration = Vector3D.ZERO;
213             }
214         } else {
215             rotationRate         = Vector3D.ZERO;
216             rotationAcceleration = Vector3D.ZERO;
217         }
218 
219     }
220 
221     /** Find a vector from two known cross products.
222      * <p>
223      * We want to find Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
224      * </p>
225      * <p>
226      * The first equation (Ω ⨯ v₁ = c₁) will always be fulfilled exactly,
227      * and the second one will be fulfilled if possible.
228      * </p>
229      * @param v1 vector forming the first known cross product
230      * @param c1 know vector for cross product Ω ⨯ v₁
231      * @param v2 vector forming the second known cross product
232      * @param c2 know vector for cross product Ω ⨯ v₂
233      * @param tolerance relative tolerance factor used to check singularities
234      * @return vector Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
235      * @exception MathIllegalArgumentException if vectors are inconsistent and
236      * no solution can be found
237      */
238     private static Vector3D inverseCrossProducts(final Vector3D v1, final Vector3D c1,
239                                                  final Vector3D v2, final Vector3D c2,
240                                                  final double tolerance)
241         throws MathIllegalArgumentException {
242 
243         final double v12 = v1.getNormSq();
244         final double v1n = FastMath.sqrt(v12);
245         final double v22 = v2.getNormSq();
246         final double v2n = FastMath.sqrt(v22);
247         final double threshold = tolerance * FastMath.max(v1n, v2n);
248 
249         Vector3D omega;
250 
251         try {
252             // create the over-determined linear system representing the two cross products
253             final RealMatrix m = MatrixUtils.createRealMatrix(6, 3);
254             m.setEntry(0, 1,  v1.getZ());
255             m.setEntry(0, 2, -v1.getY());
256             m.setEntry(1, 0, -v1.getZ());
257             m.setEntry(1, 2,  v1.getX());
258             m.setEntry(2, 0,  v1.getY());
259             m.setEntry(2, 1, -v1.getX());
260             m.setEntry(3, 1,  v2.getZ());
261             m.setEntry(3, 2, -v2.getY());
262             m.setEntry(4, 0, -v2.getZ());
263             m.setEntry(4, 2,  v2.getX());
264             m.setEntry(5, 0,  v2.getY());
265             m.setEntry(5, 1, -v2.getX());
266 
267             final RealVector rhs = MatrixUtils.createRealVector(new double[] {
268                 c1.getX(), c1.getY(), c1.getZ(),
269                 c2.getX(), c2.getY(), c2.getZ()
270             });
271 
272             // find the best solution we can
273             final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver();
274             final RealVector v = solver.solve(rhs);
275             omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2));
276 
277         } catch (MathIllegalArgumentException miae) {
278             if (miae.getSpecifier() == LocalizedCoreFormats.SINGULAR_MATRIX) {
279 
280                 // handle some special cases for which we can compute a solution
281                 final double c12 = c1.getNormSq();
282                 final double c1n = FastMath.sqrt(c12);
283                 final double c22 = c2.getNormSq();
284                 final double c2n = FastMath.sqrt(c22);
285 
286                 if (c1n <= threshold && c2n <= threshold) {
287                     // simple special case, velocities are cancelled
288                     return Vector3D.ZERO;
289                 } else if (v1n <= threshold && c1n >= threshold) {
290                     // this is inconsistent, if v₁ is zero, c₁ must be 0 too
291                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c1n, 0, true);
292                 } else if (v2n <= threshold && c2n >= threshold) {
293                     // this is inconsistent, if v₂ is zero, c₂ must be 0 too
294                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c2n, 0, true);
295                 } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) {
296                     // simple special case, v₂ is redundant with v₁, we just ignore it
297                     // use the simplest Ω: orthogonal to both v₁ and c₁
298                     omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1));
299                 } else {
300                     throw miae;
301                 }
302             } else {
303                 throw miae;
304             }
305 
306         }
307 
308         // check results
309         final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1);
310         if (d1 > threshold) {
311             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d1, 0, true);
312         }
313 
314         final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2);
315         if (d2 > threshold) {
316             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d2, 0, true);
317         }
318 
319         return omega;
320 
321     }
322 
323     /** Transform the instance to a {@link FieldRotation}&lt;{@link DerivativeStructure}&gt;.
324      * <p>
325      * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
326      * to the user-specified order.
327      * </p>
328      * @param order derivation order for the vector components
329      * @return rotation with time-derivatives embedded within the coordinates
330      */
331     public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order) {
332 
333         // quaternion components
334         final double q0 = rotation.getQ0();
335         final double q1 = rotation.getQ1();
336         final double q2 = rotation.getQ2();
337         final double q3 = rotation.getQ3();
338 
339         // first time-derivatives of the quaternion
340         final double oX    = rotationRate.getX();
341         final double oY    = rotationRate.getY();
342         final double oZ    = rotationRate.getZ();
343         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
344         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
345         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
346         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);
347 
348         // second time-derivatives of the quaternion
349         final double oXDot = rotationAcceleration.getX();
350         final double oYDot = rotationAcceleration.getY();
351         final double oZDot = rotationAcceleration.getZ();
352         final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
353             q1, q2,  q3, q1Dot, q2Dot,  q3Dot
354         }, new double[] {
355             oXDot, oYDot, oZDot, oX, oY, oZ
356         });
357         final double q1DotDot =  0.5 * MathArrays.linearCombination(new double[] {
358             q0, q2, -q3, q0Dot, q2Dot, -q3Dot
359         }, new double[] {
360             oXDot, oZDot, oYDot, oX, oZ, oY
361         });
362         final double q2DotDot =  0.5 * MathArrays.linearCombination(new double[] {
363             q0, q3, -q1, q0Dot, q3Dot, -q1Dot
364         }, new double[] {
365             oYDot, oXDot, oZDot, oY, oX, oZ
366         });
367         final double q3DotDot =  0.5 * MathArrays.linearCombination(new double[] {
368             q0, q1, -q2, q0Dot, q1Dot, -q2Dot
369         }, new double[] {
370             oZDot, oYDot, oXDot, oZ, oY, oX
371         });
372 
373         final DSFactory factory;
374         final DerivativeStructure q0DS;
375         final DerivativeStructure q1DS;
376         final DerivativeStructure q2DS;
377         final DerivativeStructure q3DS;
378         switch(order) {
379             case 0 :
380                 factory = new DSFactory(1, order);
381                 q0DS = factory.build(q0);
382                 q1DS = factory.build(q1);
383                 q2DS = factory.build(q2);
384                 q3DS = factory.build(q3);
385                 break;
386             case 1 :
387                 factory = new DSFactory(1, order);
388                 q0DS = factory.build(q0, q0Dot);
389                 q1DS = factory.build(q1, q1Dot);
390                 q2DS = factory.build(q2, q2Dot);
391                 q3DS = factory.build(q3, q3Dot);
392                 break;
393             case 2 :
394                 factory = new DSFactory(1, order);
395                 q0DS = factory.build(q0, q0Dot, q0DotDot);
396                 q1DS = factory.build(q1, q1Dot, q1DotDot);
397                 q2DS = factory.build(q2, q2Dot, q2DotDot);
398                 q3DS = factory.build(q3, q3Dot, q3DotDot);
399                 break;
400             default :
401                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
402         }
403 
404         return new FieldRotation<>(q0DS, q1DS, q2DS, q3DS, false);
405 
406     }
407 
408     /** Transform the instance to a {@link FieldRotation}&lt;{@link UnivariateDerivative1}&gt;.
409      * <p>
410      * The {@link UnivariateDerivative1} coordinates correspond to time-derivatives up
411      * to the order 1.
412      * </p>
413      * @return rotation with time-derivatives embedded within the coordinates
414      */
415     public FieldRotation<UnivariateDerivative1> toUnivariateDerivative1Rotation() {
416 
417         // quaternion components
418         final double q0 = rotation.getQ0();
419         final double q1 = rotation.getQ1();
420         final double q2 = rotation.getQ2();
421         final double q3 = rotation.getQ3();
422 
423         // first time-derivatives of the quaternion
424         final double oX    = rotationRate.getX();
425         final double oY    = rotationRate.getY();
426         final double oZ    = rotationRate.getZ();
427         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
428         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
429         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
430         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);
431 
432         final UnivariateDerivative1 q0UD = new UnivariateDerivative1(q0, q0Dot);
433         final UnivariateDerivative1 q1UD = new UnivariateDerivative1(q1, q1Dot);
434         final UnivariateDerivative1 q2UD = new UnivariateDerivative1(q2, q2Dot);
435         final UnivariateDerivative1 q3UD = new UnivariateDerivative1(q3, q3Dot);
436 
437         return new FieldRotation<>(q0UD, q1UD, q2UD, q3UD, false);
438 
439     }
440 
441     /** Estimate rotation rate between two orientations.
442      * <p>Estimation is based on a simple fixed rate rotation
443      * during the time interval between the two orientations.</p>
444      * @param start start orientation
445      * @param end end orientation
446      * @param dt time elapsed between the dates of the two orientations
447      * @return rotation rate allowing to go from start to end orientations
448      */
449     public static Vector3D estimateRate(final Rotation start, final Rotation end, final double dt) {
450         final Rotation evolution = start.compose(end.revert(), RotationConvention.VECTOR_OPERATOR);
451         return new Vector3D(evolution.getAngle() / dt, evolution.getAxis(RotationConvention.VECTOR_OPERATOR));
452     }
453 
454     /** Revert a rotation/rotation rate/ rotation acceleration triplet.
455      * Build a triplet which reverse the effect of another triplet.
456      * @return a new triplet whose effect is the reverse of the effect
457      * of the instance
458      */
459     public AngularCoordinates revert() {
460         return new AngularCoordinates(rotation.revert(),
461                                       rotation.applyInverseTo(rotationRate).negate(),
462                                       rotation.applyInverseTo(rotationAcceleration).negate());
463     }
464 
465     /** Get a time-shifted state.
466      * <p>
467      * The state can be slightly shifted to close dates. This shift is based on
468      * an approximate solution of the fixed acceleration motion. It is <em>not</em>
469      * intended as a replacement for proper attitude propagation but should be
470      * sufficient for either small time shifts or coarse accuracy.
471      * </p>
472      * @param dt time shift in seconds
473      * @return a new state, shifted with respect to the instance (which is immutable)
474      */
475     public AngularCoordinates shiftedBy(final double dt) {
476 
477         // the shiftedBy method is based on a local approximation.
478         // It considers separately the contribution of the constant
479         // rotation, the linear contribution or the rate and the
480         // quadratic contribution of the acceleration. The rate
481         // and acceleration contributions are small rotations as long
482         // as the time shift is small, which is the crux of the algorithm.
483         // Small rotations are almost commutative, so we append these small
484         // contributions one after the other, as if they really occurred
485         // successively, despite this is not what really happens.
486 
487         // compute the linear contribution first, ignoring acceleration
488         // BEWARE: there is really a minus sign here, because if
489         // the target frame rotates in one direction, the vectors in the origin
490         // frame seem to rotate in the opposite direction
491         final double rate = rotationRate.getNorm();
492         final Rotation rateContribution = (rate == 0.0) ?
493                                           Rotation.IDENTITY :
494                                           new Rotation(rotationRate, rate * dt, RotationConvention.FRAME_TRANSFORM);
495 
496         // append rotation and rate contribution
497         final AngularCoordinates linearPart =
498                 new AngularCoordinates(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR), rotationRate);
499 
500         final double acc  = rotationAcceleration.getNorm();
501         if (acc == 0.0) {
502             // no acceleration, the linear part is sufficient
503             return linearPart;
504         }
505 
506         // compute the quadratic contribution, ignoring initial rotation and rotation rate
507         // BEWARE: there is really a minus sign here, because if
508         // the target frame rotates in one direction, the vectors in the origin
509         // frame seem to rotate in the opposite direction
510         final AngularCoordinates quadraticContribution =
511                 new AngularCoordinates(new Rotation(rotationAcceleration,
512                                                     0.5 * acc * dt * dt,
513                                                     RotationConvention.FRAME_TRANSFORM),
514                                        new Vector3D(dt, rotationAcceleration),
515                                        rotationAcceleration);
516 
517         // the quadratic contribution is a small rotation:
518         // its initial angle and angular rate are both zero.
519         // small rotations are almost commutative, so we append the small
520         // quadratic part after the linear part as a simple offset
521         return quadraticContribution.addOffset(linearPart);
522 
523     }
524 
525     /** Get the rotation.
526      * @return the rotation.
527      */
528     public Rotation getRotation() {
529         return rotation;
530     }
531 
532     /** Get the rotation rate.
533      * @return the rotation rate vector Ω (rad/s).
534      */
535     public Vector3D getRotationRate() {
536         return rotationRate;
537     }
538 
539     /** Get the rotation acceleration.
540      * @return the rotation acceleration vector dΩ/dt (rad²/s²).
541      */
542     public Vector3D getRotationAcceleration() {
543         return rotationAcceleration;
544     }
545 
546     /** Add an offset from the instance.
547      * <p>
548      * We consider here that the offset rotation is applied first and the
549      * instance is applied afterward. Note that angular coordinates do <em>not</em>
550      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
551      * b.addOffset(a)} lead to <em>different</em> results in most cases.
552      * </p>
553      * <p>
554      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
555      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
556      * so that round trip applications are possible. This means that both {@code
557      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
558      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
559      * </p>
560      * @param offset offset to subtract
561      * @return new instance, with offset subtracted
562      * @see #subtractOffset(AngularCoordinates)
563      */
564     public AngularCoordinatesarCoordinates">AngularCoordinates addOffset(final AngularCoordinates offset) {
565         final Vector3D rOmega    = rotation.applyTo(offset.rotationRate);
566         final Vector3D rOmegaDot = rotation.applyTo(offset.rotationAcceleration);
567         return new AngularCoordinates(rotation.compose(offset.rotation, RotationConvention.VECTOR_OPERATOR),
568                                       rotationRate.add(rOmega),
569                                       new Vector3D( 1.0, rotationAcceleration,
570                                                     1.0, rOmegaDot,
571                                                    -1.0, Vector3D.crossProduct(rotationRate, rOmega)));
572     }
573 
574     /** Subtract an offset from the instance.
575      * <p>
576      * We consider here that the offset rotation is applied first and the
577      * instance is applied afterward. Note that angular coordinates do <em>not</em>
578      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
579      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
580      * </p>
581      * <p>
582      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
583      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
584      * so that round trip applications are possible. This means that both {@code
585      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
586      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
587      * </p>
588      * @param offset offset to subtract
589      * @return new instance, with offset subtracted
590      * @see #addOffset(AngularCoordinates)
591      */
592     public AngularCoordinatesrdinates">AngularCoordinates subtractOffset(final AngularCoordinates offset) {
593         return addOffset(offset.revert());
594     }
595 
596     /** Apply the rotation to a pv coordinates.
597      * @param pv vector to apply the rotation to
598      * @return a new pv coordinates which is the image of u by the rotation
599      */
600     public PVCoordinatesoordinates">PVCoordinates applyTo(final PVCoordinates pv) {
601 
602         final Vector3D transformedP = rotation.applyTo(pv.getPosition());
603         final Vector3D crossP       = Vector3D.crossProduct(rotationRate, transformedP);
604         final Vector3D transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
605         final Vector3D crossV       = Vector3D.crossProduct(rotationRate, transformedV);
606         final Vector3D crossCrossP  = Vector3D.crossProduct(rotationRate, crossP);
607         final Vector3D crossDotP    = Vector3D.crossProduct(rotationAcceleration, transformedP);
608         final Vector3D transformedA = new Vector3D( 1, rotation.applyTo(pv.getAcceleration()),
609                                                    -2, crossV,
610                                                    -1, crossCrossP,
611                                                    -1, crossDotP);
612 
613         return new PVCoordinates(transformedP, transformedV, transformedA);
614 
615     }
616 
617     /** Apply the rotation to a pv coordinates.
618      * @param pv vector to apply the rotation to
619      * @return a new pv coordinates which is the image of u by the rotation
620      */
621     public TimeStampedPVCoordinateseStampedPVCoordinates">TimeStampedPVCoordinates applyTo(final TimeStampedPVCoordinates pv) {
622 
623         final Vector3D transformedP = getRotation().applyTo(pv.getPosition());
624         final Vector3D crossP       = Vector3D.crossProduct(getRotationRate(), transformedP);
625         final Vector3D transformedV = getRotation().applyTo(pv.getVelocity()).subtract(crossP);
626         final Vector3D crossV       = Vector3D.crossProduct(getRotationRate(), transformedV);
627         final Vector3D crossCrossP  = Vector3D.crossProduct(getRotationRate(), crossP);
628         final Vector3D crossDotP    = Vector3D.crossProduct(getRotationAcceleration(), transformedP);
629         final Vector3D transformedA = new Vector3D( 1, getRotation().applyTo(pv.getAcceleration()),
630                                                    -2, crossV,
631                                                    -1, crossCrossP,
632                                                    -1, crossDotP);
633 
634         return new TimeStampedPVCoordinates(pv.getDate(), transformedP, transformedV, transformedA);
635 
636     }
637 
638     /** Apply the rotation to a pv coordinates.
639      * @param pv vector to apply the rotation to
640      * @param <T> type of the field elements
641      * @return a new pv coordinates which is the image of u by the rotation
642      * @since 9.0
643      */
644     public <T extends RealFieldElement<T>> FieldPVCoordinates<T> applyTo(final FieldPVCoordinates<T> pv) {
645 
646         final FieldVector3D<T> transformedP = FieldRotation.applyTo(rotation, pv.getPosition());
647         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
648         final FieldVector3D<T> transformedV = FieldRotation.applyTo(rotation, pv.getVelocity()).subtract(crossP);
649         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
650         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
651         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
652         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, FieldRotation.applyTo(rotation, pv.getAcceleration()),
653                                                                   -2, crossV,
654                                                                   -1, crossCrossP,
655                                                                   -1, crossDotP);
656 
657         return new FieldPVCoordinates<>(transformedP, transformedV, transformedA);
658 
659     }
660 
661     /** Apply the rotation to a pv coordinates.
662      * @param pv vector to apply the rotation to
663      * @param <T> type of the field elements
664      * @return a new pv coordinates which is the image of u by the rotation
665      * @since 9.0
666      */
667     public <T extends RealFieldElement<T>> TimeStampedFieldPVCoordinates<T> applyTo(final TimeStampedFieldPVCoordinates<T> pv) {
668 
669         final FieldVector3D<T> transformedP = FieldRotation.applyTo(rotation, pv.getPosition());
670         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
671         final FieldVector3D<T> transformedV = FieldRotation.applyTo(rotation, pv.getVelocity()).subtract(crossP);
672         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
673         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
674         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
675         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, FieldRotation.applyTo(rotation, pv.getAcceleration()),
676                                                                   -2, crossV,
677                                                                   -1, crossCrossP,
678                                                                   -1, crossDotP);
679 
680         return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV, transformedA);
681 
682     }
683 
684     /** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives.
685      * <p>
686      * The modified Rodrigues vector is tan(θ/4) u where θ and u are the
687      * rotation angle and axis respectively.
688      * </p>
689      * @param sign multiplicative sign for quaternion components
690      * @return modified Rodrigues vector and derivatives (vector on row 0, first derivative
691      * on row 1, second derivative on row 2)
692      * @see #createFromModifiedRodrigues(double[][])
693      */
694     public double[][] getModifiedRodrigues(final double sign) {
695 
696         final double q0    = sign * getRotation().getQ0();
697         final double q1    = sign * getRotation().getQ1();
698         final double q2    = sign * getRotation().getQ2();
699         final double q3    = sign * getRotation().getQ3();
700         final double oX    = getRotationRate().getX();
701         final double oY    = getRotationRate().getY();
702         final double oZ    = getRotationRate().getZ();
703         final double oXDot = getRotationAcceleration().getX();
704         final double oYDot = getRotationAcceleration().getY();
705         final double oZDot = getRotationAcceleration().getZ();
706 
707         // first time-derivatives of the quaternion
708         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
709         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
710         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
711         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);
712 
713         // second time-derivatives of the quaternion
714         final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
715             q1, q2,  q3, q1Dot, q2Dot,  q3Dot
716         }, new double[] {
717             oXDot, oYDot, oZDot, oX, oY, oZ
718         });
719         final double q1DotDot =  0.5 * MathArrays.linearCombination(new double[] {
720             q0, q2, -q3, q0Dot, q2Dot, -q3Dot
721         }, new double[] {
722             oXDot, oZDot, oYDot, oX, oZ, oY
723         });
724         final double q2DotDot =  0.5 * MathArrays.linearCombination(new double[] {
725             q0, q3, -q1, q0Dot, q3Dot, -q1Dot
726         }, new double[] {
727             oYDot, oXDot, oZDot, oY, oX, oZ
728         });
729         final double q3DotDot =  0.5 * MathArrays.linearCombination(new double[] {
730             q0, q1, -q2, q0Dot, q1Dot, -q2Dot
731         }, new double[] {
732             oZDot, oYDot, oXDot, oZ, oY, oX
733         });
734 
735         // the modified Rodrigues is tan(θ/4) u where θ and u are the rotation angle and axis respectively
736         // this can be rewritten using quaternion components:
737         //      r (q₁ / (1+q₀), q₂ / (1+q₀), q₃ / (1+q₀))
738         // applying the derivation chain rule to previous expression gives rDot and rDotDot
739         final double inv          = 1.0 / (1.0 + q0);
740         final double mTwoInvQ0Dot = -2 * inv * q0Dot;
741 
742         final double r1       = inv * q1;
743         final double r2       = inv * q2;
744         final double r3       = inv * q3;
745 
746         final double mInvR1   = -inv * r1;
747         final double mInvR2   = -inv * r2;
748         final double mInvR3   = -inv * r3;
749 
750         final double r1Dot    = MathArrays.linearCombination(inv, q1Dot, mInvR1, q0Dot);
751         final double r2Dot    = MathArrays.linearCombination(inv, q2Dot, mInvR2, q0Dot);
752         final double r3Dot    = MathArrays.linearCombination(inv, q3Dot, mInvR3, q0Dot);
753 
754         final double r1DotDot = MathArrays.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot);
755         final double r2DotDot = MathArrays.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot);
756         final double r3DotDot = MathArrays.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot);
757 
758         return new double[][] {
759             {
760                 r1,       r2,       r3
761             }, {
762                 r1Dot,    r2Dot,    r3Dot
763             }, {
764                 r1DotDot, r2DotDot, r3DotDot
765             }
766         };
767 
768     }
769 
770     /** Convert a modified Rodrigues vector and derivatives to angular coordinates.
771      * @param r modified Rodrigues vector (with first and second times derivatives)
772      * @return angular coordinates
773      * @see #getModifiedRodrigues(double)
774      */
775     public static AngularCoordinates createFromModifiedRodrigues(final double[][] r) {
776 
777         // rotation
778         final double rSquared = r[0][0] * r[0][0] + r[0][1] * r[0][1] + r[0][2] * r[0][2];
779         final double oPQ0     = 2 / (1 + rSquared);
780         final double q0       = oPQ0 - 1;
781         final double q1       = oPQ0 * r[0][0];
782         final double q2       = oPQ0 * r[0][1];
783         final double q3       = oPQ0 * r[0][2];
784 
785         // rotation rate
786         final double oPQ02    = oPQ0 * oPQ0;
787         final double q0Dot    = -oPQ02 * MathArrays.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1],  r[0][2], r[1][2]);
788         final double q1Dot    = oPQ0 * r[1][0] + r[0][0] * q0Dot;
789         final double q2Dot    = oPQ0 * r[1][1] + r[0][1] * q0Dot;
790         final double q3Dot    = oPQ0 * r[1][2] + r[0][2] * q0Dot;
791         final double oX       = 2 * MathArrays.linearCombination(-q1, q0Dot,  q0, q1Dot,  q3, q2Dot, -q2, q3Dot);
792         final double oY       = 2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot,  q0, q2Dot,  q1, q3Dot);
793         final double oZ       = 2 * MathArrays.linearCombination(-q3, q0Dot,  q2, q1Dot, -q1, q2Dot,  q0, q3Dot);
794 
795         // rotation acceleration
796         final double q0DotDot = (1 - q0) / oPQ0 * q0Dot * q0Dot -
797                                 oPQ02 * MathArrays.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]) -
798                                 (q1Dot * q1Dot + q2Dot * q2Dot + q3Dot * q3Dot);
799         final double q1DotDot = MathArrays.linearCombination(oPQ0, r[2][0], 2 * r[1][0], q0Dot, r[0][0], q0DotDot);
800         final double q2DotDot = MathArrays.linearCombination(oPQ0, r[2][1], 2 * r[1][1], q0Dot, r[0][1], q0DotDot);
801         final double q3DotDot = MathArrays.linearCombination(oPQ0, r[2][2], 2 * r[1][2], q0Dot, r[0][2], q0DotDot);
802         final double oXDot    = 2 * MathArrays.linearCombination(-q1, q0DotDot,  q0, q1DotDot,  q3, q2DotDot, -q2, q3DotDot);
803         final double oYDot    = 2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot,  q0, q2DotDot,  q1, q3DotDot);
804         final double oZDot    = 2 * MathArrays.linearCombination(-q3, q0DotDot,  q2, q1DotDot, -q1, q2DotDot,  q0, q3DotDot);
805 
806         return new AngularCoordinates(new Rotation(q0, q1, q2, q3, false),
807                                       new Vector3D(oX, oY, oZ),
808                                       new Vector3D(oXDot, oYDot, oZDot));
809 
810     }
811 
812 }