AttitudeType.java

  1. /* Copyright 2002-2025 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.Formatter;
  44. import org.orekit.utils.TimeStampedAngularCoordinates;
  45. import org.orekit.utils.units.Unit;

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

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

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

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

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

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

  79.             return data;

  81.         }

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

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

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

  102.         }

  104.     },

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

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

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

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

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

  139.             return data;

  141.         }

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

  167.             return new TimeStampedAngularCoordinates(date, rotation);

  169.         }

  171.     },

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

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

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

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

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

  205.             return data;

  207.         }

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

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

  232.         }

  234.     },

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

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

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

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

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

  265.             return data;

  267.         }

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

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

  290.         }

  292.     },

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

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

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

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

  313.         }

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

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

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

  335.     },

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

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

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

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

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

  367.         }

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

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

  388.             return new TimeStampedAngularCoordinates(date, rotation);

  390.         }

  392.     },

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

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

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

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

  422.         }

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

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

  447.         }

  449.     },

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

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

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

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

  471.         }

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

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

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

  495.         }

  497.     },

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

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

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

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

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

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

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

  538.         }

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

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

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

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

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

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

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

  584.         }

  586.     },

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

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

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

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

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

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

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

  630.         }

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

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

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

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

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

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

  674.         }

  676.     };

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

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

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

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

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

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

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

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

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

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

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

  772.         return fields;

  774.     }

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

  793.         return this.createDataFields(isFirst, isExternal2SpacecraftBody, eulerRotSequence, isSpacecraftBodyRate, attitude, new AccurateFormatter());
  794.     }

  796.     /**
  797.      * Generate the attitude data corresponding to the attitude type.
  798.      * <p>
  799.      * This method returns the components in SI units.
  800.      * </p>
  801.      * @param isFirst if true the first quaternion component is the scalar component
  802.      * @param isExternal2SpacecraftBody true attitude is from external frame to spacecraft body frame
  803.      * @param eulerRotSequence sequance of Euler angles
  804.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  805.      * @param attitude angular coordinates, using {@link Attitude Attitude} convention
  806.      * (i.e. from inertial frame to spacecraft frame)
  807.      * @return the attitude data in CCSDS units
  808.      * @since 12.0
  809.      */
  810.     public abstract double[] generateData(boolean isFirst, boolean isExternal2SpacecraftBody,
  811.                                           RotationOrder eulerRotSequence, boolean isSpacecraftBodyRate,
  812.                                           TimeStampedAngularCoordinates attitude);

  814.     /**
  815.      * Get the angular coordinates corresponding to the attitude data.
  816.      * <p>
  817.      * This method assumes the text fields are in CCSDS units and will convert to SI units.
  818.      * </p>
  819.      * @param isFirst if true the first quaternion component is the scalar component
  820.      * @param isExternal2SpacecraftBody true attitude is from external frame to spacecraft body frame
  821.      * @param eulerRotSequence sequance of Euler angles
  822.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  823.      * @param context context binding
  824.      * @param fields raw data fields
  825.      * @return the angular coordinates, using {@link Attitude Attitude} convention
  826.      * (i.e. from inertial frame to spacecraft frame)
  827.      */
  828.     public TimeStampedAngularCoordinates parse(final boolean isFirst, final boolean isExternal2SpacecraftBody,
  829.                                                final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
  830.                                                final ContextBinding context, final String[] fields) {

  832.         // parse the text fields
  833.         final AbsoluteDate date = context.getTimeSystem().getConverter(context).parse(fields[0]);
  834.         final double[] components = new double[fields.length - 1];
  835.         for (int i = 0; i < components.length; ++i) {
  836.             components[i] = units[i].toSI(Double.parseDouble(fields[i + 1]));
  837.         }

  839.         // build the coordinates
  840.         return build(isFirst, isExternal2SpacecraftBody, eulerRotSequence, isSpacecraftBodyRate,
  841.                      date, components);

  843.     }

  845.     /** Get the angular coordinates corresponding to the attitude data.
  846.      * @param isFirst if true the first quaternion component is the scalar component
  847.      * @param isExternal2SpacecraftBody true attitude is from external frame to spacecraft body frame
  848.      * @param eulerRotSequence sequance of Euler angles
  849.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  850.      * @param date entry date
  851.      * @param components entry components with SI units, semantic depends on attitude type
  852.      * @return the angular coordinates, using {@link Attitude Attitude} convention
  853.      * (i.e. from inertial frame to spacecraft frame)
  854.      */
  855.     public abstract TimeStampedAngularCoordinates build(boolean isFirst, boolean isExternal2SpacecraftBody,
  856.                                                         RotationOrder eulerRotSequence, boolean isSpacecraftBodyRate,
  857.                                                         AbsoluteDate date, double... components);

  859.     /**
  860.      * Get the angular derivative filter corresponding to the attitude data.
  861.      * @return the angular derivative filter corresponding to the attitude data
  862.      */
  863.     public AngularDerivativesFilter getAngularDerivativesFilter() {
  864.         return filter;
  865.     }

  867.     /** Convert a rotation rate for Orekit convention to metadata convention.
  868.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  869.      * @param rate rotation rate from Orekit attitude
  870.      * @param rotation corresponding rotation
  871.      * @return rotation rate in metadata convention
  872.      */
  873.     private Vector3D metadataRate(final boolean isSpacecraftBodyRate, final Vector3D rate, final Rotation rotation) {
  874.         return isSpacecraftBodyRate ? rate : rotation.applyInverseTo(rate);
  875.     }

  877.     /** Convert a rotation rate for metadata convention to Orekit convention.
  878.      * @param isSpacecraftBodyRate if true Euler rates are specified in spacecraft body frame
  879.      * @param rate rotation rate read from the data line
  880.      * @param rotation corresponding rotation
  881.      * @return rotation rate in Orekit convention (i.e. in spacecraft body local frame)
  882.      */
  883.     private Vector3D orekitRate(final boolean isSpacecraftBodyRate, final Vector3D rate, final Rotation rotation) {
  884.         return isSpacecraftBodyRate ? rate : rotation.applyTo(rate);
  885.     }

  887.     /** Container for a name associated to a format version.
  888.      * @since 12.0
  889.      */
  890.     private static class VersionedName {

  892.         /** Version at which this name was defined. */
  893.         private final double since;

  895.         /** Name. */
  896.         private final String name;

  898.         /** Simple constructor.
  899.          * @param since version at which this name was defined
  900.          * @param name name
  901.          */
  902.         VersionedName(final double since, final String name) {
  903.             this.since = since;
  904.             this.name  = name;
  905.         }

  907.     }

  909. }
Created with JaCoCo 0.8.12.202403310830