FieldAngularCoordinates.java

  1. /* Copyright 2002-2024 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;

  19. import org.hipparchus.Field;
  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.analysis.differentiation.FDSFactory;
  22. import org.hipparchus.analysis.differentiation.FieldDerivative;
  23. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  24. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
  25. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  26. import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
  27. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  28. import org.hipparchus.exception.LocalizedCoreFormats;
  29. import org.hipparchus.exception.MathIllegalArgumentException;
  30. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  31. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  32. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  33. import org.hipparchus.linear.FieldDecompositionSolver;
  34. import org.hipparchus.linear.FieldMatrix;
  35. import org.hipparchus.linear.FieldQRDecomposition;
  36. import org.hipparchus.linear.FieldVector;
  37. import org.hipparchus.linear.MatrixUtils;
  38. import org.hipparchus.util.MathArrays;
  39. import org.orekit.errors.OrekitException;
  40. import org.orekit.errors.OrekitMessages;
  41. import org.orekit.time.FieldTimeShiftable;

  43. /** Simple container for rotation / rotation rate pairs, using {@link
  44.  * CalculusFieldElement}.
  45.  * <p>
  46.  * The state can be slightly shifted to close dates. This shift is based on
  47.  * a simple quadratic model. It is <em>not</em> intended as a replacement for
  48.  * proper attitude propagation but should be sufficient for either small
  49.  * time shifts or coarse accuracy.
  50.  * </p>
  51.  * <p>
  52.  * This class is the angular counterpart to {@link FieldPVCoordinates}.
  53.  * </p>
  54.  * <p>Instances of this class are guaranteed to be immutable.</p>
  55.  * @param <T> the type of the field elements
  56.  * @author Luc Maisonobe
  57.  * @since 6.0
  58.  * @see AngularCoordinates
  59.  */
  60. public class FieldAngularCoordinates<T extends CalculusFieldElement<T>>
  61.         implements FieldTimeShiftable<FieldAngularCoordinates<T>, T> {

  63.     /** rotation. */
  64.     private final FieldRotation<T> rotation;

  66.     /** rotation rate. */
  67.     private final FieldVector3D<T> rotationRate;

  69.     /** rotation acceleration. */
  70.     private final FieldVector3D<T> rotationAcceleration;

  72.     /** Builds a rotation/rotation rate pair.
  73.      * @param rotation rotation
  74.      * @param rotationRate rotation rate Ω (rad/s)
  75.      */
  76.     public FieldAngularCoordinates(final FieldRotation<T> rotation,
  77.                                    final FieldVector3D<T> rotationRate) {
  78.         this(rotation, rotationRate,
  79.              new FieldVector3D<>(rotation.getQ0().getField().getZero(),
  80.                                  rotation.getQ0().getField().getZero(),
  81.                                  rotation.getQ0().getField().getZero()));
  82.     }

  84.     /** Builds a rotation / rotation rate / rotation acceleration triplet.
  85.      * @param rotation i.e. the orientation of the vehicle
  86.      * @param rotationRate rotation rate Ω, i.e. the spin vector (rad/s)
  87.      * @param rotationAcceleration angular acceleration vector dΩ/dt (rad/s²)
  88.      */
  89.     public FieldAngularCoordinates(final FieldRotation<T> rotation,
  90.                                    final FieldVector3D<T> rotationRate,
  91.                                    final FieldVector3D<T> rotationAcceleration) {
  92.         this.rotation             = rotation;
  93.         this.rotationRate         = rotationRate;
  94.         this.rotationAcceleration = rotationAcceleration;
  95.     }

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

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

  121.         try {
  122.             // find the initial fixed rotation
  123.             rotation = new FieldRotation<>(u1.getPosition(), u2.getPosition(),
  124.                                            v1.getPosition(), v2.getPosition());

  126.             // find rotation rate Ω such that
  127.             //  Ω ⨯ v₁ = r(dot(u₁)) - dot(v₁)
  128.             //  Ω ⨯ v₂ = r(dot(u₂)) - dot(v₂)
  129.             final FieldVector3D<T> ru1Dot = rotation.applyTo(u1.getVelocity());
  130.             final FieldVector3D<T> ru2Dot = rotation.applyTo(u2.getVelocity());


  133.             rotationRate = inverseCrossProducts(v1.getPosition(), ru1Dot.subtract(v1.getVelocity()),
  134.                                                 v2.getPosition(), ru2Dot.subtract(v2.getVelocity()),
  135.                                                 tolerance);


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

  151.         } catch (MathIllegalArgumentException miae) {
  152.             throw new OrekitException(miae);
  153.         }

  155.     }

  157.     /** Builds a FieldAngularCoordinates from a field and a regular AngularCoordinates.
  158.      * @param field field for the components
  159.      * @param ang AngularCoordinates to convert
  160.      */
  161.     public FieldAngularCoordinates(final Field<T> field, final AngularCoordinates ang) {
  162.         this.rotation             = new FieldRotation<>(field, ang.getRotation());
  163.         this.rotationRate         = new FieldVector3D<>(field, ang.getRotationRate());
  164.         this.rotationAcceleration = new FieldVector3D<>(field, ang.getRotationAcceleration());
  165.     }

  167.     /** Builds a FieldAngularCoordinates from  a {@link FieldRotation}&lt;{@link FieldDerivativeStructure}&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.      * @since 9.2
  175.      */
  176.     public <U extends FieldDerivative<T, U>> FieldAngularCoordinates(final FieldRotation<U> r) {

  178.         final T q0       = r.getQ0().getValue();
  179.         final T q1       = r.getQ1().getValue();
  180.         final T q2       = r.getQ2().getValue();
  181.         final T q3       = r.getQ3().getValue();

  183.         rotation     = new FieldRotation<>(q0, q1, q2, q3, false);
  184.         if (r.getQ0().getOrder() >= 1) {
  185.             final T q0Dot    = r.getQ0().getPartialDerivative(1);
  186.             final T q1Dot    = r.getQ1().getPartialDerivative(1);
  187.             final T q2Dot    = r.getQ2().getPartialDerivative(1);
  188.             final T q3Dot    = r.getQ3().getPartialDerivative(1);
  189.             rotationRate =
  190.                     new FieldVector3D<>(q0.linearCombination(q1.negate(), q0Dot, q0,          q1Dot,
  191.                                                              q3,          q2Dot, q2.negate(), q3Dot).multiply(2),
  192.                                         q0.linearCombination(q2.negate(), q0Dot, q3.negate(), q1Dot,
  193.                                                              q0,          q2Dot, q1,          q3Dot).multiply(2),
  194.                                         q0.linearCombination(q3.negate(), q0Dot, q2,          q1Dot,
  195.                                                              q1.negate(), q2Dot, q0,          q3Dot).multiply(2));
  196.             if (r.getQ0().getOrder() >= 2) {
  197.                 final T q0DotDot = r.getQ0().getPartialDerivative(2);
  198.                 final T q1DotDot = r.getQ1().getPartialDerivative(2);
  199.                 final T q2DotDot = r.getQ2().getPartialDerivative(2);
  200.                 final T q3DotDot = r.getQ3().getPartialDerivative(2);
  201.                 rotationAcceleration =
  202.                         new FieldVector3D<>(q0.linearCombination(q1.negate(), q0DotDot, q0,          q1DotDot,
  203.                                                                  q3,          q2DotDot, q2.negate(), q3DotDot).multiply(2),
  204.                                             q0.linearCombination(q2.negate(), q0DotDot, q3.negate(), q1DotDot,
  205.                                                                  q0,          q2DotDot, q1,          q3DotDot).multiply(2),
  206.                                             q0.linearCombination(q3.negate(), q0DotDot, q2,          q1DotDot,
  207.                                                                  q1.negate(), q2DotDot, q0,          q3DotDot).multiply(2));
  208.             } else {
  209.                 rotationAcceleration = FieldVector3D.getZero(q0.getField());
  210.             }
  211.         } else {
  212.             rotationRate         = FieldVector3D.getZero(q0.getField());
  213.             rotationAcceleration = FieldVector3D.getZero(q0.getField());
  214.         }

  216.     }

  218.     /** Fixed orientation parallel with reference frame
  219.      * (identity rotation, zero rotation rate and acceleration).
  220.      * @param field field for the components
  221.      * @param <T> the type of the field elements
  222.      * @return a new fixed orientation parallel with reference frame
  223.      */
  224.     public static <T extends CalculusFieldElement<T>> FieldAngularCoordinates<T> getIdentity(final Field<T> field) {
  225.         return new FieldAngularCoordinates<>(field, AngularCoordinates.IDENTITY);
  226.     }

  228.     /** Find a vector from two known cross products.
  229.      * <p>
  230.      * We want to find Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
  231.      * </p>
  232.      * <p>
  233.      * The first equation (Ω ⨯ v₁ = c₁) will always be fulfilled exactly,
  234.      * and the second one will be fulfilled if possible.
  235.      * </p>
  236.      * @param v1 vector forming the first known cross product
  237.      * @param c1 know vector for cross product Ω ⨯ v₁
  238.      * @param v2 vector forming the second known cross product
  239.      * @param c2 know vector for cross product Ω ⨯ v₂
  240.      * @param tolerance relative tolerance factor used to check singularities
  241.      * @param <T> the type of the field elements
  242.      * @return vector Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
  243.      * @exception MathIllegalArgumentException if vectors are inconsistent and
  244.      * no solution can be found
  245.      */
  246.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> inverseCrossProducts(final FieldVector3D<T> v1, final FieldVector3D<T> c1,
  247.                                                                                          final FieldVector3D<T> v2, final FieldVector3D<T> c2,
  248.                                                                                          final double tolerance)
  249.         throws MathIllegalArgumentException {

  251.         final T v12 = v1.getNormSq();
  252.         final T v1n = v12.sqrt();
  253.         final T v22 = v2.getNormSq();
  254.         final T v2n = v22.sqrt();
  255.         final T threshold;
  256.         if (v1n.getReal() >= v2n.getReal()) {
  257.             threshold = v1n.multiply(tolerance);
  258.         }
  259.         else {
  260.             threshold = v2n.multiply(tolerance);
  261.         }
  262.         FieldVector3D<T> omega = null;

  264.         try {
  265.             // create the over-determined linear system representing the two cross products
  266.             final FieldMatrix<T> m = MatrixUtils.createFieldMatrix(v12.getField(), 6, 3);
  267.             m.setEntry(0, 1, v1.getZ());
  268.             m.setEntry(0, 2, v1.getY().negate());
  269.             m.setEntry(1, 0, v1.getZ().negate());
  270.             m.setEntry(1, 2, v1.getX());
  271.             m.setEntry(2, 0, v1.getY());
  272.             m.setEntry(2, 1, v1.getX().negate());
  273.             m.setEntry(3, 1, v2.getZ());
  274.             m.setEntry(3, 2, v2.getY().negate());
  275.             m.setEntry(4, 0, v2.getZ().negate());
  276.             m.setEntry(4, 2, v2.getX());
  277.             m.setEntry(5, 0, v2.getY());
  278.             m.setEntry(5, 1, v2.getX().negate());

  280.             final T[] kk = MathArrays.buildArray(v2n.getField(), 6);
  281.             kk[0] = c1.getX();
  282.             kk[1] = c1.getY();
  283.             kk[2] = c1.getZ();
  284.             kk[3] = c2.getX();
  285.             kk[4] = c2.getY();
  286.             kk[5] = c2.getZ();
  287.             final FieldVector<T> rhs = MatrixUtils.createFieldVector(kk);

  289.             // find the best solution we can
  290.             final FieldDecompositionSolver<T> solver = new FieldQRDecomposition<>(m).getSolver();
  291.             final FieldVector<T> v = solver.solve(rhs);
  292.             omega = new FieldVector3D<>(v.getEntry(0), v.getEntry(1), v.getEntry(2));

  294.         } catch (MathIllegalArgumentException miae) {
  295.             if (miae.getSpecifier() == LocalizedCoreFormats.SINGULAR_MATRIX) {

  297.                 // handle some special cases for which we can compute a solution
  298.                 final T c12 = c1.getNormSq();
  299.                 final T c1n = c12.sqrt();
  300.                 final T c22 = c2.getNormSq();
  301.                 final T c2n = c22.sqrt();
  302.                 if (c1n.getReal() <= threshold.getReal() && c2n.getReal() <= threshold.getReal()) {
  303.                     // simple special case, velocities are cancelled
  304.                     return new FieldVector3D<>(v12.getField().getZero(), v12.getField().getZero(), v12.getField().getZero());
  305.                 } else if (v1n.getReal() <= threshold.getReal() && c1n.getReal() >= threshold.getReal()) {
  306.                     // this is inconsistent, if v₁ is zero, c₁ must be 0 too
  307.                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE,
  308.                                                            c1n.getReal(), 0, true);
  309.                 } else if (v2n.getReal() <= threshold.getReal() && c2n.getReal() >= threshold.getReal()) {
  310.                     // this is inconsistent, if v₂ is zero, c₂ must be 0 too
  311.                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE,
  312.                                                            c2n.getReal(), 0, true);
  313.                 } else if (v1.crossProduct(v1).getNorm().getReal() <= threshold.getReal() && v12.getReal() > threshold.getReal()) {
  314.                     // simple special case, v₂ is redundant with v₁, we just ignore it
  315.                     // use the simplest Ω: orthogonal to both v₁ and c₁
  316.                     omega = new FieldVector3D<>(v12.reciprocal(), v1.crossProduct(c1));
  317.                 } else {
  318.                     throw miae;
  319.                 }
  320.             } else {
  321.                 throw miae;
  322.             }
  323.         }
  324.         // check results
  325.         final T d1 = FieldVector3D.distance(omega.crossProduct(v1), c1);
  326.         if (d1.getReal() > threshold.getReal()) {
  327.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, 0, true);
  328.         }

  330.         final T d2 = FieldVector3D.distance(omega.crossProduct(v2), c2);
  331.         if (d2.getReal() > threshold.getReal()) {
  332.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, 0, true);
  333.         }

  335.         return omega;

  337.     }

  339.     /** Transform the instance to a {@link FieldRotation}&lt;{@link FieldDerivativeStructure}&gt;.
  340.      * <p>
  341.      * The {@link FieldDerivativeStructure} coordinates correspond to time-derivatives up
  342.      * to the user-specified order.
  343.      * </p>
  344.      * @param order derivation order for the vector components
  345.      * @return rotation with time-derivatives embedded within the coordinates
  346.           * @since 9.2
  347.      */
  348.     public FieldRotation<FieldDerivativeStructure<T>> toDerivativeStructureRotation(final int order) {

  350.         // quaternion components
  351.         final T q0 = rotation.getQ0();
  352.         final T q1 = rotation.getQ1();
  353.         final T q2 = rotation.getQ2();
  354.         final T q3 = rotation.getQ3();

  356.         // first time-derivatives of the quaternion
  357.         final T oX    = rotationRate.getX();
  358.         final T oY    = rotationRate.getY();
  359.         final T oZ    = rotationRate.getZ();
  360.         final T q0Dot = q0.linearCombination(q1.negate(), oX, q2.negate(), oY, q3.negate(), oZ).multiply(0.5);
  361.         final T q1Dot = q0.linearCombination(q0,          oX, q3.negate(), oY, q2,          oZ).multiply(0.5);
  362.         final T q2Dot = q0.linearCombination(q3,          oX, q0,          oY, q1.negate(), oZ).multiply(0.5);
  363.         final T q3Dot = q0.linearCombination(q2.negate(), oX, q1,          oY, q0,          oZ).multiply(0.5);

  365.         // second time-derivatives of the quaternion
  366.         final T oXDot = rotationAcceleration.getX();
  367.         final T oYDot = rotationAcceleration.getY();
  368.         final T oZDot = rotationAcceleration.getZ();
  369.         final T q0DotDot = q0.linearCombination(array6(q1, q2,  q3, q1Dot, q2Dot,  q3Dot),
  370.                                                 array6(oXDot, oYDot, oZDot, oX, oY, oZ)).multiply(-0.5);
  371.         final T q1DotDot = q0.linearCombination(array6(q0, q2, q3.negate(), q0Dot, q2Dot, q3Dot.negate()),
  372.                                                 array6(oXDot, oZDot, oYDot, oX, oZ, oY)).multiply(0.5);
  373.         final T q2DotDot = q0.linearCombination(array6(q0, q3, q1.negate(), q0Dot, q3Dot, q1Dot.negate()),
  374.                                                 array6(oYDot, oXDot, oZDot, oY, oX, oZ)).multiply(0.5);
  375.         final T q3DotDot = q0.linearCombination(array6(q0, q1, q2.negate(), q0Dot, q1Dot, q2Dot.negate()),
  376.                                                 array6(oZDot, oYDot, oXDot, oZ, oY, oX)).multiply(0.5);

  378.         final FDSFactory<T> factory;
  379.         final FieldDerivativeStructure<T> q0DS;
  380.         final FieldDerivativeStructure<T> q1DS;
  381.         final FieldDerivativeStructure<T> q2DS;
  382.         final FieldDerivativeStructure<T> q3DS;
  383.         switch (order) {
  384.             case 0 :
  385.                 factory = new FDSFactory<>(q0.getField(), 1, order);
  386.                 q0DS = factory.build(q0);
  387.                 q1DS = factory.build(q1);
  388.                 q2DS = factory.build(q2);
  389.                 q3DS = factory.build(q3);
  390.                 break;
  391.             case 1 :
  392.                 factory = new FDSFactory<>(q0.getField(), 1, order);
  393.                 q0DS = factory.build(q0, q0Dot);
  394.                 q1DS = factory.build(q1, q1Dot);
  395.                 q2DS = factory.build(q2, q2Dot);
  396.                 q3DS = factory.build(q3, q3Dot);
  397.                 break;
  398.             case 2 :
  399.                 factory = new FDSFactory<>(q0.getField(), 1, order);
  400.                 q0DS = factory.build(q0, q0Dot, q0DotDot);
  401.                 q1DS = factory.build(q1, q1Dot, q1DotDot);
  402.                 q2DS = factory.build(q2, q2Dot, q2DotDot);
  403.                 q3DS = factory.build(q3, q3Dot, q3DotDot);
  404.                 break;
  405.             default :
  406.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  407.         }

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

  411.     }

  413.     /** Transform the instance to a {@link FieldRotation}&lt;{@link UnivariateDerivative1}&gt;.
  414.      * <p>
  415.      * The {@link UnivariateDerivative1} coordinates correspond to time-derivatives up
  416.      * to the order 1.
  417.      * </p>
  418.      * @return rotation with time-derivatives embedded within the coordinates
  419.      */
  420.     public FieldRotation<FieldUnivariateDerivative1<T>> toUnivariateDerivative1Rotation() {

  422.         // quaternion components
  423.         final T q0 = rotation.getQ0();
  424.         final T q1 = rotation.getQ1();
  425.         final T q2 = rotation.getQ2();
  426.         final T q3 = rotation.getQ3();

  428.         // first time-derivatives of the quaternion
  429.         final T oX    = rotationRate.getX();
  430.         final T oY    = rotationRate.getY();
  431.         final T oZ    = rotationRate.getZ();
  432.         final T q0Dot = q0.linearCombination(q1.negate(), oX, q2.negate(), oY, q3.negate(), oZ).multiply(0.5);
  433.         final T q1Dot = q0.linearCombination(q0,          oX, q3.negate(), oY, q2,          oZ).multiply(0.5);
  434.         final T q2Dot = q0.linearCombination(q3,          oX, q0,          oY, q1.negate(), oZ).multiply(0.5);
  435.         final T q3Dot = q0.linearCombination(q2.negate(), oX, q1,          oY, q0,          oZ).multiply(0.5);

  437.         final FieldUnivariateDerivative1<T> q0UD = new FieldUnivariateDerivative1<>(q0, q0Dot);
  438.         final FieldUnivariateDerivative1<T> q1UD = new FieldUnivariateDerivative1<>(q1, q1Dot);
  439.         final FieldUnivariateDerivative1<T> q2UD = new FieldUnivariateDerivative1<>(q2, q2Dot);
  440.         final FieldUnivariateDerivative1<T> q3UD = new FieldUnivariateDerivative1<>(q3, q3Dot);

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

  444.     }

  446.     /** Transform the instance to a {@link FieldRotation}&lt;{@link UnivariateDerivative2}&gt;.
  447.      * <p>
  448.      * The {@link UnivariateDerivative2} coordinates correspond to time-derivatives up
  449.      * to the order 2.
  450.      * </p>
  451.      * @return rotation with time-derivatives embedded within the coordinates
  452.      */
  453.     public FieldRotation<FieldUnivariateDerivative2<T>> toUnivariateDerivative2Rotation() {

  455.         // quaternion components
  456.         final T q0 = rotation.getQ0();
  457.         final T q1 = rotation.getQ1();
  458.         final T q2 = rotation.getQ2();
  459.         final T q3 = rotation.getQ3();

  461.         // first time-derivatives of the quaternion
  462.         final T oX    = rotationRate.getX();
  463.         final T oY    = rotationRate.getY();
  464.         final T oZ    = rotationRate.getZ();
  465.         final T q0Dot = q0.linearCombination(q1.negate(), oX, q2.negate(), oY, q3.negate(), oZ).multiply(0.5);
  466.         final T q1Dot = q0.linearCombination(q0,          oX, q3.negate(), oY, q2,          oZ).multiply(0.5);
  467.         final T q2Dot = q0.linearCombination(q3,          oX, q0,          oY, q1.negate(), oZ).multiply(0.5);
  468.         final T q3Dot = q0.linearCombination(q2.negate(), oX, q1,          oY, q0,          oZ).multiply(0.5);

  470.         // second time-derivatives of the quaternion
  471.         final T oXDot = rotationAcceleration.getX();
  472.         final T oYDot = rotationAcceleration.getY();
  473.         final T oZDot = rotationAcceleration.getZ();
  474.         final T q0DotDot = q0.linearCombination(array6(q1, q2,  q3, q1Dot, q2Dot,  q3Dot),
  475.                                                 array6(oXDot, oYDot, oZDot, oX, oY, oZ)).multiply(-0.5);
  476.         final T q1DotDot = q0.linearCombination(array6(q0, q2, q3.negate(), q0Dot, q2Dot, q3Dot.negate()),
  477.                                                 array6(oXDot, oZDot, oYDot, oX, oZ, oY)).multiply(0.5);
  478.         final T q2DotDot = q0.linearCombination(array6(q0, q3, q1.negate(), q0Dot, q3Dot, q1Dot.negate()),
  479.                                                 array6(oYDot, oXDot, oZDot, oY, oX, oZ)).multiply(0.5);
  480.         final T q3DotDot = q0.linearCombination(array6(q0, q1, q2.negate(), q0Dot, q1Dot, q2Dot.negate()),
  481.                                                 array6(oZDot, oYDot, oXDot, oZ, oY, oX)).multiply(0.5);

  483.         final FieldUnivariateDerivative2<T> q0UD = new FieldUnivariateDerivative2<>(q0, q0Dot, q0DotDot);
  484.         final FieldUnivariateDerivative2<T> q1UD = new FieldUnivariateDerivative2<>(q1, q1Dot, q1DotDot);
  485.         final FieldUnivariateDerivative2<T> q2UD = new FieldUnivariateDerivative2<>(q2, q2Dot, q2DotDot);
  486.         final FieldUnivariateDerivative2<T> q3UD = new FieldUnivariateDerivative2<>(q3, q3Dot, q3DotDot);

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

  490.     }

  492.     /** Build an arry of 6 elements.
  493.      * @param e1 first element
  494.      * @param e2 second element
  495.      * @param e3 third element
  496.      * @param e4 fourth element
  497.      * @param e5 fifth element
  498.      * @param e6 sixth element
  499.      * @return a new array
  500.      * @since 9.2
  501.      */
  502.     private T[] array6(final T e1, final T e2, final T e3, final T e4, final T e5, final T e6) {
  503.         final T[] array = MathArrays.buildArray(e1.getField(), 6);
  504.         array[0] = e1;
  505.         array[1] = e2;
  506.         array[2] = e3;
  507.         array[3] = e4;
  508.         array[4] = e5;
  509.         array[5] = e6;
  510.         return array;
  511.     }

  513.     /** Estimate rotation rate between two orientations.
  514.      * <p>Estimation is based on a simple fixed rate rotation
  515.      * during the time interval between the two orientations.</p>
  516.      * @param start start orientation
  517.      * @param end end orientation
  518.      * @param dt time elapsed between the dates of the two orientations
  519.      * @param <T> the type of the field elements
  520.      * @return rotation rate allowing to go from start to end orientations
  521.      */
  522.     public static <T extends CalculusFieldElement<T>>
  523.         FieldVector3D<T> estimateRate(final FieldRotation<T> start,
  524.                                       final FieldRotation<T> end,
  525.                                       final double dt) {
  526.         return estimateRate(start, end, start.getQ0().getField().getZero().add(dt));
  527.     }

  529.     /** Estimate rotation rate between two orientations.
  530.      * <p>Estimation is based on a simple fixed rate rotation
  531.      * during the time interval between the two orientations.</p>
  532.      * @param start start orientation
  533.      * @param end end orientation
  534.      * @param dt time elapsed between the dates of the two orientations
  535.      * @param <T> the type of the field elements
  536.      * @return rotation rate allowing to go from start to end orientations
  537.      */
  538.     public static <T extends CalculusFieldElement<T>>
  539.         FieldVector3D<T> estimateRate(final FieldRotation<T> start,
  540.                                       final FieldRotation<T> end,
  541.                                       final T dt) {
  542.         final FieldRotation<T> evolution = start.compose(end.revert(), RotationConvention.VECTOR_OPERATOR);
  543.         return new FieldVector3D<>(evolution.getAngle().divide(dt),
  544.                                    evolution.getAxis(RotationConvention.VECTOR_OPERATOR));
  545.     }

  547.     /**
  548.      * Revert a rotation / rotation rate / rotation acceleration triplet.
  549.      *
  550.      * <p> Build a triplet which reverse the effect of another triplet.
  551.      *
  552.      * @return a new triplet whose effect is the reverse of the effect
  553.      * of the instance
  554.      */
  555.     public FieldAngularCoordinates<T> revert() {
  556.         return new FieldAngularCoordinates<>(rotation.revert(),
  557.                                              rotation.applyInverseTo(rotationRate.negate()),
  558.                                              rotation.applyInverseTo(rotationAcceleration.negate()));
  559.     }

  561.     /** Get a time-shifted rotation. Same as {@link #shiftedBy(double)} except
  562.      * only the shifted rotation is computed.
  563.      * <p>
  564.      * The state can be slightly shifted to close dates. This shift is based on
  565.      * an approximate solution of the fixed acceleration motion. It is <em>not</em>
  566.      * intended as a replacement for proper attitude propagation but should be
  567.      * sufficient for either small time shifts or coarse accuracy.
  568.      * </p>
  569.      * @param dt time shift in seconds
  570.      * @return a new state, shifted with respect to the instance (which is immutable)
  571.      * @see  #shiftedBy(CalculusFieldElement)
  572.      * @since 11.2
  573.      */
  574.     public FieldRotation<T> rotationShiftedBy(final T dt) {

  576.         // the shiftedBy method is based on a local approximation.
  577.         // It considers separately the contribution of the constant
  578.         // rotation, the linear contribution or the rate and the
  579.         // quadratic contribution of the acceleration. The rate
  580.         // and acceleration contributions are small rotations as long
  581.         // as the time shift is small, which is the crux of the algorithm.
  582.         // Small rotations are almost commutative, so we append these small
  583.         // contributions one after the other, as if they really occurred
  584.         // successively, despite this is not what really happens.

  586.         // compute the linear contribution first, ignoring acceleration
  587.         // BEWARE: there is really a minus sign here, because if
  588.         // the target frame rotates in one direction, the vectors in the origin
  589.         // frame seem to rotate in the opposite direction
  590.         final T rate = rotationRate.getNorm();
  591.         final FieldRotation<T> rateContribution = (rate.getReal() == 0.0) ?
  592.                 FieldRotation.getIdentity(dt.getField()) :
  593.                 new FieldRotation<>(rotationRate, rate.multiply(dt), RotationConvention.FRAME_TRANSFORM);

  595.         // append rotation and rate contribution
  596.         final FieldRotation<T> linearPart =
  597.                 rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR);

  599.         final T acc  = rotationAcceleration.getNorm();
  600.         if (acc.getReal() == 0.0) {
  601.             // no acceleration, the linear part is sufficient
  602.             return linearPart;
  603.         }

  605.         // compute the quadratic contribution, ignoring initial rotation and rotation rate
  606.         // BEWARE: there is really a minus sign here, because if
  607.         // the target frame rotates in one direction, the vectors in the origin
  608.         // frame seem to rotate in the opposite direction
  609.         final FieldRotation<T> quadraticContribution =
  610.                 new FieldRotation<>(rotationAcceleration,
  611.                         acc.multiply(dt).multiply(dt).multiply(0.5),
  612.                         RotationConvention.FRAME_TRANSFORM);

  614.         // the quadratic contribution is a small rotation:
  615.         // its initial angle and angular rate are both zero.
  616.         // small rotations are almost commutative, so we append the small
  617.         // quadratic part after the linear part as a simple offset
  618.         return quadraticContribution
  619.                 .compose(linearPart, RotationConvention.VECTOR_OPERATOR);

  621.     }

  623.     /** Get a time-shifted state.
  624.      * <p>
  625.      * The state can be slightly shifted to close dates. This shift is based on
  626.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  627.      * proper attitude propagation but should be sufficient for either small
  628.      * time shifts or coarse accuracy.
  629.      * </p>
  630.      * @param dt time shift in seconds
  631.      * @return a new state, shifted with respect to the instance (which is immutable)
  632.      */
  633.     @Override
  634.     public FieldAngularCoordinates<T> shiftedBy(final double dt) {
  635.         return shiftedBy(rotation.getQ0().getField().getZero().add(dt));
  636.     }

  638.     /** Get a time-shifted state.
  639.      * <p>
  640.      * The state can be slightly shifted to close dates. This shift is based on
  641.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  642.      * proper attitude propagation but should be sufficient for either small
  643.      * time shifts or coarse accuracy.
  644.      * </p>
  645.      * @param dt time shift in seconds
  646.      * @return a new state, shifted with respect to the instance (which is immutable)
  647.      */
  648.     @Override
  649.     public FieldAngularCoordinates<T> shiftedBy(final T dt) {

  651.         // the shiftedBy method is based on a local approximation.
  652.         // It considers separately the contribution of the constant
  653.         // rotation, the linear contribution or the rate and the
  654.         // quadratic contribution of the acceleration. The rate
  655.         // and acceleration contributions are small rotations as long
  656.         // as the time shift is small, which is the crux of the algorithm.
  657.         // Small rotations are almost commutative, so we append these small
  658.         // contributions one after the other, as if they really occurred
  659.         // successively, despite this is not what really happens.

  661.         // compute the linear contribution first, ignoring acceleration
  662.         // BEWARE: there is really a minus sign here, because if
  663.         // the target frame rotates in one direction, the vectors in the origin
  664.         // frame seem to rotate in the opposite direction
  665.         final T rate = rotationRate.getNorm();
  666.         final T zero = rate.getField().getZero();
  667.         final T one  = rate.getField().getOne();
  668.         final FieldRotation<T> rateContribution = (rate.getReal() == 0.0) ?
  669.                                                   new FieldRotation<>(one, zero, zero, zero, false) :
  670.                                                   new FieldRotation<>(rotationRate,
  671.                                                                       rate.multiply(dt),
  672.                                                                       RotationConvention.FRAME_TRANSFORM);

  674.         // append rotation and rate contribution
  675.         final FieldAngularCoordinates<T> linearPart =
  676.                 new FieldAngularCoordinates<>(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR),
  677.                                               rotationRate);

  679.         final T acc  = rotationAcceleration.getNorm();
  680.         if (acc.getReal() == 0.0) {
  681.             // no acceleration, the linear part is sufficient
  682.             return linearPart;
  683.         }

  685.         // compute the quadratic contribution, ignoring initial rotation and rotation rate
  686.         // BEWARE: there is really a minus sign here, because if
  687.         // the target frame rotates in one direction, the vectors in the origin
  688.         // frame seem to rotate in the opposite direction
  689.         final FieldAngularCoordinates<T> quadraticContribution =
  690.                 new FieldAngularCoordinates<>(new FieldRotation<>(rotationAcceleration,
  691.                                                                   acc.multiply(dt.multiply(0.5).multiply(dt)),
  692.                                                                   RotationConvention.FRAME_TRANSFORM),
  693.                                               new FieldVector3D<>(dt, rotationAcceleration),
  694.                                               rotationAcceleration);

  696.         // the quadratic contribution is a small rotation:
  697.         // its initial angle and angular rate are both zero.
  698.         // small rotations are almost commutative, so we append the small
  699.         // quadratic part after the linear part as a simple offset
  700.         return quadraticContribution.addOffset(linearPart);

  702.     }

  704.     /** Get the rotation.
  705.      * @return the rotation.
  706.      */
  707.     public FieldRotation<T> getRotation() {
  708.         return rotation;
  709.     }

  711.     /** Get the rotation rate.
  712.      * @return the rotation rate vector (rad/s).
  713.      */
  714.     public FieldVector3D<T> getRotationRate() {
  715.         return rotationRate;
  716.     }

  718.     /** Get the rotation acceleration.
  719.      * @return the rotation acceleration vector dΩ/dt (rad/s²).
  720.      */
  721.     public FieldVector3D<T> getRotationAcceleration() {
  722.         return rotationAcceleration;
  723.     }

  725.     /** Add an offset from the instance.
  726.      * <p>
  727.      * We consider here that the offset rotation is applied first and the
  728.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  729.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  730.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  731.      * </p>
  732.      * <p>
  733.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  734.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  735.      * so that round trip applications are possible. This means that both {@code
  736.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  737.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  738.      * </p>
  739.      * @param offset offset to subtract
  740.      * @return new instance, with offset subtracted
  741.      * @see #subtractOffset(FieldAngularCoordinates)
  742.      */
  743.     public FieldAngularCoordinates<T> addOffset(final FieldAngularCoordinates<T> offset) {
  744.         final FieldVector3D<T> rOmega    = rotation.applyTo(offset.rotationRate);
  745.         final FieldVector3D<T> rOmegaDot = rotation.applyTo(offset.rotationAcceleration);
  746.         return new FieldAngularCoordinates<>(rotation.compose(offset.rotation, RotationConvention.VECTOR_OPERATOR),
  747.                                              rotationRate.add(rOmega),
  748.                                              new FieldVector3D<>( 1.0, rotationAcceleration,
  749.                                                                   1.0, rOmegaDot,
  750.                                                                  -1.0, FieldVector3D.crossProduct(rotationRate, rOmega)));
  751.     }

  753.     /** Subtract an offset from the instance.
  754.      * <p>
  755.      * We consider here that the offset Rotation is applied first and the
  756.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  757.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  758.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  759.      * </p>
  760.      * <p>
  761.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  762.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  763.      * so that round trip applications are possible. This means that both {@code
  764.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  765.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  766.      * </p>
  767.      * @param offset offset to subtract
  768.      * @return new instance, with offset subtracted
  769.      * @see #addOffset(FieldAngularCoordinates)
  770.      */
  771.     public FieldAngularCoordinates<T> subtractOffset(final FieldAngularCoordinates<T> offset) {
  772.         return addOffset(offset.revert());
  773.     }

  775.     /** Convert to a regular angular coordinates.
  776.      * @return a regular angular coordinates
  777.      */
  778.     public AngularCoordinates toAngularCoordinates() {
  779.         return new AngularCoordinates(rotation.toRotation(), rotationRate.toVector3D(),
  780.                                       rotationAcceleration.toVector3D());
  781.     }

  783.     /** Apply the rotation to a pv coordinates.
  784.      * @param pv vector to apply the rotation to
  785.      * @return a new pv coordinates which is the image of pv by the rotation
  786.      */
  787.     public FieldPVCoordinates<T> applyTo(final PVCoordinates pv) {

  789.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  790.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  791.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  792.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  793.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  794.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  795.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  796.                                                                   -2, crossV,
  797.                                                                   -1, crossCrossP,
  798.                                                                   -1, crossDotP);

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

  802.     }

  804.     /** Apply the rotation to a pv coordinates.
  805.      * @param pv vector to apply the rotation to
  806.      * @return a new pv coordinates which is the image of pv by the rotation
  807.      */
  808.     public TimeStampedFieldPVCoordinates<T> applyTo(final TimeStampedPVCoordinates pv) {

  810.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  811.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  812.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  813.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  814.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  815.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  816.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  817.                                                                   -2, crossV,
  818.                                                                   -1, crossCrossP,
  819.                                                                   -1, crossDotP);

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

  823.     }

  825.     /** Apply the rotation to a pv coordinates.
  826.      * @param pv vector to apply the rotation to
  827.      * @return a new pv coordinates which is the image of pv by the rotation
  828.      * @since 9.0
  829.      */
  830.     public FieldPVCoordinates<T> applyTo(final FieldPVCoordinates<T> pv) {

  832.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  833.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  834.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  835.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  836.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  837.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  838.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  839.                                                                   -2, crossV,
  840.                                                                   -1, crossCrossP,
  841.                                                                   -1, crossDotP);

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

  845.     }

  847.     /** Apply the rotation to a pv coordinates.
  848.      * @param pv vector to apply the rotation to
  849.      * @return a new pv coordinates which is the image of pv by the rotation
  850.      * @since 9.0
  851.      */
  852.     public TimeStampedFieldPVCoordinates<T> applyTo(final TimeStampedFieldPVCoordinates<T> pv) {

  854.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  855.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  856.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  857.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  858.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  859.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  860.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  861.                                                                   -2, crossV,
  862.                                                                   -1, crossCrossP,
  863.                                                                   -1, crossDotP);

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

  867.     }

  869.     /** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives.
  870.      * <p>
  871.      * The modified Rodrigues vector is tan(θ/4) u where θ and u are the
  872.      * rotation angle and axis respectively.
  873.      * </p>
  874.      * @param sign multiplicative sign for quaternion components
  875.      * @return modified Rodrigues vector and derivatives (vector on row 0, first derivative
  876.      * on row 1, second derivative on row 2)
  877.      * @see #createFromModifiedRodrigues(CalculusFieldElement[][])
  878.      * @since 9.0
  879.      */
  880.     public T[][] getModifiedRodrigues(final double sign) {

  882.         final T q0    = getRotation().getQ0().multiply(sign);
  883.         final T q1    = getRotation().getQ1().multiply(sign);
  884.         final T q2    = getRotation().getQ2().multiply(sign);
  885.         final T q3    = getRotation().getQ3().multiply(sign);
  886.         final T oX    = getRotationRate().getX();
  887.         final T oY    = getRotationRate().getY();
  888.         final T oZ    = getRotationRate().getZ();
  889.         final T oXDot = getRotationAcceleration().getX();
  890.         final T oYDot = getRotationAcceleration().getY();
  891.         final T oZDot = getRotationAcceleration().getZ();

  893.         // first time-derivatives of the quaternion
  894.         final T q0Dot = q0.linearCombination(q1.negate(), oX, q2.negate(), oY, q3.negate(), oZ).multiply(0.5);
  895.         final T q1Dot = q0.linearCombination( q0, oX, q3.negate(), oY,  q2, oZ).multiply(0.5);
  896.         final T q2Dot = q0.linearCombination( q3, oX,  q0, oY, q1.negate(), oZ).multiply(0.5);
  897.         final T q3Dot = q0.linearCombination(q2.negate(), oX,  q1, oY,  q0, oZ).multiply(0.5);

  899.         // second time-derivatives of the quaternion
  900.         final T q0DotDot = linearCombination(q1, oXDot, q2, oYDot, q3, oZDot,
  901.                                              q1Dot, oX, q2Dot, oY, q3Dot, oZ).
  902.                            multiply(-0.5);
  903.         final T q1DotDot = linearCombination(q0, oXDot, q2, oZDot, q3.negate(), oYDot,
  904.                                              q0Dot, oX, q2Dot, oZ, q3Dot.negate(), oY).
  905.                            multiply(0.5);
  906.         final T q2DotDot = linearCombination(q0, oYDot, q3, oXDot, q1.negate(), oZDot,
  907.                                              q0Dot, oY, q3Dot, oX, q1Dot.negate(), oZ).
  908.                            multiply(0.5);
  909.         final T q3DotDot = linearCombination(q0, oZDot, q1, oYDot, q2.negate(), oXDot,
  910.                                              q0Dot, oZ, q1Dot, oY, q2Dot.negate(), oX).
  911.                            multiply(0.5);

  913.         // the modified Rodrigues is tan(θ/4) u where θ and u are the rotation angle and axis respectively
  914.         // this can be rewritten using quaternion components:
  915.         //      r (q₁ / (1+q₀), q₂ / (1+q₀), q₃ / (1+q₀))
  916.         // applying the derivation chain rule to previous expression gives rDot and rDotDot
  917.         final T inv          = q0.add(1).reciprocal();
  918.         final T mTwoInvQ0Dot = inv.multiply(q0Dot).multiply(-2);

  920.         final T r1       = inv.multiply(q1);
  921.         final T r2       = inv.multiply(q2);
  922.         final T r3       = inv.multiply(q3);

  924.         final T mInvR1   = inv.multiply(r1).negate();
  925.         final T mInvR2   = inv.multiply(r2).negate();
  926.         final T mInvR3   = inv.multiply(r3).negate();

  928.         final T r1Dot    = q0.linearCombination(inv, q1Dot, mInvR1, q0Dot);
  929.         final T r2Dot    = q0.linearCombination(inv, q2Dot, mInvR2, q0Dot);
  930.         final T r3Dot    = q0.linearCombination(inv, q3Dot, mInvR3, q0Dot);

  932.         final T r1DotDot = q0.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot);
  933.         final T r2DotDot = q0.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot);
  934.         final T r3DotDot = q0.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot);

  936.         final T[][] rodrigues = MathArrays.buildArray(q0.getField(), 3, 3);
  937.         rodrigues[0][0] = r1;
  938.         rodrigues[0][1] = r2;
  939.         rodrigues[0][2] = r3;
  940.         rodrigues[1][0] = r1Dot;
  941.         rodrigues[1][1] = r2Dot;
  942.         rodrigues[1][2] = r3Dot;
  943.         rodrigues[2][0] = r1DotDot;
  944.         rodrigues[2][1] = r2DotDot;
  945.         rodrigues[2][2] = r3DotDot;
  946.         return rodrigues;

  948.     }

  950.     /**
  951.      * Compute a linear combination.
  952.      * @param a1 first factor of the first term
  953.      * @param b1 second factor of the first term
  954.      * @param a2 first factor of the second term
  955.      * @param b2 second factor of the second term
  956.      * @param a3 first factor of the third term
  957.      * @param b3 second factor of the third term
  958.      * @param a4 first factor of the fourth term
  959.      * @param b4 second factor of the fourth term
  960.      * @param a5 first factor of the fifth term
  961.      * @param b5 second factor of the fifth term
  962.      * @param a6 first factor of the sixth term
  963.      * @param b6 second factor of the sicth term
  964.      * @return a<sub>1</sub>&times;b<sub>1</sub> + a<sub>2</sub>&times;b<sub>2</sub> +
  965.      * a<sub>3</sub>&times;b<sub>3</sub> + a<sub>4</sub>&times;b<sub>4</sub> +
  966.      * a<sub>5</sub>&times;b<sub>5</sub> + a<sub>6</sub>&times;b<sub>6</sub>
  967.      */
  968.     private T linearCombination(final T a1, final T b1, final T a2, final T b2, final T a3, final T b3,
  969.                                 final T a4, final T b4, final T a5, final T b5, final T a6, final T b6) {

  971.         final T[] a = MathArrays.buildArray(a1.getField(), 6);
  972.         a[0] = a1;
  973.         a[1] = a2;
  974.         a[2] = a3;
  975.         a[3] = a4;
  976.         a[4] = a5;
  977.         a[5] = a6;

  979.         final T[] b = MathArrays.buildArray(b1.getField(), 6);
  980.         b[0] = b1;
  981.         b[1] = b2;
  982.         b[2] = b3;
  983.         b[3] = b4;
  984.         b[4] = b5;
  985.         b[5] = b6;

  987.         return a1.linearCombination(a, b);

  989.     }

  991.     /** Convert a modified Rodrigues vector and derivatives to angular coordinates.
  992.      * @param r modified Rodrigues vector (with first and second times derivatives)
  993.      * @param <T> the type of the field elements
  994.      * @return angular coordinates
  995.      * @see #getModifiedRodrigues(double)
  996.      * @since 9.0
  997.      */
  998.     public static <T extends CalculusFieldElement<T>>  FieldAngularCoordinates<T> createFromModifiedRodrigues(final T[][] r) {

  1000.         // rotation
  1001.         final T rSquared = r[0][0].multiply(r[0][0]).add(r[0][1].multiply(r[0][1])).add(r[0][2].multiply(r[0][2]));
  1002.         final T oPQ0     = rSquared.add(1).reciprocal().multiply(2);
  1003.         final T q0       = oPQ0.subtract(1);
  1004.         final T q1       = oPQ0.multiply(r[0][0]);
  1005.         final T q2       = oPQ0.multiply(r[0][1]);
  1006.         final T q3       = oPQ0.multiply(r[0][2]);

  1008.         // rotation rate
  1009.         final T oPQ02    = oPQ0.multiply(oPQ0);
  1010.         final T q0Dot    = oPQ02.multiply(q0.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1],  r[0][2], r[1][2])).negate();
  1011.         final T q1Dot    = oPQ0.multiply(r[1][0]).add(r[0][0].multiply(q0Dot));
  1012.         final T q2Dot    = oPQ0.multiply(r[1][1]).add(r[0][1].multiply(q0Dot));
  1013.         final T q3Dot    = oPQ0.multiply(r[1][2]).add(r[0][2].multiply(q0Dot));
  1014.         final T oX       = q0.linearCombination(q1.negate(), q0Dot,  q0, q1Dot,  q3, q2Dot, q2.negate(), q3Dot).multiply(2);
  1015.         final T oY       = q0.linearCombination(q2.negate(), q0Dot, q3.negate(), q1Dot,  q0, q2Dot,  q1, q3Dot).multiply(2);
  1016.         final T oZ       = q0.linearCombination(q3.negate(), q0Dot,  q2, q1Dot, q1.negate(), q2Dot,  q0, q3Dot).multiply(2);

  1018.         // rotation acceleration
  1019.         final T q0DotDot = q0.subtract(1).negate().divide(oPQ0).multiply(q0Dot).multiply(q0Dot).
  1020.                            subtract(oPQ02.multiply(q0.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]))).
  1021.                            subtract(q1Dot.multiply(q1Dot).add(q2Dot.multiply(q2Dot)).add(q3Dot.multiply(q3Dot)));
  1022.         final T q1DotDot = q0.linearCombination(oPQ0, r[2][0], r[1][0].add(r[1][0]), q0Dot, r[0][0], q0DotDot);
  1023.         final T q2DotDot = q0.linearCombination(oPQ0, r[2][1], r[1][1].add(r[1][1]), q0Dot, r[0][1], q0DotDot);
  1024.         final T q3DotDot = q0.linearCombination(oPQ0, r[2][2], r[1][2].add(r[1][2]), q0Dot, r[0][2], q0DotDot);
  1025.         final T oXDot    = q0.linearCombination(q1.negate(), q0DotDot,  q0, q1DotDot,  q3, q2DotDot, q2.negate(), q3DotDot).multiply(2);
  1026.         final T oYDot    = q0.linearCombination(q2.negate(), q0DotDot, q3.negate(), q1DotDot,  q0, q2DotDot,  q1, q3DotDot).multiply(2);
  1027.         final T oZDot    = q0.linearCombination(q3.negate(), q0DotDot,  q2, q1DotDot, q1.negate(), q2DotDot,  q0, q3DotDot).multiply(2);

  1029.         return new FieldAngularCoordinates<>(new FieldRotation<>(q0, q1, q2, q3, false),
  1030.                                              new FieldVector3D<>(oX, oY, oZ),
  1031.                                              new FieldVector3D<>(oXDot, oYDot, oZDot));

  1033.     }

  1035. }
Created with JaCoCo 0.8.11.202310140853