FieldAngularCoordinates.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.RealFieldElement;
  25. import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
  26. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  27. import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
  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 quadratic 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 = 20140414L;

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

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

  61.     /** FieldRotation<T> acceleration. */
  62.     private final FieldVector3D<T> rotationAcceleration;

  64.     /** Builds a rotation/rotation rate pair.
  65.      * @param rotation rotation
  66.      * @param rotationRate rotation rate Ω (rad/s)
  67.      */
  68.     public FieldAngularCoordinates(final FieldRotation<T> rotation, final FieldVector3D<T> rotationRate) {
  69.         this(rotation, rotationRate,
  70.              new FieldVector3D<T>(rotation.getQ0().getField().getZero(),
  71.                                   rotation.getQ0().getField().getZero(),
  72.                                   rotation.getQ0().getField().getZero()));
  73.     }

  75.     /** Builds a FieldRotation<T>/FieldRotation<T> rate/FieldRotation<T> acceleration triplet.
  76.      * @param rotation FieldRotation<T>
  77.      * @param rotationRate FieldRotation<T> rate Ω (rad/s)
  78.      * @param rotationAcceleration FieldRotation<T> acceleration dΩ/dt (rad²/s²)
  79.      */
  80.     public FieldAngularCoordinates(final FieldRotation<T> rotation, final FieldVector3D<T> rotationRate,
  81.                                    final FieldVector3D<T> rotationAcceleration) {
  82.         this.rotation             = rotation;
  83.         this.rotationRate         = rotationRate;
  84.         this.rotationAcceleration = rotationAcceleration;
  85.     }

  87.     /** Estimate FieldRotation<T> rate between two orientations.
  88.      * <p>Estimation is based on a simple fixed rate FieldRotation<T>
  89.      * during the time interval between the two orientations.</p>
  90.      * @param start start orientation
  91.      * @param end end orientation
  92.      * @param dt time elapsed between the dates of the two orientations
  93.      * @param <T> the type of the field elements
  94.      * @return FieldRotation<T> rate allowing to go from start to end orientations
  95.      */
  96.     public static <T extends RealFieldElement<T>>
  97.         FieldVector3D<T> estimateRate(final FieldRotation<T> start,
  98.                                       final FieldRotation<T> end,
  99.                                       final double dt) {
  100.         final FieldRotation<T> evolution = start.compose(end.revert(), RotationConvention.VECTOR_OPERATOR);
  101.         return new FieldVector3D<T>(evolution.getAngle().divide(dt),
  102.                                     evolution.getAxis(RotationConvention.VECTOR_OPERATOR));
  103.     }

  105.     /** Revert a FieldRotation<T>/FieldRotation<T>/FieldRotation<T> acceleration triplet.
  106.      * Build a triplet which reverse the effect of another triplet.
  107.      * @return a new triplet whose effect is the reverse of the effect
  108.      * of the instance
  109.      */
  110.     public FieldAngularCoordinates<T> revert() {
  111.         return new FieldAngularCoordinates<T>(rotation.revert(),
  112.                                               rotation.applyInverseTo(rotationRate.negate()),
  113.                                               rotation.applyInverseTo(rotationAcceleration.negate()));
  114.     }

  116.     /** Get a time-shifted state.
  117.      * <p>
  118.      * The state can be slightly shifted to close dates. This shift is based on
  119.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  120.      * proper attitude propagation but should be sufficient for either small
  121.      * time shifts or coarse accuracy.
  122.      * </p>
  123.      * @param dt time shift in seconds
  124.      * @return a new state, shifted with respect to the instance (which is immutable)
  125.      */
  126.     public FieldAngularCoordinates<T> shiftedBy(final double dt) {

  128.         // the shiftedBy method is based on a local approximation.
  129.         // It considers separately the contribution of the constant
  130.         // rotation, the linear contribution or the rate and the
  131.         // quadratic contribution of the acceleration. The rate
  132.         // and acceleration contributions are small rotations as long
  133.         // as the time shift is small, which is the crux of the algorithm.
  134.         // Small rotations are almost commutative, so we append these small
  135.         // contributions one after the other, as if they really occurred
  136.         // successively, despite this is not what really happens.

  138.         // compute the linear contribution first, ignoring acceleration
  139.         // BEWARE: there is really a minus sign here, because if
  140.         // the target frame rotates in one direction, the vectors in the origin
  141.         // frame seem to rotate in the opposite direction
  142.         final T rate = rotationRate.getNorm();
  143.         final T zero = rate.getField().getZero();
  144.         final T one  = rate.getField().getOne();
  145.         final FieldRotation<T> rateContribution = (rate.getReal() == 0.0) ?
  146.                                                   new FieldRotation<T>(one, zero, zero, zero, false) :
  147.                                                   new FieldRotation<T>(rotationRate,
  148.                                                                        rate.multiply(dt),
  149.                                                                        RotationConvention.FRAME_TRANSFORM);

  151.         // append rotation and rate contribution
  152.         final FieldAngularCoordinates<T> linearPart =
  153.                 new FieldAngularCoordinates<T>(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR),
  154.                                                rotationRate);

  156.         final T acc  = rotationAcceleration.getNorm();
  157.         if (acc.getReal() == 0.0) {
  158.             // no acceleration, the linear part is sufficient
  159.             return linearPart;
  160.         }

  162.         // compute the quadratic contribution, ignoring initial rotation and rotation rate
  163.         // BEWARE: there is really a minus sign here, because if
  164.         // the target frame rotates in one direction, the vectors in the origin
  165.         // frame seem to rotate in the opposite direction
  166.         final FieldAngularCoordinates<T> quadraticContribution =
  167.                 new FieldAngularCoordinates<T>(new FieldRotation<T>(rotationAcceleration,
  168.                                                                     acc.multiply(0.5 * dt * dt),
  169.                                                                     RotationConvention.FRAME_TRANSFORM),
  170.                                                new FieldVector3D<T>(dt, rotationAcceleration),
  171.                                                rotationAcceleration);

  173.         // the quadratic contribution is a small rotation:
  174.         // its initial angle and angular rate are both zero.
  175.         // small rotations are almost commutative, so we append the small
  176.         // quadratic part after the linear part as a simple offset
  177.         return quadraticContribution.addOffset(linearPart);

  179.     }

  181.     /** Get the FieldRotation<T>.
  182.      * @return the FieldRotation<T>.
  183.      */
  184.     public FieldRotation<T> getRotation() {
  185.         return rotation;
  186.     }

  188.     /** Get the FieldRotation<T> rate.
  189.      * @return the FieldRotation<T> rate vector (rad/s).
  190.      */
  191.     public FieldVector3D<T> getRotationRate() {
  192.         return rotationRate;
  193.     }

  195.     /** Get the rotation acceleration.
  196.      * @return the rotation acceleration vector dΩ/dt (rad²/s²).
  197.      */
  198.     public FieldVector3D<T> getRotationAcceleration() {
  199.         return rotationAcceleration;
  200.     }

  202.     /** Add an offset from the instance.
  203.      * <p>
  204.      * We consider here that the offset FieldRotation<T> is applied first and the
  205.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  206.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  207.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  208.      * </p>
  209.      * <p>
  210.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  211.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  212.      * so that round trip applications are possible. This means that both {@code
  213.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  214.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  215.      * </p>
  216.      * @param offset offset to subtract
  217.      * @return new instance, with offset subtracted
  218.      * @see #subtractOffset(FieldAngularCoordinates)
  219.      */
  220.     public FieldAngularCoordinates<T> addOffset(final FieldAngularCoordinates<T> offset) {
  221.         final FieldVector3D<T> rOmega    = rotation.applyTo(offset.rotationRate);
  222.         final FieldVector3D<T> rOmegaDot = rotation.applyTo(offset.rotationAcceleration);
  223.         return new FieldAngularCoordinates<T>(rotation.compose(offset.rotation, RotationConvention.VECTOR_OPERATOR),
  224.                                               rotationRate.add(rOmega),
  225.                                               new FieldVector3D<T>( 1.0, rotationAcceleration,
  226.                                                                     1.0, rOmegaDot,
  227.                                                                    -1.0, FieldVector3D.crossProduct(rotationRate, rOmega)));
  228.     }

  230.     /** Subtract an offset from the instance.
  231.      * <p>
  232.      * We consider here that the offset Rotation is applied first and the
  233.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  234.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  235.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  236.      * </p>
  237.      * <p>
  238.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  239.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  240.      * so that round trip applications are possible. This means that both {@code
  241.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  242.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  243.      * </p>
  244.      * @param offset offset to subtract
  245.      * @return new instance, with offset subtracted
  246.      * @see #addOffset(FieldAngularCoordinates)
  247.      */
  248.     public FieldAngularCoordinates<T> subtractOffset(final FieldAngularCoordinates<T> offset) {
  249.         return addOffset(offset.revert());
  250.     }

  252.     /** Convert to a regular angular coordinates.
  253.      * @return a regular angular coordinates
  254.      */
  255.     public AngularCoordinates toAngularCoordinates() {
  256.         return new AngularCoordinates(rotation.toRotation(), rotationRate.toVector3D(),
  257.                                       rotationAcceleration.toVector3D());
  258.     }

  260.     /** Interpolate angular coordinates.
  261.      * <p>
  262.      * The interpolated instance is created by polynomial Hermite interpolation
  263.      * on Rodrigues vector ensuring FieldRotation<T> rate remains the exact derivative of FieldRotation<T>.
  264.      * </p>
  265.      * <p>
  266.      * This method is based on Sergei Tanygin's paper <a
  267.      * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude
  268.      * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues
  269.      * vector as described in Malcolm D. Shuster's paper <a
  270.      * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
  271.      * Survey of Attitude Representations</a>. This change avoids the singularity at π.
  272.      * There is still a singularity at 2π, which is handled by slightly offsetting all FieldRotation<T>s
  273.      * when this singularity is detected.
  274.      * </p>
  275.      * <p>
  276.      * Note that even if first time derivatives (FieldRotation<T> rates)
  277.      * from sample can be ignored, the interpolated instance always includes
  278.      * interpolated derivatives. This feature can be used explicitly to
  279.      * compute these derivatives when it would be too complex to compute them
  280.      * from an analytical formula: just compute a few sample points from the
  281.      * explicit formula and set the derivatives to zero in these sample points,
  282.      * then use interpolation to add derivatives consistent with the FieldRotation<T>s.
  283.      * </p>
  284.      * @param date interpolation date
  285.      * @param useRotationRates if true, use sample points FieldRotation<T> rates,
  286.      * otherwise ignore them and use only FieldRotation<T>s
  287.      * @param sample sample points on which interpolation should be done
  288.      * @param <T> the type of the field elements
  289.      * @return a new position-velocity, interpolated at specified date
  290.      * @exception OrekitException if the number of point is too small for interpolating
  291.      * @deprecated since 7.0 replaced with {@link TimeStampedFieldAngularCoordinates#interpolate(AbsoluteDate,
  292.      * AngularDerivativesFilter, Collection)}
  293.      */
  294.     @Deprecated
  295.     public static <T extends RealFieldElement<T>>
  296.         FieldAngularCoordinates<T> interpolate(final AbsoluteDate date,
  297.                                                final boolean useRotationRates,
  298.                                                final Collection<Pair<AbsoluteDate,
  299.                                                FieldAngularCoordinates<T>>> sample)
  300.         throws OrekitException {
  301.         final List<TimeStampedFieldAngularCoordinates<T>> list = new ArrayList<TimeStampedFieldAngularCoordinates<T>>(sample.size());
  302.         for (final Pair<AbsoluteDate, FieldAngularCoordinates<T>> pair : sample) {
  303.             list.add(new TimeStampedFieldAngularCoordinates<T>(pair.getFirst(),
  304.                                                                pair.getSecond().getRotation(),
  305.                                                                pair.getSecond().getRotationRate(),
  306.                                                                pair.getSecond().getRotationAcceleration()));
  307.         }
  308.         return TimeStampedFieldAngularCoordinates.interpolate(date,
  309.                                                               useRotationRates ? AngularDerivativesFilter.USE_RR : AngularDerivativesFilter.USE_R,
  310.                                                               list);
  311.     }

  313. }
Created with JaCoCo 0.7.6.201602180812