AttitudeType.java

  1. /* Copyright 2002-2023 CS GROUP
  2.  * Licensed to CS GROUP (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.files.ccsds.ndm.adm;

  19. import java.util.Arrays;
  20. import java.util.Collections;
  21. import java.util.HashMap;
  22. import java.util.Map;

  24. import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
  25. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  26. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  27. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  28. import org.hipparchus.geometry.euclidean.threed.Rotation;
  29. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  30. import org.hipparchus.geometry.euclidean.threed.RotationOrder;
  31. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  32. import org.hipparchus.util.FastMath;
  33. import org.hipparchus.util.MathUtils;
  34. import org.hipparchus.util.SinCos;
  35. import org.orekit.attitudes.Attitude;
  36. import org.orekit.errors.OrekitException;
  37. import org.orekit.errors.OrekitMessages;
  38. import org.orekit.files.ccsds.definitions.Units;
  39. import org.orekit.files.ccsds.utils.ContextBinding;
  40. import org.orekit.time.AbsoluteDate;
  41. import org.orekit.utils.AccurateFormatter;
  42. import org.orekit.utils.AngularDerivativesFilter;
  43. import org.orekit.utils.TimeStampedAngularCoordinates;
  44. import org.orekit.utils.units.Unit;

  46. /** Enumerate for ADM attitude type.
  47.  * @author Bryan Cazabonne
  48.  * @since 10.2
  49.  */
  50. public enum AttitudeType {

  52.     /** Quaternion. */
  53.     QUATERNION(Collections.singleton(new VersionedName(1.0, "QUATERNION")),
  54.                AngularDerivativesFilter.USE_R,
  55.                Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE) {

  57.         /** {@inheritDoc} */
  58.         @Override
  59.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  60.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  61.                                      final TimeStampedAngularCoordinates coordinates) {

  63.             // Data index
  64.             final int[] quaternionIndex = isFirst ? new int[] {0, 1, 2, 3} : new int[] {3, 0, 1, 2};

  66.             Rotation rotation  = coordinates.getRotation();
  67.             if (!isExternal2SpacecraftBody) {
  68.                 rotation = rotation.revert();
  69.             }

  71.             // Fill the array, taking care of quaternion ordering
  72.             final double[] data = new double[4];
  73.             data[quaternionIndex[0]] = rotation.getQ0();
  74.             data[quaternionIndex[1]] = rotation.getQ1();
  75.             data[quaternionIndex[2]] = rotation.getQ2();
  76.             data[quaternionIndex[3]] = rotation.getQ3();

  78.             return data;

  80.         }

  82.         /** {@inheritDoc} */
  83.         @Override
  84.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  85.                                                    final boolean isExternal2SpacecraftBody,
  86.                                                    final RotationOrder eulerRotSequence,
  87.                                                    final boolean isSpacecraftBodyRate,
  88.                                                    final AbsoluteDate date,
  89.                                                    final double... components) {

  91.             Rotation rotation = isFirst ?
  92.                                 new Rotation(components[0], components[1], components[2], components[3], true) :
  93.                                 new Rotation(components[3], components[0], components[1], components[2], true);
  94.             if (!isExternal2SpacecraftBody) {
  95.                 rotation = rotation.revert();
  96.             }

  98.             // Return
  99.             return new TimeStampedAngularCoordinates(date, rotation, Vector3D.ZERO, Vector3D.ZERO);

  101.         }

  103.     },

  105.     /** Quaternion and derivatives. */
  106.     QUATERNION_DERIVATIVE(Collections.singleton(new VersionedName(1.0, "QUATERNION/DERIVATIVE")),
  107.                           AngularDerivativesFilter.USE_RR,
  108.                           Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE,
  109.                           Units.ONE_PER_S, Units.ONE_PER_S, Units.ONE_PER_S, Units.ONE_PER_S) {

  111.         /** {@inheritDoc} */
  112.         @Override
  113.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  114.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  115.                                      final TimeStampedAngularCoordinates coordinates) {

  117.             FieldRotation<UnivariateDerivative1> rotation = coordinates.toUnivariateDerivative1Rotation();
  118.             if (!isExternal2SpacecraftBody) {
  119.                 rotation = rotation.revert();
  120.             }

  122.             // Data index
  123.             final int[] quaternionIndex = isFirst ?
  124.                                           new int[] {0, 1, 2, 3, 4, 5, 6, 7} :
  125.                                           new int[] {3, 0, 1, 2, 7, 4, 5, 6};

  127.             // Fill the array, taking care of quaternion ordering
  128.             final double[] data = new double[8];
  129.             data[quaternionIndex[0]] = rotation.getQ0().getValue();
  130.             data[quaternionIndex[1]] = rotation.getQ1().getValue();
  131.             data[quaternionIndex[2]] = rotation.getQ2().getValue();
  132.             data[quaternionIndex[3]] = rotation.getQ3().getValue();
  133.             data[quaternionIndex[4]] = rotation.getQ0().getFirstDerivative();
  134.             data[quaternionIndex[5]] = rotation.getQ1().getFirstDerivative();
  135.             data[quaternionIndex[6]] = rotation.getQ2().getFirstDerivative();
  136.             data[quaternionIndex[7]] = rotation.getQ3().getFirstDerivative();

  138.             return data;

  140.         }

  142.         /** {@inheritDoc} */
  143.         @Override
  144.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  145.                                                    final boolean isExternal2SpacecraftBody,
  146.                                                    final RotationOrder eulerRotSequence,
  147.                                                    final boolean isSpacecraftBodyRate,
  148.                                                    final AbsoluteDate date,
  149.                                                    final double... components) {
  150.             FieldRotation<UnivariateDerivative1> rotation =
  151.                             isFirst ?
  152.                             new FieldRotation<>(new UnivariateDerivative1(components[0], components[4]),
  153.                                                 new UnivariateDerivative1(components[1], components[5]),
  154.                                                 new UnivariateDerivative1(components[2], components[6]),
  155.                                                 new UnivariateDerivative1(components[3], components[7]),
  156.                                                 true) :
  157.                             new FieldRotation<>(new UnivariateDerivative1(components[3], components[7]),
  158.                                                 new UnivariateDerivative1(components[0], components[4]),
  159.                                                 new UnivariateDerivative1(components[1], components[5]),
  160.                                                 new UnivariateDerivative1(components[2], components[6]),
  161.                                                 true);
  162.             if (!isExternal2SpacecraftBody) {
  163.                 rotation = rotation.revert();
  164.             }

  166.             return new TimeStampedAngularCoordinates(date, rotation);

  168.         }

  170.     },

  172.     /** Quaternion and Euler angles rates (only in ADM V1). */
  173.     QUATERNION_EULER_RATES(Collections.singleton(new VersionedName(1.0, "QUATERNION/RATE")),
  174.                            AngularDerivativesFilter.USE_RR,
  175.                            Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE,
  176.                            Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {

  178.         /** {@inheritDoc} */
  179.         @Override
  180.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  181.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  182.                                      final TimeStampedAngularCoordinates coordinates) {

  184.             // Data index
  185.             final int[] quaternionIndex = isFirst ? new int[] {0, 1, 2, 3} : new int[] {3, 0, 1, 2};

  187.             // Attitude
  188.             FieldRotation<UnivariateDerivative1> rotation = coordinates.toUnivariateDerivative1Rotation();
  189.             if (!isExternal2SpacecraftBody) {
  190.                 rotation = rotation.revert();
  191.             }
  192.             final UnivariateDerivative1[] euler = rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);

  194.             // Fill the array, taking care of quaternion ordering
  195.             final double[] data = new double[7];
  196.             data[quaternionIndex[0]] = rotation.getQ0().getValue();
  197.             data[quaternionIndex[1]] = rotation.getQ1().getValue();
  198.             data[quaternionIndex[2]] = rotation.getQ2().getValue();
  199.             data[quaternionIndex[3]] = rotation.getQ3().getValue();
  200.             data[4]                  = euler[0].getFirstDerivative();
  201.             data[5]                  = euler[1].getFirstDerivative();
  202.             data[6]                  = euler[2].getFirstDerivative();

  204.             return data;

  206.         }

  208.         /** {@inheritDoc} */
  209.         @Override
  210.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  211.                                                    final boolean isExternal2SpacecraftBody,
  212.                                                    final RotationOrder eulerRotSequence,
  213.                                                    final boolean isSpacecraftBodyRate,
  214.                                                    final AbsoluteDate date,
  215.                                                    final double... components) {
  216.             // Build the needed objects
  217.             final Rotation rotation = isFirst ?
  218.                                       new Rotation(components[0], components[1], components[2], components[3], true) :
  219.                                       new Rotation(components[3], components[0], components[1], components[2], true);
  220.             final double[] euler = rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);
  221.             final FieldRotation<UnivariateDerivative1> rUD1 =
  222.                             new FieldRotation<>(eulerRotSequence, RotationConvention.FRAME_TRANSFORM,
  223.                                                 new UnivariateDerivative1(euler[0], components[4]),
  224.                                                 new UnivariateDerivative1(euler[1], components[5]),
  225.                                                 new UnivariateDerivative1(euler[2], components[6]));

  227.             // Return
  228.             final TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(date, rUD1);
  229.             return isExternal2SpacecraftBody ? ac : ac.revert();

  231.         }

  233.     },

  235.     /** Quaternion and angular velocity. */
  236.     QUATERNION_ANGVEL(Collections.singleton(new VersionedName(2.0, "QUATERNION/ANGVEL")),
  237.                       AngularDerivativesFilter.USE_RR,
  238.                       Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE,
  239.                       Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {

  241.         /** {@inheritDoc} */
  242.         @Override
  243.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  244.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  245.                                      final TimeStampedAngularCoordinates coordinates) {

  247.             // Data index
  248.             final int[] quaternionIndex = isFirst ? new int[] {0, 1, 2, 3} : new int[] {3, 0, 1, 2};

  250.             // Attitude
  251.             final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
  252.             final Vector3D rotationRate = QUATERNION_ANGVEL.metadataRate(isSpacecraftBodyRate, c.getRotationRate(), c.getRotation());

  254.             // Fill the array, taking care of quaternion ordering
  255.             final double[] data = new double[7];
  256.             data[quaternionIndex[0]] = c.getRotation().getQ0();
  257.             data[quaternionIndex[1]] = c.getRotation().getQ1();
  258.             data[quaternionIndex[2]] = c.getRotation().getQ2();
  259.             data[quaternionIndex[3]] = c.getRotation().getQ3();
  260.             data[4] = rotationRate.getX();
  261.             data[5] = rotationRate.getY();
  262.             data[6] = rotationRate.getZ();

  264.             return data;

  266.         }

  268.         /** {@inheritDoc} */
  269.         @Override
  270.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  271.                                                    final boolean isExternal2SpacecraftBody,
  272.                                                    final RotationOrder eulerRotSequence,
  273.                                                    final boolean isSpacecraftBodyRate,
  274.                                                    final AbsoluteDate date,
  275.                                                    final double... components) {
  276.             // Build the needed objects
  277.             final Rotation rotation = isFirst ?
  278.                                       new Rotation(components[0], components[1], components[2], components[3], true) :
  279.                                       new Rotation(components[3], components[0], components[1], components[2], true);
  280.             final Vector3D rotationRate = QUATERNION_ANGVEL.orekitRate(isSpacecraftBodyRate,
  281.                                                                        new Vector3D(components[4], components[5], components[6]),
  282.                                                                        rotation);

  284.             // Return
  285.             final TimeStampedAngularCoordinates ac =
  286.                             new TimeStampedAngularCoordinates(date, rotation, rotationRate, Vector3D.ZERO);
  287.             return isExternal2SpacecraftBody ? ac : ac.revert();

  289.         }

  291.     },

  293.     /** Euler angles. */
  294.     EULER_ANGLE(Collections.singleton(new VersionedName(1.0, "EULER_ANGLE")),
  295.                 AngularDerivativesFilter.USE_R,
  296.                 Unit.DEGREE, Unit.DEGREE, Unit.DEGREE) {

  298.         /** {@inheritDoc} */
  299.         @Override
  300.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  301.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  302.                                      final TimeStampedAngularCoordinates coordinates) {

  304.             // Attitude
  305.             Rotation rotation = coordinates.getRotation();
  306.             if (!isExternal2SpacecraftBody) {
  307.                 rotation = rotation.revert();
  308.             }

  310.             return rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);

  312.         }

  314.         /** {@inheritDoc} */
  315.         @Override
  316.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  317.                                                    final boolean isExternal2SpacecraftBody,
  318.                                                    final RotationOrder eulerRotSequence,
  319.                                                    final boolean isSpacecraftBodyRate,
  320.                                                    final AbsoluteDate date,
  321.                                                    final double... components) {

  323.             // Build the needed objects
  324.             Rotation rotation = new Rotation(eulerRotSequence, RotationConvention.FRAME_TRANSFORM,
  325.                                              components[0], components[1], components[2]);
  326.             if (!isExternal2SpacecraftBody) {
  327.                 rotation = rotation.revert();
  328.             }

  330.             // Return
  331.             return new TimeStampedAngularCoordinates(date, rotation, Vector3D.ZERO, Vector3D.ZERO);
  332.         }

  334.     },

  336.     /** Euler angles and rotation rate. */
  337.     EULER_ANGLE_DERIVATIVE(Arrays.asList(new VersionedName(1.0, "EULER_ANGLE/RATE"),
  338.                                          new VersionedName(2.0, "EULER_ANGLE/DERIVATIVE")),
  339.                            AngularDerivativesFilter.USE_RR,
  340.                            Unit.DEGREE, Unit.DEGREE, Unit.DEGREE,
  341.                            Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {

  343.         /** {@inheritDoc} */
  344.         @Override
  345.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  346.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  347.                                      final TimeStampedAngularCoordinates coordinates) {

  349.             // Attitude
  350.             FieldRotation<UnivariateDerivative1> rotation = coordinates.toUnivariateDerivative1Rotation();
  351.             if (!isExternal2SpacecraftBody) {
  352.                 rotation = rotation.revert();
  353.             }

  355.             final UnivariateDerivative1[] angles = rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);

  357.             return new double[] {
  358.                 angles[0].getValue(),
  359.                 angles[1].getValue(),
  360.                 angles[2].getValue(),
  361.                 angles[0].getFirstDerivative(),
  362.                 angles[1].getFirstDerivative(),
  363.                 angles[2].getFirstDerivative()
  364.             };

  366.         }

  368.         /** {@inheritDoc} */
  369.         @Override
  370.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  371.                                                    final boolean isExternal2SpacecraftBody,
  372.                                                    final RotationOrder eulerRotSequence,
  373.                                                    final boolean isSpacecraftBodyRate,
  374.                                                    final AbsoluteDate date,
  375.                                                    final double... components) {

  377.             // Build the needed objects
  378.             FieldRotation<UnivariateDerivative1> rotation =
  379.                             new FieldRotation<>(eulerRotSequence, RotationConvention.FRAME_TRANSFORM,
  380.                                                 new UnivariateDerivative1(components[0], components[3]),
  381.                                                 new UnivariateDerivative1(components[1], components[4]),
  382.                                                 new UnivariateDerivative1(components[2], components[5]));
  383.             if (!isExternal2SpacecraftBody) {
  384.                 rotation = rotation.revert();
  385.             }

  387.             return new TimeStampedAngularCoordinates(date, rotation);

  389.         }

  391.     },

  393.     /** Euler angles and angular velocity.
  394.      * @since 12.0
  395.      */
  396.     EULER_ANGLE_ANGVEL(Collections.singleton(new VersionedName(2.0, "EULER_ANGLE/ANGVEL")),
  397.                        AngularDerivativesFilter.USE_RR,
  398.                        Unit.DEGREE, Unit.DEGREE, Unit.DEGREE,
  399.                        Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {

  401.         /** {@inheritDoc} */
  402.         @Override
  403.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  404.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  405.                                      final TimeStampedAngularCoordinates coordinates) {

  407.             // Attitude
  408.             final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
  409.             final Vector3D rotationRate = EULER_ANGLE_ANGVEL.metadataRate(isSpacecraftBodyRate, c.getRotationRate(), c.getRotation());
  410.             final double[] angles       = c.getRotation().getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);

  412.             return new double[] {
  413.                 angles[0],
  414.                 angles[1],
  415.                 angles[2],
  416.                 rotationRate.getX(),
  417.                 rotationRate.getY(),
  418.                 rotationRate.getZ()
  419.             };

  421.         }

  423.         /** {@inheritDoc} */
  424.         @Override
  425.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  426.                                                    final boolean isExternal2SpacecraftBody,
  427.                                                    final RotationOrder eulerRotSequence,
  428.                                                    final boolean isSpacecraftBodyRate,
  429.                                                    final AbsoluteDate date,
  430.                                                    final double... components) {

  432.             // Build the needed objects
  433.             final Rotation rotation = new Rotation(eulerRotSequence,
  434.                                                    RotationConvention.FRAME_TRANSFORM,
  435.                                                    components[0],
  436.                                                    components[1],
  437.                                                    components[2]);
  438.             final Vector3D rotationRate = EULER_ANGLE_ANGVEL.orekitRate(isSpacecraftBodyRate,
  439.                                                                         new Vector3D(components[3], components[4], components[5]),
  440.                                                                         rotation);
  441.             // Return
  442.             final TimeStampedAngularCoordinates ac =
  443.                             new TimeStampedAngularCoordinates(date, rotation, rotationRate, Vector3D.ZERO);
  444.             return isExternal2SpacecraftBody ? ac : ac.revert();

  446.         }

  448.     },

  450.     /** Spin. */
  451.     SPIN(Collections.singleton(new VersionedName(1.0, "SPIN")),
  452.          AngularDerivativesFilter.USE_R,
  453.          Unit.DEGREE, Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S) {

  455.         /** {@inheritDoc} */
  456.         @Override
  457.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  458.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  459.                                      final TimeStampedAngularCoordinates coordinates) {

  461.             // spin axis is forced to Z (but it is not the instantaneous rotation rate as it also moves)
  462.             final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
  463.             final SpinFinder sf = new SpinFinder(c);
  464.             final double spinAngleVel = coordinates.getRotationRate().getZ();

  466.             return new double[] {
  467.                 sf.getSpinAlpha(), sf.getSpinDelta(), sf.getSpinAngle(), spinAngleVel
  468.             };

  470.         }

  472.         /** {@inheritDoc} */
  473.         @Override
  474.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  475.                                                    final boolean isExternal2SpacecraftBody,
  476.                                                    final RotationOrder eulerRotSequence,
  477.                                                    final boolean isSpacecraftBodyRate,
  478.                                                    final AbsoluteDate date,
  479.                                                    final double... components) {

  481.             // Build the needed objects
  482.             final Rotation rotation = new Rotation(RotationOrder.ZXZ,
  483.                                                    RotationConvention.FRAME_TRANSFORM,
  484.                                                    MathUtils.SEMI_PI + components[0],
  485.                                                    MathUtils.SEMI_PI - components[1],
  486.                                                    components[2]);
  487.             final Vector3D rotationRate = new Vector3D(0, 0, components[3]);

  489.             // Return
  490.             final TimeStampedAngularCoordinates ac =
  491.                             new TimeStampedAngularCoordinates(date, rotation, rotationRate, Vector3D.ZERO);
  492.             return isExternal2SpacecraftBody ? ac : ac.revert();

  494.         }

  496.     },

  498.     /** Spin and nutation. */
  499.     SPIN_NUTATION(Collections.singleton(new VersionedName(1.0, "SPIN/NUTATION")),
  500.                   AngularDerivativesFilter.USE_RR,
  501.                   Unit.DEGREE, Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S,
  502.                   Unit.DEGREE, Unit.SECOND, Unit.DEGREE) {

  504.         /** {@inheritDoc} */
  505.         @Override
  506.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  507.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  508.                                      final TimeStampedAngularCoordinates coordinates) {

  510.             // spin data
  511.             final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
  512.             final SpinFinder sf = new SpinFinder(c);

  514.             // Orekit/CCSDS naming difference: for CCSDS this is nutation, for Orekit this is precession
  515.             final FieldRotation<UnivariateDerivative2> c2       = c.toUnivariateDerivative2Rotation();
  516.             final FieldVector3D<UnivariateDerivative2> spinAxis = c2.applyInverseTo(Vector3D.PLUS_K);
  517.             final PrecessionFinder                     pf       = new PrecessionFinder(spinAxis);

  519.             // intermediate inertial frame, with Z axis aligned with angular momentum
  520.             final Rotation intermediate2Inert = new Rotation(Vector3D.PLUS_K, pf.getAxis());

  522.             // recover Euler rotations starting from frame aligned with angular momentum
  523.             final FieldRotation<UnivariateDerivative2> intermediate2Body = c2.applyTo(intermediate2Inert);
  524.             final UnivariateDerivative2[] euler = intermediate2Body.
  525.                                                   getAngles(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM);

  527.             return new double[] {
  528.                 sf.getSpinAlpha(),
  529.                 sf.getSpinDelta(),
  530.                 sf.getSpinAngle(),
  531.                 euler[2].getFirstDerivative(),
  532.                 pf.getPrecessionAngle(),
  533.                 MathUtils.TWO_PI / pf.getAngularVelocity(),
  534.                 euler[2].getValue() - MathUtils.SEMI_PI
  535.             };

  537.         }

  539.         /** {@inheritDoc} */
  540.         @Override
  541.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  542.                                                    final boolean isExternal2SpacecraftBody,
  543.                                                    final RotationOrder eulerRotSequence,
  544.                                                    final boolean isSpacecraftBodyRate,
  545.                                                    final AbsoluteDate date,
  546.                                                    final double... components) {

  548.             // Build the needed objects
  549.             final Rotation inert2Body0 = new Rotation(RotationOrder.ZXZ,
  550.                                                       RotationConvention.FRAME_TRANSFORM,
  551.                                                       MathUtils.SEMI_PI + components[0],
  552.                                                       MathUtils.SEMI_PI - components[1],
  553.                                                       components[2]);

  555.             // intermediate inertial frame, with Z axis aligned with angular momentum
  556.             final SinCos   scNutation         = FastMath.sinCos(components[4]);
  557.             final SinCos   scPhase            = FastMath.sinCos(components[6]);
  558.             final Vector3D momentumBody       = new Vector3D( scNutation.sin() * scPhase.cos(),
  559.                                                              -scNutation.sin() * scPhase.sin(),
  560.                                                               scNutation.cos());
  561.             final Vector3D momentumInert      = inert2Body0.applyInverseTo(momentumBody);
  562.             final Rotation inert2Intermediate = new Rotation(momentumInert, Vector3D.PLUS_K);

  564.             // base Euler angles from the intermediate frame to body
  565.             final Rotation intermediate2Body0 = inert2Body0.applyTo(inert2Intermediate.revert());
  566.             final double[] euler0             = intermediate2Body0.getAngles(RotationOrder.ZXZ,
  567.                                                                              RotationConvention.FRAME_TRANSFORM);

  569.             // add Euler angular rates to base Euler angles
  570.             final FieldRotation<UnivariateDerivative2> intermediate2Body =
  571.                             new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
  572.                                                 new UnivariateDerivative2(euler0[0], MathUtils.TWO_PI / components[5], 0.0),
  573.                                                 new UnivariateDerivative2(euler0[1], 0.0,           0.0),
  574.                                                 new UnivariateDerivative2(euler0[2], components[3], 0.0));

  576.             // final rotation, including derivatives
  577.             final FieldRotation<UnivariateDerivative2> inert2Body = intermediate2Body.applyTo(inert2Intermediate);

  579.             final TimeStampedAngularCoordinates ac =
  580.                             new TimeStampedAngularCoordinates(date, inert2Body);
  581.             return isExternal2SpacecraftBody ? ac : ac.revert();

  583.         }

  585.     },

  587.     /** Spin and momentum.
  588.      * @since 12.0
  589.      */
  590.     SPIN_NUTATION_MOMENTUM(Collections.singleton(new VersionedName(2.0, "SPIN/NUTATION_MOM")),
  591.                            AngularDerivativesFilter.USE_RR,
  592.                            Unit.DEGREE, Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S,
  593.                            Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S) {

  595.         /** {@inheritDoc} */
  596.         @Override
  597.         public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  598.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  599.                                      final TimeStampedAngularCoordinates coordinates) {

  601.             // spin data
  602.             final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
  603.             final SpinFinder sf = new SpinFinder(c);

  605.             // Orekit/CCSDS naming difference: for CCSDS this is nutation, for Orekit this is precession
  606.             final FieldRotation<UnivariateDerivative2> c2       = c.toUnivariateDerivative2Rotation();
  607.             final FieldVector3D<UnivariateDerivative2> spinAxis = c2.applyInverseTo(Vector3D.PLUS_K);
  608.             final PrecessionFinder                     pf       = new PrecessionFinder(spinAxis);

  610.             // intermediate inertial frame, with Z axis aligned with angular momentum
  611.             final Rotation intermediate2Inert = new Rotation(Vector3D.PLUS_K, pf.getAxis());

  613.             // recover spin angle velocity
  614.             final FieldRotation<UnivariateDerivative2> intermediate2Body = c2.applyTo(intermediate2Inert);
  615.             final double spinAngleVel = intermediate2Body.
  616.                                         getAngles(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM)[2].
  617.                                         getFirstDerivative();

  619.             return new double[] {
  620.                 sf.getSpinAlpha(),
  621.                 sf.getSpinDelta(),
  622.                 sf.getSpinAngle(),
  623.                 spinAngleVel,
  624.                 pf.getAxis().getAlpha(),
  625.                 pf.getAxis().getDelta(),
  626.                 pf.getAngularVelocity()
  627.             };

  629.         }

  631.         /** {@inheritDoc} */
  632.         @Override
  633.         public TimeStampedAngularCoordinates build(final boolean isFirst,
  634.                                                    final boolean isExternal2SpacecraftBody,
  635.                                                    final RotationOrder eulerRotSequence,
  636.                                                    final boolean isSpacecraftBodyRate,
  637.                                                    final AbsoluteDate date,
  638.                                                    final double... components) {

  640.             // Build the needed objects
  641.             final SinCos   scAlpha            = FastMath.sinCos(components[4]);
  642.             final SinCos   scDelta            = FastMath.sinCos(components[5]);
  643.             final Vector3D momentumInert      = new Vector3D(scAlpha.cos() * scDelta.cos(),
  644.                                                              scAlpha.sin() * scDelta.cos(),
  645.                                                              scDelta.sin());
  646.             final Rotation inert2Intermediate = new Rotation(momentumInert, Vector3D.PLUS_K);

  648.             // base Euler angles from the intermediate frame to body
  649.             final Rotation inert2Body0 = new Rotation(RotationOrder.ZXZ,
  650.                                                       RotationConvention.FRAME_TRANSFORM,
  651.                                                       MathUtils.SEMI_PI + components[0],
  652.                                                       MathUtils.SEMI_PI - components[1],
  653.                                                       components[2]);
  654.             final Rotation intermediate2Body0 = inert2Body0.applyTo(inert2Intermediate.revert());
  655.             final double[] euler0             = intermediate2Body0.getAngles(RotationOrder.ZXZ,
  656.                                                                              RotationConvention.FRAME_TRANSFORM);

  658.             // add Euler angular rates to base Euler angles
  659.             final FieldRotation<UnivariateDerivative2> intermediate2Body =
  660.                             new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
  661.                                                 new UnivariateDerivative2(euler0[0], components[6], 0.0),
  662.                                                 new UnivariateDerivative2(euler0[1], 0.0,           0.0),
  663.                                                 new UnivariateDerivative2(euler0[2], components[3], 0.0));

  665.             // final rotation, including derivatives
  666.             final FieldRotation<UnivariateDerivative2> inert2Body = intermediate2Body.applyTo(inert2Intermediate);

  668.             // return
  669.             final TimeStampedAngularCoordinates ac =
  670.                             new TimeStampedAngularCoordinates(date, inert2Body);
  671.             return isExternal2SpacecraftBody ? ac : ac.revert();

  673.         }

  675.     };

  677.     /** Names map.
  678.      * @since 12.0
  679.      */
  680.     private static final Map<String, AttitudeType> MAP = new HashMap<>();
  681.     static {
  682.         for (final AttitudeType type : values()) {
  683.             for (final VersionedName vn : type.ccsdsNames) {
  684.                 MAP.put(vn.name, type);
  685.             }
  686.         }
  687.     }

  689.     /** CCSDS names of the attitude type. */
  690.     private final Iterable<VersionedName> ccsdsNames;

  692.     /** Derivatives filter. */
  693.     private final AngularDerivativesFilter filter;

  695.     /** Components units (used only for parsing). */
  696.     private final Unit[] units;

  698.     /** Private constructor.
  699.      * @param ccsdsNames CCSDS names of the attitude type
  700.      * @param filter derivative filter
  701.      * @param units components units (used only for parsing)
  702.      */
  703.     AttitudeType(final Iterable<VersionedName> ccsdsNames, final AngularDerivativesFilter filter, final Unit... units) {
  704.         this.ccsdsNames = ccsdsNames;
  705.         this.filter     = filter;
  706.         this.units      = units.clone();
  707.     }

  709.     /** Get the type name for a given format version.
  710.      * @param formatVersion format version
  711.      * @return type name
  712.      * @since 12.0
  713.      */
  714.     public String getName(final double formatVersion) {
  715.         String name = null;
  716.         for (final VersionedName vn : ccsdsNames) {
  717.             if (name == null || formatVersion >= vn.since) {
  718.                 name = vn.name;
  719.             }
  720.         }
  721.         return name;
  722.     }

  724.     /** {@inheritDoc} */
  725.     @Override
  726.     public String toString() {
  727.         // use the most recent name by default
  728.         return getName(Double.POSITIVE_INFINITY);
  729.     }

  731.     /** Parse an attitude type.
  732.      * @param typeSpecification unnormalized type name
  733.      * @return parsed type
  734.      */
  735.     public static AttitudeType parseType(final String typeSpecification) {
  736.         final AttitudeType type = MAP.get(typeSpecification);
  737.         if (type == null) {
  738.             throw new OrekitException(OrekitMessages.CCSDS_UNKNOWN_ATTITUDE_TYPE, typeSpecification);
  739.         }
  740.         return type;
  741.     }

  743.     /**
  744.      * Get the attitude data fields corresponding to the attitude type.
  745.      * <p>
  746.      * This method returns the components in CCSDS units (i.e. degrees, degrees per seconds…).
  747.      * </p>
  748.      * @param isFirst if true the first quaternion component is the scalar component
  749.      * @param isExternal2SpacecraftBody true attitude is from external frame to spacecraft body frame
  750.      * @param eulerRotSequence sequance of Euler angles
  751.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  752.      * @param attitude angular coordinates, using {@link Attitude Attitude} convention
  753.      * (i.e. from inertial frame to spacecraft frame)
  754.      * @return the attitude data in CCSDS units
  755.      */
  756.     public String[] createDataFields(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  757.                                      final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  758.                                      final TimeStampedAngularCoordinates attitude) {

  760.         // generate the double data
  761.         final double[] data = generateData(isFirst, isExternal2SpacecraftBody,
  762.                                            eulerRotSequence, isSpacecraftBodyRate, attitude);

  764.         // format as string array with CCSDS units
  765.         final String[] fields = new String[data.length];
  766.         for (int i = 0; i < data.length; ++i) {
  767.             fields[i] = AccurateFormatter.format(units[i].fromSI(data[i]));
  768.         }

  770.         return fields;

  772.     }

  774.     /**
  775.      * Generate the attitude data corresponding to the attitude type.
  776.      * <p>
  777.      * This method returns the components in SI units.
  778.      * </p>
  779.      * @param isFirst if true the first quaternion component is the scalar component
  780.      * @param isExternal2SpacecraftBody true attitude is from external frame to spacecraft body frame
  781.      * @param eulerRotSequence sequance of Euler angles
  782.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  783.      * @param attitude angular coordinates, using {@link Attitude Attitude} convention
  784.      * (i.e. from inertial frame to spacecraft frame)
  785.      * @return the attitude data in CCSDS units
  786.      * @since 12.0
  787.      */
  788.     public abstract double[] generateData(boolean isFirst, boolean isExternal2SpacecraftBody,
  789.                                           RotationOrder eulerRotSequence, boolean isSpacecraftBodyRate,
  790.                                           TimeStampedAngularCoordinates attitude);

  792.     /**
  793.      * Get the angular coordinates corresponding to the attitude data.
  794.      * <p>
  795.      * This method assumes the text fields are in CCSDS units and will convert to SI units.
  796.      * </p>
  797.      * @param isFirst if true the first quaternion component is the scalar component
  798.      * @param isExternal2SpacecraftBody true attitude is from external frame to spacecraft body frame
  799.      * @param eulerRotSequence sequance of Euler angles
  800.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  801.      * @param context context binding
  802.      * @param fields raw data fields
  803.      * @return the angular coordinates, using {@link Attitude Attitude} convention
  804.      * (i.e. from inertial frame to spacecraft frame)
  805.      */
  806.     public TimeStampedAngularCoordinates parse(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  807.                                                final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  808.                                                final ContextBinding context, final String[] fields) {

  810.         // parse the text fields
  811.         final AbsoluteDate date = context.getTimeSystem().getConverter(context).parse(fields[0]);
  812.         final double[] components = new double[fields.length - 1];
  813.         for (int i = 0; i < components.length; ++i) {
  814.             components[i] = units[i].toSI(Double.parseDouble(fields[i + 1]));
  815.         }

  817.         // build the coordinates
  818.         return build(isFirst, isExternal2SpacecraftBody, eulerRotSequence, isSpacecraftBodyRate,
  819.                      date, components);

  821.     }

  823.     /** Get the angular coordinates corresponding to the attitude data.
  824.      * @param isFirst if true the first quaternion component is the scalar component
  825.      * @param isExternal2SpacecraftBody true attitude is from external frame to spacecraft body frame
  826.      * @param eulerRotSequence sequance of Euler angles
  827.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  828.      * @param date entry date
  829.      * @param components entry components with SI units, semantic depends on attitude type
  830.      * @return the angular coordinates, using {@link Attitude Attitude} convention
  831.      * (i.e. from inertial frame to spacecraft frame)
  832.      */
  833.     public abstract TimeStampedAngularCoordinates build(boolean isFirst, boolean isExternal2SpacecraftBody,
  834.                                                         RotationOrder eulerRotSequence, boolean isSpacecraftBodyRate,
  835.                                                         AbsoluteDate date, double... components);

  837.     /**
  838.      * Get the angular derivative filter corresponding to the attitude data.
  839.      * @return the angular derivative filter corresponding to the attitude data
  840.      */
  841.     public AngularDerivativesFilter getAngularDerivativesFilter() {
  842.         return filter;
  843.     }

  845.     /** Convert a rotation rate for Orekit convention to metadata convention.
  846.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  847.      * @param rate rotation rate from Orekit attitude
  848.      * @param rotation corresponding rotation
  849.      * @return rotation rate in metadata convention
  850.      */
  851.     private Vector3D metadataRate(final boolean isSpacecraftBodyRate, final Vector3D rate, final Rotation rotation) {
  852.         return isSpacecraftBodyRate ? rate : rotation.applyInverseTo(rate);
  853.     }

  855.     /** Convert a rotation rate for metadata convention to Orekit convention.
  856.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  857.      * @param rate rotation rate read from the data line
  858.      * @param rotation corresponding rotation
  859.      * @return rotation rate in Orekit convention (i.e. in spacecraft body local frame)
  860.      */
  861.     private Vector3D orekitRate(final boolean isSpacecraftBodyRate, final Vector3D rate, final Rotation rotation) {
  862.         return isSpacecraftBodyRate ? rate : rotation.applyTo(rate);
  863.     }

  865.     /** Container for a name associated to a format version.
  866.      * @since 12.0
  867.      */
  868.     private static class VersionedName {

  870.         /** Version at which this name was defined. */
  871.         private final double since;

  873.         /** Name. */
  874.         private final String name;

  876.         /** Simple constructor.
  877.          * @param since version at which this name was defined
  878.          * @param name name
  879.          */
  880.         VersionedName(final double since, final String name) {
  881.             this.since = since;
  882.             this.name  = name;
  883.         }

  885.     }

  887. }
Created with JaCoCo 0.8.11.202310140853