FieldTransform.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.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.hipparchus.geometry.euclidean.threed.Vector3D;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.time.FieldAbsoluteDate;
  33. import org.orekit.time.FieldTimeInterpolator;
  34. import org.orekit.time.FieldTimeShiftable;
  35. import org.orekit.utils.AngularDerivativesFilter;
  36. import org.orekit.utils.CartesianDerivativesFilter;
  37. import org.orekit.utils.FieldAngularCoordinates;
  38. import org.orekit.utils.FieldPVCoordinates;
  39. import org.orekit.utils.PVCoordinates;
  40. import org.orekit.utils.TimeStampedFieldAngularCoordinates;
  41. import org.orekit.utils.TimeStampedFieldAngularCoordinatesHermiteInterpolator;
  42. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  43. import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
  44. import org.orekit.utils.TimeStampedPVCoordinates;


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

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

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

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

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

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

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

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

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

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

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

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

  215.     /** Build a combined translation and rotation transform.
  216.      * @param date date of the transform
  217.      * @param translation translation to apply (i.e. coordinates of
  218.      * the transformed origin, or coordinates of the origin of the
  219.      * old frame in the new frame)
  220.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  221.      * coordinates of a vector expressed in the old frame to obtain the
  222.      * same vector expressed in the new frame )
  223.      * @since 12.1
  224.      */
  225.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation,
  226.                           final FieldRotation<T> rotation) {
  227.         this(date, date.toAbsoluteDate(), new FieldPVCoordinates<>(translation, FieldVector3D.getZero(date.getField())),
  228.                 new FieldAngularCoordinates<>(rotation, FieldVector3D.getZero(date.getField())));
  229.     }

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

  248.     /** Build a rotation transform.
  249.      * @param date date of the transform
  250.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  251.      * coordinates of a vector expressed in the old frame to obtain the
  252.      * same vector expressed in the new frame )
  253.      * @param rotationRate the axis of the instant rotation
  254.      * @param rotationAcceleration the axis of the instant rotation
  255.      * expressed in the new frame. (norm representing angular rate)
  256.      */
  257.     public FieldTransform(final FieldAbsoluteDate<T> date,
  258.                           final FieldRotation<T> rotation,
  259.                           final FieldVector3D<T> rotationRate,
  260.                           final FieldVector3D<T> rotationAcceleration) {
  261.         this(date, new FieldAngularCoordinates<>(rotation, rotationRate, rotationAcceleration));
  262.     }

  264.     /** Build a rotation transform.
  265.      * @param date date of the transform
  266.      * @param angular angular part of the transformation to apply (i.e. rotation to
  267.      * apply to the coordinates of a vector expressed in the old frame to obtain the
  268.      * same vector expressed in the new frame, with its rotation rate)
  269.      */
  270.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldAngularCoordinates<T> angular) {
  271.         this(date, date.toAbsoluteDate(),
  272.              FieldPVCoordinates.getZero(date.getField()),
  273.              angular);
  274.     }

  276.     /** Build a transform by combining two existing ones.
  277.      * <p>
  278.      * Note that the dates of the two existing transformed are <em>ignored</em>,
  279.      * and the combined transform date is set to the date supplied in this constructor
  280.      * without any attempt to shift the raw transforms. This is a design choice allowing
  281.      * user full control of the combination.
  282.      * </p>
  283.      * @param date date of the transform
  284.      * @param first first transform applied
  285.      * @param second second transform applied
  286.      */
  287.     public FieldTransform(final FieldAbsoluteDate<T> date,
  288.                           final FieldTransform<T> first,
  289.                           final FieldTransform<T> second) {
  290.         this(date, date.toAbsoluteDate(),
  291.              new FieldPVCoordinates<>(FieldStaticTransform.compositeTranslation(first, second),
  292.                                       compositeVelocity(first, second),
  293.                                       compositeAcceleration(first, second)),
  294.              new FieldAngularCoordinates<>(FieldStaticTransform.compositeRotation(first, second),
  295.                                            compositeRotationRate(first, second),
  296.                                            compositeRotationAcceleration(first, second)));
  297.     }

  299.     /** Get the identity transform.
  300.      * @param field field for the components
  301.      * @param <T> the type of the field elements
  302.      * @return identity transform
  303.      */
  304.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> getIdentity(final Field<T> field) {
  305.         return new FieldIdentityTransform<>(field);
  306.     }

  308.     /** Compute a composite velocity.
  309.      * @param first first applied transform
  310.      * @param second second applied transform
  311.      * @param <T> the type of the field elements
  312.      * @return velocity part of the composite transform
  313.      */
  314.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeVelocity(final FieldTransform<T> first, final FieldTransform<T> second) {

  316.         final FieldVector3D<T> v1 = first.cartesian.getVelocity();
  317.         final FieldRotation<T> r1 = first.angular.getRotation();
  318.         final FieldVector3D<T> o1 = first.angular.getRotationRate();
  319.         final FieldVector3D<T> p2 = second.cartesian.getPosition();
  320.         final FieldVector3D<T> v2 = second.cartesian.getVelocity();

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

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

  326.     }

  328.     /** Compute a composite acceleration.
  329.      * @param first first applied transform
  330.      * @param second second applied transform
  331.      * @param <T> the type of the field elements
  332.      * @return acceleration part of the composite transform
  333.      */
  334.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {

  336.         final FieldVector3D<T> a1    = first.cartesian.getAcceleration();
  337.         final FieldRotation<T> r1    = first.angular.getRotation();
  338.         final FieldVector3D<T> o1    = first.angular.getRotationRate();
  339.         final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
  340.         final FieldVector3D<T> p2    = second.cartesian.getPosition();
  341.         final FieldVector3D<T> v2    = second.cartesian.getVelocity();
  342.         final FieldVector3D<T> a2    = second.cartesian.getAcceleration();

  344.         final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o1,    FieldVector3D.crossProduct(o1, p2));
  345.         final FieldVector3D<T> crossV      = FieldVector3D.crossProduct(o1,    v2);
  346.         final FieldVector3D<T> crossDotP   = FieldVector3D.crossProduct(oDot1, p2);

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

  350.     }

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

  360.         final FieldVector3D<T> o1 = first.angular.getRotationRate();
  361.         final FieldRotation<T> r2 = second.angular.getRotation();
  362.         final FieldVector3D<T> o2 = second.angular.getRotationRate();

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

  366.     }

  368.     /** Compute a composite rotation acceleration.
  369.      * @param first first applied transform
  370.      * @param second second applied transform
  371.      * @param <T> the type of the field elements
  372.      * @return rotation acceleration part of the composite transform
  373.      */
  374.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {

  376.         final FieldVector3D<T> o1    = first.angular.getRotationRate();
  377.         final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
  378.         final FieldRotation<T> r2    = second.angular.getRotation();
  379.         final FieldVector3D<T> o2    = second.angular.getRotationRate();
  380.         final FieldVector3D<T> oDot2 = second.angular.getRotationAcceleration();

  382.         return new FieldVector3D<>( 1, oDot2,
  383.                                     1, r2.applyTo(oDot1),
  384.                                    -1, FieldVector3D.crossProduct(o2, r2.applyTo(o1)));

  386.     }

  388.     /** {@inheritDoc} */
  389.     @Override
  390.     public AbsoluteDate getDate() {
  391.         return aDate;
  392.     }

  394.     /** Get the date.
  395.      * @return date attached to the object
  396.      */
  397.     @Override
  398.     public FieldAbsoluteDate<T> getFieldDate() {
  399.         return date;
  400.     }

  402.     /** {@inheritDoc} */
  403.     @Override
  404.     public FieldTransform<T> shiftedBy(final double dt) {
  405.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt),
  406.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  407.     }

  409.     /** Get a time-shifted instance.
  410.      * @param dt time shift in seconds
  411.      * @return a new instance, shifted with respect to instance (which is not changed)
  412.      */
  413.     public FieldTransform<T> shiftedBy(final T dt) {
  414.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt.getReal()),
  415.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  416.     }

  418.     /**
  419.      * Shift the transform in time considering all rates, then return only the
  420.      * translation and rotation portion of the transform.
  421.      *
  422.      * @param dt time shift in seconds.
  423.      * @return shifted transform as a static transform. It is static in the
  424.      * sense that it can only be used to transform directions and positions, but
  425.      * not velocities or accelerations.
  426.      * @see #shiftedBy(double)
  427.      */
  428.     public FieldStaticTransform<T> staticShiftedBy(final T dt) {
  429.         return FieldStaticTransform.of(date.shiftedBy(dt),
  430.                                        cartesian.positionShiftedBy(dt),
  431.                                        angular.rotationShiftedBy(dt));
  432.     }

  434.     /**
  435.      * Create a so-called static transform from the instance.
  436.      *
  437.      * @return static part of the transform. It is static in the
  438.      * sense that it can only be used to transform directions and positions, but
  439.      * not velocities or accelerations.
  440.      * @see FieldStaticTransform
  441.      */
  442.     public FieldStaticTransform<T> toStaticTransform() {
  443.         return FieldStaticTransform.of(date, cartesian.getPosition(), angular.getRotation());
  444.     }

  446.     /** Interpolate a transform from a sample set of existing transforms.
  447.      * <p>
  448.      * Calling this method is equivalent to call {@link #interpolate(FieldAbsoluteDate,
  449.      * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
  450.      * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
  451.      * {@link AngularDerivativesFilter#USE_RRA}
  452.      * set to true.
  453.      * </p>
  454.      * @param interpolationDate interpolation date
  455.      * @param sample sample points on which interpolation should be done
  456.      * @param <T> the type of the field elements
  457.      * @return a new instance, interpolated at specified date
  458.      */
  459.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> interpolationDate,
  460.                                                                                 final Collection<FieldTransform<T>> sample) {
  461.         return interpolate(interpolationDate,
  462.                            CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
  463.                            sample);
  464.     }

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

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

  526.         // Create samples
  527.         final List<TimeStampedFieldPVCoordinates<T>>      datedPV = new ArrayList<>();
  528.         final List<TimeStampedFieldAngularCoordinates<T>> datedAC = new ArrayList<>();
  529.         sample.forEach(t -> {
  530.             datedPV.add(new TimeStampedFieldPVCoordinates<>(t.getDate(), t.getTranslation(), t.getVelocity(), t.getAcceleration()));
  531.             datedAC.add(new TimeStampedFieldAngularCoordinates<>(t.getDate(), t.getRotation(), t.getRotationRate(), t.getRotationAcceleration()));
  532.         });

  534.         // Create interpolators
  535.         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> pvInterpolator =
  536.                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(datedPV.size(), cFilter);

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

  541.         // Interpolate
  542.         final TimeStampedFieldPVCoordinates<T>      interpolatedPV = pvInterpolator.interpolate(date, datedPV);
  543.         final TimeStampedFieldAngularCoordinates<T> interpolatedAC = angularInterpolator.interpolate(date, datedAC);

  545.         return new FieldTransform<>(date, date.toAbsoluteDate(), interpolatedPV, interpolatedAC);
  546.     }

  548.     /** Get the inverse transform of the instance.
  549.      * @return inverse transform of the instance
  550.      */
  551.     @Override
  552.     public FieldTransform<T> getInverse() {

  554.         final FieldRotation<T> r    = angular.getRotation();
  555.         final FieldVector3D<T> o    = angular.getRotationRate();
  556.         final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  557.         final FieldVector3D<T> rp   = r.applyTo(cartesian.getPosition());
  558.         final FieldVector3D<T> rv   = r.applyTo(cartesian.getVelocity());
  559.         final FieldVector3D<T> ra   = r.applyTo(cartesian.getAcceleration());

  561.         final FieldVector3D<T> pInv        = rp.negate();
  562.         final FieldVector3D<T> crossP      = FieldVector3D.crossProduct(o, rp);
  563.         final FieldVector3D<T> vInv        = crossP.subtract(rv);
  564.         final FieldVector3D<T> crossV      = FieldVector3D.crossProduct(o, rv);
  565.         final FieldVector3D<T> crossDotP   = FieldVector3D.crossProduct(oDot, rp);
  566.         final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o, crossP);
  567.         final FieldVector3D<T> aInv        = new FieldVector3D<>(-1, ra,
  568.                                                                   2, crossV,
  569.                                                                   1, crossDotP,
  570.                                                                  -1, crossCrossP);

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

  574.     }

  576.     /** Get a frozen transform.
  577.      * <p>
  578.      * This method creates a copy of the instance but frozen in time,
  579.      * i.e. with velocity, acceleration and rotation rate forced to zero.
  580.      * </p>
  581.      * @return a new transform, without any time-dependent parts
  582.      */
  583.     public FieldTransform<T> freeze() {
  584.         return new FieldTransform<>(date, aDate,
  585.                                     new FieldPVCoordinates<>(cartesian.getPosition(),
  586.                                                              FieldVector3D.getZero(date.getField()),
  587.                                                              FieldVector3D.getZero(date.getField())),
  588.                                     new FieldAngularCoordinates<>(angular.getRotation(),
  589.                                                                   FieldVector3D.getZero(date.getField()),
  590.                                                                   FieldVector3D.getZero(date.getField())));
  591.     }

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

  610.     /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
  611.      * <p>
  612.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  613.      * consistency between the transform {@link #getDate() date} and the time-stamped
  614.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  615.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  616.      * the input argument, regardless of the instance {@link #getDate() date}.
  617.      * </p>
  618.      * @param pv time-stamped  position-velocity to transform.
  619.      * @return transformed time-stamped position-velocity
  620.      */
  621.     public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedPVCoordinates pv) {
  622.         return angular.applyTo(new TimeStampedFieldPVCoordinates<>(pv.getDate(),
  623.                                                                    cartesian.getPosition().add(pv.getPosition()),
  624.                                                                    cartesian.getVelocity().add(pv.getVelocity()),
  625.                                                                    cartesian.getAcceleration().add(pv.getAcceleration())));
  626.     }

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

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

  697.     /** Compute the Jacobian of the {@link #transformPVCoordinates(FieldPVCoordinates)}
  698.      * method of the transform.
  699.      * <p>
  700.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  701.      * of the transformed {@link FieldPVCoordinates} with respect to Cartesian coordinate j
  702.      * of the input {@link FieldPVCoordinates} in method {@link #transformPVCoordinates(FieldPVCoordinates)}.
  703.      * </p>
  704.      * <p>
  705.      * This definition implies that if we define position-velocity coordinates
  706.      * <pre>PV₁ = transform.transformPVCoordinates(PV₀)</pre>
  707.      * then their differentials dPV₁ and dPV₀ will obey the following relation
  708.      * where J is the matrix computed by this method:
  709.      * <pre>dPV₁ = J &times; dPV₀</pre>
  710.      *
  711.      * @param selector selector specifying the size of the upper left corner that must be filled
  712.      * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
  713.      * velocities and accelerations)
  714.      * @param jacobian placeholder matrix whose upper-left corner is to be filled with
  715.      * the Jacobian, the rest of the matrix remaining untouched
  716.      */
  717.     public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {

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

  721.         if (selector.getMaxOrder() == 0) {
  722.             // elementary matrix for rotation
  723.             final T[][] mData = angular.getRotation().getMatrix();

  725.             // dP1/dP0
  726.             System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  727.             System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  728.             System.arraycopy(mData[2], 0, jacobian[2], 0, 3);
  729.         }

  731.         else if (selector.getMaxOrder() == 1) {
  732.             // use KinematicTransform Jacobian
  733.             final T[][] mData = getPVJacobian();
  734.             for (int i = 0; i < mData.length; i++) {
  735.                 System.arraycopy(mData[i], 0, jacobian[i], 0, mData[i].length);
  736.             }
  737.         }

  739.         else if (selector.getMaxOrder() >= 2) {
  740.             getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);

  742.             // dP1/dA0
  743.             Arrays.fill(jacobian[0], 6, 9, zero);
  744.             Arrays.fill(jacobian[1], 6, 9, zero);
  745.             Arrays.fill(jacobian[2], 6, 9, zero);

  747.             // dV1/dA0
  748.             Arrays.fill(jacobian[3], 6, 9, zero);
  749.             Arrays.fill(jacobian[4], 6, 9, zero);
  750.             Arrays.fill(jacobian[5], 6, 9, zero);

  752.             // dA1/dP0
  753.             final FieldVector3D<T> o = angular.getRotationRate();
  754.             final T ox = o.getX();
  755.             final T oy = o.getY();
  756.             final T oz = o.getZ();
  757.             final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  758.             final T oDotx  = oDot.getX();
  759.             final T oDoty  = oDot.getY();
  760.             final T oDotz  = oDot.getZ();
  761.             for (int i = 0; i < 3; ++i) {
  762.                 jacobian[6][i] = oDotz.multiply(jacobian[1][i]).subtract(oDoty.multiply(jacobian[2][i])).add(oz.multiply(jacobian[4][i]).subtract(oy.multiply(jacobian[5][i])));
  763.                 jacobian[7][i] = oDotx.multiply(jacobian[2][i]).subtract(oDotz.multiply(jacobian[0][i])).add(ox.multiply(jacobian[5][i]).subtract(oz.multiply(jacobian[3][i])));
  764.                 jacobian[8][i] = oDoty.multiply(jacobian[0][i]).subtract(oDotx.multiply(jacobian[1][i])).add(oy.multiply(jacobian[3][i]).subtract(ox.multiply(jacobian[4][i])));
  765.             }

  767.             // dA1/dV0
  768.             for (int i = 0; i < 3; ++i) {
  769.                 jacobian[6][i + 3] = oz.multiply(jacobian[1][i]).subtract(oy.multiply(jacobian[2][i])).multiply(2);
  770.                 jacobian[7][i + 3] = ox.multiply(jacobian[2][i]).subtract(oz.multiply(jacobian[0][i])).multiply(2);
  771.                 jacobian[8][i + 3] = oy.multiply(jacobian[0][i]).subtract(ox.multiply(jacobian[1][i])).multiply(2);
  772.             }

  774.             // dA1/dA0
  775.             System.arraycopy(jacobian[0], 0, jacobian[6], 6, 3);
  776.             System.arraycopy(jacobian[1], 0, jacobian[7], 6, 3);
  777.             System.arraycopy(jacobian[2], 0, jacobian[8], 6, 3);

  779.         }

  781.     }

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

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

  808.     /** Get the first time derivative of the translation.
  809.      * @return first time derivative of the translation
  810.      * @see #getCartesian()
  811.      * @see #getTranslation()
  812.      * @see #getAcceleration()
  813.      */
  814.     public FieldVector3D<T> getVelocity() {
  815.         return cartesian.getVelocity();
  816.     }

  818.     /** Get the second time derivative of the translation.
  819.      * @return second time derivative of the translation
  820.      * @see #getCartesian()
  821.      * @see #getTranslation()
  822.      * @see #getVelocity()
  823.      */
  824.     public FieldVector3D<T> getAcceleration() {
  825.         return cartesian.getAcceleration();
  826.     }

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

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

  854.     /** Get the first time derivative of the rotation.
  855.      * <p>The norm represents the angular rate.</p>
  856.      * @return First time derivative of the rotation
  857.      * @see #getAngular()
  858.      * @see #getRotation()
  859.      * @see #getRotationAcceleration()
  860.      */
  861.     public FieldVector3D<T> getRotationRate() {
  862.         return angular.getRotationRate();
  863.     }

  865.     /** Get the second time derivative of the rotation.
  866.      * @return Second time derivative of the rotation
  867.      * @see #getAngular()
  868.      * @see #getRotation()
  869.      * @see #getRotationRate()
  870.      */
  871.     public FieldVector3D<T> getRotationAcceleration() {
  872.         return angular.getRotationAcceleration();
  873.     }

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

  878.         /** Field. */
  879.         private final Field<T> field;

  881.         /** Simple constructor.
  882.          * @param field field for the components
  883.          */
  884.         FieldIdentityTransform(final Field<T> field) {
  885.             super(FieldAbsoluteDate.getArbitraryEpoch(field),
  886.                   FieldAbsoluteDate.getArbitraryEpoch(field).toAbsoluteDate(),
  887.                   FieldPVCoordinates.getZero(field),
  888.                   FieldAngularCoordinates.getIdentity(field));
  889.             this.field = field;
  890.         }

  892.         @Override
  893.         public FieldStaticTransform<T> staticShiftedBy(final T dt) {
  894.             return toStaticTransform();
  895.         }

  897.         @Override
  898.         public FieldTransform<T> shiftedBy(final T dt) {
  899.             return this;
  900.         }

  902.         /** {@inheritDoc} */
  903.         @Override
  904.         public FieldTransform<T> shiftedBy(final double dt) {
  905.             return this;
  906.         }

  908.         @Override
  909.         public FieldStaticTransform<T> getStaticInverse() {
  910.             return toStaticTransform();
  911.         }

  913.         /** {@inheritDoc} */
  914.         @Override
  915.         public FieldTransform<T> getInverse() {
  916.             return this;
  917.         }

  919.         @Override
  920.         public FieldStaticTransform<T> toStaticTransform() {
  921.             return FieldStaticTransform.getIdentity(field);
  922.         }

  924.         /** {@inheritDoc} */
  925.         @Override
  926.         public FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  927.             return position;
  928.         }

  930.         @Override
  931.         public FieldVector3D<T> transformPosition(final Vector3D position) {
  932.             return transformVector(position);
  933.         }

  935.         /** {@inheritDoc} */
  936.         @Override
  937.         public FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  938.             return vector;
  939.         }

  941.         @Override
  942.         public FieldVector3D<T> transformVector(final Vector3D vector) {
  943.             return new FieldVector3D<>(field, vector);
  944.         }

  946.         /** {@inheritDoc} */
  947.         @Override
  948.         public FieldLine<T> transformLine(final FieldLine<T> line) {
  949.             return line;
  950.         }

  952.         /** {@inheritDoc} */
  953.         @Override
  954.         public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  955.             return pv;
  956.         }

  958.         @Override
  959.         public FieldPVCoordinates<T> transformPVCoordinates(final PVCoordinates pv) {
  960.             return new FieldPVCoordinates<>(field, pv);
  961.         }

  963.         @Override
  964.         public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedPVCoordinates pv) {
  965.             return new TimeStampedFieldPVCoordinates<>(field, pv);
  966.         }

  968.         @Override
  969.         public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedFieldPVCoordinates<T> pv) {
  970.             return pv;
  971.         }

  973.         @Override
  974.         public FieldPVCoordinates<T> transformOnlyPV(final FieldPVCoordinates<T> pv) {
  975.             return new FieldPVCoordinates<>(pv.getPosition(), pv.getVelocity());
  976.         }

  978.         @Override
  979.         public TimeStampedFieldPVCoordinates<T> transformOnlyPV(final TimeStampedFieldPVCoordinates<T> pv) {
  980.             return new TimeStampedFieldPVCoordinates<>(pv.getDate(), pv.getPosition(), pv.getVelocity(), FieldVector3D.getZero(field));
  981.         }

  983.         /** {@inheritDoc} */
  984.         @Override
  985.         public FieldTransform<T> freeze() {
  986.             return this;
  987.         }

  989.         /** {@inheritDoc} */
  990.         @Override
  991.         public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {
  992.             final int n = 3 * (selector.getMaxOrder() + 1);
  993.             for (int i = 0; i < n; ++i) {
  994.                 Arrays.fill(jacobian[i], 0, n, getFieldDate().getField().getZero());
  995.                 jacobian[i][i] = getFieldDate().getField().getOne();
  996.             }
  997.         }

  999.     }

  1001. }
Created with JaCoCo 0.8.12.202403310830