AngularCoordinates.java

  1. /* Copyright 2002-2022 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. import java.io.Serializable;

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.analysis.differentiation.DSFactory;
  21. import org.hipparchus.analysis.differentiation.Derivative;
  22. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  23. import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
  24. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  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. /** Simple container for rotation/rotation rate/rotation acceleration triplets.
  44.  * <p>
  45.  * The state can be slightly shifted to close dates. This shift is based on
  46.  * an approximate solution of the fixed acceleration motion. It is <em>not</em>
  47.  * intended as a replacement for proper attitude propagation but should be
  48.  * sufficient for either small time shifts or coarse accuracy.
  49.  * </p>
  50.  * <p>
  51.  * This class is the angular counterpart to {@link PVCoordinates}.
  52.  * </p>
  53.  * <p>Instances of this class are guaranteed to be immutable.</p>
  54.  * @author Luc Maisonobe
  55.  */
  56. public class AngularCoordinates implements TimeShiftable<AngularCoordinates>, Serializable {

  57.     /** Fixed orientation parallel with reference frame
  58.      * (identity rotation, zero rotation rate and acceleration).
  59.      */
  60.     public static final AngularCoordinates IDENTITY =
  61.             new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);

  62.     /** Serializable UID. */
  63.     private static final long serialVersionUID = 20140414L;

  64.     /** Rotation. */
  65.     private final Rotation rotation;

  66.     /** Rotation rate. */
  67.     private final Vector3D rotationRate;

  68.     /** Rotation acceleration. */
  69.     private final Vector3D rotationAcceleration;

  70.     /** Simple constructor.
  71.      * <p> Sets the Coordinates to default : Identity, Ω = (0 0 0), dΩ/dt = (0 0 0).</p>
  72.      */
  73.     public AngularCoordinates() {
  74.         this(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
  75.     }

  76.     /** Builds a rotation/rotation rate pair.
  77.      * @param rotation rotation
  78.      * @param rotationRate rotation rate Ω (rad/s)
  79.      */
  80.     public AngularCoordinates(final Rotation rotation, final Vector3D rotationRate) {
  81.         this(rotation, rotationRate, Vector3D.ZERO);
  82.     }

  83.     /** Builds a rotation/rotation rate/rotation acceleration triplet.
  84.      * @param rotation rotation
  85.      * @param rotationRate rotation rate Ω (rad/s)
  86.      * @param rotationAcceleration rotation acceleration dΩ/dt (rad/s²)
  87.      */
  88.     public AngularCoordinates(final Rotation rotation,
  89.                               final Vector3D rotationRate, final Vector3D rotationAcceleration) {
  90.         this.rotation             = rotation;
  91.         this.rotationRate         = rotationRate;
  92.         this.rotationAcceleration = rotationAcceleration;
  93.     }

  94.     /**
  95.      * Builds angular coordinates with the given rotation, zero angular
  96.      * velocity, and zero angular acceleration.
  97.      *
  98.      * @param rotation rotation
  99.      */
  100.     public AngularCoordinates(final Rotation rotation) {
  101.         this(rotation, Vector3D.ZERO);
  102.     }

  103.     /** Build the rotation that transforms a pair of pv coordinates into another one.

  104.      * <p><em>WARNING</em>! This method requires much more stringent assumptions on
  105.      * its parameters than the similar {@link Rotation#Rotation(Vector3D, Vector3D,
  106.      * Vector3D, Vector3D) constructor} from the {@link Rotation Rotation} class.
  107.      * As far as the Rotation constructor is concerned, the {@code v₂} vector from
  108.      * the second pair can be slightly misaligned. The Rotation constructor will
  109.      * compensate for this misalignment and create a rotation that ensure {@code
  110.      * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
  111.      * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
  112.      * preserved, this constructor works <em>only</em> if the two pairs are fully
  113.      * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
  114.      * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
  115.      * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>
  116.      * @param u1 first vector of the origin pair
  117.      * @param u2 second vector of the origin pair
  118.      * @param v1 desired image of u1 by the rotation
  119.      * @param v2 desired image of u2 by the rotation
  120.      * @param tolerance relative tolerance factor used to check singularities
  121.      */
  122.     public AngularCoordinates(final PVCoordinates u1, final PVCoordinates u2,
  123.                               final PVCoordinates v1, final PVCoordinates v2,
  124.                               final double tolerance) {

  125.         try {
  126.             // find the initial fixed rotation
  127.             rotation = new Rotation(u1.getPosition(), u2.getPosition(),
  128.                                     v1.getPosition(), v2.getPosition());

  129.             // find rotation rate Ω such that
  130.             //  Ω ⨯ v₁ = r(dot(u₁)) - dot(v₁)
  131.             //  Ω ⨯ v₂ = r(dot(u₂)) - dot(v₂)
  132.             final Vector3D ru1Dot = rotation.applyTo(u1.getVelocity());
  133.             final Vector3D ru2Dot = rotation.applyTo(u2.getVelocity());
  134.             rotationRate = inverseCrossProducts(v1.getPosition(), ru1Dot.subtract(v1.getVelocity()),
  135.                                                 v2.getPosition(), ru2Dot.subtract(v2.getVelocity()),
  136.                                                 tolerance);

  137.             // find rotation acceleration dot(Ω) such that
  138.             // dot(Ω) ⨯ v₁ = r(dotdot(u₁)) - 2 Ω ⨯ dot(v₁) - Ω ⨯  (Ω ⨯ v₁) - dotdot(v₁)
  139.             // dot(Ω) ⨯ v₂ = r(dotdot(u₂)) - 2 Ω ⨯ dot(v₂) - Ω ⨯  (Ω ⨯ v₂) - dotdot(v₂)
  140.             final Vector3D ru1DotDot = rotation.applyTo(u1.getAcceleration());
  141.             final Vector3D oDotv1    = Vector3D.crossProduct(rotationRate, v1.getVelocity());
  142.             final Vector3D oov1      = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v1.getPosition()));
  143.             final Vector3D c1        = new Vector3D(1, ru1DotDot, -2, oDotv1, -1, oov1, -1, v1.getAcceleration());
  144.             final Vector3D ru2DotDot = rotation.applyTo(u2.getAcceleration());
  145.             final Vector3D oDotv2    = Vector3D.crossProduct(rotationRate, v2.getVelocity());
  146.             final Vector3D oov2      = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v2.getPosition()));
  147.             final Vector3D c2        = new Vector3D(1, ru2DotDot, -2, oDotv2, -1, oov2, -1, v2.getAcceleration());
  148.             rotationAcceleration     = inverseCrossProducts(v1.getPosition(), c1, v2.getPosition(), c2, tolerance);

  149.         } catch (MathRuntimeException mrte) {
  150.             throw new OrekitException(mrte);
  151.         }

  152.     }

  153.     /** Build one of the rotations that transform one pv coordinates into another one.

  154.      * <p>Except for a possible scale factor, if the instance were
  155.      * applied to the vector u it will produce the vector v. There is an
  156.      * infinite number of such rotations, this constructor choose the
  157.      * one with the smallest associated angle (i.e. the one whose axis
  158.      * is orthogonal to the (u, v) plane). If u and v are collinear, an
  159.      * arbitrary rotation axis is chosen.</p>

  160.      * @param u origin vector
  161.      * @param v desired image of u by the rotation
  162.      */
  163.     public AngularCoordinates(final PVCoordinates u, final PVCoordinates v) {
  164.         this(new FieldRotation<>(u.toDerivativeStructureVector(2),
  165.                                  v.toDerivativeStructureVector(2)));
  166.     }

  167.     /** Builds a AngularCoordinates from  a {@link FieldRotation}&lt;{@link Derivative}&gt;.
  168.      * <p>
  169.      * The rotation components must have time as their only derivation parameter and
  170.      * have consistent derivation orders.
  171.      * </p>
  172.      * @param r rotation with time-derivatives embedded within the coordinates
  173.      * @param <U> type of the derivative
  174.      */
  175.     public <U extends Derivative<U>> AngularCoordinates(final FieldRotation<U> r) {

  176.         final double q0       = r.getQ0().getReal();
  177.         final double q1       = r.getQ1().getReal();
  178.         final double q2       = r.getQ2().getReal();
  179.         final double q3       = r.getQ3().getReal();

  180.         rotation     = new Rotation(q0, q1, q2, q3, false);
  181.         if (r.getQ0().getOrder() >= 1) {
  182.             final double q0Dot    = r.getQ0().getPartialDerivative(1);
  183.             final double q1Dot    = r.getQ1().getPartialDerivative(1);
  184.             final double q2Dot    = r.getQ2().getPartialDerivative(1);
  185.             final double q3Dot    = r.getQ3().getPartialDerivative(1);
  186.             rotationRate =
  187.                     new Vector3D(2 * MathArrays.linearCombination(-q1, q0Dot,  q0, q1Dot,  q3, q2Dot, -q2, q3Dot),
  188.                                  2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot,  q0, q2Dot,  q1, q3Dot),
  189.                                  2 * MathArrays.linearCombination(-q3, q0Dot,  q2, q1Dot, -q1, q2Dot,  q0, q3Dot));
  190.             if (r.getQ0().getOrder() >= 2) {
  191.                 final double q0DotDot = r.getQ0().getPartialDerivative(2);
  192.                 final double q1DotDot = r.getQ1().getPartialDerivative(2);
  193.                 final double q2DotDot = r.getQ2().getPartialDerivative(2);
  194.                 final double q3DotDot = r.getQ3().getPartialDerivative(2);
  195.                 rotationAcceleration =
  196.                         new Vector3D(2 * MathArrays.linearCombination(-q1, q0DotDot,  q0, q1DotDot,  q3, q2DotDot, -q2, q3DotDot),
  197.                                      2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot,  q0, q2DotDot,  q1, q3DotDot),
  198.                                      2 * MathArrays.linearCombination(-q3, q0DotDot,  q2, q1DotDot, -q1, q2DotDot,  q0, q3DotDot));
  199.             } else {
  200.                 rotationAcceleration = Vector3D.ZERO;
  201.             }
  202.         } else {
  203.             rotationRate         = Vector3D.ZERO;
  204.             rotationAcceleration = Vector3D.ZERO;
  205.         }

  206.     }

  207.     /** Find a vector from two known cross products.
  208.      * <p>
  209.      * We want to find Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
  210.      * </p>
  211.      * <p>
  212.      * The first equation (Ω ⨯ v₁ = c₁) will always be fulfilled exactly,
  213.      * and the second one will be fulfilled if possible.
  214.      * </p>
  215.      * @param v1 vector forming the first known cross product
  216.      * @param c1 know vector for cross product Ω ⨯ v₁
  217.      * @param v2 vector forming the second known cross product
  218.      * @param c2 know vector for cross product Ω ⨯ v₂
  219.      * @param tolerance relative tolerance factor used to check singularities
  220.      * @return vector Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
  221.      * @exception MathIllegalArgumentException if vectors are inconsistent and
  222.      * no solution can be found
  223.      */
  224.     private static Vector3D inverseCrossProducts(final Vector3D v1, final Vector3D c1,
  225.                                                  final Vector3D v2, final Vector3D c2,
  226.                                                  final double tolerance)
  227.         throws MathIllegalArgumentException {

  228.         final double v12 = v1.getNormSq();
  229.         final double v1n = FastMath.sqrt(v12);
  230.         final double v22 = v2.getNormSq();
  231.         final double v2n = FastMath.sqrt(v22);
  232.         final double threshold = tolerance * FastMath.max(v1n, v2n);

  233.         Vector3D omega;

  234.         try {
  235.             // create the over-determined linear system representing the two cross products
  236.             final RealMatrix m = MatrixUtils.createRealMatrix(6, 3);
  237.             m.setEntry(0, 1,  v1.getZ());
  238.             m.setEntry(0, 2, -v1.getY());
  239.             m.setEntry(1, 0, -v1.getZ());
  240.             m.setEntry(1, 2,  v1.getX());
  241.             m.setEntry(2, 0,  v1.getY());
  242.             m.setEntry(2, 1, -v1.getX());
  243.             m.setEntry(3, 1,  v2.getZ());
  244.             m.setEntry(3, 2, -v2.getY());
  245.             m.setEntry(4, 0, -v2.getZ());
  246.             m.setEntry(4, 2,  v2.getX());
  247.             m.setEntry(5, 0,  v2.getY());
  248.             m.setEntry(5, 1, -v2.getX());

  249.             final RealVector rhs = MatrixUtils.createRealVector(new double[] {
  250.                 c1.getX(), c1.getY(), c1.getZ(),
  251.                 c2.getX(), c2.getY(), c2.getZ()
  252.             });

  253.             // find the best solution we can
  254.             final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver();
  255.             final RealVector v = solver.solve(rhs);
  256.             omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2));

  257.         } catch (MathIllegalArgumentException miae) {
  258.             if (miae.getSpecifier() == LocalizedCoreFormats.SINGULAR_MATRIX) {

  259.                 // handle some special cases for which we can compute a solution
  260.                 final double c12 = c1.getNormSq();
  261.                 final double c1n = FastMath.sqrt(c12);
  262.                 final double c22 = c2.getNormSq();
  263.                 final double c2n = FastMath.sqrt(c22);

  264.                 if (c1n <= threshold && c2n <= threshold) {
  265.                     // simple special case, velocities are cancelled
  266.                     return Vector3D.ZERO;
  267.                 } else if (v1n <= threshold && c1n >= threshold) {
  268.                     // this is inconsistent, if v₁ is zero, c₁ must be 0 too
  269.                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c1n, 0, true);
  270.                 } else if (v2n <= threshold && c2n >= threshold) {
  271.                     // this is inconsistent, if v₂ is zero, c₂ must be 0 too
  272.                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c2n, 0, true);
  273.                 } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) {
  274.                     // simple special case, v₂ is redundant with v₁, we just ignore it
  275.                     // use the simplest Ω: orthogonal to both v₁ and c₁
  276.                     omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1));
  277.                 } else {
  278.                     throw miae;
  279.                 }
  280.             } else {
  281.                 throw miae;
  282.             }

  283.         }

  284.         // check results
  285.         final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1);
  286.         if (d1 > threshold) {
  287.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d1, 0, true);
  288.         }

  289.         final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2);
  290.         if (d2 > threshold) {
  291.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d2, 0, true);
  292.         }

  293.         return omega;

  294.     }

  295.     /** Transform the instance to a {@link FieldRotation}&lt;{@link DerivativeStructure}&gt;.
  296.      * <p>
  297.      * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
  298.      * to the user-specified order.
  299.      * </p>
  300.      * @param order derivation order for the vector components
  301.      * @return rotation with time-derivatives embedded within the coordinates
  302.      */
  303.     public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order) {

  304.         // quaternion components
  305.         final double q0 = rotation.getQ0();
  306.         final double q1 = rotation.getQ1();
  307.         final double q2 = rotation.getQ2();
  308.         final double q3 = rotation.getQ3();

  309.         // first time-derivatives of the quaternion
  310.         final double oX    = rotationRate.getX();
  311.         final double oY    = rotationRate.getY();
  312.         final double oZ    = rotationRate.getZ();
  313.         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
  314.         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
  315.         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
  316.         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);

  317.         // second time-derivatives of the quaternion
  318.         final double oXDot = rotationAcceleration.getX();
  319.         final double oYDot = rotationAcceleration.getY();
  320.         final double oZDot = rotationAcceleration.getZ();
  321.         final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
  322.             q1, q2,  q3, q1Dot, q2Dot,  q3Dot
  323.         }, new double[] {
  324.             oXDot, oYDot, oZDot, oX, oY, oZ
  325.         });
  326.         final double q1DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  327.             q0, q2, -q3, q0Dot, q2Dot, -q3Dot
  328.         }, new double[] {
  329.             oXDot, oZDot, oYDot, oX, oZ, oY
  330.         });
  331.         final double q2DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  332.             q0, q3, -q1, q0Dot, q3Dot, -q1Dot
  333.         }, new double[] {
  334.             oYDot, oXDot, oZDot, oY, oX, oZ
  335.         });
  336.         final double q3DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  337.             q0, q1, -q2, q0Dot, q1Dot, -q2Dot
  338.         }, new double[] {
  339.             oZDot, oYDot, oXDot, oZ, oY, oX
  340.         });

  341.         final DSFactory factory;
  342.         final DerivativeStructure q0DS;
  343.         final DerivativeStructure q1DS;
  344.         final DerivativeStructure q2DS;
  345.         final DerivativeStructure q3DS;
  346.         switch (order) {
  347.             case 0 :
  348.                 factory = new DSFactory(1, order);
  349.                 q0DS = factory.build(q0);
  350.                 q1DS = factory.build(q1);
  351.                 q2DS = factory.build(q2);
  352.                 q3DS = factory.build(q3);
  353.                 break;
  354.             case 1 :
  355.                 factory = new DSFactory(1, order);
  356.                 q0DS = factory.build(q0, q0Dot);
  357.                 q1DS = factory.build(q1, q1Dot);
  358.                 q2DS = factory.build(q2, q2Dot);
  359.                 q3DS = factory.build(q3, q3Dot);
  360.                 break;
  361.             case 2 :
  362.                 factory = new DSFactory(1, order);
  363.                 q0DS = factory.build(q0, q0Dot, q0DotDot);
  364.                 q1DS = factory.build(q1, q1Dot, q1DotDot);
  365.                 q2DS = factory.build(q2, q2Dot, q2DotDot);
  366.                 q3DS = factory.build(q3, q3Dot, q3DotDot);
  367.                 break;
  368.             default :
  369.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  370.         }

  371.         return new FieldRotation<>(q0DS, q1DS, q2DS, q3DS, false);

  372.     }

  373.     /** Transform the instance to a {@link FieldRotation}&lt;{@link UnivariateDerivative1}&gt;.
  374.      * <p>
  375.      * The {@link UnivariateDerivative1} coordinates correspond to time-derivatives up
  376.      * to the order 1.
  377.      * </p>
  378.      * @return rotation with time-derivatives embedded within the coordinates
  379.      */
  380.     public FieldRotation<UnivariateDerivative1> toUnivariateDerivative1Rotation() {

  381.         // quaternion components
  382.         final double q0 = rotation.getQ0();
  383.         final double q1 = rotation.getQ1();
  384.         final double q2 = rotation.getQ2();
  385.         final double q3 = rotation.getQ3();

  386.         // first time-derivatives of the quaternion
  387.         final double oX    = rotationRate.getX();
  388.         final double oY    = rotationRate.getY();
  389.         final double oZ    = rotationRate.getZ();
  390.         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
  391.         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
  392.         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
  393.         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);

  394.         final UnivariateDerivative1 q0UD = new UnivariateDerivative1(q0, q0Dot);
  395.         final UnivariateDerivative1 q1UD = new UnivariateDerivative1(q1, q1Dot);
  396.         final UnivariateDerivative1 q2UD = new UnivariateDerivative1(q2, q2Dot);
  397.         final UnivariateDerivative1 q3UD = new UnivariateDerivative1(q3, q3Dot);

  398.         return new FieldRotation<>(q0UD, q1UD, q2UD, q3UD, false);

  399.     }

  400.     /** Transform the instance to a {@link FieldRotation}&lt;{@link UnivariateDerivative2}&gt;.
  401.      * <p>
  402.      * The {@link UnivariateDerivative2} coordinates correspond to time-derivatives up
  403.      * to the order 2.
  404.      * </p>
  405.      * @return rotation with time-derivatives embedded within the coordinates
  406.      */
  407.     public FieldRotation<UnivariateDerivative2> toUnivariateDerivative2Rotation() {

  408.         // quaternion components
  409.         final double q0 = rotation.getQ0();
  410.         final double q1 = rotation.getQ1();
  411.         final double q2 = rotation.getQ2();
  412.         final double q3 = rotation.getQ3();

  413.         // first time-derivatives of the quaternion
  414.         final double oX    = rotationRate.getX();
  415.         final double oY    = rotationRate.getY();
  416.         final double oZ    = rotationRate.getZ();
  417.         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
  418.         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
  419.         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
  420.         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);

  421.         // second time-derivatives of the quaternion
  422.         final double oXDot = rotationAcceleration.getX();
  423.         final double oYDot = rotationAcceleration.getY();
  424.         final double oZDot = rotationAcceleration.getZ();
  425.         final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
  426.             q1, q2,  q3, q1Dot, q2Dot,  q3Dot
  427.         }, new double[] {
  428.             oXDot, oYDot, oZDot, oX, oY, oZ
  429.         });
  430.         final double q1DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  431.             q0, q2, -q3, q0Dot, q2Dot, -q3Dot
  432.         }, new double[] {
  433.             oXDot, oZDot, oYDot, oX, oZ, oY
  434.         });
  435.         final double q2DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  436.             q0, q3, -q1, q0Dot, q3Dot, -q1Dot
  437.         }, new double[] {
  438.             oYDot, oXDot, oZDot, oY, oX, oZ
  439.         });
  440.         final double q3DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  441.             q0, q1, -q2, q0Dot, q1Dot, -q2Dot
  442.         }, new double[] {
  443.             oZDot, oYDot, oXDot, oZ, oY, oX
  444.         });

  445.         final UnivariateDerivative2 q0UD = new UnivariateDerivative2(q0, q0Dot, q0DotDot);
  446.         final UnivariateDerivative2 q1UD = new UnivariateDerivative2(q1, q1Dot, q1DotDot);
  447.         final UnivariateDerivative2 q2UD = new UnivariateDerivative2(q2, q2Dot, q2DotDot);
  448.         final UnivariateDerivative2 q3UD = new UnivariateDerivative2(q3, q3Dot, q3DotDot);

  449.         return new FieldRotation<>(q0UD, q1UD, q2UD, q3UD, false);

  450.     }

  451.     /** Estimate rotation rate between two orientations.
  452.      * <p>Estimation is based on a simple fixed rate rotation
  453.      * during the time interval between the two orientations.</p>
  454.      * @param start start orientation
  455.      * @param end end orientation
  456.      * @param dt time elapsed between the dates of the two orientations
  457.      * @return rotation rate allowing to go from start to end orientations
  458.      */
  459.     public static Vector3D estimateRate(final Rotation start, final Rotation end, final double dt) {
  460.         final Rotation evolution = start.compose(end.revert(), RotationConvention.VECTOR_OPERATOR);
  461.         return new Vector3D(evolution.getAngle() / dt, evolution.getAxis(RotationConvention.VECTOR_OPERATOR));
  462.     }

  463.     /** Revert a rotation/rotation rate/ rotation acceleration triplet.
  464.      * Build a triplet which reverse the effect of another triplet.
  465.      * @return a new triplet whose effect is the reverse of the effect
  466.      * of the instance
  467.      */
  468.     public AngularCoordinates revert() {
  469.         return new AngularCoordinates(rotation.revert(),
  470.                                       rotation.applyInverseTo(rotationRate).negate(),
  471.                                       rotation.applyInverseTo(rotationAcceleration).negate());
  472.     }


  473.     /** Get a time-shifted rotation. Same as {@link #shiftedBy(double)} except
  474.      * only the shifted rotation is computed.
  475.      * <p>
  476.      * The state can be slightly shifted to close dates. This shift is based on
  477.      * an approximate solution of the fixed acceleration motion. It is <em>not</em>
  478.      * intended as a replacement for proper attitude propagation but should be
  479.      * sufficient for either small time shifts or coarse accuracy.
  480.      * </p>
  481.      * @param dt time shift in seconds
  482.      * @return a new state, shifted with respect to the instance (which is immutable)
  483.      * @see  #shiftedBy(double)
  484.      */
  485.     public Rotation rotationShiftedBy(final double dt) {

  486.         // the shiftedBy method is based on a local approximation.
  487.         // It considers separately the contribution of the constant
  488.         // rotation, the linear contribution or the rate and the
  489.         // quadratic contribution of the acceleration. The rate
  490.         // and acceleration contributions are small rotations as long
  491.         // as the time shift is small, which is the crux of the algorithm.
  492.         // Small rotations are almost commutative, so we append these small
  493.         // contributions one after the other, as if they really occurred
  494.         // successively, despite this is not what really happens.

  495.         // compute the linear contribution first, ignoring acceleration
  496.         // BEWARE: there is really a minus sign here, because if
  497.         // the target frame rotates in one direction, the vectors in the origin
  498.         // frame seem to rotate in the opposite direction
  499.         final double rate = rotationRate.getNorm();
  500.         final Rotation rateContribution = (rate == 0.0) ?
  501.                 Rotation.IDENTITY :
  502.                 new Rotation(rotationRate, rate * dt, RotationConvention.FRAME_TRANSFORM);

  503.         // append rotation and rate contribution
  504.         final Rotation linearPart =
  505.                 rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR);

  506.         final double acc  = rotationAcceleration.getNorm();
  507.         if (acc == 0.0) {
  508.             // no acceleration, the linear part is sufficient
  509.             return linearPart;
  510.         }

  511.         // compute the quadratic contribution, ignoring initial rotation and rotation rate
  512.         // BEWARE: there is really a minus sign here, because if
  513.         // the target frame rotates in one direction, the vectors in the origin
  514.         // frame seem to rotate in the opposite direction
  515.         final Rotation quadraticContribution =
  516.                 new Rotation(rotationAcceleration,
  517.                         0.5 * acc * dt * dt,
  518.                         RotationConvention.FRAME_TRANSFORM);

  519.         // the quadratic contribution is a small rotation:
  520.         // its initial angle and angular rate are both zero.
  521.         // small rotations are almost commutative, so we append the small
  522.         // quadratic part after the linear part as a simple offset
  523.         return quadraticContribution
  524.                 .compose(linearPart, RotationConvention.VECTOR_OPERATOR);

  525.     }


  526.     /** Get a time-shifted state.
  527.      * <p>
  528.      * The state can be slightly shifted to close dates. This shift is based on
  529.      * an approximate solution of the fixed acceleration motion. It is <em>not</em>
  530.      * intended as a replacement for proper attitude propagation but should be
  531.      * sufficient for either small time shifts or coarse accuracy.
  532.      * </p>
  533.      * @param dt time shift in seconds
  534.      * @return a new state, shifted with respect to the instance (which is immutable)
  535.      */
  536.     public AngularCoordinates shiftedBy(final double dt) {

  537.         // the shiftedBy method is based on a local approximation.
  538.         // It considers separately the contribution of the constant
  539.         // rotation, the linear contribution or the rate and the
  540.         // quadratic contribution of the acceleration. The rate
  541.         // and acceleration contributions are small rotations as long
  542.         // as the time shift is small, which is the crux of the algorithm.
  543.         // Small rotations are almost commutative, so we append these small
  544.         // contributions one after the other, as if they really occurred
  545.         // successively, despite this is not what really happens.

  546.         // compute the linear contribution first, ignoring acceleration
  547.         // BEWARE: there is really a minus sign here, because if
  548.         // the target frame rotates in one direction, the vectors in the origin
  549.         // frame seem to rotate in the opposite direction
  550.         final double rate = rotationRate.getNorm();
  551.         final Rotation rateContribution = (rate == 0.0) ?
  552.                                           Rotation.IDENTITY :
  553.                                           new Rotation(rotationRate, rate * dt, RotationConvention.FRAME_TRANSFORM);

  554.         // append rotation and rate contribution
  555.         final AngularCoordinates linearPart =
  556.                 new AngularCoordinates(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR), rotationRate);

  557.         final double acc  = rotationAcceleration.getNorm();
  558.         if (acc == 0.0) {
  559.             // no acceleration, the linear part is sufficient
  560.             return linearPart;
  561.         }

  562.         // compute the quadratic contribution, ignoring initial rotation and rotation rate
  563.         // BEWARE: there is really a minus sign here, because if
  564.         // the target frame rotates in one direction, the vectors in the origin
  565.         // frame seem to rotate in the opposite direction
  566.         final AngularCoordinates quadraticContribution =
  567.                 new AngularCoordinates(new Rotation(rotationAcceleration,
  568.                                                     0.5 * acc * dt * dt,
  569.                                                     RotationConvention.FRAME_TRANSFORM),
  570.                                        new Vector3D(dt, rotationAcceleration),
  571.                                        rotationAcceleration);

  572.         // the quadratic contribution is a small rotation:
  573.         // its initial angle and angular rate are both zero.
  574.         // small rotations are almost commutative, so we append the small
  575.         // quadratic part after the linear part as a simple offset
  576.         return quadraticContribution.addOffset(linearPart);

  577.     }

  578.     /** Get the rotation.
  579.      * @return the rotation.
  580.      */
  581.     public Rotation getRotation() {
  582.         return rotation;
  583.     }

  584.     /** Get the rotation rate.
  585.      * @return the rotation rate vector Ω (rad/s).
  586.      */
  587.     public Vector3D getRotationRate() {
  588.         return rotationRate;
  589.     }

  590.     /** Get the rotation acceleration.
  591.      * @return the rotation acceleration vector dΩ/dt (rad/s²).
  592.      */
  593.     public Vector3D getRotationAcceleration() {
  594.         return rotationAcceleration;
  595.     }

  596.     /** Add an offset from the instance.
  597.      * <p>
  598.      * We consider here that the offset rotation is applied first and the
  599.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  600.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  601.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  602.      * </p>
  603.      * <p>
  604.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  605.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  606.      * so that round trip applications are possible. This means that both {@code
  607.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  608.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  609.      * </p>
  610.      * @param offset offset to subtract
  611.      * @return new instance, with offset subtracted
  612.      * @see #subtractOffset(AngularCoordinates)
  613.      */
  614.     public AngularCoordinates addOffset(final AngularCoordinates offset) {
  615.         final Vector3D rOmega    = rotation.applyTo(offset.rotationRate);
  616.         final Vector3D rOmegaDot = rotation.applyTo(offset.rotationAcceleration);
  617.         return new AngularCoordinates(rotation.compose(offset.rotation, RotationConvention.VECTOR_OPERATOR),
  618.                                       rotationRate.add(rOmega),
  619.                                       new Vector3D( 1.0, rotationAcceleration,
  620.                                                     1.0, rOmegaDot,
  621.                                                    -1.0, Vector3D.crossProduct(rotationRate, rOmega)));
  622.     }

  623.     /** Subtract an offset from the instance.
  624.      * <p>
  625.      * We consider here that the offset rotation is applied first and the
  626.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  627.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  628.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  629.      * </p>
  630.      * <p>
  631.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  632.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  633.      * so that round trip applications are possible. This means that both {@code
  634.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  635.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  636.      * </p>
  637.      * @param offset offset to subtract
  638.      * @return new instance, with offset subtracted
  639.      * @see #addOffset(AngularCoordinates)
  640.      */
  641.     public AngularCoordinates subtractOffset(final AngularCoordinates offset) {
  642.         return addOffset(offset.revert());
  643.     }

  644.     /** Apply the rotation to a pv coordinates.
  645.      * @param pv vector to apply the rotation to
  646.      * @return a new pv coordinates which is the image of u by the rotation
  647.      */
  648.     public PVCoordinates applyTo(final PVCoordinates pv) {

  649.         final Vector3D transformedP = rotation.applyTo(pv.getPosition());
  650.         final Vector3D crossP       = Vector3D.crossProduct(rotationRate, transformedP);
  651.         final Vector3D transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  652.         final Vector3D crossV       = Vector3D.crossProduct(rotationRate, transformedV);
  653.         final Vector3D crossCrossP  = Vector3D.crossProduct(rotationRate, crossP);
  654.         final Vector3D crossDotP    = Vector3D.crossProduct(rotationAcceleration, transformedP);
  655.         final Vector3D transformedA = new Vector3D( 1, rotation.applyTo(pv.getAcceleration()),
  656.                                                    -2, crossV,
  657.                                                    -1, crossCrossP,
  658.                                                    -1, crossDotP);

  659.         return new PVCoordinates(transformedP, transformedV, transformedA);

  660.     }

  661.     /** Apply the rotation to a pv coordinates.
  662.      * @param pv vector to apply the rotation to
  663.      * @return a new pv coordinates which is the image of u by the rotation
  664.      */
  665.     public TimeStampedPVCoordinates applyTo(final TimeStampedPVCoordinates pv) {

  666.         final Vector3D transformedP = getRotation().applyTo(pv.getPosition());
  667.         final Vector3D crossP       = Vector3D.crossProduct(getRotationRate(), transformedP);
  668.         final Vector3D transformedV = getRotation().applyTo(pv.getVelocity()).subtract(crossP);
  669.         final Vector3D crossV       = Vector3D.crossProduct(getRotationRate(), transformedV);
  670.         final Vector3D crossCrossP  = Vector3D.crossProduct(getRotationRate(), crossP);
  671.         final Vector3D crossDotP    = Vector3D.crossProduct(getRotationAcceleration(), transformedP);
  672.         final Vector3D transformedA = new Vector3D( 1, getRotation().applyTo(pv.getAcceleration()),
  673.                                                    -2, crossV,
  674.                                                    -1, crossCrossP,
  675.                                                    -1, crossDotP);

  676.         return new TimeStampedPVCoordinates(pv.getDate(), transformedP, transformedV, transformedA);

  677.     }

  678.     /** Apply the rotation to a pv coordinates.
  679.      * @param pv vector to apply the rotation to
  680.      * @param <T> type of the field elements
  681.      * @return a new pv coordinates which is the image of u by the rotation
  682.      * @since 9.0
  683.      */
  684.     public <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> applyTo(final FieldPVCoordinates<T> pv) {

  685.         final FieldVector3D<T> transformedP = FieldRotation.applyTo(rotation, pv.getPosition());
  686.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  687.         final FieldVector3D<T> transformedV = FieldRotation.applyTo(rotation, pv.getVelocity()).subtract(crossP);
  688.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  689.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  690.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  691.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, FieldRotation.applyTo(rotation, pv.getAcceleration()),
  692.                                                                   -2, crossV,
  693.                                                                   -1, crossCrossP,
  694.                                                                   -1, crossDotP);

  695.         return new FieldPVCoordinates<>(transformedP, transformedV, transformedA);

  696.     }

  697.     /** Apply the rotation to a pv coordinates.
  698.      * @param pv vector to apply the rotation to
  699.      * @param <T> type of the field elements
  700.      * @return a new pv coordinates which is the image of u by the rotation
  701.      * @since 9.0
  702.      */
  703.     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> applyTo(final TimeStampedFieldPVCoordinates<T> pv) {

  704.         final FieldVector3D<T> transformedP = FieldRotation.applyTo(rotation, pv.getPosition());
  705.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  706.         final FieldVector3D<T> transformedV = FieldRotation.applyTo(rotation, pv.getVelocity()).subtract(crossP);
  707.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  708.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  709.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  710.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, FieldRotation.applyTo(rotation, pv.getAcceleration()),
  711.                                                                   -2, crossV,
  712.                                                                   -1, crossCrossP,
  713.                                                                   -1, crossDotP);

  714.         return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV, transformedA);

  715.     }

  716.     /** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives.
  717.      * <p>
  718.      * The modified Rodrigues vector is tan(θ/4) u where θ and u are the
  719.      * rotation angle and axis respectively.
  720.      * </p>
  721.      * @param sign multiplicative sign for quaternion components
  722.      * @return modified Rodrigues vector and derivatives (vector on row 0, first derivative
  723.      * on row 1, second derivative on row 2)
  724.      * @see #createFromModifiedRodrigues(double[][])
  725.      */
  726.     public double[][] getModifiedRodrigues(final double sign) {

  727.         final double q0    = sign * getRotation().getQ0();
  728.         final double q1    = sign * getRotation().getQ1();
  729.         final double q2    = sign * getRotation().getQ2();
  730.         final double q3    = sign * getRotation().getQ3();
  731.         final double oX    = getRotationRate().getX();
  732.         final double oY    = getRotationRate().getY();
  733.         final double oZ    = getRotationRate().getZ();
  734.         final double oXDot = getRotationAcceleration().getX();
  735.         final double oYDot = getRotationAcceleration().getY();
  736.         final double oZDot = getRotationAcceleration().getZ();

  737.         // first time-derivatives of the quaternion
  738.         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
  739.         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
  740.         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
  741.         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);

  742.         // second time-derivatives of the quaternion
  743.         final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
  744.             q1, q2,  q3, q1Dot, q2Dot,  q3Dot
  745.         }, new double[] {
  746.             oXDot, oYDot, oZDot, oX, oY, oZ
  747.         });
  748.         final double q1DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  749.             q0, q2, -q3, q0Dot, q2Dot, -q3Dot
  750.         }, new double[] {
  751.             oXDot, oZDot, oYDot, oX, oZ, oY
  752.         });
  753.         final double q2DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  754.             q0, q3, -q1, q0Dot, q3Dot, -q1Dot
  755.         }, new double[] {
  756.             oYDot, oXDot, oZDot, oY, oX, oZ
  757.         });
  758.         final double q3DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  759.             q0, q1, -q2, q0Dot, q1Dot, -q2Dot
  760.         }, new double[] {
  761.             oZDot, oYDot, oXDot, oZ, oY, oX
  762.         });

  763.         // the modified Rodrigues is tan(θ/4) u where θ and u are the rotation angle and axis respectively
  764.         // this can be rewritten using quaternion components:
  765.         //      r (q₁ / (1+q₀), q₂ / (1+q₀), q₃ / (1+q₀))
  766.         // applying the derivation chain rule to previous expression gives rDot and rDotDot
  767.         final double inv          = 1.0 / (1.0 + q0);
  768.         final double mTwoInvQ0Dot = -2 * inv * q0Dot;

  769.         final double r1       = inv * q1;
  770.         final double r2       = inv * q2;
  771.         final double r3       = inv * q3;

  772.         final double mInvR1   = -inv * r1;
  773.         final double mInvR2   = -inv * r2;
  774.         final double mInvR3   = -inv * r3;

  775.         final double r1Dot    = MathArrays.linearCombination(inv, q1Dot, mInvR1, q0Dot);
  776.         final double r2Dot    = MathArrays.linearCombination(inv, q2Dot, mInvR2, q0Dot);
  777.         final double r3Dot    = MathArrays.linearCombination(inv, q3Dot, mInvR3, q0Dot);

  778.         final double r1DotDot = MathArrays.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot);
  779.         final double r2DotDot = MathArrays.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot);
  780.         final double r3DotDot = MathArrays.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot);

  781.         return new double[][] {
  782.             {
  783.                 r1,       r2,       r3
  784.             }, {
  785.                 r1Dot,    r2Dot,    r3Dot
  786.             }, {
  787.                 r1DotDot, r2DotDot, r3DotDot
  788.             }
  789.         };

  790.     }

  791.     /** Convert a modified Rodrigues vector and derivatives to angular coordinates.
  792.      * @param r modified Rodrigues vector (with first and second times derivatives)
  793.      * @return angular coordinates
  794.      * @see #getModifiedRodrigues(double)
  795.      */
  796.     public static AngularCoordinates createFromModifiedRodrigues(final double[][] r) {

  797.         // rotation
  798.         final double rSquared = r[0][0] * r[0][0] + r[0][1] * r[0][1] + r[0][2] * r[0][2];
  799.         final double oPQ0     = 2 / (1 + rSquared);
  800.         final double q0       = oPQ0 - 1;
  801.         final double q1       = oPQ0 * r[0][0];
  802.         final double q2       = oPQ0 * r[0][1];
  803.         final double q3       = oPQ0 * r[0][2];

  804.         // rotation rate
  805.         final double oPQ02    = oPQ0 * oPQ0;
  806.         final double q0Dot    = -oPQ02 * MathArrays.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1],  r[0][2], r[1][2]);
  807.         final double q1Dot    = oPQ0 * r[1][0] + r[0][0] * q0Dot;
  808.         final double q2Dot    = oPQ0 * r[1][1] + r[0][1] * q0Dot;
  809.         final double q3Dot    = oPQ0 * r[1][2] + r[0][2] * q0Dot;
  810.         final double oX       = 2 * MathArrays.linearCombination(-q1, q0Dot,  q0, q1Dot,  q3, q2Dot, -q2, q3Dot);
  811.         final double oY       = 2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot,  q0, q2Dot,  q1, q3Dot);
  812.         final double oZ       = 2 * MathArrays.linearCombination(-q3, q0Dot,  q2, q1Dot, -q1, q2Dot,  q0, q3Dot);

  813.         // rotation acceleration
  814.         final double q0DotDot = (1 - q0) / oPQ0 * q0Dot * q0Dot -
  815.                                 oPQ02 * MathArrays.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]) -
  816.                                 (q1Dot * q1Dot + q2Dot * q2Dot + q3Dot * q3Dot);
  817.         final double q1DotDot = MathArrays.linearCombination(oPQ0, r[2][0], 2 * r[1][0], q0Dot, r[0][0], q0DotDot);
  818.         final double q2DotDot = MathArrays.linearCombination(oPQ0, r[2][1], 2 * r[1][1], q0Dot, r[0][1], q0DotDot);
  819.         final double q3DotDot = MathArrays.linearCombination(oPQ0, r[2][2], 2 * r[1][2], q0Dot, r[0][2], q0DotDot);
  820.         final double oXDot    = 2 * MathArrays.linearCombination(-q1, q0DotDot,  q0, q1DotDot,  q3, q2DotDot, -q2, q3DotDot);
  821.         final double oYDot    = 2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot,  q0, q2DotDot,  q1, q3DotDot);
  822.         final double oZDot    = 2 * MathArrays.linearCombination(-q3, q0DotDot,  q2, q1DotDot, -q1, q2DotDot,  q0, q3DotDot);

  823.         return new AngularCoordinates(new Rotation(q0, q1, q2, q3, false),
  824.                                       new Vector3D(oX, oY, oZ),
  825.                                       new Vector3D(oXDot, oYDot, oZDot));

  826.     }

  827. }