FieldAngularCoordinates.java

  1. /* Copyright 2002-2017 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 org.hipparchus.Field;
  20. import org.hipparchus.RealFieldElement;
  21. import org.hipparchus.exception.LocalizedCoreFormats;
  22. import org.hipparchus.exception.MathIllegalArgumentException;
  23. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  24. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  25. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  26. import org.hipparchus.linear.FieldDecompositionSolver;
  27. import org.hipparchus.linear.FieldMatrix;
  28. import org.hipparchus.linear.FieldQRDecomposition;
  29. import org.hipparchus.linear.FieldVector;
  30. import org.hipparchus.linear.MatrixUtils;
  31. import org.hipparchus.util.MathArrays;
  32. import org.orekit.errors.OrekitException;

  34. /** Simple container for rotation / rotation rate pairs, using {@link
  35.  * RealFieldElement}.
  36.  * <p>
  37.  * The state can be slightly shifted to close dates. This shift is based on
  38.  * a simple quadratic model. It is <em>not</em> intended as a replacement for
  39.  * proper attitude propagation but should be sufficient for either small
  40.  * time shifts or coarse accuracy.
  41.  * </p>
  42.  * <p>
  43.  * This class is the angular counterpart to {@link FieldPVCoordinates}.
  44.  * </p>
  45.  * <p>Instances of this class are guaranteed to be immutable.</p>
  46.  * @param <T> the type of the field elements
  47.  * @author Luc Maisonobe
  48.  * @since 6.0
  49.  * @see AngularCoordinates
  50.  */
  51. public class FieldAngularCoordinates<T extends RealFieldElement<T>> {


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

  57.     /** rotation rate. */
  58.     private final FieldVector3D<T> rotationRate;

  60.     /** rotation acceleration. */
  61.     private final FieldVector3D<T> rotationAcceleration;

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

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

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

  90.      * <p><em>WARNING</em>! This method requires much more stringent assumptions on
  91.      * its parameters than the similar {@link FieldRotation#FieldRotation(FieldVector3D, FieldVector3D,
  92.      * FieldVector3D, FieldVector3D) constructor} from the {@link FieldRotation FieldRotation} class.
  93.      * As far as the FieldRotation constructor is concerned, the {@code v₂} vector from
  94.      * the second pair can be slightly misaligned. The FieldRotation constructor will
  95.      * compensate for this misalignment and create a rotation that ensure {@code
  96.      * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
  97.      * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
  98.      * preserved, this constructor works <em>only</em> if the two pairs are fully
  99.      * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
  100.      * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
  101.      * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>
  102.      * @param u1 first vector of the origin pair
  103.      * @param u2 second vector of the origin pair
  104.      * @param v1 desired image of u1 by the rotation
  105.      * @param v2 desired image of u2 by the rotation
  106.      * @param tolerance relative tolerance factor used to check singularities
  107.      * @exception OrekitException if the vectors are inconsistent for the
  108.      * rotation to be found (null, aligned, ...)
  109.      */
  110.     public FieldAngularCoordinates(final FieldPVCoordinates<T> u1, final FieldPVCoordinates<T> u2,
  111.                                    final FieldPVCoordinates<T> v1, final FieldPVCoordinates<T> v2,
  112.                                    final double tolerance)
  113.         throws OrekitException {

  115.         try {
  116.             // find the initial fixed rotation
  117.             rotation = new FieldRotation<>(u1.getPosition(), u2.getPosition(),
  118.                                            v1.getPosition(), v2.getPosition());

  120.             // find rotation rate Ω such that
  121.             //  Ω ⨯ v₁ = r(dot(u₁)) - dot(v₁)
  122.             //  Ω ⨯ v₂ = r(dot(u₂)) - dot(v₂)
  123.             final FieldVector3D<T> ru1Dot = rotation.applyTo(u1.getVelocity());
  124.             final FieldVector3D<T> ru2Dot = rotation.applyTo(u2.getVelocity());


  127.             rotationRate = inverseCrossProducts(v1.getPosition(), ru1Dot.subtract(v1.getVelocity()),
  128.                                                 v2.getPosition(), ru2Dot.subtract(v2.getVelocity()),
  129.                                                 tolerance);


  132.             // find rotation acceleration dot(Ω) such that
  133.             // dot(Ω) ⨯ v₁ = r(dotdot(u₁)) - 2 Ω ⨯ dot(v₁) - Ω ⨯  (Ω ⨯ v₁) - dotdot(v₁)
  134.             // dot(Ω) ⨯ v₂ = r(dotdot(u₂)) - 2 Ω ⨯ dot(v₂) - Ω ⨯  (Ω ⨯ v₂) - dotdot(v₂)
  135.             final FieldVector3D<T> ru1DotDot = rotation.applyTo(u1.getAcceleration());
  136.             final FieldVector3D<T> oDotv1    = FieldVector3D.crossProduct(rotationRate, v1.getVelocity());
  137.             final FieldVector3D<T> oov1      = FieldVector3D.crossProduct(rotationRate, rotationRate.crossProduct(v1.getPosition()));
  138.             final FieldVector3D<T> c1        = new FieldVector3D<>(1, ru1DotDot, -2, oDotv1, -1, oov1, -1, v1.getAcceleration());
  139.             final FieldVector3D<T> ru2DotDot = rotation.applyTo(u2.getAcceleration());
  140.             final FieldVector3D<T> oDotv2    = FieldVector3D.crossProduct(rotationRate, v2.getVelocity());
  141.             final FieldVector3D<T> oov2      = FieldVector3D.crossProduct(rotationRate, rotationRate.crossProduct( v2.getPosition()));
  142.             final FieldVector3D<T> c2        = new FieldVector3D<>(1, ru2DotDot, -2, oDotv2, -1, oov2, -1, v2.getAcceleration());
  143.             rotationAcceleration     = inverseCrossProducts(v1.getPosition(), c1, v2.getPosition(), c2, tolerance);

  145.         } catch (MathIllegalArgumentException miae) {
  146.             throw new OrekitException(miae);
  147.         }

  149.     }

  151.     /** Builds a FieldAngularCoordinates from a field and a regular AngularCoordinates.
  152.      * @param field field for the components
  153.      * @param ang AngularCoordinates to convert
  154.      */
  155.     public FieldAngularCoordinates(final Field<T> field, final AngularCoordinates ang) {
  156.         this.rotation             = new FieldRotation<>(field, ang.getRotation());
  157.         this.rotationRate         = new FieldVector3D<>(field, ang.getRotationRate());
  158.         this.rotationAcceleration = new FieldVector3D<>(field, ang.getRotationAcceleration());
  159.     }

  161.     /** Fixed orientation parallel with reference frame
  162.      * (identity rotation, zero rotation rate and acceleration).
  163.      * @param field field for the components
  164.      * @param <T> the type of the field elements
  165.      * @return a new fixed orientation parallel with reference frame
  166.      */
  167.     public static <T extends RealFieldElement<T>> FieldAngularCoordinates<T> getIdentity(final Field<T> field) {
  168.         return new FieldAngularCoordinates<>(field, AngularCoordinates.IDENTITY);
  169.     }

  171.     /** Find a vector from two known cross products.
  172.      * <p>
  173.      * We want to find Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
  174.      * </p>
  175.      * <p>
  176.      * The first equation (Ω ⨯ v₁ = c₁) will always be fulfilled exactly,
  177.      * and the second one will be fulfilled if possible.
  178.      * </p>
  179.      * @param v1 vector forming the first known cross product
  180.      * @param c1 know vector for cross product Ω ⨯ v₁
  181.      * @param v2 vector forming the second known cross product
  182.      * @param c2 know vector for cross product Ω ⨯ v₂
  183.      * @param tolerance relative tolerance factor used to check singularities
  184.      * @param <T> the type of the field elements
  185.      * @return vector Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
  186.      * @exception MathIllegalArgumentException if vectors are inconsistent and
  187.      * no solution can be found
  188.      */
  189.     private static <T extends RealFieldElement<T>> FieldVector3D<T> inverseCrossProducts(final FieldVector3D<T> v1, final FieldVector3D<T> c1,
  190.                                                                                          final FieldVector3D<T> v2, final FieldVector3D<T> c2,
  191.                                                                                          final double tolerance)
  192.         throws MathIllegalArgumentException {

  194.         final T v12 = v1.getNormSq();
  195.         final T v1n = v12.sqrt();
  196.         final T v22 = v2.getNormSq();
  197.         final T v2n = v22.sqrt();
  198.         final T threshold;
  199.         if (v1n.getReal() >= v2n.getReal()) {
  200.             threshold = v1n.multiply(tolerance);
  201.         }
  202.         else {
  203.             threshold = v2n.multiply(tolerance);
  204.         }
  205.         FieldVector3D<T> omega = null;

  207.         try {
  208.             // create the over-determined linear system representing the two cross products
  209.             final FieldMatrix<T> m = MatrixUtils.createFieldMatrix(v12.getField(), 6, 3);
  210.             m.setEntry(0, 1, v1.getZ());
  211.             m.setEntry(0, 2, v1.getY().negate());
  212.             m.setEntry(1, 0, v1.getZ().negate());
  213.             m.setEntry(1, 2, v1.getX());
  214.             m.setEntry(2, 0, v1.getY());
  215.             m.setEntry(2, 1, v1.getX().negate());
  216.             m.setEntry(3, 1, v2.getZ());
  217.             m.setEntry(3, 2, v2.getY().negate());
  218.             m.setEntry(4, 0, v2.getZ().negate());
  219.             m.setEntry(4, 2, v2.getX());
  220.             m.setEntry(5, 0, v2.getY());
  221.             m.setEntry(5, 1, v2.getX().negate());

  223.             final T[] kk = MathArrays.buildArray(v2n.getField(), 6);
  224.             kk[0] = c1.getX();
  225.             kk[1] = c1.getY();
  226.             kk[2] = c1.getZ();
  227.             kk[3] = c2.getX();
  228.             kk[4] = c2.getY();
  229.             kk[5] = c2.getZ();
  230.             final FieldVector<T> rhs = MatrixUtils.createFieldVector(kk);

  232.             // find the best solution we can
  233.             final FieldDecompositionSolver<T> solver = new FieldQRDecomposition<>(m).getSolver();
  234.             final FieldVector<T> v = solver.solve(rhs);
  235.             omega = new FieldVector3D<>(v.getEntry(0), v.getEntry(1), v.getEntry(2));

  237.         } catch (MathIllegalArgumentException miae) {
  238.             if (miae.getSpecifier() == LocalizedCoreFormats.SINGULAR_MATRIX) {

  240.                 // handle some special cases for which we can compute a solution
  241.                 final T c12 = c1.getNormSq();
  242.                 final T c1n = c12.sqrt();
  243.                 final T c22 = c2.getNormSq();
  244.                 final T c2n = c22.sqrt();
  245.                 if (c1n.getReal() <= threshold.getReal() && c2n.getReal() <= threshold.getReal()) {
  246.                     // simple special case, velocities are cancelled
  247.                     return new FieldVector3D<>(v12.getField().getZero(), v12.getField().getZero(), v12.getField().getZero());
  248.                 } else if (v1n.getReal() <= threshold.getReal() && c1n.getReal() >= threshold.getReal()) {
  249.                     // this is inconsistent, if v₁ is zero, c₁ must be 0 too
  250.                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE,
  251.                                                            c1n.getReal(), 0, true);
  252.                 } else if (v2n.getReal() <= threshold.getReal() && c2n.getReal() >= threshold.getReal()) {
  253.                     // this is inconsistent, if v₂ is zero, c₂ must be 0 too
  254.                     throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE,
  255.                                                            c2n.getReal(), 0, true);
  256.                 } else if (v1.crossProduct(v1).getNorm().getReal() <= threshold.getReal() && v12.getReal() > threshold.getReal()) {
  257.                     // simple special case, v₂ is redundant with v₁, we just ignore it
  258.                     // use the simplest Ω: orthogonal to both v₁ and c₁
  259.                     omega = new FieldVector3D<>(v12.reciprocal(), v1.crossProduct(c1));
  260.                 }
  261.             } else {
  262.                 throw miae;
  263.             }
  264.         }
  265.         // check results
  266.         final T d1 = FieldVector3D.distance(omega.crossProduct(v1), c1);
  267.         if (d1.getReal() > threshold.getReal()) {
  268.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, 0, true);
  269.         }

  271.         final T d2 = FieldVector3D.distance(omega.crossProduct(v2), c2);
  272.         if (d2.getReal() > threshold.getReal()) {
  273.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, 0, true);
  274.         }

  276.         return omega;

  278.     }

  280.     /** Estimate rotation rate between two orientations.
  281.      * <p>Estimation is based on a simple fixed rate rotation
  282.      * during the time interval between the two orientations.</p>
  283.      * @param start start orientation
  284.      * @param end end orientation
  285.      * @param dt time elapsed between the dates of the two orientations
  286.      * @param <T> the type of the field elements
  287.      * @return rotation rate allowing to go from start to end orientations
  288.      */
  289.     public static <T extends RealFieldElement<T>>
  290.         FieldVector3D<T> estimateRate(final FieldRotation<T> start,
  291.                                       final FieldRotation<T> end,
  292.                                       final double dt) {
  293.         return estimateRate(start, end, start.getQ0().getField().getZero().add(dt));
  294.     }

  296.     /** Estimate rotation rate between two orientations.
  297.      * <p>Estimation is based on a simple fixed rate rotation
  298.      * during the time interval between the two orientations.</p>
  299.      * @param start start orientation
  300.      * @param end end orientation
  301.      * @param dt time elapsed between the dates of the two orientations
  302.      * @param <T> the type of the field elements
  303.      * @return rotation rate allowing to go from start to end orientations
  304.      */
  305.     public static <T extends RealFieldElement<T>>
  306.         FieldVector3D<T> estimateRate(final FieldRotation<T> start,
  307.                                       final FieldRotation<T> end,
  308.                                       final T dt) {
  309.         final FieldRotation<T> evolution = start.compose(end.revert(), RotationConvention.VECTOR_OPERATOR);
  310.         return new FieldVector3D<>(evolution.getAngle().divide(dt),
  311.                                    evolution.getAxis(RotationConvention.VECTOR_OPERATOR));
  312.     }

  314.     /**
  315.      * Revert a rotation / rotation rate / rotation acceleration triplet.
  316.      *
  317.      * <p> Build a triplet which reverse the effect of another triplet.
  318.      *
  319.      * @return a new triplet whose effect is the reverse of the effect
  320.      * of the instance
  321.      */
  322.     public FieldAngularCoordinates<T> revert() {
  323.         return new FieldAngularCoordinates<>(rotation.revert(),
  324.                                              rotation.applyInverseTo(rotationRate.negate()),
  325.                                              rotation.applyInverseTo(rotationAcceleration.negate()));
  326.     }

  328.     /** Get a time-shifted state.
  329.      * <p>
  330.      * The state can be slightly shifted to close dates. This shift is based on
  331.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  332.      * proper attitude propagation but should be sufficient for either small
  333.      * time shifts or coarse accuracy.
  334.      * </p>
  335.      * @param dt time shift in seconds
  336.      * @return a new state, shifted with respect to the instance (which is immutable)
  337.      */
  338.     public FieldAngularCoordinates<T> shiftedBy(final double dt) {
  339.         return shiftedBy(rotation.getQ0().getField().getZero().add(dt));
  340.     }

  342.     /** Get a time-shifted state.
  343.      * <p>
  344.      * The state can be slightly shifted to close dates. This shift is based on
  345.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  346.      * proper attitude propagation but should be sufficient for either small
  347.      * time shifts or coarse accuracy.
  348.      * </p>
  349.      * @param dt time shift in seconds
  350.      * @return a new state, shifted with respect to the instance (which is immutable)
  351.      */
  352.     public FieldAngularCoordinates<T> shiftedBy(final T dt) {

  354.         // the shiftedBy method is based on a local approximation.
  355.         // It considers separately the contribution of the constant
  356.         // rotation, the linear contribution or the rate and the
  357.         // quadratic contribution of the acceleration. The rate
  358.         // and acceleration contributions are small rotations as long
  359.         // as the time shift is small, which is the crux of the algorithm.
  360.         // Small rotations are almost commutative, so we append these small
  361.         // contributions one after the other, as if they really occurred
  362.         // successively, despite this is not what really happens.

  364.         // compute the linear contribution first, ignoring acceleration
  365.         // BEWARE: there is really a minus sign here, because if
  366.         // the target frame rotates in one direction, the vectors in the origin
  367.         // frame seem to rotate in the opposite direction
  368.         final T rate = rotationRate.getNorm();
  369.         final T zero = rate.getField().getZero();
  370.         final T one  = rate.getField().getOne();
  371.         final FieldRotation<T> rateContribution = (rate.getReal() == 0.0) ?
  372.                                                   new FieldRotation<>(one, zero, zero, zero, false) :
  373.                                                   new FieldRotation<>(rotationRate,
  374.                                                                       rate.multiply(dt),
  375.                                                                       RotationConvention.FRAME_TRANSFORM);

  377.         // append rotation and rate contribution
  378.         final FieldAngularCoordinates<T> linearPart =
  379.                 new FieldAngularCoordinates<>(rateContribution.compose(rotation, RotationConvention.VECTOR_OPERATOR),
  380.                                               rotationRate);

  382.         final T acc  = rotationAcceleration.getNorm();
  383.         if (acc.getReal() == 0.0) {
  384.             // no acceleration, the linear part is sufficient
  385.             return linearPart;
  386.         }

  388.         // compute the quadratic contribution, ignoring initial rotation and rotation rate
  389.         // BEWARE: there is really a minus sign here, because if
  390.         // the target frame rotates in one direction, the vectors in the origin
  391.         // frame seem to rotate in the opposite direction
  392.         final FieldAngularCoordinates<T> quadraticContribution =
  393.                 new FieldAngularCoordinates<>(new FieldRotation<>(rotationAcceleration,
  394.                                                                   acc.multiply(dt.multiply(0.5).multiply(dt)),
  395.                                                                   RotationConvention.FRAME_TRANSFORM),
  396.                                               new FieldVector3D<>(dt, rotationAcceleration),
  397.                                               rotationAcceleration);

  399.         // the quadratic contribution is a small rotation:
  400.         // its initial angle and angular rate are both zero.
  401.         // small rotations are almost commutative, so we append the small
  402.         // quadratic part after the linear part as a simple offset
  403.         return quadraticContribution.addOffset(linearPart);

  405.     }

  407.     /** Get the rotation.
  408.      * @return the rotation.
  409.      */
  410.     public FieldRotation<T> getRotation() {
  411.         return rotation;
  412.     }

  414.     /** Get the rotation rate.
  415.      * @return the rotation rate vector (rad/s).
  416.      */
  417.     public FieldVector3D<T> getRotationRate() {
  418.         return rotationRate;
  419.     }

  421.     /** Get the rotation acceleration.
  422.      * @return the rotation acceleration vector dΩ/dt (rad²/s²).
  423.      */
  424.     public FieldVector3D<T> getRotationAcceleration() {
  425.         return rotationAcceleration;
  426.     }

  428.     /** Add an offset from the instance.
  429.      * <p>
  430.      * We consider here that the offset rotation is applied first and the
  431.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  432.      * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
  433.      * b.addOffset(a)} lead to <em>different</em> results in most cases.
  434.      * </p>
  435.      * <p>
  436.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  437.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  438.      * so that round trip applications are possible. This means that both {@code
  439.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  440.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  441.      * </p>
  442.      * @param offset offset to subtract
  443.      * @return new instance, with offset subtracted
  444.      * @see #subtractOffset(FieldAngularCoordinates)
  445.      */
  446.     public FieldAngularCoordinates<T> addOffset(final FieldAngularCoordinates<T> offset) {
  447.         final FieldVector3D<T> rOmega    = rotation.applyTo(offset.rotationRate);
  448.         final FieldVector3D<T> rOmegaDot = rotation.applyTo(offset.rotationAcceleration);
  449.         return new FieldAngularCoordinates<>(rotation.compose(offset.rotation, RotationConvention.VECTOR_OPERATOR),
  450.                                              rotationRate.add(rOmega),
  451.                                              new FieldVector3D<>( 1.0, rotationAcceleration,
  452.                                                                   1.0, rOmegaDot,
  453.                                                                  -1.0, FieldVector3D.crossProduct(rotationRate, rOmega)));
  454.     }

  456.     /** Subtract an offset from the instance.
  457.      * <p>
  458.      * We consider here that the offset Rotation is applied first and the
  459.      * instance is applied afterward. Note that angular coordinates do <em>not</em>
  460.      * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
  461.      * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
  462.      * </p>
  463.      * <p>
  464.      * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
  465.      * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
  466.      * so that round trip applications are possible. This means that both {@code
  467.      * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
  468.      * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
  469.      * </p>
  470.      * @param offset offset to subtract
  471.      * @return new instance, with offset subtracted
  472.      * @see #addOffset(FieldAngularCoordinates)
  473.      */
  474.     public FieldAngularCoordinates<T> subtractOffset(final FieldAngularCoordinates<T> offset) {
  475.         return addOffset(offset.revert());
  476.     }

  478.     /** Convert to a regular angular coordinates.
  479.      * @return a regular angular coordinates
  480.      */
  481.     public AngularCoordinates toAngularCoordinates() {
  482.         return new AngularCoordinates(rotation.toRotation(), rotationRate.toVector3D(),
  483.                                       rotationAcceleration.toVector3D());
  484.     }

  486.     /** Apply the rotation to a pv coordinates.
  487.      * @param pv vector to apply the rotation to
  488.      * @return a new pv coordinates which is the image of u by the rotation
  489.      */
  490.     public FieldPVCoordinates<T> applyTo(final PVCoordinates pv) {

  492.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  493.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  494.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  495.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  496.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  497.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  498.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  499.                                                                   -2, crossV,
  500.                                                                   -1, crossCrossP,
  501.                                                                   -1, crossDotP);

  503.         return new FieldPVCoordinates<>(transformedP, transformedV, transformedA);

  505.     }

  507.     /** Apply the rotation to a pv coordinates.
  508.      * @param pv vector to apply the rotation to
  509.      * @return a new pv coordinates which is the image of u by the rotation
  510.      */
  511.     public TimeStampedFieldPVCoordinates<T> applyTo(final TimeStampedPVCoordinates pv) {

  513.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  514.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  515.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  516.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  517.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  518.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  519.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  520.                                                                   -2, crossV,
  521.                                                                   -1, crossCrossP,
  522.                                                                   -1, crossDotP);

  524.         return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV, transformedA);

  526.     }

  528.     /** Apply the rotation to a pv coordinates.
  529.      * @param pv vector to apply the rotation to
  530.      * @return a new pv coordinates which is the image of u by the rotation
  531.      * @since 9.0
  532.      */
  533.     public FieldPVCoordinates<T> applyTo(final FieldPVCoordinates<T> pv) {

  535.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  536.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  537.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  538.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  539.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  540.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  541.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  542.                                                                   -2, crossV,
  543.                                                                   -1, crossCrossP,
  544.                                                                   -1, crossDotP);

  546.         return new FieldPVCoordinates<>(transformedP, transformedV, transformedA);

  548.     }

  550.     /** Apply the rotation to a pv coordinates.
  551.      * @param pv vector to apply the rotation to
  552.      * @return a new pv coordinates which is the image of u by the rotation
  553.      * @since 9.0
  554.      */
  555.     public TimeStampedFieldPVCoordinates<T> applyTo(final TimeStampedFieldPVCoordinates<T> pv) {

  557.         final FieldVector3D<T> transformedP = rotation.applyTo(pv.getPosition());
  558.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(rotationRate, transformedP);
  559.         final FieldVector3D<T> transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP);
  560.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(rotationRate, transformedV);
  561.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(rotationRate, crossP);
  562.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(rotationAcceleration, transformedP);
  563.         final FieldVector3D<T> transformedA = new FieldVector3D<>( 1, rotation.applyTo(pv.getAcceleration()),
  564.                                                                   -2, crossV,
  565.                                                                   -1, crossCrossP,
  566.                                                                   -1, crossDotP);

  568.         return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV, transformedA);

  570.     }

  572.     /** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives.
  573.      * <p>
  574.      * The modified Rodrigues vector is tan(θ/4) u where θ and u are the
  575.      * rotation angle and axis respectively.
  576.      * </p>
  577.      * @param sign multiplicative sign for quaternion components
  578.      * @return modified Rodrigues vector and derivatives (vector on row 0, first derivative
  579.      * on row 1, second derivative on row 2)
  580.      * @see #createFromModifiedRodrigues(RealFieldElement[][])
  581.      * @since 9.0
  582.      */
  583.     public T[][] getModifiedRodrigues(final double sign) {

  585.         final T q0    = getRotation().getQ0().multiply(sign);
  586.         final T q1    = getRotation().getQ1().multiply(sign);
  587.         final T q2    = getRotation().getQ2().multiply(sign);
  588.         final T q3    = getRotation().getQ3().multiply(sign);
  589.         final T oX    = getRotationRate().getX();
  590.         final T oY    = getRotationRate().getY();
  591.         final T oZ    = getRotationRate().getZ();
  592.         final T oXDot = getRotationAcceleration().getX();
  593.         final T oYDot = getRotationAcceleration().getY();
  594.         final T oZDot = getRotationAcceleration().getZ();

  596.         // first time-derivatives of the quaternion
  597.         final T q0Dot = q0.linearCombination(q1.negate(), oX, q2.negate(), oY, q3.negate(), oZ).multiply(0.5);
  598.         final T q1Dot = q0.linearCombination( q0, oX, q3.negate(), oY,  q2, oZ).multiply(0.5);
  599.         final T q2Dot = q0.linearCombination( q3, oX,  q0, oY, q1.negate(), oZ).multiply(0.5);
  600.         final T q3Dot = q0.linearCombination(q2.negate(), oX,  q1, oY,  q0, oZ).multiply(0.5);

  602.         // second time-derivatives of the quaternion
  603.         final T q0DotDot = linearCombination(q1, oXDot, q2, oYDot, q3, oZDot,
  604.                                              q1Dot, oX, q2Dot, oY, q3Dot, oZ).
  605.                            multiply(-0.5);
  606.         final T q1DotDot = linearCombination(q0, oXDot, q2, oZDot, q3.negate(), oYDot,
  607.                                              q0Dot, oX, q2Dot, oZ, q3Dot.negate(), oY).
  608.                            multiply(0.5);
  609.         final T q2DotDot = linearCombination(q0, oYDot, q3, oXDot, q1.negate(), oZDot,
  610.                                              q0Dot, oY, q3Dot, oX, q1Dot.negate(), oZ).
  611.                            multiply(0.5);
  612.         final T q3DotDot = linearCombination(q0, oZDot, q1, oYDot, q2.negate(), oXDot,
  613.                                              q0Dot, oZ, q1Dot, oY, q2Dot.negate(), oX).
  614.                            multiply(0.5);

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

  623.         final T r1       = inv.multiply(q1);
  624.         final T r2       = inv.multiply(q2);
  625.         final T r3       = inv.multiply(q3);

  627.         final T mInvR1   = inv.multiply(r1).negate();
  628.         final T mInvR2   = inv.multiply(r2).negate();
  629.         final T mInvR3   = inv.multiply(r3).negate();

  631.         final T r1Dot    = q0.linearCombination(inv, q1Dot, mInvR1, q0Dot);
  632.         final T r2Dot    = q0.linearCombination(inv, q2Dot, mInvR2, q0Dot);
  633.         final T r3Dot    = q0.linearCombination(inv, q3Dot, mInvR3, q0Dot);

  635.         final T r1DotDot = q0.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot);
  636.         final T r2DotDot = q0.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot);
  637.         final T r3DotDot = q0.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot);

  639.         final T[][] rodrigues = MathArrays.buildArray(q0.getField(), 3, 3);
  640.         rodrigues[0][0] = r1;
  641.         rodrigues[0][1] = r2;
  642.         rodrigues[0][2] = r3;
  643.         rodrigues[1][0] = r1Dot;
  644.         rodrigues[1][1] = r2Dot;
  645.         rodrigues[1][2] = r3Dot;
  646.         rodrigues[2][0] = r1DotDot;
  647.         rodrigues[2][1] = r2DotDot;
  648.         rodrigues[2][2] = r3DotDot;
  649.         return rodrigues;

  651.     }

  653.     /**
  654.      * Compute a linear combination.
  655.      * @param a1 first factor of the first term
  656.      * @param b1 second factor of the first term
  657.      * @param a2 first factor of the second term
  658.      * @param b2 second factor of the second term
  659.      * @param a3 first factor of the third term
  660.      * @param b3 second factor of the third term
  661.      * @param a4 first factor of the fourth term
  662.      * @param b4 second factor of the fourth term
  663.      * @param a5 first factor of the fifth term
  664.      * @param b5 second factor of the fifth term
  665.      * @param a6 first factor of the sixth term
  666.      * @param b6 second factor of the sicth term
  667.      * @return a<sub>1</sub>&times;b<sub>1</sub> + a<sub>2</sub>&times;b<sub>2</sub> +
  668.      * a<sub>3</sub>&times;b<sub>3</sub> + a<sub>4</sub>&times;b<sub>4</sub> +
  669.      * a<sub>5</sub>&times;b<sub>5</sub> + a<sub>6</sub>&times;b<sub>6</sub>
  670.      */
  671.     private T linearCombination(final T a1, final T b1, final T a2, final T b2, final T a3, final T b3,
  672.                                 final T a4, final T b4, final T a5, final T b5, final T a6, final T b6) {

  674.         final T[] a = MathArrays.buildArray(a1.getField(), 6);
  675.         a[0] = a1;
  676.         a[1] = a2;
  677.         a[2] = a3;
  678.         a[3] = a4;
  679.         a[4] = a5;
  680.         a[5] = a6;

  682.         final T[] b = MathArrays.buildArray(b1.getField(), 6);
  683.         b[0] = b1;
  684.         b[1] = b2;
  685.         b[2] = b3;
  686.         b[3] = b4;
  687.         b[4] = b5;
  688.         b[5] = b6;

  690.         return a1.linearCombination(a, b);

  692.     }

  694.     /** Convert a modified Rodrigues vector and derivatives to angular coordinates.
  695.      * @param r modified Rodrigues vector (with first and second times derivatives)
  696.      * @param <T> the type of the field elements
  697.      * @return angular coordinates
  698.      * @see #getModifiedRodrigues(double)
  699.      * @since 9.0
  700.      */
  701.     public static <T extends RealFieldElement<T>>  FieldAngularCoordinates<T> createFromModifiedRodrigues(final T[][] r) {

  703.         // rotation
  704.         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]));
  705.         final T oPQ0     = rSquared.add(1).reciprocal().multiply(2);
  706.         final T q0       = oPQ0.subtract(1);
  707.         final T q1       = oPQ0.multiply(r[0][0]);
  708.         final T q2       = oPQ0.multiply(r[0][1]);
  709.         final T q3       = oPQ0.multiply(r[0][2]);

  711.         // rotation rate
  712.         final T oPQ02    = oPQ0.multiply(oPQ0);
  713.         final T q0Dot    = oPQ02.multiply(q0.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1],  r[0][2], r[1][2])).negate();
  714.         final T q1Dot    = oPQ0.multiply(r[1][0]).add(r[0][0].multiply(q0Dot));
  715.         final T q2Dot    = oPQ0.multiply(r[1][1]).add(r[0][1].multiply(q0Dot));
  716.         final T q3Dot    = oPQ0.multiply(r[1][2]).add(r[0][2].multiply(q0Dot));
  717.         final T oX       = q0.linearCombination(q1.negate(), q0Dot,  q0, q1Dot,  q3, q2Dot, q2.negate(), q3Dot).multiply(2);
  718.         final T oY       = q0.linearCombination(q2.negate(), q0Dot, q3.negate(), q1Dot,  q0, q2Dot,  q1, q3Dot).multiply(2);
  719.         final T oZ       = q0.linearCombination(q3.negate(), q0Dot,  q2, q1Dot, q1.negate(), q2Dot,  q0, q3Dot).multiply(2);

  721.         // rotation acceleration
  722.         final T q0DotDot = q0.subtract(1).negate().divide(oPQ0).multiply(q0Dot).multiply(q0Dot).
  723.                            subtract(oPQ02.multiply(q0.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]))).
  724.                            subtract(q1Dot.multiply(q1Dot).add(q2Dot.multiply(q2Dot)).add(q3Dot.multiply(q3Dot)));
  725.         final T q1DotDot = q0.linearCombination(oPQ0, r[2][0], r[1][0].add(r[1][0]), q0Dot, r[0][0], q0DotDot);
  726.         final T q2DotDot = q0.linearCombination(oPQ0, r[2][1], r[1][1].add(r[1][1]), q0Dot, r[0][1], q0DotDot);
  727.         final T q3DotDot = q0.linearCombination(oPQ0, r[2][2], r[1][2].add(r[1][2]), q0Dot, r[0][2], q0DotDot);
  728.         final T oXDot    = q0.linearCombination(q1.negate(), q0DotDot,  q0, q1DotDot,  q3, q2DotDot, q2.negate(), q3DotDot).multiply(2);
  729.         final T oYDot    = q0.linearCombination(q2.negate(), q0DotDot, q3.negate(), q1DotDot,  q0, q2DotDot,  q1, q3DotDot).multiply(2);
  730.         final T oZDot    = q0.linearCombination(q3.negate(), q0DotDot,  q2, q1DotDot, q1.negate(), q2DotDot,  q0, q3DotDot).multiply(2);

  732.         return new FieldAngularCoordinates<>(new FieldRotation<>(q0, q1, q2, q3, false),
  733.                                              new FieldVector3D<>(oX, oY, oZ),
  734.                                              new FieldVector3D<>(oXDot, oYDot, oZDot));

  736.     }

  738. }
Created with JaCoCo 0.7.9.201702052155