AngularCoordinates.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.io.Serializable;
  20. import java.util.ArrayList;
  21. import java.util.Collection;
  22. import java.util.List;

  24. import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
  25. import org.apache.commons.math3.exception.MathArithmeticException;
  26. import org.apache.commons.math3.exception.MathIllegalArgumentException;
  27. import org.apache.commons.math3.exception.NumberIsTooLargeException;
  28. import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
  29. import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
  30. import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
  31. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  32. import org.apache.commons.math3.linear.DecompositionSolver;
  33. import org.apache.commons.math3.linear.MatrixUtils;
  34. import org.apache.commons.math3.linear.QRDecomposition;
  35. import org.apache.commons.math3.linear.RealMatrix;
  36. import org.apache.commons.math3.linear.RealVector;
  37. import org.apache.commons.math3.linear.SingularMatrixException;
  38. import org.apache.commons.math3.util.FastMath;
  39. import org.apache.commons.math3.util.MathArrays;
  40. import org.apache.commons.math3.util.Pair;
  41. import org.orekit.errors.OrekitException;
  42. import org.orekit.errors.OrekitMessages;
  43. import org.orekit.time.AbsoluteDate;
  44. import org.orekit.time.TimeShiftable;

  46. /** Simple container for rotation/rotation rate/rotation acceleration triplets.
  47.  * <p>
  48.  * The state can be slightly shifted to close dates. This shift is based on
  49.  * an approximate solution of the fixed acceleration motion. It is <em>not</em>
  50.  * intended as a replacement for proper attitude propagation but should be
  51.  * sufficient for either small time shifts or coarse accuracy.
  52.  * </p>
  53.  * <p>
  54.  * This class is the angular counterpart to {@link PVCoordinates}.
  55.  * </p>
  56.  * <p>Instances of this class are guaranteed to be immutable.</p>
  57.  * @author Luc Maisonobe
  58.  */
  59. public class AngularCoordinates implements TimeShiftable<AngularCoordinates>, Serializable {

  61.     /** Fixed orientation parallel with reference frame
  62.      * (identity rotation, zero rotation rate and acceleration).
  63.      */
  64.     public static final AngularCoordinates IDENTITY =
  65.             new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);

  67.     /** Serializable UID. */
  68.     private static final long serialVersionUID = 20140414L;

  70.     /** Rotation. */
  71.     private final Rotation rotation;

  73.     /** Rotation rate. */
  74.     private final Vector3D rotationRate;

  76.     /** Rotation acceleration. */
  77.     private final Vector3D rotationAcceleration;

  79.     /** Simple constructor.
  80.      * <p> Sets the Coordinates to default : Identity, Ω = (0 0 0), dΩ/dt = (0 0 0).</p>
  81.      */
  82.     public AngularCoordinates() {
  83.         this(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
  84.     }

  86.     /** Builds a rotation/rotation rate pair.
  87.      * @param rotation rotation
  88.      * @param rotationRate rotation rate Ω (rad/s)
  89.      */
  90.     public AngularCoordinates(final Rotation rotation, final Vector3D rotationRate) {
  91.         this(rotation, rotationRate, Vector3D.ZERO);
  92.     }

  94.     /** Builds a rotation/rotation rate/rotation acceleration triplet.
  95.      * @param rotation rotation
  96.      * @param rotationRate rotation rate Ω (rad/s)
  97.      * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²)
  98.      */
  99.     public AngularCoordinates(final Rotation rotation,
  100.                               final Vector3D rotationRate, final Vector3D rotationAcceleration) {
  101.         this.rotation             = rotation;
  102.         this.rotationRate         = rotationRate;
  103.         this.rotationAcceleration = rotationAcceleration;
  104.     }

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

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

  133.         try {
  134.             // find the initial fixed rotation
  135.             rotation = new Rotation(u1.getPosition(), u2.getPosition(),
  136.                                     v1.getPosition(), v2.getPosition());

  138.             // find rotation rate Ω such that
  139.             //  Ω ⨯ v₁ = r(dot(u₁)) - dot(v₁)
  140.             //  Ω ⨯ v₂ = r(dot(u₂)) - dot(v₂)
  141.             final Vector3D ru1Dot = rotation.applyTo(u1.getVelocity());
  142.             final Vector3D ru2Dot = rotation.applyTo(u2.getVelocity());
  143.             rotationRate = inverseCrossProducts(v1.getPosition(), ru1Dot.subtract(v1.getVelocity()),
  144.                                                 v2.getPosition(), ru2Dot.subtract(v2.getVelocity()),
  145.                                                 tolerance);

  147.             // find rotation acceleration dot(Ω) such that
  148.             // dot(Ω) ⨯ v₁ = r(dotdot(u₁)) - 2 Ω ⨯ dot(v₁) - Ω ⨯  (Ω ⨯ v₁) - dotdot(v₁)
  149.             // dot(Ω) ⨯ v₂ = r(dotdot(u₂)) - 2 Ω ⨯ dot(v₂) - Ω ⨯  (Ω ⨯ v₂) - dotdot(v₂)
  150.             final Vector3D ru1DotDot = rotation.applyTo(u1.getAcceleration());
  151.             final Vector3D oDotv1    = Vector3D.crossProduct(rotationRate, v1.getVelocity());
  152.             final Vector3D oov1      = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v1.getPosition()));
  153.             final Vector3D c1        = new Vector3D(1, ru1DotDot, -2, oDotv1, -1, oov1, -1, v1.getAcceleration());
  154.             final Vector3D ru2DotDot = rotation.applyTo(u2.getAcceleration());
  155.             final Vector3D oDotv2    = Vector3D.crossProduct(rotationRate, v2.getVelocity());
  156.             final Vector3D oov2      = Vector3D.crossProduct(rotationRate, Vector3D.crossProduct(rotationRate, v2.getPosition()));
  157.             final Vector3D c2        = new Vector3D(1, ru2DotDot, -2, oDotv2, -1, oov2, -1, v2.getAcceleration());
  158.             rotationAcceleration     = inverseCrossProducts(v1.getPosition(), c1, v2.getPosition(), c2, tolerance);

  160.         } catch (MathIllegalArgumentException miae) {
  161.             throw new OrekitException(miae);
  162.         } catch (MathArithmeticException mae) {
  163.             throw new OrekitException(mae);
  164.         }

  166.     }

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

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

  177.      * @param u origin vector
  178.      * @param v desired image of u by the rotation
  179.      * @exception OrekitException if the vectors components cannot be converted to
  180.      * {@link DerivativeStructure} with proper order
  181.      */
  182.     public AngularCoordinates(final PVCoordinates u, final PVCoordinates v) throws OrekitException {
  183.         this(new FieldRotation<DerivativeStructure>(u.toDerivativeStructureVector(2),
  184.                                                     v.toDerivativeStructureVector(2)));
  185.     }

  187.     /** Builds a AngularCoordinates from  a {@link FieldRotation}&lt;{@link DerivativeStructure}&gt;.
  188.      * <p>
  189.      * The rotation components must have time as their only derivation parameter and
  190.      * have consistent derivation orders.
  191.      * </p>
  192.      * @param r rotation with time-derivatives embedded within the coordinates
  193.      */
  194.     public AngularCoordinates(final FieldRotation<DerivativeStructure> r) {

  196.         final double q0       = r.getQ0().getReal();
  197.         final double q1       = r.getQ1().getReal();
  198.         final double q2       = r.getQ2().getReal();
  199.         final double q3       = r.getQ3().getReal();

  201.         rotation     = new Rotation(q0, q1, q2, q3, false);
  202.         if (r.getQ0().getOrder() >= 1) {
  203.             final double q0Dot    = r.getQ0().getPartialDerivative(1);
  204.             final double q1Dot    = r.getQ1().getPartialDerivative(1);
  205.             final double q2Dot    = r.getQ2().getPartialDerivative(1);
  206.             final double q3Dot    = r.getQ3().getPartialDerivative(1);
  207.             rotationRate =
  208.                     new Vector3D(2 * MathArrays.linearCombination(-q1, q0Dot,  q0, q1Dot,  q3, q2Dot, -q2, q3Dot),
  209.                                  2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot,  q0, q2Dot,  q1, q3Dot),
  210.                                  2 * MathArrays.linearCombination(-q3, q0Dot,  q2, q1Dot, -q1, q2Dot,  q0, q3Dot));
  211.             if (r.getQ0().getOrder() >= 2) {
  212.                 final double q0DotDot = r.getQ0().getPartialDerivative(2);
  213.                 final double q1DotDot = r.getQ1().getPartialDerivative(2);
  214.                 final double q2DotDot = r.getQ2().getPartialDerivative(2);
  215.                 final double q3DotDot = r.getQ3().getPartialDerivative(2);
  216.                 rotationAcceleration =
  217.                         new Vector3D(2 * MathArrays.linearCombination(-q1, q0DotDot,  q0, q1DotDot,  q3, q2DotDot, -q2, q3DotDot),
  218.                                      2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot,  q0, q2DotDot,  q1, q3DotDot),
  219.                                      2 * MathArrays.linearCombination(-q3, q0DotDot,  q2, q1DotDot, -q1, q2DotDot,  q0, q3DotDot));
  220.             } else {
  221.                 rotationAcceleration = Vector3D.ZERO;
  222.             }
  223.         } else {
  224.             rotationRate         = Vector3D.ZERO;
  225.             rotationAcceleration = Vector3D.ZERO;
  226.         }

  228.     }

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

  252.         final double v12 = v1.getNormSq();
  253.         final double v1n = FastMath.sqrt(v12);
  254.         final double v22 = v2.getNormSq();
  255.         final double v2n = FastMath.sqrt(v22);
  256.         final double threshold = tolerance * FastMath.max(v1n, v2n);

  258.         Vector3D omega;

  260.         try {
  261.             // create the over-determined linear system representing the two cross products
  262.             final RealMatrix m = MatrixUtils.createRealMatrix(6, 3);
  263.             m.setEntry(0, 1,  v1.getZ());
  264.             m.setEntry(0, 2, -v1.getY());
  265.             m.setEntry(1, 0, -v1.getZ());
  266.             m.setEntry(1, 2,  v1.getX());
  267.             m.setEntry(2, 0,  v1.getY());
  268.             m.setEntry(2, 1, -v1.getX());
  269.             m.setEntry(3, 1,  v2.getZ());
  270.             m.setEntry(3, 2, -v2.getY());
  271.             m.setEntry(4, 0, -v2.getZ());
  272.             m.setEntry(4, 2,  v2.getX());
  273.             m.setEntry(5, 0,  v2.getY());
  274.             m.setEntry(5, 1, -v2.getX());

  276.             final RealVector rhs = MatrixUtils.createRealVector(new double[] {
  277.                 c1.getX(), c1.getY(), c1.getZ(),
  278.                 c2.getX(), c2.getY(), c2.getZ()
  279.             });

  281.             // find the best solution we can
  282.             final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver();
  283.             final RealVector v = solver.solve(rhs);
  284.             omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2));

  286.         } catch (SingularMatrixException sme) {

  288.             // handle some special cases for which we can compute a solution
  289.             final double c12 = c1.getNormSq();
  290.             final double c1n = FastMath.sqrt(c12);
  291.             final double c22 = c2.getNormSq();
  292.             final double c2n = FastMath.sqrt(c22);

  294.             if (c1n <= threshold && c2n <= threshold) {
  295.                 // simple special case, velocities are cancelled
  296.                 return Vector3D.ZERO;
  297.             } else if (v1n <= threshold && c1n >= threshold) {
  298.                 // this is inconsistent, if v₁ is zero, c₁ must be 0 too
  299.                 throw new NumberIsTooLargeException(c1n, 0, true);
  300.             } else if (v2n <= threshold && c2n >= threshold) {
  301.                 // this is inconsistent, if v₂ is zero, c₂ must be 0 too
  302.                 throw new NumberIsTooLargeException(c2n, 0, true);
  303.             } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) {
  304.                 // simple special case, v₂ is redundant with v₁, we just ignore it
  305.                 // use the simplest Ω: orthogonal to both v₁ and c₁
  306.                 omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1));
  307.             } else {
  308.                 throw sme;
  309.             }

  311.         }

  313.         // check results
  314.         final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1);
  315.         if (d1 > threshold) {
  316.             throw new NumberIsTooLargeException(d1, 0, true);
  317.         }

  319.         final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2);
  320.         if (d2 > threshold) {
  321.             throw new NumberIsTooLargeException(d2, 0, true);
  322.         }

  324.         return omega;

  326.     }

  328.     /** Transform the instance to a {@link FieldRotation}&lt;{@link DerivativeStructure}&gt;.
  329.      * <p>
  330.      * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
  331.      * to the user-specified order.
  332.      * </p>
  333.      * @param order derivation order for the vector components
  334.      * @return rotation with time-derivatives embedded within the coordinates
  335.      * @exception OrekitException if the user specified order is too large
  336.      */
  337.     public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order)
  338.         throws OrekitException {

  340.         // quaternion components
  341.         final double q0 = rotation.getQ0();
  342.         final double q1 = rotation.getQ1();
  343.         final double q2 = rotation.getQ2();
  344.         final double q3 = rotation.getQ3();

  346.         // first time-derivatives of the quaternion
  347.         final double oX    = rotationRate.getX();
  348.         final double oY    = rotationRate.getY();
  349.         final double oZ    = rotationRate.getZ();
  350.         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
  351.         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
  352.         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
  353.         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);

  355.         // second time-derivatives of the quaternion
  356.         final double oXDot = rotationAcceleration.getX();
  357.         final double oYDot = rotationAcceleration.getY();
  358.         final double oZDot = rotationAcceleration.getZ();
  359.         final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
  360.             q1, q2,  q3, q1Dot, q2Dot,  q3Dot
  361.         }, new double[] {
  362.             oXDot, oYDot, oZDot, oX, oY, oZ
  363.         });
  364.         final double q1DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  365.             q0, q2, -q3, q0Dot, q2Dot, -q3Dot
  366.         }, new double[] {
  367.             oXDot, oZDot, oYDot, oX, oZ, oY
  368.         });
  369.         final double q2DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  370.             q0, q3, -q1, q0Dot, q3Dot, -q1Dot
  371.         }, new double[] {
  372.             oYDot, oXDot, oZDot, oY, oX, oZ
  373.         });
  374.         final double q3DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  375.             q0, q1, -q2, q0Dot, q1Dot, -q2Dot
  376.         }, new double[] {
  377.             oZDot, oYDot, oXDot, oZ, oY, oX
  378.         });

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

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

  409.     }

  411.     /** Estimate rotation rate between two orientations.
  412.      * <p>Estimation is based on a simple fixed rate rotation
  413.      * during the time interval between the two orientations.</p>
  414.      * @param start start orientation
  415.      * @param end end orientation
  416.      * @param dt time elapsed between the dates of the two orientations
  417.      * @return rotation rate allowing to go from start to end orientations
  418.      */
  419.     public static Vector3D estimateRate(final Rotation start, final Rotation end, final double dt) {
  420.         final Rotation evolution = start.compose(end.revert(), RotationConvention.VECTOR_OPERATOR);
  421.         return new Vector3D(evolution.getAngle() / dt, evolution.getAxis(RotationConvention.VECTOR_OPERATOR));
  422.     }

  424.     /** Revert a rotation/rotation rate/ rotation acceleration triplet.
  425.      * Build a triplet which reverse the effect of another triplet.
  426.      * @return a new triplet whose effect is the reverse of the effect
  427.      * of the instance
  428.      */
  429.     public AngularCoordinates revert() {
  430.         return new AngularCoordinates(rotation.revert(),
  431.                                       rotation.applyInverseTo(rotationRate).negate(),
  432.                                       rotation.applyInverseTo(rotationAcceleration).negate());
  433.     }

  435.     /** Get a time-shifted state.
  436.      * <p>
  437.      * The state can be slightly shifted to close dates. This shift is based on
  438.      * an approximate solution of the fixed acceleration motion. It is <em>not</em>
  439.      * intended as a replacement for proper attitude propagation but should be
  440.      * sufficient for either small time shifts or coarse accuracy.
  441.      * </p>
  442.      * @param dt time shift in seconds
  443.      * @return a new state, shifted with respect to the instance (which is immutable)
  444.      */
  445.     public AngularCoordinates shiftedBy(final double dt) {

  447.         // the shiftedBy method is based on a local approximation.
  448.         // It considers separately the contribution of the constant
  449.         // rotation, the linear contribution or the rate and the
  450.         // quadratic contribution of the acceleration. The rate
  451.         // and acceleration contributions are small rotations as long
  452.         // as the time shift is small, which is the crux of the algorithm.
  453.         // Small rotations are almost commutative, so we append these small
  454.         // contributions one after the other, as if they really occurred
  455.         // successively, despite this is not what really happens.

  457.         // compute the linear contribution first, ignoring acceleration
  458.         // BEWARE: there is really a minus sign here, because if
  459.         // the target frame rotates in one direction, the vectors in the origin
  460.         // frame seem to rotate in the opposite direction
  461.         final double rate = rotationRate.getNorm();
  462.         final Rotation rateContribution = (rate == 0.0) ?
  463.                                           Rotation.IDENTITY :
  464.                                           new Rotation(rotationRate, rate * dt, RotationConvention.FRAME_TRANSFORM);

  466.         // append rotation and rate contribution
  467.         final AngularCoordinates linearPart =
  468.                 new AngularCoordinates(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR), rotationRate);

  470.         final double acc  = rotationAcceleration.getNorm();
  471.         if (acc == 0.0) {
  472.             // no acceleration, the linear part is sufficient
  473.             return linearPart;
  474.         }

  476.         // compute the quadratic contribution, ignoring initial rotation and rotation rate
  477.         // BEWARE: there is really a minus sign here, because if
  478.         // the target frame rotates in one direction, the vectors in the origin
  479.         // frame seem to rotate in the opposite direction
  480.         final AngularCoordinates quadraticContribution =
  481.                 new AngularCoordinates(new Rotation(rotationAcceleration,
  482.                                                     0.5 * acc * dt * dt,
  483.                                                     RotationConvention.FRAME_TRANSFORM),
  484.                                        new Vector3D(dt, rotationAcceleration),
  485.                                        rotationAcceleration);

  487.         // the quadratic contribution is a small rotation:
  488.         // its initial angle and angular rate are both zero.
  489.         // small rotations are almost commutative, so we append the small
  490.         // quadratic part after the linear part as a simple offset
  491.         return quadraticContribution.addOffset(linearPart);

  493.     }

  495.     /** Get the rotation.
  496.      * @return the rotation.
  497.      */
  498.     public Rotation getRotation() {
  499.         return rotation;
  500.     }

  502.     /** Get the rotation rate.
  503.      * @return the rotation rate vector Ω (rad/s).
  504.      */
  505.     public Vector3D getRotationRate() {
  506.         return rotationRate;
  507.     }

  509.     /** Get the rotation acceleration.
  510.      * @return the rotation acceleration vector dΩ/dt (rad²/s²).
  511.      */
  512.     public Vector3D getRotationAcceleration() {
  513.         return rotationAcceleration;
  514.     }

  516.     /** Add an offset from the instance.
  517.      * <p>
  518.      * We consider here that the offset rotation is applied first and the
  519.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  520.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  521.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  522.      * </p>
  523.      * <p>
  524.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  525.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  526.      * so that round trip applications are possible. This means that both {@code
  527.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  528.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  529.      * </p>
  530.      * @param offset offset to subtract
  531.      * @return new instance, with offset subtracted
  532.      * @see #subtractOffset(AngularCoordinates)
  533.      */
  534.     public AngularCoordinates addOffset(final AngularCoordinates offset) {
  535.         final Vector3D rOmega    = rotation.applyTo(offset.rotationRate);
  536.         final Vector3D rOmegaDot = rotation.applyTo(offset.rotationAcceleration);
  537.         return new AngularCoordinates(rotation.compose(offset.rotation, RotationConvention.VECTOR_OPERATOR),
  538.                                       rotationRate.add(rOmega),
  539.                                       new Vector3D( 1.0, rotationAcceleration,
  540.                                                     1.0, rOmegaDot,
  541.                                                    -1.0, Vector3D.crossProduct(rotationRate, rOmega)));
  542.     }

  544.     /** Subtract an offset from the instance.
  545.      * <p>
  546.      * We consider here that the offset rotation is applied first and the
  547.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  548.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  549.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  550.      * </p>
  551.      * <p>
  552.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  553.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  554.      * so that round trip applications are possible. This means that both {@code
  555.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  556.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  557.      * </p>
  558.      * @param offset offset to subtract
  559.      * @return new instance, with offset subtracted
  560.      * @see #addOffset(AngularCoordinates)
  561.      */
  562.     public AngularCoordinates subtractOffset(final AngularCoordinates offset) {
  563.         return addOffset(offset.revert());
  564.     }

  566.     /** Apply the rotation to a pv coordinates.
  567.      * @param pv vector to apply the rotation to
  568.      * @return a new pv coordinates which is the image of u by the rotation
  569.      */
  570.     public PVCoordinates applyTo(final PVCoordinates pv) {

  572.         final Vector3D transformedP = rotation.applyTo(pv.getPosition());
  573.         final Vector3D crossP       = Vector3D.crossProduct(rotationRate, transformedP);
  574.         final Vector3D transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  575.         final Vector3D crossV       = Vector3D.crossProduct(rotationRate, transformedV);
  576.         final Vector3D crossCrossP  = Vector3D.crossProduct(rotationRate, crossP);
  577.         final Vector3D crossDotP    = Vector3D.crossProduct(rotationAcceleration, transformedP);
  578.         final Vector3D transformedA = new Vector3D( 1, rotation.applyTo(pv.getAcceleration()),
  579.                                                    -2, crossV,
  580.                                                    -1, crossCrossP,
  581.                                                    -1, crossDotP);

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

  585.     }

  587.     /** Apply the rotation to a pv coordinates.
  588.      * @param pv vector to apply the rotation to
  589.      * @return a new pv coordinates which is the image of u by the rotation
  590.      */
  591.     public TimeStampedPVCoordinates applyTo(final TimeStampedPVCoordinates pv) {

  593.         final Vector3D transformedP = getRotation().applyTo(pv.getPosition());
  594.         final Vector3D crossP       = Vector3D.crossProduct(getRotationRate(), transformedP);
  595.         final Vector3D transformedV = getRotation().applyTo(pv.getVelocity()).subtract(crossP);
  596.         final Vector3D crossV       = Vector3D.crossProduct(getRotationRate(), transformedV);
  597.         final Vector3D crossCrossP  = Vector3D.crossProduct(getRotationRate(), crossP);
  598.         final Vector3D crossDotP    = Vector3D.crossProduct(getRotationAcceleration(), transformedP);
  599.         final Vector3D transformedA = new Vector3D( 1, getRotation().applyTo(pv.getAcceleration()),
  600.                                                    -2, crossV,
  601.                                                    -1, crossCrossP,
  602.                                                    -1, crossDotP);

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

  606.     }

  608.     /** Interpolate angular coordinates.
  609.      * <p>
  610.      * The interpolated instance is created by polynomial Hermite interpolation
  611.      * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
  612.      * </p>
  613.      * <p>
  614.      * This method is based on Sergei Tanygin's paper <a
  615.      * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
  616.      * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
  617.      * vector as described in Malcolm D. Shuster's paper <a
  618.      * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
  619.      * Survey of Attitude Representations</a>. This change avoids the singularity at π.
  620.      * There is still a singularity at 2π, which is handled by slightly offsetting all rotations
  621.      * when this singularity is detected.
  622.      * </p>
  623.      * <p>
  624.      * Note that even if first time derivatives (rotation rates)
  625.      * from sample can be ignored, the interpolated instance always includes
  626.      * interpolated derivatives. This feature can be used explicitly to
  627.      * compute these derivatives when it would be too complex to compute them
  628.      * from an analytical formula: just compute a few sample points from the
  629.      * explicit formula and set the derivatives to zero in these sample points,
  630.      * then use interpolation to add derivatives consistent with the rotations.
  631.      * </p>
  632.      * @param date interpolation date
  633.      * @param useRotationRates if true, use sample points rotation rates,
  634.      * otherwise ignore them and use only rotations
  635.      * @param sample sample points on which interpolation should be done
  636.      * @return a new position-velocity, interpolated at specified date
  637.      * @exception OrekitException if the number of point is too small for interpolating
  638.      * @deprecated since 7.0 replaced with {@link TimeStampedAngularCoordinates#interpolate(AbsoluteDate, AngularDerivativesFilter, Collection)}
  639.      */
  640.     @Deprecated
  641.     public static AngularCoordinates interpolate(final AbsoluteDate date, final boolean useRotationRates,
  642.                                                  final Collection<Pair<AbsoluteDate, AngularCoordinates>> sample)
  643.         throws OrekitException {
  644.         final List<TimeStampedAngularCoordinates> list = new ArrayList<TimeStampedAngularCoordinates>(sample.size());
  645.         for (final Pair<AbsoluteDate, AngularCoordinates> pair : sample) {
  646.             list.add(new TimeStampedAngularCoordinates(pair.getFirst(),
  647.                                                        pair.getSecond().getRotation(),
  648.                                                        pair.getSecond().getRotationRate(),
  649.                                                        pair.getSecond().getRotationAcceleration()));
  650.         }
  651.         return TimeStampedAngularCoordinates.interpolate(date,
  652.                                                          useRotationRates ? AngularDerivativesFilter.USE_RR : AngularDerivativesFilter.USE_R,
  653.                                                          list);
  654.     }

  656.     /** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives.
  657.      * <p>
  658.      * The modified Rodrigues vector is tan(θ/4) u where θ and u are the
  659.      * rotation angle and axis respectively.
  660.      * </p>
  661.      * @param sign multiplicative sign for quaternion components
  662.      * @return modified Rodrigues vector and derivatives (vector on row 0, first derivative
  663.      * on row 1, second derivative on row 2)
  664.      * @see #createFromModifiedRodrigues(double[][])
  665.      */
  666.     public double[][] getModifiedRodrigues(final double sign) {

  668.         final double q0    = sign * getRotation().getQ0();
  669.         final double q1    = sign * getRotation().getQ1();
  670.         final double q2    = sign * getRotation().getQ2();
  671.         final double q3    = sign * getRotation().getQ3();
  672.         final double oX    = getRotationRate().getX();
  673.         final double oY    = getRotationRate().getY();
  674.         final double oZ    = getRotationRate().getZ();
  675.         final double oXDot = getRotationAcceleration().getX();
  676.         final double oYDot = getRotationAcceleration().getY();
  677.         final double oZDot = getRotationAcceleration().getZ();

  679.         // first time-derivatives of the quaternion
  680.         final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
  681.         final double q1Dot = 0.5 * MathArrays.linearCombination( q0, oX, -q3, oY,  q2, oZ);
  682.         final double q2Dot = 0.5 * MathArrays.linearCombination( q3, oX,  q0, oY, -q1, oZ);
  683.         final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX,  q1, oY,  q0, oZ);

  685.         // second time-derivatives of the quaternion
  686.         final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] {
  687.             q1, q2,  q3, q1Dot, q2Dot,  q3Dot
  688.         }, new double[] {
  689.             oXDot, oYDot, oZDot, oX, oY, oZ
  690.         });
  691.         final double q1DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  692.             q0, q2, -q3, q0Dot, q2Dot, -q3Dot
  693.         }, new double[] {
  694.             oXDot, oZDot, oYDot, oX, oZ, oY
  695.         });
  696.         final double q2DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  697.             q0, q3, -q1, q0Dot, q3Dot, -q1Dot
  698.         }, new double[] {
  699.             oYDot, oXDot, oZDot, oY, oX, oZ
  700.         });
  701.         final double q3DotDot =  0.5 * MathArrays.linearCombination(new double[] {
  702.             q0, q1, -q2, q0Dot, q1Dot, -q2Dot
  703.         }, new double[] {
  704.             oZDot, oYDot, oXDot, oZ, oY, oX
  705.         });

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

  714.         final double r1       = inv * q1;
  715.         final double r2       = inv * q2;
  716.         final double r3       = inv * q3;

  718.         final double mInvR1   = -inv * r1;
  719.         final double mInvR2   = -inv * r2;
  720.         final double mInvR3   = -inv * r3;

  722.         final double r1Dot    = MathArrays.linearCombination(inv, q1Dot, mInvR1, q0Dot);
  723.         final double r2Dot    = MathArrays.linearCombination(inv, q2Dot, mInvR2, q0Dot);
  724.         final double r3Dot    = MathArrays.linearCombination(inv, q3Dot, mInvR3, q0Dot);

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

  730.         return new double[][] {
  731.             {
  732.                 r1,       r2,       r3
  733.             }, {
  734.                 r1Dot,    r2Dot,    r3Dot
  735.             }, {
  736.                 r1DotDot, r2DotDot, r3DotDot
  737.             }
  738.         };

  740.     }

  742.     /** Convert a modified Rodrigues vector and derivatives to angular coordinates.
  743.      * @param r modified Rodrigues vector (with first and second times derivatives)
  744.      * @return angular coordinates
  745.      * @see #getModifiedRodrigues(double)
  746.      */
  747.     public static AngularCoordinates createFromModifiedRodrigues(final double[][] r) {

  749.         // rotation
  750.         final double rSquared = r[0][0] * r[0][0] + r[0][1] * r[0][1] + r[0][2] * r[0][2];
  751.         final double oPQ0     = 2 / (1 + rSquared);
  752.         final double q0       = oPQ0 - 1;
  753.         final double q1       = oPQ0 * r[0][0];
  754.         final double q2       = oPQ0 * r[0][1];
  755.         final double q3       = oPQ0 * r[0][2];

  757.         // rotation rate
  758.         final double oPQ02    = oPQ0 * oPQ0;
  759.         final double q0Dot    = -oPQ02 * MathArrays.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1],  r[0][2], r[1][2]);
  760.         final double q1Dot    = oPQ0 * r[1][0] + r[0][0] * q0Dot;
  761.         final double q2Dot    = oPQ0 * r[1][1] + r[0][1] * q0Dot;
  762.         final double q3Dot    = oPQ0 * r[1][2] + r[0][2] * q0Dot;
  763.         final double oX       = 2 * MathArrays.linearCombination(-q1, q0Dot,  q0, q1Dot,  q3, q2Dot, -q2, q3Dot);
  764.         final double oY       = 2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot,  q0, q2Dot,  q1, q3Dot);
  765.         final double oZ       = 2 * MathArrays.linearCombination(-q3, q0Dot,  q2, q1Dot, -q1, q2Dot,  q0, q3Dot);

  767.         // rotation acceleration
  768.         final double q0DotDot = (1 - q0) / oPQ0 * q0Dot * q0Dot -
  769.                                 oPQ02 * MathArrays.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]) -
  770.                                 (q1Dot * q1Dot + q2Dot * q2Dot + q3Dot * q3Dot);
  771.         final double q1DotDot = MathArrays.linearCombination(oPQ0, r[2][0], 2 * r[1][0], q0Dot, r[0][0], q0DotDot);
  772.         final double q2DotDot = MathArrays.linearCombination(oPQ0, r[2][1], 2 * r[1][1], q0Dot, r[0][1], q0DotDot);
  773.         final double q3DotDot = MathArrays.linearCombination(oPQ0, r[2][2], 2 * r[1][2], q0Dot, r[0][2], q0DotDot);
  774.         final double oXDot    = 2 * MathArrays.linearCombination(-q1, q0DotDot,  q0, q1DotDot,  q3, q2DotDot, -q2, q3DotDot);
  775.         final double oYDot    = 2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot,  q0, q2DotDot,  q1, q3DotDot);
  776.         final double oZDot    = 2 * MathArrays.linearCombination(-q3, q0DotDot,  q2, q1DotDot, -q1, q2DotDot,  q0, q3DotDot);

  778.         return new AngularCoordinates(new Rotation(q0, q1, q2, q3, false),
  779.                                       new Vector3D(oX, oY, oZ),
  780.                                       new Vector3D(oXDot, oYDot, oZDot));

  782.     }

  784. }
Created with JaCoCo 0.7.6.201602180812