FieldTransform.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.frames;

  19. import java.util.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.Collection;
  22. import java.util.List;
  23. import java.util.stream.Stream;

  25. import org.hipparchus.CalculusFieldElement;
  26. import org.hipparchus.Field;
  27. import org.hipparchus.geometry.euclidean.threed.FieldLine;
  28. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  29. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  30. import org.orekit.time.AbsoluteDate;
  31. import org.orekit.time.FieldAbsoluteDate;
  32. import org.orekit.time.FieldTimeInterpolator;
  33. import org.orekit.time.FieldTimeShiftable;
  34. import org.orekit.utils.AngularDerivativesFilter;
  35. import org.orekit.utils.CartesianDerivativesFilter;
  36. import org.orekit.utils.FieldAngularCoordinates;
  37. import org.orekit.utils.FieldPVCoordinates;
  38. import org.orekit.utils.PVCoordinates;
  39. import org.orekit.utils.TimeStampedFieldAngularCoordinates;
  40. import org.orekit.utils.TimeStampedFieldAngularCoordinatesHermiteInterpolator;
  41. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  42. import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
  43. import org.orekit.utils.TimeStampedPVCoordinates;


  46. /** Transformation class in three-dimensional space.
  47.  *
  48.  * <p>This class represents the transformation engine between {@link Frame frames}.
  49.  * It is used both to define the relationship between each frame and its
  50.  * parent frame and to gather all individual transforms into one
  51.  * operation when converting between frames far away from each other.</p>
  52.  * <p>The convention used in OREKIT is vectorial transformation. It means
  53.  * that a transformation is defined as a transform to apply to the
  54.  * coordinates of a vector expressed in the old frame to obtain the
  55.  * same vector expressed in the new frame.
  56.  *
  57.  * <p>Instances of this class are guaranteed to be immutable.</p>
  58.  *
  59.  * <h2> Examples </h2>
  60.  *
  61.  * <h3> Example of translation from R<sub>A</sub> to R<sub>B</sub> </h3>
  62.  *
  63.  * <p> We want to transform the {@link FieldPVCoordinates} PV<sub>A</sub> to
  64.  * PV<sub>B</sub> with :
  65.  * <p> PV<sub>A</sub> = ({1, 0, 0}, {2, 0, 0}, {3, 0, 0}); <br>
  66.  *     PV<sub>B</sub> = ({0, 0, 0}, {0, 0, 0}, {0, 0, 0});
  67.  *
  68.  * <p> The transform to apply then is defined as follows :
  69.  *
  70.  * <pre>
  71.  * Vector3D translation  = new Vector3D(-1, 0, 0);
  72.  * Vector3D velocity     = new Vector3D(-2, 0, 0);
  73.  * Vector3D acceleration = new Vector3D(-3, 0, 0);
  74.  *
  75.  * Transform R1toR2 = new Transform(date, translation, velocity, acceleration);
  76.  *
  77.  * PVB = R1toR2.transformPVCoordinate(PVA);
  78.  * </pre>
  79.  *
  80.  * <h3> Example of rotation from R<sub>A</sub> to R<sub>B</sub> </h3>
  81.  * <p> We want to transform the {@link FieldPVCoordinates} PV<sub>A</sub> to
  82.  * PV<sub>B</sub> with
  83.  *
  84.  * <p> PV<sub>A</sub> = ({1, 0, 0}, { 1, 0, 0}); <br>
  85.  *     PV<sub>B</sub> = ({0, 1, 0}, {-2, 1, 0});
  86.  *
  87.  * <p> The transform to apply then is defined as follows :
  88.  *
  89.  * <pre>
  90.  * Rotation rotation = new Rotation(Vector3D.PLUS_K, FastMath.PI / 2);
  91.  * Vector3D rotationRate = new Vector3D(0, 0, -2);
  92.  *
  93.  * Transform R1toR2 = new Transform(rotation, rotationRate);
  94.  *
  95.  * PVB = R1toR2.transformPVCoordinates(PVA);
  96.  * </pre>
  97.  *
  98.  * @author Luc Maisonobe
  99.  * @author Fabien Maussion
  100.  * @param <T> the type of the field elements
  101.  * @since 9.0
  102.  */
  103. public class FieldTransform<T extends CalculusFieldElement<T>>
  104.     implements FieldTimeShiftable<FieldTransform<T>, T>, FieldStaticTransform<T> {

  106.     /** Date of the transform. */
  107.     private final FieldAbsoluteDate<T> date;

  109.     /** Date of the transform. */
  110.     private final AbsoluteDate aDate;

  112.     /** Cartesian coordinates of the target frame with respect to the original frame. */
  113.     private final FieldPVCoordinates<T> cartesian;

  115.     /** Angular coordinates of the target frame with respect to the original frame. */
  116.     private final FieldAngularCoordinates<T> angular;

  118.     /** Build a transform from its primitive operations.
  119.      * @param date date of the transform
  120.      * @param aDate date of the transform
  121.      * @param cartesian Cartesian coordinates of the target frame with respect to the original frame
  122.      * @param angular angular coordinates of the target frame with respect to the original frame
  123.      */
  124.     private FieldTransform(final FieldAbsoluteDate<T> date, final AbsoluteDate aDate,
  125.                            final FieldPVCoordinates<T> cartesian,
  126.                            final FieldAngularCoordinates<T> angular) {
  127.         this.date      = date;
  128.         this.aDate     = aDate;
  129.         this.cartesian = cartesian;
  130.         this.angular   = angular;
  131.     }

  133.     /** Build a transform from a regular transform.
  134.      * @param field field of the elements
  135.      * @param transform regular transform to convert
  136.      */
  137.     public FieldTransform(final Field<T> field, final Transform transform) {
  138.         this(new FieldAbsoluteDate<>(field, transform.getDate()), transform.getDate(),
  139.              new FieldPVCoordinates<>(field, transform.getCartesian()),
  140.              new FieldAngularCoordinates<>(field, transform.getAngular()));
  141.     }

  143.     /** Build a translation transform.
  144.      * @param date date of the transform
  145.      * @param translation translation to apply (i.e. coordinates of
  146.      * the transformed origin, or coordinates of the origin of the
  147.      * old frame in the new frame)
  148.      */
  149.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation) {
  150.         this(date, date.toAbsoluteDate(),
  151.              new FieldPVCoordinates<>(translation,
  152.                                       FieldVector3D.getZero(date.getField()),
  153.                                       FieldVector3D.getZero(date.getField())),
  154.              FieldAngularCoordinates.getIdentity(date.getField()));
  155.     }

  157.     /** Build a rotation transform.
  158.      * @param date date of the transform
  159.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  160.      * coordinates of a vector expressed in the old frame to obtain the
  161.      * same vector expressed in the new frame )
  162.      */
  163.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldRotation<T> rotation) {
  164.         this(date, date.toAbsoluteDate(),
  165.              FieldPVCoordinates.getZero(date.getField()),
  166.              new FieldAngularCoordinates<>(rotation,
  167.                                            FieldVector3D.getZero(date.getField()),
  168.                                            FieldVector3D.getZero(date.getField())));
  169.     }

  171.     /** Build a translation transform, with its first time derivative.
  172.      * @param date date of the transform
  173.      * @param translation translation to apply (i.e. coordinates of
  174.      * the transformed origin, or coordinates of the origin of the
  175.      * old frame in the new frame)
  176.      * @param velocity the velocity of the translation (i.e. origin
  177.      * of the old frame velocity in the new frame)
  178.      */
  179.     public FieldTransform(final FieldAbsoluteDate<T> date,
  180.                           final FieldVector3D<T> translation,
  181.                           final FieldVector3D<T> velocity) {
  182.         this(date,
  183.              new FieldPVCoordinates<>(translation,
  184.                                       velocity,
  185.                                       FieldVector3D.getZero(date.getField())));
  186.     }

  188.     /** Build a translation transform, with its first and second time derivatives.
  189.      * @param date date of the transform
  190.      * @param translation translation to apply (i.e. coordinates of
  191.      * the transformed origin, or coordinates of the origin of the
  192.      * old frame in the new frame)
  193.      * @param velocity the velocity of the translation (i.e. origin
  194.      * of the old frame velocity in the new frame)
  195.      * @param acceleration the acceleration of the translation (i.e. origin
  196.      * of the old frame acceleration in the new frame)
  197.      */
  198.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation,
  199.                           final FieldVector3D<T> velocity, final FieldVector3D<T> acceleration) {
  200.         this(date,
  201.              new FieldPVCoordinates<>(translation, velocity, acceleration));
  202.     }

  204.     /** Build a translation transform, with its first time derivative.
  205.      * @param date date of the transform
  206.      * @param cartesian Cartesian part of the transformation to apply (i.e. coordinates of
  207.      * the transformed origin, or coordinates of the origin of the
  208.      * old frame in the new frame, with their derivatives)
  209.      */
  210.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldPVCoordinates<T> cartesian) {
  211.         this(date, date.toAbsoluteDate(),
  212.              cartesian,
  213.              FieldAngularCoordinates.getIdentity(date.getField()));
  214.     }

  216.     /** Build a rotation transform.
  217.      * @param date date of the transform
  218.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  219.      * coordinates of a vector expressed in the old frame to obtain the
  220.      * same vector expressed in the new frame )
  221.      * @param rotationRate the axis of the instant rotation
  222.      * expressed in the new frame. (norm representing angular rate)
  223.      */
  224.     public FieldTransform(final FieldAbsoluteDate<T> date,
  225.                           final FieldRotation<T> rotation,
  226.                           final FieldVector3D<T> rotationRate) {
  227.         this(date,
  228.              new FieldAngularCoordinates<>(rotation,
  229.                                            rotationRate,
  230.                                            FieldVector3D.getZero(date.getField())));
  231.     }

  233.     /** Build a rotation transform.
  234.      * @param date date of the transform
  235.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  236.      * coordinates of a vector expressed in the old frame to obtain the
  237.      * same vector expressed in the new frame )
  238.      * @param rotationRate the axis of the instant rotation
  239.      * @param rotationAcceleration the axis of the instant rotation
  240.      * expressed in the new frame. (norm representing angular rate)
  241.      */
  242.     public FieldTransform(final FieldAbsoluteDate<T> date,
  243.                           final FieldRotation<T> rotation,
  244.                           final FieldVector3D<T> rotationRate,
  245.                           final FieldVector3D<T> rotationAcceleration) {
  246.         this(date, new FieldAngularCoordinates<>(rotation, rotationRate, rotationAcceleration));
  247.     }

  249.     /** Build a rotation transform.
  250.      * @param date date of the transform
  251.      * @param angular angular part of the transformation to apply (i.e. rotation to
  252.      * apply to the coordinates of a vector expressed in the old frame to obtain the
  253.      * same vector expressed in the new frame, with its rotation rate)
  254.      */
  255.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldAngularCoordinates<T> angular) {
  256.         this(date, date.toAbsoluteDate(),
  257.              FieldPVCoordinates.getZero(date.getField()),
  258.              angular);
  259.     }

  261.     /** Build a transform by combining two existing ones.
  262.      * <p>
  263.      * Note that the dates of the two existing transformed are <em>ignored</em>,
  264.      * and the combined transform date is set to the date supplied in this constructor
  265.      * without any attempt to shift the raw transforms. This is a design choice allowing
  266.      * user full control of the combination.
  267.      * </p>
  268.      * @param date date of the transform
  269.      * @param first first transform applied
  270.      * @param second second transform applied
  271.      */
  272.     public FieldTransform(final FieldAbsoluteDate<T> date,
  273.                           final FieldTransform<T> first,
  274.                           final FieldTransform<T> second) {
  275.         this(date, date.toAbsoluteDate(),
  276.              new FieldPVCoordinates<>(FieldStaticTransform.compositeTranslation(first, second),
  277.                                       compositeVelocity(first, second),
  278.                                       compositeAcceleration(first, second)),
  279.              new FieldAngularCoordinates<>(FieldStaticTransform.compositeRotation(first, second),
  280.                                            compositeRotationRate(first, second),
  281.                                            compositeRotationAcceleration(first, second)));
  282.     }

  284.     /** Get the identity transform.
  285.      * @param field field for the components
  286.      * @param <T> the type of the field elements
  287.      * @return identity transform
  288.      */
  289.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> getIdentity(final Field<T> field) {
  290.         return new FieldIdentityTransform<>(field);
  291.     }

  293.     /** Compute a composite velocity.
  294.      * @param first first applied transform
  295.      * @param second second applied transform
  296.      * @param <T> the type of the field elements
  297.      * @return velocity part of the composite transform
  298.      */
  299.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeVelocity(final FieldTransform<T> first, final FieldTransform<T> second) {

  301.         final FieldVector3D<T> v1 = first.cartesian.getVelocity();
  302.         final FieldRotation<T> r1 = first.angular.getRotation();
  303.         final FieldVector3D<T> o1 = first.angular.getRotationRate();
  304.         final FieldVector3D<T> p2 = second.cartesian.getPosition();
  305.         final FieldVector3D<T> v2 = second.cartesian.getVelocity();

  307.         final FieldVector3D<T> crossP = FieldVector3D.crossProduct(o1, p2);

  309.         return v1.add(r1.applyInverseTo(v2.add(crossP)));

  311.     }

  313.     /** Compute a composite acceleration.
  314.      * @param first first applied transform
  315.      * @param second second applied transform
  316.      * @param <T> the type of the field elements
  317.      * @return acceleration part of the composite transform
  318.      */
  319.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {

  321.         final FieldVector3D<T> a1    = first.cartesian.getAcceleration();
  322.         final FieldRotation<T> r1    = first.angular.getRotation();
  323.         final FieldVector3D<T> o1    = first.angular.getRotationRate();
  324.         final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
  325.         final FieldVector3D<T> p2    = second.cartesian.getPosition();
  326.         final FieldVector3D<T> v2    = second.cartesian.getVelocity();
  327.         final FieldVector3D<T> a2    = second.cartesian.getAcceleration();

  329.         final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o1,    FieldVector3D.crossProduct(o1, p2));
  330.         final FieldVector3D<T> crossV      = FieldVector3D.crossProduct(o1,    v2);
  331.         final FieldVector3D<T> crossDotP   = FieldVector3D.crossProduct(oDot1, p2);

  333.         return a1.add(r1.applyInverseTo(new FieldVector3D<>(1, a2, 2, crossV, 1, crossCrossP, 1, crossDotP)));

  335.     }

  337.     /** Compute a composite rotation rate.
  338.      * @param first first applied transform
  339.      * @param second second applied transform
  340.      * @param <T> the type of the field elements
  341.      * @return rotation rate part of the composite transform
  342.      */
  343.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationRate(final FieldTransform<T> first, final FieldTransform<T> second) {

  345.         final FieldVector3D<T> o1 = first.angular.getRotationRate();
  346.         final FieldRotation<T> r2 = second.angular.getRotation();
  347.         final FieldVector3D<T> o2 = second.angular.getRotationRate();

  349.         return o2.add(r2.applyTo(o1));

  351.     }

  353.     /** Compute a composite rotation acceleration.
  354.      * @param first first applied transform
  355.      * @param second second applied transform
  356.      * @param <T> the type of the field elements
  357.      * @return rotation acceleration part of the composite transform
  358.      */
  359.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {

  361.         final FieldVector3D<T> o1    = first.angular.getRotationRate();
  362.         final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
  363.         final FieldRotation<T> r2    = second.angular.getRotation();
  364.         final FieldVector3D<T> o2    = second.angular.getRotationRate();
  365.         final FieldVector3D<T> oDot2 = second.angular.getRotationAcceleration();

  367.         return new FieldVector3D<>( 1, oDot2,
  368.                                     1, r2.applyTo(oDot1),
  369.                                    -1, FieldVector3D.crossProduct(o2, r2.applyTo(o1)));

  371.     }

  373.     /** {@inheritDoc} */
  374.     @Override
  375.     public AbsoluteDate getDate() {
  376.         return aDate;
  377.     }

  379.     /** Get the date.
  380.      * @return date attached to the object
  381.      */
  382.     public FieldAbsoluteDate<T> getFieldDate() {
  383.         return date;
  384.     }

  386.     /** {@inheritDoc} */
  387.     @Override
  388.     public FieldTransform<T> shiftedBy(final double dt) {
  389.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt),
  390.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  391.     }

  393.     /** Get a time-shifted instance.
  394.      * @param dt time shift in seconds
  395.      * @return a new instance, shifted with respect to instance (which is not changed)
  396.      */
  397.     public FieldTransform<T> shiftedBy(final T dt) {
  398.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt.getReal()),
  399.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  400.     }

  402.     /**
  403.      * Shift the transform in time considering all rates, then return only the
  404.      * translation and rotation portion of the transform.
  405.      *
  406.      * @param dt time shift in seconds.
  407.      * @return shifted transform as a static transform. It is static in the
  408.      * sense that it can only be used to transform directions and positions, but
  409.      * not velocities or accelerations.
  410.      * @see #shiftedBy(double)
  411.      */
  412.     public FieldStaticTransform<T> staticShiftedBy(final T dt) {
  413.         return FieldStaticTransform.of(date.shiftedBy(dt),
  414.                                        cartesian.positionShiftedBy(dt),
  415.                                        angular.rotationShiftedBy(dt));
  416.     }

  418.     /**
  419.      * Create a so-called static transform from the instance.
  420.      *
  421.      * @return static part of the transform. It is static in the
  422.      * sense that it can only be used to transform directions and positions, but
  423.      * not velocities or accelerations.
  424.      * @see FieldStaticTransform
  425.      */
  426.     public FieldStaticTransform<T> toStaticTransform() {
  427.         return FieldStaticTransform.of(date, cartesian.getPosition(), angular.getRotation());
  428.     }

  430.     /** Interpolate a transform from a sample set of existing transforms.
  431.      * <p>
  432.      * Calling this method is equivalent to call {@link #interpolate(FieldAbsoluteDate,
  433.      * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
  434.      * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
  435.      * {@link AngularDerivativesFilter#USE_RRA}
  436.      * set to true.
  437.      * </p>
  438.      * @param interpolationDate interpolation date
  439.      * @param sample sample points on which interpolation should be done
  440.      * @param <T> the type of the field elements
  441.      * @return a new instance, interpolated at specified date
  442.      */
  443.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> interpolationDate,
  444.                                                                                 final Collection<FieldTransform<T>> sample) {
  445.         return interpolate(interpolationDate,
  446.                            CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
  447.                            sample);
  448.     }

  450.     /** Interpolate a transform from a sample set of existing transforms.
  451.      * <p>
  452.      * Note that even if first time derivatives (velocities and rotation rates)
  453.      * from sample can be ignored, the interpolated instance always includes
  454.      * interpolated derivatives. This feature can be used explicitly to
  455.      * compute these derivatives when it would be too complex to compute them
  456.      * from an analytical formula: just compute a few sample points from the
  457.      * explicit formula and set the derivatives to zero in these sample points,
  458.      * then use interpolation to add derivatives consistent with the positions
  459.      * and rotations.
  460.      * </p>
  461.      * <p>
  462.      * As this implementation of interpolation is polynomial, it should be used only
  463.      * with small samples (about 10-20 points) in order to avoid <a
  464.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  465.      * and numerical problems (including NaN appearing).
  466.      * </p>
  467.      * @param date interpolation date
  468.      * @param cFilter filter for derivatives from the sample to use in interpolation
  469.      * @param aFilter filter for derivatives from the sample to use in interpolation
  470.      * @param sample sample points on which interpolation should be done
  471.      * @return a new instance, interpolated at specified date
  472.           * @param <T> the type of the field elements
  473.      */
  474.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> date,
  475.                                                                                 final CartesianDerivativesFilter cFilter,
  476.                                                                                 final AngularDerivativesFilter aFilter,
  477.                                                                                 final Collection<FieldTransform<T>> sample) {
  478.         return interpolate(date, cFilter, aFilter, sample.stream());
  479.     }

  481.     /** Interpolate a transform from a sample set of existing transforms.
  482.      * <p>
  483.      * Note that even if first time derivatives (velocities and rotation rates)
  484.      * from sample can be ignored, the interpolated instance always includes
  485.      * interpolated derivatives. This feature can be used explicitly to
  486.      * compute these derivatives when it would be too complex to compute them
  487.      * from an analytical formula: just compute a few sample points from the
  488.      * explicit formula and set the derivatives to zero in these sample points,
  489.      * then use interpolation to add derivatives consistent with the positions
  490.      * and rotations.
  491.      * </p>
  492.      * <p>
  493.      * As this implementation of interpolation is polynomial, it should be used only
  494.      * with small samples (about 10-20 points) in order to avoid <a
  495.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  496.      * and numerical problems (including NaN appearing).
  497.      * </p>
  498.      * @param date interpolation date
  499.      * @param cFilter filter for derivatives from the sample to use in interpolation
  500.      * @param aFilter filter for derivatives from the sample to use in interpolation
  501.      * @param sample sample points on which interpolation should be done
  502.      * @return a new instance, interpolated at specified date
  503.           * @param <T> the type of the field elements
  504.      */
  505.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> date,
  506.                                                                                 final CartesianDerivativesFilter cFilter,
  507.                                                                                 final AngularDerivativesFilter aFilter,
  508.                                                                                 final Stream<FieldTransform<T>> sample) {

  510.         // Create samples
  511.         final List<TimeStampedFieldPVCoordinates<T>>      datedPV = new ArrayList<>();
  512.         final List<TimeStampedFieldAngularCoordinates<T>> datedAC = new ArrayList<>();
  513.         sample.forEach(t -> {
  514.             datedPV.add(new TimeStampedFieldPVCoordinates<>(t.getDate(), t.getTranslation(), t.getVelocity(), t.getAcceleration()));
  515.             datedAC.add(new TimeStampedFieldAngularCoordinates<>(t.getDate(), t.getRotation(), t.getRotationRate(), t.getRotationAcceleration()));
  516.         });

  518.         // Create interpolators
  519.         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> pvInterpolator =
  520.                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(datedPV.size(), cFilter);

  522.         final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<T>, T> angularInterpolator =
  523.                 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(datedPV.size(), aFilter);

  525.         // Interpolate
  526.         final TimeStampedFieldPVCoordinates<T>      interpolatedPV = pvInterpolator.interpolate(date, datedPV);
  527.         final TimeStampedFieldAngularCoordinates<T> interpolatedAC = angularInterpolator.interpolate(date, datedAC);

  529.         return new FieldTransform<>(date, date.toAbsoluteDate(), interpolatedPV, interpolatedAC);
  530.     }

  532.     /** Get the inverse transform of the instance.
  533.      * @return inverse transform of the instance
  534.      */
  535.     @Override
  536.     public FieldTransform<T> getInverse() {

  538.         final FieldRotation<T> r    = angular.getRotation();
  539.         final FieldVector3D<T> o    = angular.getRotationRate();
  540.         final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  541.         final FieldVector3D<T> rp   = r.applyTo(cartesian.getPosition());
  542.         final FieldVector3D<T> rv   = r.applyTo(cartesian.getVelocity());
  543.         final FieldVector3D<T> ra   = r.applyTo(cartesian.getAcceleration());

  545.         final FieldVector3D<T> pInv        = rp.negate();
  546.         final FieldVector3D<T> crossP      = FieldVector3D.crossProduct(o, rp);
  547.         final FieldVector3D<T> vInv        = crossP.subtract(rv);
  548.         final FieldVector3D<T> crossV      = FieldVector3D.crossProduct(o, rv);
  549.         final FieldVector3D<T> crossDotP   = FieldVector3D.crossProduct(oDot, rp);
  550.         final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o, crossP);
  551.         final FieldVector3D<T> aInv        = new FieldVector3D<>(-1, ra,
  552.                                                                   2, crossV,
  553.                                                                   1, crossDotP,
  554.                                                                  -1, crossCrossP);

  556.         return new FieldTransform<>(date, aDate, new FieldPVCoordinates<>(pInv, vInv, aInv), angular.revert());

  558.     }

  560.     /** Get a frozen transform.
  561.      * <p>
  562.      * This method creates a copy of the instance but frozen in time,
  563.      * i.e. with velocity, acceleration and rotation rate forced to zero.
  564.      * </p>
  565.      * @return a new transform, without any time-dependent parts
  566.      */
  567.     public FieldTransform<T> freeze() {
  568.         return new FieldTransform<>(date, aDate,
  569.                                     new FieldPVCoordinates<>(cartesian.getPosition(),
  570.                                                              FieldVector3D.getZero(date.getField()),
  571.                                                              FieldVector3D.getZero(date.getField())),
  572.                                     new FieldAngularCoordinates<>(angular.getRotation(),
  573.                                                                   FieldVector3D.getZero(date.getField()),
  574.                                                                   FieldVector3D.getZero(date.getField())));
  575.     }

  577.     /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
  578.      * <p>
  579.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  580.      * consistency between the transform {@link #getDate() date} and the time-stamped
  581.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  582.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  583.      * the input argument, regardless of the instance {@link #getDate() date}.
  584.      * </p>
  585.      * @param pv time-stamped  position-velocity to transform.
  586.      * @return transformed time-stamped position-velocity
  587.      */
  588.     public FieldPVCoordinates<T> transformPVCoordinates(final PVCoordinates pv) {
  589.         return angular.applyTo(new FieldPVCoordinates<>(cartesian.getPosition().add(pv.getPosition()),
  590.                                                         cartesian.getVelocity().add(pv.getVelocity()),
  591.                                                         cartesian.getAcceleration().add(pv.getAcceleration())));
  592.     }

  594.     /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
  595.      * <p>
  596.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  597.      * consistency between the transform {@link #getDate() date} and the time-stamped
  598.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  599.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  600.      * the input argument, regardless of the instance {@link #getDate() date}.
  601.      * </p>
  602.      * @param pv time-stamped  position-velocity to transform.
  603.      * @return transformed time-stamped position-velocity
  604.      */
  605.     public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedPVCoordinates pv) {
  606.         return angular.applyTo(new TimeStampedFieldPVCoordinates<>(pv.getDate(),
  607.                                                                    cartesian.getPosition().add(pv.getPosition()),
  608.                                                                    cartesian.getVelocity().add(pv.getVelocity()),
  609.                                                                    cartesian.getAcceleration().add(pv.getAcceleration())));
  610.     }

  612.     /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
  613.      * <p>
  614.      * BEWARE! This method does explicit computation of velocity and acceleration by combining
  615.      * the transform velocity, acceleration, rotation rate and rotation acceleration with the
  616.      * velocity and acceleration from the argument. This implies that this method should
  617.      * <em>not</em> be used when derivatives are contained in the {@link CalculusFieldElement field
  618.      * elements} (typically when using {@link org.hipparchus.analysis.differentiation.DerivativeStructure
  619.      * DerivativeStructure} elements where time is one of the differentiation parameter), otherwise
  620.      * the time derivatives would be computed twice, once explicitly in this method and once implicitly
  621.      * in the field operations. If time derivatives are contained in the field elements themselves,
  622.      * then rather than this method the {@link #transformPosition(FieldVector3D) transformPosition}
  623.      * method should be used, so the derivatives are computed once, as part of the field. This
  624.      * method is rather expected to be used when the field elements are {@link
  625.      * org.hipparchus.analysis.differentiation.DerivativeStructure DerivativeStructure} instances
  626.      * where the differentiation parameters are not time (they can typically be initial state
  627.      * for computing state transition matrices or force models parameters, or ground stations
  628.      * positions, ...).
  629.      * </p>
  630.      * <p>
  631.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  632.      * consistency between the transform {@link #getDate() date} and the time-stamped
  633.      * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
  634.      * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
  635.      * the input argument, regardless of the instance {@link #getDate() date}.
  636.      * </p>
  637.      * @param pv time-stamped position-velocity to transform.
  638.      * @return transformed time-stamped position-velocity
  639.      */
  640.     public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  641.         return angular.applyTo(new FieldPVCoordinates<>(pv.getPosition().add(cartesian.getPosition()),
  642.                                                         pv.getVelocity().add(cartesian.getVelocity()),
  643.                                                         pv.getAcceleration().add(cartesian.getAcceleration())));
  644.     }

  646.     /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
  647.      * <p>
  648.      * BEWARE! This method does explicit computation of velocity and acceleration by combining
  649.      * the transform velocity, acceleration, rotation rate and rotation acceleration with the
  650.      * velocity and acceleration from the argument. This implies that this method should
  651.      * <em>not</em> be used when derivatives are contained in the {@link CalculusFieldElement field
  652.      * elements} (typically when using {@link org.hipparchus.analysis.differentiation.DerivativeStructure
  653.      * DerivativeStructure} elements where time is one of the differentiation parameter), otherwise
  654.      * the time derivatives would be computed twice, once explicitly in this method and once implicitly
  655.      * in the field operations. If time derivatives are contained in the field elements themselves,
  656.      * then rather than this method the {@link #transformPosition(FieldVector3D) transformPosition}
  657.      * method should be used, so the derivatives are computed once, as part of the field. This
  658.      * method is rather expected to be used when the field elements are {@link
  659.      * org.hipparchus.analysis.differentiation.DerivativeStructure DerivativeStructure} instances
  660.      * where the differentiation parameters are not time (they can typically be initial state
  661.      * for computing state transition matrices or force models parameters, or ground stations
  662.      * positions, ...).
  663.      * </p>
  664.      * <p>
  665.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  666.      * consistency between the transform {@link #getDate() date} and the time-stamped
  667.      * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
  668.      * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
  669.      * the input argument, regardless of the instance {@link #getDate() date}.
  670.      * </p>
  671.      * @param pv time-stamped position-velocity to transform.
  672.      * @return transformed time-stamped position-velocity
  673.      */
  674.     public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedFieldPVCoordinates<T> pv) {
  675.         return angular.applyTo(new TimeStampedFieldPVCoordinates<>(pv.getDate(),
  676.                                                                    pv.getPosition().add(cartesian.getPosition()),
  677.                                                                    pv.getVelocity().add(cartesian.getVelocity()),
  678.                                                                    pv.getAcceleration().add(cartesian.getAcceleration())));
  679.     }

  681.     /** Compute the Jacobian of the {@link #transformPVCoordinates(FieldPVCoordinates)}
  682.      * method of the transform.
  683.      * <p>
  684.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  685.      * of the transformed {@link FieldPVCoordinates} with respect to Cartesian coordinate j
  686.      * of the input {@link FieldPVCoordinates} in method {@link #transformPVCoordinates(FieldPVCoordinates)}.
  687.      * </p>
  688.      * <p>
  689.      * This definition implies that if we define position-velocity coordinates
  690.      * <pre>PV₁ = transform.transformPVCoordinates(PV₀)</pre>
  691.      * then their differentials dPV₁ and dPV₀ will obey the following relation
  692.      * where J is the matrix computed by this method:
  693.      * <pre>dPV₁ = J &times; dPV₀</pre>
  694.      *
  695.      * @param selector selector specifying the size of the upper left corner that must be filled
  696.      * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
  697.      * velocities and accelerations)
  698.      * @param jacobian placeholder matrix whose upper-left corner is to be filled with
  699.      * the Jacobian, the rest of the matrix remaining untouched
  700.      */
  701.     public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {

  703.         final T zero = date.getField().getZero();

  705.         // elementary matrix for rotation
  706.         final T[][] mData = angular.getRotation().getMatrix();

  708.         // dP1/dP0
  709.         System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  710.         System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  711.         System.arraycopy(mData[2], 0, jacobian[2], 0, 3);

  713.         if (selector.getMaxOrder() >= 1) {

  715.             // dP1/dV0
  716.             Arrays.fill(jacobian[0], 3, 6, zero);
  717.             Arrays.fill(jacobian[1], 3, 6, zero);
  718.             Arrays.fill(jacobian[2], 3, 6, zero);

  720.             // dV1/dP0
  721.             final FieldVector3D<T> o = angular.getRotationRate();
  722.             final T ox = o.getX();
  723.             final T oy = o.getY();
  724.             final T oz = o.getZ();
  725.             for (int i = 0; i < 3; ++i) {
  726.                 jacobian[3][i] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i]));
  727.                 jacobian[4][i] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i]));
  728.                 jacobian[5][i] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i]));
  729.             }

  731.             // dV1/dV0
  732.             System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
  733.             System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
  734.             System.arraycopy(mData[2], 0, jacobian[5], 3, 3);

  736.             if (selector.getMaxOrder() >= 2) {

  738.                 // dP1/dA0
  739.                 Arrays.fill(jacobian[0], 6, 9, zero);
  740.                 Arrays.fill(jacobian[1], 6, 9, zero);
  741.                 Arrays.fill(jacobian[2], 6, 9, zero);

  743.                 // dV1/dA0
  744.                 Arrays.fill(jacobian[3], 6, 9, zero);
  745.                 Arrays.fill(jacobian[4], 6, 9, zero);
  746.                 Arrays.fill(jacobian[5], 6, 9, zero);

  748.                 // dA1/dP0
  749.                 final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  750.                 final T oDotx  = oDot.getX();
  751.                 final T oDoty  = oDot.getY();
  752.                 final T oDotz  = oDot.getZ();
  753.                 for (int i = 0; i < 3; ++i) {
  754.                     jacobian[6][i] = oDotz.multiply(mData[1][i]).subtract(oDoty.multiply(mData[2][i])).add(oz.multiply(jacobian[4][i]).subtract(oy.multiply(jacobian[5][i])));
  755.                     jacobian[7][i] = oDotx.multiply(mData[2][i]).subtract(oDotz.multiply(mData[0][i])).add(ox.multiply(jacobian[5][i]).subtract(oz.multiply(jacobian[3][i])));
  756.                     jacobian[8][i] = oDoty.multiply(mData[0][i]).subtract(oDotx.multiply(mData[1][i])).add(oy.multiply(jacobian[3][i]).subtract(ox.multiply(jacobian[4][i])));
  757.                 }

  759.                 // dA1/dV0
  760.                 for (int i = 0; i < 3; ++i) {
  761.                     jacobian[6][i + 3] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i])).multiply(2);
  762.                     jacobian[7][i + 3] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i])).multiply(2);
  763.                     jacobian[8][i + 3] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i])).multiply(2);
  764.                 }

  766.                 // dA1/dA0
  767.                 System.arraycopy(mData[0], 0, jacobian[6], 6, 3);
  768.                 System.arraycopy(mData[1], 0, jacobian[7], 6, 3);
  769.                 System.arraycopy(mData[2], 0, jacobian[8], 6, 3);

  771.             }

  773.         }

  775.     }

  777.     /** Get the underlying elementary Cartesian part.
  778.      * <p>A transform can be uniquely represented as an elementary
  779.      * translation followed by an elementary rotation. This method
  780.      * returns this unique elementary translation with its derivative.</p>
  781.      * @return underlying elementary Cartesian part
  782.      * @see #getTranslation()
  783.      * @see #getVelocity()
  784.      */
  785.     public FieldPVCoordinates<T> getCartesian() {
  786.         return cartesian;
  787.     }

  789.     /** Get the underlying elementary translation.
  790.      * <p>A transform can be uniquely represented as an elementary
  791.      * translation followed by an elementary rotation. This method
  792.      * returns this unique elementary translation.</p>
  793.      * @return underlying elementary translation
  794.      * @see #getCartesian()
  795.      * @see #getVelocity()
  796.      * @see #getAcceleration()
  797.      */
  798.     public FieldVector3D<T> getTranslation() {
  799.         return cartesian.getPosition();
  800.     }

  802.     /** Get the first time derivative of the translation.
  803.      * @return first time derivative of the translation
  804.      * @see #getCartesian()
  805.      * @see #getTranslation()
  806.      * @see #getAcceleration()
  807.      */
  808.     public FieldVector3D<T> getVelocity() {
  809.         return cartesian.getVelocity();
  810.     }

  812.     /** Get the second time derivative of the translation.
  813.      * @return second time derivative of the translation
  814.      * @see #getCartesian()
  815.      * @see #getTranslation()
  816.      * @see #getVelocity()
  817.      */
  818.     public FieldVector3D<T> getAcceleration() {
  819.         return cartesian.getAcceleration();
  820.     }

  822.     /** Get the underlying elementary angular part.
  823.      * <p>A transform can be uniquely represented as an elementary
  824.      * translation followed by an elementary rotation. This method
  825.      * returns this unique elementary rotation with its derivative.</p>
  826.      * @return underlying elementary angular part
  827.      * @see #getRotation()
  828.      * @see #getRotationRate()
  829.      * @see #getRotationAcceleration()
  830.      */
  831.     public FieldAngularCoordinates<T> getAngular() {
  832.         return angular;
  833.     }

  835.     /** Get the underlying elementary rotation.
  836.      * <p>A transform can be uniquely represented as an elementary
  837.      * translation followed by an elementary rotation. This method
  838.      * returns this unique elementary rotation.</p>
  839.      * @return underlying elementary rotation
  840.      * @see #getAngular()
  841.      * @see #getRotationRate()
  842.      * @see #getRotationAcceleration()
  843.      */
  844.     public FieldRotation<T> getRotation() {
  845.         return angular.getRotation();
  846.     }

  848.     /** Get the first time derivative of the rotation.
  849.      * <p>The norm represents the angular rate.</p>
  850.      * @return First time derivative of the rotation
  851.      * @see #getAngular()
  852.      * @see #getRotation()
  853.      * @see #getRotationAcceleration()
  854.      */
  855.     public FieldVector3D<T> getRotationRate() {
  856.         return angular.getRotationRate();
  857.     }

  859.     /** Get the second time derivative of the rotation.
  860.      * @return Second time derivative of the rotation
  861.      * @see #getAngular()
  862.      * @see #getRotation()
  863.      * @see #getRotationRate()
  864.      */
  865.     public FieldVector3D<T> getRotationAcceleration() {
  866.         return angular.getRotationAcceleration();
  867.     }

  869.     /** Specialized class for identity transform. */
  870.     private static class FieldIdentityTransform<T extends CalculusFieldElement<T>> extends FieldTransform<T> {

  872.         /** Simple constructor.
  873.          * @param field field for the components
  874.          */
  875.         FieldIdentityTransform(final Field<T> field) {
  876.             super(FieldAbsoluteDate.getArbitraryEpoch(field),
  877.                   FieldAbsoluteDate.getArbitraryEpoch(field).toAbsoluteDate(),
  878.                   FieldPVCoordinates.getZero(field),
  879.                   FieldAngularCoordinates.getIdentity(field));
  880.         }

  882.         /** {@inheritDoc} */
  883.         @Override
  884.         public FieldTransform<T> shiftedBy(final double dt) {
  885.             return this;
  886.         }

  888.         /** {@inheritDoc} */
  889.         @Override
  890.         public FieldTransform<T> getInverse() {
  891.             return this;
  892.         }

  894.         /** {@inheritDoc} */
  895.         @Override
  896.         public FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  897.             return position;
  898.         }

  900.         /** {@inheritDoc} */
  901.         @Override
  902.         public FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  903.             return vector;
  904.         }

  906.         /** {@inheritDoc} */
  907.         @Override
  908.         public FieldLine<T> transformLine(final FieldLine<T> line) {
  909.             return line;
  910.         }

  912.         /** {@inheritDoc} */
  913.         @Override
  914.         public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  915.             return pv;
  916.         }

  918.         /** {@inheritDoc} */
  919.         @Override
  920.         public FieldTransform<T> freeze() {
  921.             return this;
  922.         }

  924.         /** {@inheritDoc} */
  925.         @Override
  926.         public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {
  927.             final int n = 3 * (selector.getMaxOrder() + 1);
  928.             for (int i = 0; i < n; ++i) {
  929.                 Arrays.fill(jacobian[i], 0, n, getFieldDate().getField().getZero());
  930.                 jacobian[i][i] = getFieldDate().getField().getOne();
  931.             }
  932.         }

  934.     }

  936. }
Created with JaCoCo 0.8.11.202310140853