AngularCoordinates.java

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

  22. import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
  23. import org.apache.commons.math3.analysis.interpolation.HermiteInterpolator;
  24. import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
  25. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  26. import org.apache.commons.math3.util.FastMath;
  27. import org.apache.commons.math3.util.MathArrays;
  28. import org.apache.commons.math3.util.Pair;
  29. import org.orekit.errors.OrekitException;
  30. import org.orekit.time.AbsoluteDate;
  31. import org.orekit.time.TimeShiftable;

  33. /** Simple container for rotation/rotation rate pairs.
  34.  * <p>
  35.  * The state can be slightly shifted to close dates. This shift is based on
  36.  * a simple linear model. It is <em>not</em> intended as a replacement for
  37.  * proper attitude propagation but should be sufficient for either small
  38.  * time shifts or coarse accuracy.
  39.  * </p>
  40.  * <p>
  41.  * This class is the angular counterpart to {@link PVCoordinates}.
  42.  * </p>
  43.  * <p>Instances of this class are guaranteed to be immutable.</p>
  44.  * @author Luc Maisonobe
  45.  */
  46. public class AngularCoordinates implements TimeShiftable<AngularCoordinates>, Serializable {

  48.     /** Fixed orientation parallel with reference frame (identity rotation and zero rotation rate). */
  49.     public static final AngularCoordinates IDENTITY =
  50.             new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO);

  52.     /** Serializable UID. */
  53.     private static final long serialVersionUID = 3750363056414336775L;

  55.     /** Rotation. */
  56.     private final Rotation rotation;

  58.     /** Rotation rate. */
  59.     private final Vector3D rotationRate;

  61.     /** Simple constructor.
  62.      * <p> Sets the Coordinates to default : Identity (0 0 0).</p>
  63.      */
  64.     public AngularCoordinates() {
  65.         rotation     = Rotation.IDENTITY;
  66.         rotationRate = Vector3D.ZERO;
  67.     }

  69.     /** Builds a rotation/rotation rate pair.
  70.      * @param rotation rotation
  71.      * @param rotationRate rotation rate (rad/s)
  72.      */
  73.     public AngularCoordinates(final Rotation rotation, final Vector3D rotationRate) {
  74.         this.rotation     = rotation;
  75.         this.rotationRate = rotationRate;
  76.     }

  78.     /** Estimate rotation rate between two orientations.
  79.      * <p>Estimation is based on a simple fixed rate rotation
  80.      * during the time interval between the two orientations.</p>
  81.      * @param start start orientation
  82.      * @param end end orientation
  83.      * @param dt time elapsed between the dates of the two orientations
  84.      * @return rotation rate allowing to go from start to end orientations
  85.      */
  86.     public static Vector3D estimateRate(final Rotation start, final Rotation end, final double dt) {
  87.         final Rotation evolution = start.applyTo(end.revert());
  88.         return new Vector3D(evolution.getAngle() / dt, evolution.getAxis());
  89.     }

  91.     /** Revert a rotation/rotation rate pair.
  92.      * Build a pair which reverse the effect of another pair.
  93.      * @return a new pair whose effect is the reverse of the effect
  94.      * of the instance
  95.      */
  96.     public AngularCoordinates revert() {
  97.         return new AngularCoordinates(rotation.revert(), rotation.applyInverseTo(rotationRate.negate()));
  98.     }

  100.     /** Get a time-shifted state.
  101.      * <p>
  102.      * The state can be slightly shifted to close dates. This shift is based on
  103.      * a simple linear model. It is <em>not</em> intended as a replacement for
  104.      * proper attitude propagation but should be sufficient for either small
  105.      * time shifts or coarse accuracy.
  106.      * </p>
  107.      * @param dt time shift in seconds
  108.      * @return a new state, shifted with respect to the instance (which is immutable)
  109.      */
  110.     public AngularCoordinates shiftedBy(final double dt) {
  111.         final double rate = rotationRate.getNorm();
  112.         if (rate == 0.0) {
  113.             // special case for fixed rotations
  114.             return this;
  115.         }

  117.         // BEWARE: there is really a minus sign here, because if
  118.         // the target frame rotates in one direction, the vectors in the origin
  119.         // frame seem to rotate in the opposite direction
  120.         final Rotation evolution = new Rotation(rotationRate, -rate * dt);

  122.         return new AngularCoordinates(evolution.applyTo(rotation), rotationRate);

  124.     }

  126.     /** Get the rotation.
  127.      * @return the rotation.
  128.      */
  129.     public Rotation getRotation() {
  130.         return rotation;
  131.     }

  133.     /** Get the rotation rate.
  134.      * @return the rotation rate vector (rad/s).
  135.      */
  136.     public Vector3D getRotationRate() {
  137.         return rotationRate;
  138.     }

  140.     /** Add an offset from the instance.
  141.      * <p>
  142.      * We consider here that the offset rotation is applied first and the
  143.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  144.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  145.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  146.      * </p>
  147.      * <p>
  148.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  149.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  150.      * so that round trip applications are possible. This means that both {@code
  151.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  152.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  153.      * </p>
  154.      * @param offset offset to subtract
  155.      * @return new instance, with offset subtracted
  156.      * @see #subtractOffset(AngularCoordinates)
  157.      */
  158.     public AngularCoordinates addOffset(final AngularCoordinates offset) {
  159.         return new AngularCoordinates(rotation.applyTo(offset.rotation),
  160.                                       rotationRate.add(rotation.applyTo(offset.rotationRate)));
  161.     }

  163.     /** Subtract an offset from the instance.
  164.      * <p>
  165.      * We consider here that the offset rotation is applied first and the
  166.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  167.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  168.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  169.      * </p>
  170.      * <p>
  171.      * The two methods {@link #addOffset(AngularCoordinates) addOffset} and
  172.      * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed
  173.      * so that round trip applications are possible. This means that both {@code
  174.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  175.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  176.      * </p>
  177.      * @param offset offset to subtract
  178.      * @return new instance, with offset subtracted
  179.      * @see #addOffset(AngularCoordinates)
  180.      */
  181.     public AngularCoordinates subtractOffset(final AngularCoordinates offset) {
  182.         return addOffset(offset.revert());
  183.     }

  185.     /** Interpolate angular coordinates.
  186.      * <p>
  187.      * The interpolated instance is created by polynomial Hermite interpolation
  188.      * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation.
  189.      * </p>
  190.      * <p>
  191.      * This method is based on Sergei Tanygin's paper <a
  192.      * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
  193.      * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
  194.      * vector as described in Malcolm D. Shuster's paper <a
  195.      * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
  196.      * Survey of Attitude Representations</a>. This change avoids the singularity at &pi;.
  197.      * There is still a singularity at 2&pi;, which is handled by slightly offsetting all rotations
  198.      * when this singularity is detected.
  199.      * </p>
  200.      * <p>
  201.      * Note that even if first time derivatives (rotation rates)
  202.      * from sample can be ignored, the interpolated instance always includes
  203.      * interpolated derivatives. This feature can be used explicitly to
  204.      * compute these derivatives when it would be too complex to compute them
  205.      * from an analytical formula: just compute a few sample points from the
  206.      * explicit formula and set the derivatives to zero in these sample points,
  207.      * then use interpolation to add derivatives consistent with the rotations.
  208.      * </p>
  209.      * @param date interpolation date
  210.      * @param useRotationRates if true, use sample points rotation rates,
  211.      * otherwise ignore them and use only rotations
  212.      * @param sample sample points on which interpolation should be done
  213.      * @return a new position-velocity, interpolated at specified date
  214.      */
  215.     public static AngularCoordinates interpolate(final AbsoluteDate date, final boolean useRotationRates,
  216.                                                  final Collection<Pair<AbsoluteDate, AngularCoordinates>> sample) {

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

  222.         // set up a linear offset model canceling mean rotation rate
  223.         final Vector3D meanRate;
  224.         if (useRotationRates) {
  225.             Vector3D sum = Vector3D.ZERO;
  226.             for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
  227.                 sum = sum.add(datedAC.getValue().getRotationRate());
  228.             }
  229.             meanRate = new Vector3D(1.0 / sample.size(), sum);
  230.         } else {
  231.             Vector3D sum = Vector3D.ZERO;
  232.             Pair<AbsoluteDate, AngularCoordinates> previous = null;
  233.             for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
  234.                 if (previous != null) {
  235.                     sum = sum.add(estimateRate(previous.getValue().getRotation(),
  236.                                                          datedAC.getValue().getRotation(),
  237.                                                          datedAC.getKey().durationFrom(previous.getKey().getDate())));
  238.                 }
  239.                 previous = datedAC;
  240.             }
  241.             meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
  242.         }
  243.         AngularCoordinates offset = new AngularCoordinates(Rotation.IDENTITY, meanRate);

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

  248.             // offset adaptation parameters
  249.             restart = false;

  251.             // set up an interpolator taking derivatives into account
  252.             final HermiteInterpolator interpolator = new HermiteInterpolator();

  254.             // add sample points
  255.             if (useRotationRates) {
  256.                 // populate sample with rotation and rotation rate data
  257.                 for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
  258.                     final double[] rodrigues = getModifiedRodrigues(datedAC.getKey(), datedAC.getValue(),
  259.                                                                     date, offset, threshold);
  260.                     if (rodrigues == null) {
  261.                         // the sample point is close to a modified Rodrigues vector singularity
  262.                         // we need to change the linear offset model to avoid this
  263.                         restart = true;
  264.                         break;
  265.                     }
  266.                     interpolator.addSamplePoint(datedAC.getKey().getDate().durationFrom(date),
  267.                                                 new double[] {
  268.                                                     rodrigues[0],
  269.                                                     rodrigues[1],
  270.                                                     rodrigues[2]
  271.                                                 },
  272.                                                 new double[] {
  273.                                                     rodrigues[3],
  274.                                                     rodrigues[4],
  275.                                                     rodrigues[5]
  276.                                                 });
  277.                 }
  278.             } else {
  279.                 // populate sample with rotation data only, ignoring rotation rate
  280.                 for (final Pair<AbsoluteDate, AngularCoordinates> datedAC : sample) {
  281.                     final double[] rodrigues = getModifiedRodrigues(datedAC.getKey(), datedAC.getValue(),
  282.                                                                     date, offset, threshold);
  283.                     if (rodrigues == null) {
  284.                         // the sample point is close to a modified Rodrigues vector singularity
  285.                         // we need to change the linear offset model to avoid this
  286.                         restart = true;
  287.                         break;
  288.                     }
  289.                     interpolator.addSamplePoint(datedAC.getKey().getDate().durationFrom(date),
  290.                                                 new double[] {
  291.                                                     rodrigues[0],
  292.                                                     rodrigues[1],
  293.                                                     rodrigues[2]
  294.                                                 });
  295.                 }
  296.             }

  298.             if (restart) {
  299.                 // interpolation failed, some intermediate rotation was too close to 2PI
  300.                 // we need to offset all rotations to avoid the singularity
  301.                 offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I, epsilon),
  302.                                                                  Vector3D.ZERO));
  303.             } else {
  304.                 // interpolation succeeded with the current offset
  305.                 final DerivativeStructure zero = new DerivativeStructure(1, 1, 0, 0.0);
  306.                 final DerivativeStructure[] p = interpolator.value(zero);
  307.                 return createFromModifiedRodrigues(new double[] {
  308.                     p[0].getValue(), p[1].getValue(), p[2].getValue(),
  309.                     p[0].getPartialDerivative(1), p[1].getPartialDerivative(1), p[2].getPartialDerivative(1)
  310.                 }, offset);
  311.             }

  313.         }

  315.         // this should never happen
  316.         throw OrekitException.createInternalError(null);

  318.     }

  320.     /** Convert rotation and rate to modified Rodrigues vector and derivative.
  321.      * <p>
  322.      * The modified Rodrigues vector is tan(&theta;/4) u where &theta; and u are the
  323.      * rotation angle and axis respectively.
  324.      * </p>
  325.      * @param date date of the angular coordinates
  326.      * @param ac coordinates to convert
  327.      * @param offsetDate date of the linear offset model to remove
  328.      * @param offset linear offset model to remove
  329.      * @param threshold threshold for rotations too close to 2&pi;
  330.      * @return modified Rodrigues vector and derivative, or null if rotation is too close to 2&pi;
  331.      */
  332.     private static double[] getModifiedRodrigues(final AbsoluteDate date, final AngularCoordinates ac,
  333.                                                  final AbsoluteDate offsetDate, final AngularCoordinates offset,
  334.                                                  final double threshold) {

  336.         // remove linear offset from the current coordinates
  337.         final double dt = date.durationFrom(offsetDate);
  338.         final AngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));

  340.         // check modified Rodrigues vector singularity
  341.         double q0 = fixed.getRotation().getQ0();
  342.         double q1 = fixed.getRotation().getQ1();
  343.         double q2 = fixed.getRotation().getQ2();
  344.         double q3 = fixed.getRotation().getQ3();
  345.         if (q0 < threshold && FastMath.abs(dt) * fixed.getRotationRate().getNorm() > 1.0e-3) {
  346.             // this is an intermediate point that happens to be 2PI away from reference
  347.             // we need to change the linear offset model to avoid this point
  348.             return null;
  349.         }

  351.         // make sure all interpolated points will be on the same branch
  352.         if (q0 < 0) {
  353.             q0 = -q0;
  354.             q1 = -q1;
  355.             q2 = -q2;
  356.             q3 = -q3;
  357.         }

  359.         final double x  = fixed.getRotationRate().getX();
  360.         final double y  = fixed.getRotationRate().getY();
  361.         final double z  = fixed.getRotationRate().getZ();

  363.         // derivatives of the quaternion
  364.         final double q0Dot = -0.5 * MathArrays.linearCombination(q1, x, q2, y,  q3, z);
  365.         final double q1Dot =  0.5 * MathArrays.linearCombination(q0, x, q2, z, -q3, y);
  366.         final double q2Dot =  0.5 * MathArrays.linearCombination(q0, y, q3, x, -q1, z);
  367.         final double q3Dot =  0.5 * MathArrays.linearCombination(q0, z, q1, y, -q2, x);

  369.         final double inv = 1.0 / (1.0 + q0);
  370.         return new double[] {
  371.             inv * q1,
  372.             inv * q2,
  373.             inv * q3,
  374.             inv * (q1Dot - inv * q1 * q0Dot),
  375.             inv * (q2Dot - inv * q2 * q0Dot),
  376.             inv * (q3Dot - inv * q3 * q0Dot)
  377.         };

  379.     }

  381.     /** Convert a modified Rodrigues vector and derivative to angular coordinates.
  382.      * @param r modified Rodrigues vector (with first derivatives)
  383.      * @param offset linear offset model to add (its date must be consistent with the modified Rodrigues vector)
  384.      * @return angular coordinates
  385.      */
  386.     private static AngularCoordinates createFromModifiedRodrigues(final double[] r,
  387.                                                                   final AngularCoordinates offset) {

  389.         // rotation
  390.         final double rSquared = r[0] * r[0] + r[1] * r[1] + r[2] * r[2];
  391.         final double inv      = 1.0 / (1 + rSquared);
  392.         final double ratio    = inv * (1 - rSquared);
  393.         final Rotation rotation =
  394.                 new Rotation(ratio, 2 * inv * r[0], 2 * inv * r[1], 2 * inv * r[2], false);

  396.         // rotation rate
  397.         final Vector3D p    = new Vector3D(r[0], r[1], r[2]);
  398.         final Vector3D pDot = new Vector3D(r[3], r[4], r[5]);
  399.         final Vector3D rate = new Vector3D( 4 * ratio * inv, pDot,
  400.                                            -8 * inv * inv, p.crossProduct(pDot),
  401.                                             8 * inv * inv * p.dotProduct(pDot), p);

  403.         return new AngularCoordinates(rotation, rate).addOffset(offset);

  405.     }

  407. }
Created with JaCoCo 0.6.3.201306030806