FieldAngularCoordinates.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.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.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 FieldRotation<T>/FieldRotation<T> rate pairs, using {@link RealFieldElement}.
  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 FieldPVCoordinates}.
  42.  * </p>
  43.  * <p>Instances of this class are guaranteed to be immutable.</p>
  44.  * @param <T> the type of the field elements
  45.  * @author Luc Maisonobe
  46.  * @since 6.0
  47.  * @see AngularCoordinates
  48.  */
  49. public class FieldAngularCoordinates<T extends RealFieldElement<T>>
  50.      implements TimeShiftable<FieldAngularCoordinates<T>>, Serializable {

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

  55.     /** FieldRotation<T>. */
  56.     private final FieldRotation<T> rotation;

  58.     /** FieldRotation<T> rate. */
  59.     private final FieldVector3D<T> rotationRate;

  61.     /** Builds a FieldRotation<T>/FieldRotation<T> rate pair.
  62.      * @param rotation FieldRotation<T>
  63.      * @param rotationRate FieldRotation<T> rate (rad/s)
  64.      */
  65.     public FieldAngularCoordinates(final FieldRotation<T> rotation, final FieldVector3D<T> rotationRate) {
  66.         this.rotation     = rotation;
  67.         this.rotationRate = rotationRate;
  68.     }

  70.     /** Estimate FieldRotation<T> rate between two orientations.
  71.      * <p>Estimation is based on a simple fixed rate FieldRotation<T>
  72.      * during the time interval between the two orientations.</p>
  73.      * @param start start orientation
  74.      * @param end end orientation
  75.      * @param dt time elapsed between the dates of the two orientations
  76.      * @param <T> the type of the field elements
  77.      * @return FieldRotation<T> rate allowing to go from start to end orientations
  78.      */
  79.     public static <T extends RealFieldElement<T>> FieldVector3D<T> estimateRate(final FieldRotation<T> start, final FieldRotation<T> end, final double dt) {
  80.         final FieldRotation<T> evolution = start.applyTo(end.revert());
  81.         return new FieldVector3D<T>(evolution.getAngle().divide(dt), evolution.getAxis());
  82.     }

  84.     /** Revert a FieldRotation<T>/FieldRotation<T> rate pair.
  85.      * Build a pair which reverse the effect of another pair.
  86.      * @return a new pair whose effect is the reverse of the effect
  87.      * of the instance
  88.      */
  89.     public FieldAngularCoordinates<T> revert() {
  90.         return new FieldAngularCoordinates<T>(rotation.revert(), rotation.applyInverseTo(rotationRate.negate()));
  91.     }

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

  110.         // BEWARE: there is really a minus sign here, because if
  111.         // the target frame rotates in one direction, the vectors in the origin
  112.         // frame seem to rotate in the opposite direction
  113.         final FieldRotation<T> evolution = new FieldRotation<T>(rotationRate, rate.negate().multiply(dt));

  115.         return new FieldAngularCoordinates<T>(evolution.applyTo(rotation), rotationRate);

  117.     }

  119.     /** Get the FieldRotation<T>.
  120.      * @return the FieldRotation<T>.
  121.      */
  122.     public FieldRotation<T> getRotation() {
  123.         return rotation;
  124.     }

  126.     /** Get the FieldRotation<T> rate.
  127.      * @return the FieldRotation<T> rate vector (rad/s).
  128.      */
  129.     public FieldVector3D<T> getRotationRate() {
  130.         return rotationRate;
  131.     }

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

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

  178.     /** Convert to a constant angular coordinates without derivatives.
  179.      * @return a constant angular coordinates
  180.      */
  181.     public AngularCoordinates toAngularCoordinates() {
  182.         return new AngularCoordinates(rotation.toRotation(), rotationRate.toVector3D());
  183.     }

  185.     /** Interpolate angular coordinates.
  186.      * <p>
  187.      * The interpolated instance is created by polynomial Hermite interpolation
  188.      * on Rodrigues vector ensuring FieldRotation<T> rate remains the exact derivative of FieldRotation<T>.
  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 FieldRotation<T>s
  198.      * when this singularity is detected.
  199.      * </p>
  200.      * <p>
  201.      * Note that even if first time derivatives (FieldRotation<T> 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 FieldRotation<T>s.
  208.      * </p>
  209.      * @param date interpolation date
  210.      * @param useRotationRates if true, use sample points FieldRotation<T> rates,
  211.      * otherwise ignore them and use only FieldRotation<T>s
  212.      * @param sample sample points on which interpolation should be done
  213.      * @param <T> the type of the field elements
  214.      * @return a new position-velocity, interpolated at specified date
  215.      */
  216.     @SuppressWarnings("unchecked")
  217.     public static <T extends RealFieldElement<T>> FieldAngularCoordinates<T> interpolate(final AbsoluteDate date, final boolean useRotationRates,
  218.                                                                                          final Collection<Pair<AbsoluteDate, FieldAngularCoordinates<T>>> sample) {

  220.         // get field properties
  221.         final T prototype = sample.iterator().next().getValue().getRotation().getQ0();
  222.         final T zero = prototype.getField().getZero();
  223.         final T one  = prototype.getField().getOne();

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

  229.         // set up a linear offset model canceling mean FieldRotation<T> rate
  230.         final FieldVector3D<T> meanRate;
  231.         if (useRotationRates) {
  232.             FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero);
  233.             for (final Pair<AbsoluteDate, FieldAngularCoordinates<T>> datedAC : sample) {
  234.                 sum = sum.add(datedAC.getValue().getRotationRate());
  235.             }
  236.             meanRate = new FieldVector3D<T>(1.0 / sample.size(), sum);
  237.         } else {
  238.             FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero);
  239.             Pair<AbsoluteDate, FieldAngularCoordinates<T>> previous = null;
  240.             for (final Pair<AbsoluteDate, FieldAngularCoordinates<T>> datedAC : sample) {
  241.                 if (previous != null) {
  242.                     sum = sum.add(estimateRate(previous.getValue().getRotation(),
  243.                                                          datedAC.getValue().getRotation(),
  244.                                                          datedAC.getKey().durationFrom(previous.getKey().getDate())));
  245.                 }
  246.                 previous = datedAC;
  247.             }
  248.             meanRate = new FieldVector3D<T>(1.0 / (sample.size() - 1), sum);
  249.         }
  250.         FieldAngularCoordinates<T> offset =
  251.                 new FieldAngularCoordinates<T>(new FieldRotation<T>(one, zero, zero, zero, false), meanRate);

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

  256.             // offset adaptation parameters
  257.             restart = false;

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

  262.             // add sample points
  263.             if (useRotationRates) {
  264.                 // populate sample with FieldRotation<T> and FieldRotation<T> rate data
  265.                 for (final Pair<AbsoluteDate, FieldAngularCoordinates<T>> datedAC : sample) {
  266.                     final T[][] rodrigues = getModifiedRodrigues(datedAC.getKey(), datedAC.getValue(),
  267.                                                                  date, offset, threshold);
  268.                     if (rodrigues == null) {
  269.                         // the sample point is close to a modified Rodrigues vector singularity
  270.                         // we need to change the linear offset model to avoid this
  271.                         restart = true;
  272.                         break;
  273.                     }
  274.                     interpolator.addSamplePoint(zero.add(datedAC.getKey().getDate().durationFrom(date)),
  275.                                                 rodrigues[0], rodrigues[1]);
  276.                 }
  277.             } else {
  278.                 // populate sample with FieldRotation<T> data only, ignoring FieldRotation<T> rate
  279.                 for (final Pair<AbsoluteDate, FieldAngularCoordinates<T>> datedAC : sample) {
  280.                     final T[][] rodrigues = getModifiedRodrigues(datedAC.getKey(), datedAC.getValue(),
  281.                                                                  date, offset, threshold);
  282.                     if (rodrigues == null) {
  283.                         // the sample point is close to a modified Rodrigues vector singularity
  284.                         // we need to change the linear offset model to avoid this
  285.                         restart = true;
  286.                         break;
  287.                     }
  288.                     interpolator.addSamplePoint(zero.add(datedAC.getKey().getDate().durationFrom(date)),
  289.                                                 rodrigues[0]);
  290.                 }
  291.             }

  293.             if (restart) {
  294.                 // interpolation failed, some intermediate FieldRotation<T> was too close to 2PI
  295.                 // we need to offset all FieldRotation<T>s to avoid the singularity
  296.                 offset = offset.addOffset(new FieldAngularCoordinates<T>(new FieldRotation<T>(new FieldVector3D<T>(one, zero, zero),
  297.                                                                                               zero.add(epsilon)),
  298.                                                                          new FieldVector3D<T>(one, zero, zero)));
  299.             } else {
  300.                 // interpolation succeeded with the current offset
  301.                 final T[][] p = interpolator.derivatives(zero, 1);
  302.                 return createFromModifiedRodrigues(p, offset);
  303.             }

  305.         }

  307.         // this should never happen
  308.         throw OrekitException.createInternalError(null);

  310.     }

  312.     /** Convert rotation and rate to modified Rodrigues vector and derivative.
  313.      * <p>
  314.      * The modified Rodrigues vector is tan(&theta;/4) u where &theta; and u are the
  315.      * rotation angle and axis respectively.
  316.      * </p>
  317.      * @param date date of the angular coordinates
  318.      * @param ac coordinates to convert
  319.      * @param offsetDate date of the linear offset model to remove
  320.      * @param offset linear offset model to remove
  321.      * @param threshold threshold for rotations too close to 2&pi;
  322.      * @param <T> the type of the field elements
  323.      * @return modified Rodrigues vector and derivative, or null if rotation is too close to 2&pi;
  324.      */
  325.     private static <T extends RealFieldElement<T>> T[][] getModifiedRodrigues(final AbsoluteDate date, final FieldAngularCoordinates<T> ac,
  326.                                                                               final AbsoluteDate offsetDate, final FieldAngularCoordinates<T> offset,
  327.                                                                               final double threshold) {

  329.         // remove linear offset from the current coordinates
  330.         final double dt = date.durationFrom(offsetDate);
  331.         final FieldAngularCoordinates<T> fixed = ac.subtractOffset(offset.shiftedBy(dt));

  333.         // check modified Rodrigues vector singularity
  334.         T q0 = fixed.getRotation().getQ0();
  335.         T q1 = fixed.getRotation().getQ1();
  336.         T q2 = fixed.getRotation().getQ2();
  337.         T q3 = fixed.getRotation().getQ3();
  338.         if (q0.getReal() < threshold && FastMath.abs(dt) * fixed.getRotationRate().getNorm().getReal() > 1.0e-3) {
  339.             // this is an intermediate point that happens to be 2PI away from reference
  340.             // we need to change the linear offset model to avoid this point
  341.             return null;
  342.         }

  344.         // make sure all interpolated points will be on the same branch
  345.         if (q0.getReal() < 0) {
  346.             q0 = q0.negate();
  347.             q1 = q1.negate();
  348.             q2 = q2.negate();
  349.             q3 = q3.negate();
  350.         }

  352.         final T x  = fixed.getRotationRate().getX();
  353.         final T y  = fixed.getRotationRate().getY();
  354.         final T z  = fixed.getRotationRate().getZ();

  356.         // derivatives of the quaternion
  357.         final T q0Dot = q0.linearCombination(q1, x, q2, y,  q3, z).multiply(-0.5);
  358.         final T q1Dot = q1.linearCombination(q0, x, q2, z, q3.negate(), y).multiply(0.5);
  359.         final T q2Dot = q2.linearCombination(q0, y, q3, x, q1.negate(), z).multiply(0.5);
  360.         final T q3Dot = q3.linearCombination(q0, z, q1, y, q2.negate(), x).multiply(0.5);

  362.         final T inv = q0.add(1).reciprocal();
  363.         final T[][] rodrigues = MathArrays.buildArray(q0.getField(), 2, 3);
  364.         rodrigues[0][0] = inv.multiply(q1);
  365.         rodrigues[0][1] = inv.multiply(q2);
  366.         rodrigues[0][2] = inv.multiply(q3);
  367.         rodrigues[1][0] = inv.multiply(q1Dot.subtract(inv.multiply(q1).multiply(q0Dot)));
  368.         rodrigues[1][1] = inv.multiply(q2Dot.subtract(inv.multiply(q2).multiply(q0Dot)));
  369.         rodrigues[1][2] = inv.multiply(q3Dot.subtract(inv.multiply(q3).multiply(q0Dot)));

  371.         return rodrigues;

  373.     }

  375.     /** Convert a modified Rodrigues vector and derivative to angular coordinates.
  376.      * @param r modified Rodrigues vector (with first derivatives)
  377.      * @param offset linear offset model to add (its date must be consistent with the modified Rodrigues vector)
  378.      * @param <T> the type of the field elements
  379.      * @return angular coordinates
  380.      */
  381.     private static <T extends RealFieldElement<T>> FieldAngularCoordinates<T> createFromModifiedRodrigues(final T[][] r,
  382.                                                                                                           final FieldAngularCoordinates<T> offset) {

  384.         // rotation
  385.         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]));
  386.         final T inv      = rSquared.add(1).reciprocal();
  387.         final T ratio    = inv.multiply(rSquared.subtract(1).negate());
  388.         final FieldRotation<T> rotation          = new FieldRotation<T>(ratio,
  389.                                                             inv.multiply(2).multiply(r[0][0]),
  390.                                                             inv.multiply(2).multiply(r[0][1]),
  391.                                                             inv.multiply(2).multiply(r[0][2]),
  392.                                                             false);

  394.         // rotation rate
  395.         final FieldVector3D<T> p    = new FieldVector3D<T>(r[0]);
  396.         final FieldVector3D<T> pDot = new FieldVector3D<T>(r[1]);
  397.         final FieldVector3D<T> rate = new FieldVector3D<T>(inv.multiply(ratio).multiply(4), pDot,
  398.                                                            inv.multiply(inv).multiply(-8), FieldVector3D.crossProduct(p, pDot),
  399.                                                            inv.multiply(inv).multiply(8).multiply(FieldVector3D.dotProduct(p, pDot)), p);

  401.         return new FieldAngularCoordinates<T>(rotation, rate).addOffset(offset);

  403.     }

  405. }
Created with JaCoCo 0.6.3.201306030806