TimeStampedFieldAngularCoordinates.java

  1. /* Copyright 2002-2016 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.utils;

  19. import java.util.Collection;

  21. import org.apache.commons.math3.Field;
  22. import org.apache.commons.math3.RealFieldElement;
  23. import org.apache.commons.math3.analysis.interpolation.FieldHermiteInterpolator;
  24. import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
  25. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  26. import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
  27. import org.apache.commons.math3.util.FastMath;
  28. import org.apache.commons.math3.util.MathArrays;
  29. import org.orekit.errors.OrekitException;
  30. import org.orekit.errors.OrekitInternalError;
  31. import org.orekit.errors.OrekitMessages;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.time.TimeStamped;

  35. /** {@link TimeStamped time-stamped} version of {@link FieldAngularCoordinates}.
  36.  * <p>Instances of this class are guaranteed to be immutable.</p>
  37.  * @param <T> the type of the field elements
  38.  * @author Luc Maisonobe
  39.  * @since 7.0
  40.  */
  41. public class TimeStampedFieldAngularCoordinates<T extends RealFieldElement<T>>
  42.     extends FieldAngularCoordinates<T> implements TimeStamped {

  44.     /** Serializable UID. */
  45.     private static final long serialVersionUID = 20140723L;

  47.     /** The date. */
  48.     private final AbsoluteDate date;

  50.     /** Builds a rotation/rotation rate pair.
  51.      * @param date coordinates date
  52.      * @param rotation rotation
  53.      * @param rotationRate FieldRotation<T> rate Ω (rad/s)
  54.      * @param rotationAcceleration FieldRotation<T> acceleration dΩ/dt (rad²/s²)
  55.      */
  56.     public TimeStampedFieldAngularCoordinates(final AbsoluteDate date,
  57.                                               final FieldRotation<T> rotation,
  58.                                               final FieldVector3D<T> rotationRate,
  59.                                               final FieldVector3D<T> rotationAcceleration) {
  60.         super(rotation, rotationRate, rotationAcceleration);
  61.         this.date = date;
  62.     }

  64.     /** {@inheritDoc} */
  65.     public AbsoluteDate getDate() {
  66.         return date;
  67.     }

  69.     /** Revert a rotation/rotation rate pair.
  70.      * Build a pair which reverse the effect of another pair.
  71.      * @return a new pair whose effect is the reverse of the effect
  72.      * of the instance
  73.      */
  74.     public TimeStampedFieldAngularCoordinates<T> revert() {
  75.         return new TimeStampedFieldAngularCoordinates<T>(date,
  76.                                                          getRotation().revert(),
  77.                                                          getRotation().applyInverseTo(getRotationRate().negate()),
  78.                                                          getRotation().applyInverseTo(getRotationAcceleration().negate()));
  79.     }

  81.     /** Get a time-shifted state.
  82.      * <p>
  83.      * The state can be slightly shifted to close dates. This shift is based on
  84.      * a simple linear model. It is <em>not</em> intended as a replacement for
  85.      * proper attitude propagation but should be sufficient for either small
  86.      * time shifts or coarse accuracy.
  87.      * </p>
  88.      * @param dt time shift in seconds
  89.      * @return a new state, shifted with respect to the instance (which is immutable)
  90.      */
  91.     public TimeStampedFieldAngularCoordinates<T> shiftedBy(final double dt) {
  92.         final FieldAngularCoordinates<T> sac = super.shiftedBy(dt);
  93.         return new TimeStampedFieldAngularCoordinates<T>(date.shiftedBy(dt),
  94.                                                          sac.getRotation(), sac.getRotationRate(), sac.getRotationAcceleration());

  96.     }

  98.     /** Add an offset from the instance.
  99.      * <p>
  100.      * We consider here that the offset FieldRotation<T> is applied first and the
  101.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  102.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  103.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  104.      * </p>
  105.      * <p>
  106.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  107.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  108.      * so that round trip applications are possible. This means that both {@code
  109.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  110.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  111.      * </p>
  112.      * @param offset offset to subtract
  113.      * @return new instance, with offset subtracted
  114.      * @see #subtractOffset(FieldAngularCoordinates)
  115.      */
  116.     public TimeStampedFieldAngularCoordinates<T> addOffset(final FieldAngularCoordinates<T> offset) {
  117.         final FieldVector3D<T> rOmega    = getRotation().applyTo(offset.getRotationRate());
  118.         final FieldVector3D<T> rOmegaDot = getRotation().applyTo(offset.getRotationAcceleration());
  119.         return new TimeStampedFieldAngularCoordinates<T>(date,
  120.                                                          getRotation().compose(offset.getRotation(), RotationConvention.VECTOR_OPERATOR),
  121.                                                          getRotationRate().add(rOmega),
  122.                                                          new FieldVector3D<T>( 1.0, getRotationAcceleration(),
  123.                                                                                1.0, rOmegaDot,
  124.                                                                               -1.0, FieldVector3D.crossProduct(getRotationRate(), rOmega)));
  125.     }

  127.     /** Subtract an offset from the instance.
  128.      * <p>
  129.      * We consider here that the offset Rotation is applied first and the
  130.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  131.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  132.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  133.      * </p>
  134.      * <p>
  135.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  136.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  137.      * so that round trip applications are possible. This means that both {@code
  138.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  139.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  140.      * </p>
  141.      * @param offset offset to subtract
  142.      * @return new instance, with offset subtracted
  143.      * @see #addOffset(FieldAngularCoordinates)
  144.      */
  145.     public TimeStampedFieldAngularCoordinates<T> subtractOffset(final FieldAngularCoordinates<T> offset) {
  146.         return addOffset(offset.revert());
  147.     }

  149.     /** Interpolate angular coordinates.
  150.      * <p>
  151.      * The interpolated instance is created by polynomial Hermite interpolation
  152.      * on Rodrigues vector ensuring FieldRotation<T> rate remains the exact derivative of FieldRotation<T>.
  153.      * </p>
  154.      * <p>
  155.      * This method is based on Sergei Tanygin's paper <a
  156.      * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
  157.      * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
  158.      * vector as described in Malcolm D. Shuster's paper <a
  159.      * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
  160.      * Survey of Attitude Representations</a>. This change avoids the singularity at π.
  161.      * There is still a singularity at 2π, which is handled by slightly offsetting all FieldRotation<T>s
  162.      * when this singularity is detected.
  163.      * </p>
  164.      * <p>
  165.      * Note that even if first time derivatives (FieldRotation<T> rates)
  166.      * from sample can be ignored, the interpolated instance always includes
  167.      * interpolated derivatives. This feature can be used explicitly to
  168.      * compute these derivatives when it would be too complex to compute them
  169.      * from an analytical formula: just compute a few sample points from the
  170.      * explicit formula and set the derivatives to zero in these sample points,
  171.      * then use interpolation to add derivatives consistent with the FieldRotation<T>s.
  172.      * </p>
  173.      * @param date interpolation date
  174.      * @param filter filter for derivatives from the sample to use in interpolation
  175.      * @param sample sample points on which interpolation should be done
  176.      * @param <T> the type of the field elements
  177.      * @return a new position-velocity, interpolated at specified date
  178.      * @exception OrekitException if the number of point is too small for interpolating
  179.      */
  180.     @SuppressWarnings("unchecked")
  181.     public static <T extends RealFieldElement<T>>
  182.         TimeStampedFieldAngularCoordinates<T> interpolate(final AbsoluteDate date,
  183.                                                           final AngularDerivativesFilter filter,
  184.                                                           final Collection<TimeStampedFieldAngularCoordinates<T>> sample)
  185.         throws OrekitException {

  187.         // get field properties
  188.         final Field<T> field = sample.iterator().next().getRotation().getQ0().getField();
  189.         final T zero = field.getZero();
  190.         final T one  = field.getOne();

  192.         // set up safety elements for 2π singularity avoidance
  193.         final double epsilon   = 2 * FastMath.PI / sample.size();
  194.         final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));

  196.         // set up a linear model canceling mean rotation rate
  197.         final FieldVector3D<T> meanRate;
  198.         if (filter != AngularDerivativesFilter.USE_R) {
  199.             FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero);
  200.             for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) {
  201.                 sum = sum.add(datedAC.getRotationRate());
  202.             }
  203.             meanRate = new FieldVector3D<T>(1.0 / sample.size(), sum);
  204.         } else {
  205.             if (sample.size() < 2) {
  206.                 throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION,
  207.                                           sample.size());
  208.             }
  209.             FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero);
  210.             TimeStampedFieldAngularCoordinates<T> previous = null;
  211.             for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) {
  212.                 if (previous != null) {
  213.                     sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(),
  214.                                                datedAC.date.durationFrom(previous.getDate())));
  215.                 }
  216.                 previous = datedAC;
  217.             }
  218.             meanRate = new FieldVector3D<T>(1.0 / (sample.size() - 1), sum);
  219.         }
  220.         TimeStampedFieldAngularCoordinates<T> offset =
  221.                 new TimeStampedFieldAngularCoordinates<T>(date, new FieldRotation<T>(one, zero, zero, zero, false),
  222.                                                           meanRate, new FieldVector3D<T>(zero, zero, zero));

  224.         boolean restart = true;
  225.         for (int i = 0; restart && i < sample.size() + 2; ++i) {

  227.             // offset adaptation parameters
  228.             restart = false;

  230.             // set up an interpolator taking derivatives into account
  231.             final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<T>();

  233.             // add sample points
  234.             final double[] previous = new double[] {
  235.                 1.0, 0.0, 0.0, 0.0
  236.             };

  238.             for (final TimeStampedFieldAngularCoordinates<T> ac : sample) {

  240.                 // remove linear offset from the current coordinates
  241.                 final T dt = zero.add(ac.date.durationFrom(date));
  242.                 final TimeStampedFieldAngularCoordinates<T> fixed = ac.subtractOffset(offset.shiftedBy(dt.getReal()));

  244.                 final T[][] rodrigues = getModifiedRodrigues(fixed, previous, threshold);
  245.                 if (rodrigues == null) {
  246.                     // the sample point is close to a modified Rodrigues vector singularity
  247.                     // we need to change the linear offset model to avoid this
  248.                     restart = true;
  249.                     break;
  250.                 }
  251.                 switch (filter) {
  252.                     case USE_RRA:
  253.                         // populate sample with rotation, rotation rate and acceleration data
  254.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
  255.                         break;
  256.                     case USE_RR:
  257.                         // populate sample with rotation and rotation rate data
  258.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
  259.                         break;
  260.                     case USE_R:
  261.                         // populate sample with rotation data only
  262.                         interpolator.addSamplePoint(dt, rodrigues[0]);
  263.                         break;
  264.                     default :
  265.                         // this should never happen
  266.                         throw new OrekitInternalError(null);
  267.                 }
  268.             }

  270.             if (restart) {
  271.                 // interpolation failed, some intermediate rotation was too close to 2π
  272.                 // we need to offset all rotations to avoid the singularity
  273.                 offset = offset.addOffset(new FieldAngularCoordinates<T>(new FieldRotation<T>(new FieldVector3D<T>(one, zero, zero),
  274.                                                                                               zero.add(epsilon),
  275.                                                                                               RotationConvention.VECTOR_OPERATOR),
  276.                                                                          new FieldVector3D<T>(zero, zero, zero),
  277.                                                                          new FieldVector3D<T>(zero, zero, zero)));
  278.             } else {
  279.                 // interpolation succeeded with the current offset
  280.                 final T[][] p = interpolator.derivatives(field.getZero(), 2);
  281.                 return createFromModifiedRodrigues(p, offset);
  282.             }

  284.         }

  286.         // this should never happen
  287.         throw new OrekitInternalError(null);

  289.     }

  291.     /** Create a 6 elements array.
  292.      * @param field field to which coordinates belong
  293.      * @param a0 first element
  294.      * @param a1 second element
  295.      * @param a2 third element
  296.      * @param a3 fourth element
  297.      * @param a4 fifth element
  298.      * @param a5 sixth element
  299.      * @param <T> the type of the field elements
  300.      * @return array containing a0, a1, a2, a3, a4, a5
  301.      */
  302.     private static <T extends RealFieldElement<T>> T[] array6(final Field<T> field,
  303.                                                               final T a0, final T a1, final T a2,
  304.                                                               final T a3, final T a4, final T a5) {
  305.         final T[] array = MathArrays.buildArray(field, 6);
  306.         array[0] = a0;
  307.         array[1] = a1;
  308.         array[2] = a2;
  309.         array[3] = a3;
  310.         array[4] = a4;
  311.         array[5] = a5;
  312.         return array;
  313.     }

  315.     /** Create a 3x3 matrix.
  316.      * @param field field to which coordinates belong
  317.      * @param a00 first element, first row
  318.      * @param a01 second element, first row
  319.      * @param a02 third element, first row
  320.      * @param a10 first element, second row
  321.      * @param a11 second element, second row
  322.      * @param a12 third element, second row
  323.      * @param a20 first element, third row
  324.      * @param a21 second element, third row
  325.      * @param a22 third element, third row
  326.      * @param <T> the type of the field elements
  327.      * @return array containing a0, a1, a2
  328.      */
  329.     private static <T extends RealFieldElement<T>> T[][] matrix33(final Field<T> field,
  330.                                                                   final T a00, final T a01, final T a02,
  331.                                                                   final T a10, final T a11, final T a12,
  332.                                                                   final T a20, final T a21, final T a22) {
  333.         final T[][] matrix = MathArrays.buildArray(field, 3, 3);
  334.         matrix[0][0] = a00;
  335.         matrix[0][1] = a01;
  336.         matrix[0][2] = a02;
  337.         matrix[1][0] = a10;
  338.         matrix[1][1] = a11;
  339.         matrix[1][2] = a12;
  340.         matrix[2][0] = a20;
  341.         matrix[2][1] = a21;
  342.         matrix[2][2] = a22;
  343.         return matrix;
  344.     }

  346.     /** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives.
  347.      * <p>
  348.      * The modified Rodrigues vector is tan(θ/4) u where θ and u are the
  349.      * rotation angle and axis respectively.
  350.      * </p>
  351.      * @param fixed coordinates to convert, with offset already fixed
  352.      * @param previous previous quaternion used
  353.      * @param threshold threshold for rotations too close to 2π
  354.      * @param <T> the type of the field elements
  355.      * @return modified Rodrigues vector and derivative, or null if rotation is too close to 2π
  356.      */
  357.     private static <T extends RealFieldElement<T>> T[][] getModifiedRodrigues(final TimeStampedFieldAngularCoordinates<T> fixed,
  358.                                                                               final double[] previous, final double threshold) {

  360.         // make sure all interpolated points will be on the same branch
  361.         T q0 = fixed.getRotation().getQ0();
  362.         T q1 = fixed.getRotation().getQ1();
  363.         T q2 = fixed.getRotation().getQ2();
  364.         T q3 = fixed.getRotation().getQ3();
  365.         if (MathArrays.linearCombination(q0.getReal(), previous[0],
  366.                                          q1.getReal(), previous[1],
  367.                                          q2.getReal(), previous[2],
  368.                                          q3.getReal(), previous[3]) < 0) {
  369.             q0 = q0.negate();
  370.             q1 = q1.negate();
  371.             q2 = q2.negate();
  372.             q3 = q3.negate();
  373.         }
  374.         previous[0] = q0.getReal();
  375.         previous[1] = q1.getReal();
  376.         previous[2] = q2.getReal();
  377.         previous[3] = q3.getReal();

  379.         // check modified Rodrigues vector singularity
  380.         if (q0.getReal() < threshold) {
  381.             // this is an intermediate point that happens to be 2PI away from reference
  382.             // we need to change the linear offset model to avoid this point
  383.             return null;
  384.         }

  386.         final Field<T> field = q0.getField();

  388.         final T oX    = fixed.getRotationRate().getX();
  389.         final T oY    = fixed.getRotationRate().getY();
  390.         final T oZ    = fixed.getRotationRate().getZ();
  391.         final T oXDot = fixed.getRotationAcceleration().getX();
  392.         final T oYDot = fixed.getRotationAcceleration().getY();
  393.         final T oZDot = fixed.getRotationAcceleration().getZ();

  395.         // first time-derivatives of the quaternion
  396.         final T q0Dot = q0.linearCombination(q1.negate(), oX, q2.negate(), oY, q3.negate(), oZ).multiply(0.5);
  397.         final T q1Dot = q1.linearCombination(q0,          oX, q3.negate(), oY, q2,          oZ).multiply(0.5);
  398.         final T q2Dot = q2.linearCombination(q3,          oX, q0,          oY, q1.negate(), oZ).multiply(0.5);
  399.         final T q3Dot = q3.linearCombination(q2.negate(), oX, q1,          oY, q0,          oZ).multiply(0.5);

  401.         // second time-derivatives of the quaternion
  402.         final T q0DotDot = q0.linearCombination(array6(field, q1, q2,  q3, q1Dot, q2Dot,  q3Dot),
  403.                                                 array6(field, oXDot, oYDot, oZDot, oX, oY, oZ)).multiply(-0.5);
  404.         final T q1DotDot = q1.linearCombination(array6(field, q0, q2, q3.negate(), q0Dot, q2Dot, q3Dot.negate()),
  405.                                                 array6(field, oXDot, oZDot, oYDot, oX, oZ, oY)).multiply(0.5);
  406.         final T q2DotDot = q2.linearCombination(array6(field, q0, q3, q1.negate(), q0Dot, q3Dot, q1Dot.negate()),
  407.                                                 array6(field, oYDot, oXDot, oZDot, oY, oX, oZ)).multiply(0.5);
  408.         final T q3DotDot = q3.linearCombination(array6(field, q0, q1, q2.negate(), q0Dot, q1Dot, q2Dot.negate()),
  409.                                                 array6(field, oZDot, oYDot, oXDot, oZ, oY, oX)).multiply(0.5);

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

  418.         final T r1       = inv.multiply(q1);
  419.         final T r2       = inv.multiply(q2);
  420.         final T r3       = inv.multiply(q3);

  422.         final T mInvR1   = inv.multiply(r1).negate();
  423.         final T mInvR2   = inv.multiply(r2).negate();
  424.         final T mInvR3   = inv.multiply(r3).negate();

  426.         final T r1Dot    = r1.linearCombination(inv, q1Dot, mInvR1, q0Dot);
  427.         final T r2Dot    = r2.linearCombination(inv, q2Dot, mInvR2, q0Dot);
  428.         final T r3Dot    = r3.linearCombination(inv, q3Dot, mInvR3, q0Dot);

  430.         final T r1DotDot = r1.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot);
  431.         final T r2DotDot = r2.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot);
  432.         final T r3DotDot = r3.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot);

  434.         return matrix33(field,
  435.                         r1,       r2,       r3,
  436.                         r1Dot,    r2Dot,    r3Dot,
  437.                         r1DotDot, r2DotDot, r3DotDot);

  439.     }

  441.     /** Convert a modified Rodrigues vector and derivatives to angular coordinates.
  442.      * @param r modified Rodrigues vector (with first derivatives)
  443.      * @param offset linear offset model to add (its date must be consistent with the modified Rodrigues vector)
  444.      * @param <T> the type of the field elements
  445.      * @return angular coordinates
  446.      */
  447.     private static <T extends RealFieldElement<T>>
  448.         TimeStampedFieldAngularCoordinates<T> createFromModifiedRodrigues(final T[][] r,
  449.                                                                           final TimeStampedFieldAngularCoordinates<T> offset) {

  451.         // rotation
  452.         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]));
  453.         final T oPQ0     = rSquared.add(1).reciprocal().multiply(2);
  454.         final T q0       = oPQ0.subtract(1);
  455.         final T q1       = oPQ0.multiply(r[0][0]);
  456.         final T q2       = oPQ0.multiply(r[0][1]);
  457.         final T q3       = oPQ0.multiply(r[0][2]);

  459.         // rotation rate
  460.         final T oPQ02    = oPQ0.multiply(oPQ0);
  461.         final T q0Dot    = oPQ02.negate().multiply(q0.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1],  r[0][2], r[1][2]));
  462.         final T q1Dot    = oPQ0.multiply(r[1][0]).add(r[0][0].multiply(q0Dot));
  463.         final T q2Dot    = oPQ0.multiply(r[1][1]).add(r[0][1].multiply(q0Dot));
  464.         final T q3Dot    = oPQ0.multiply(r[1][2]).add(r[0][2].multiply(q0Dot));
  465.         final T oX       = q1.linearCombination(q1.negate(), q0Dot, q0,          q1Dot, q3,          q2Dot, q2.negate(), q3Dot).multiply(2);
  466.         final T oY       = q2.linearCombination(q2.negate(), q0Dot, q3.negate(), q1Dot, q0,          q2Dot, q1,          q3Dot).multiply(2);
  467.         final T oZ       = q3.linearCombination(q3.negate(), q0Dot, q2,          q1Dot, q1.negate(), q2Dot, q0,          q3Dot).multiply(2);

  469.         // rotation acceleration
  470.         final T q0DotDot = q0.getField().getOne().subtract(q0).divide(oPQ0).multiply(q0Dot).multiply(q0Dot).
  471.                            subtract(oPQ02.multiply(q0.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]))).
  472.                            subtract(q1Dot.multiply(q1Dot).add(q2Dot.multiply(q2Dot)).add(q3Dot.multiply(q3Dot)));
  473.         final T q1DotDot = q1.linearCombination(oPQ0, r[2][0], r[1][0].multiply(2), q0Dot, r[0][0], q0DotDot);
  474.         final T q2DotDot = q2.linearCombination(oPQ0, r[2][1], r[1][1].multiply(2), q0Dot, r[0][1], q0DotDot);
  475.         final T q3DotDot = q3.linearCombination(oPQ0, r[2][2], r[1][2].multiply(2), q0Dot, r[0][2], q0DotDot);
  476.         final T oXDot    = q1.linearCombination(q1.negate(), q0DotDot, q0,          q1DotDot, q3,          q2DotDot, q2.negate(), q3DotDot).multiply(2);
  477.         final T oYDot    = q2.linearCombination(q2.negate(), q0DotDot, q3.negate(), q1DotDot, q0,          q2DotDot, q1,          q3DotDot).multiply(2);
  478.         final T oZDot    = q3.linearCombination(q3.negate(), q0DotDot, q2,          q1DotDot, q1.negate(), q2DotDot, q0,          q3DotDot).multiply(2);

  480.         return new TimeStampedFieldAngularCoordinates<T>(offset.getDate(),
  481.                                                          new FieldRotation<T>(q0, q1, q2, q3, false),
  482.                                                          new FieldVector3D<T>(oX, oY, oZ),
  483.                                                          new FieldVector3D<T>(oXDot, oYDot, oZDot)).addOffset(offset);

  485.     }

  487. }
Created with JaCoCo 0.7.4.201502262128