FieldTransform.java

  1. /* Copyright 2002-2024 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;

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

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


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

  101.     /** Date of the transform. */
  102.     private final FieldAbsoluteDate<T> date;

  103.     /** Date of the transform. */
  104.     private final AbsoluteDate aDate;

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

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

  109.     /** Build a transform from its primitive operations.
  110.      * @param date date of the transform
  111.      * @param aDate date of the transform
  112.      * @param cartesian Cartesian coordinates of the target frame with respect to the original frame
  113.      * @param angular angular coordinates of the target frame with respect to the original frame
  114.      */
  115.     private FieldTransform(final FieldAbsoluteDate<T> date, final AbsoluteDate aDate,
  116.                            final FieldPVCoordinates<T> cartesian,
  117.                            final FieldAngularCoordinates<T> angular) {
  118.         this.date      = date;
  119.         this.aDate     = aDate;
  120.         this.cartesian = cartesian;
  121.         this.angular   = angular;
  122.     }

  123.     /** Build a transform from a regular transform.
  124.      * @param field field of the elements
  125.      * @param transform regular transform to convert
  126.      */
  127.     public FieldTransform(final Field<T> field, final Transform transform) {
  128.         this(new FieldAbsoluteDate<>(field, transform.getDate()), transform.getDate(),
  129.              new FieldPVCoordinates<>(field, transform.getCartesian()),
  130.              new FieldAngularCoordinates<>(field, transform.getAngular()));
  131.     }

  132.     /** Build a translation transform.
  133.      * @param date date of the transform
  134.      * @param translation translation to apply (i.e. coordinates of
  135.      * the transformed origin, or coordinates of the origin of the
  136.      * old frame in the new frame)
  137.      */
  138.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation) {
  139.         this(date, date.toAbsoluteDate(),
  140.              new FieldPVCoordinates<>(translation,
  141.                                       FieldVector3D.getZero(date.getField()),
  142.                                       FieldVector3D.getZero(date.getField())),
  143.              FieldAngularCoordinates.getIdentity(date.getField()));
  144.     }

  145.     /** Build a rotation transform.
  146.      * @param date date of the transform
  147.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  148.      * coordinates of a vector expressed in the old frame to obtain the
  149.      * same vector expressed in the new frame )
  150.      */
  151.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldRotation<T> rotation) {
  152.         this(date, date.toAbsoluteDate(),
  153.              FieldPVCoordinates.getZero(date.getField()),
  154.              new FieldAngularCoordinates<>(rotation, FieldVector3D.getZero(date.getField())));
  155.     }

  156.     /** Build a translation transform, with its first time derivative.
  157.      * @param date date of the transform
  158.      * @param translation translation to apply (i.e. coordinates of
  159.      * the transformed origin, or coordinates of the origin of the
  160.      * old frame in the new frame)
  161.      * @param velocity the velocity of the translation (i.e. origin
  162.      * of the old frame velocity in the new frame)
  163.      */
  164.     public FieldTransform(final FieldAbsoluteDate<T> date,
  165.                           final FieldVector3D<T> translation,
  166.                           final FieldVector3D<T> velocity) {
  167.         this(date,
  168.              new FieldPVCoordinates<>(translation,
  169.                                       velocity,
  170.                                       FieldVector3D.getZero(date.getField())));
  171.     }

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

  187.     /** Build a translation transform, with its first time derivative.
  188.      * @param date date of the transform
  189.      * @param cartesian Cartesian part of the transformation to apply (i.e. coordinates of
  190.      * the transformed origin, or coordinates of the origin of the
  191.      * old frame in the new frame, with their derivatives)
  192.      */
  193.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldPVCoordinates<T> cartesian) {
  194.         this(date, date.toAbsoluteDate(),
  195.              cartesian,
  196.              FieldAngularCoordinates.getIdentity(date.getField()));
  197.     }

  198.     /** Build a combined translation and rotation transform.
  199.      * @param date date of the transform
  200.      * @param translation translation to apply (i.e. coordinates of
  201.      * the transformed origin, or coordinates of the origin of the
  202.      * old frame in the new frame)
  203.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  204.      * coordinates of a vector expressed in the old frame to obtain the
  205.      * same vector expressed in the new frame )
  206.      * @since 12.1
  207.      */
  208.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation,
  209.                           final FieldRotation<T> rotation) {
  210.         this(date, date.toAbsoluteDate(), new FieldPVCoordinates<>(translation, FieldVector3D.getZero(date.getField())),
  211.                 new FieldAngularCoordinates<>(rotation, FieldVector3D.getZero(date.getField())));
  212.     }

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

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

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

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

  277.     /** Get the identity transform.
  278.      * @param field field for the components
  279.      * @param <T> the type of the field elements
  280.      * @return identity transform
  281.      */
  282.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> getIdentity(final Field<T> field) {
  283.         return new FieldIdentityTransform<>(field);
  284.     }

  285.     /** Compute a composite velocity.
  286.      * @param first first applied transform
  287.      * @param second second applied transform
  288.      * @param <T> the type of the field elements
  289.      * @return velocity part of the composite transform
  290.      */
  291.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeVelocity(final FieldTransform<T> first, final FieldTransform<T> second) {

  292.         final FieldVector3D<T> v1 = first.cartesian.getVelocity();
  293.         final FieldRotation<T> r1 = first.angular.getRotation();
  294.         final FieldVector3D<T> o1 = first.angular.getRotationRate();
  295.         final FieldVector3D<T> p2 = second.cartesian.getPosition();
  296.         final FieldVector3D<T> v2 = second.cartesian.getVelocity();

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

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

  299.     }

  300.     /** Compute a composite acceleration.
  301.      * @param first first applied transform
  302.      * @param second second applied transform
  303.      * @param <T> the type of the field elements
  304.      * @return acceleration part of the composite transform
  305.      */
  306.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {

  307.         final FieldVector3D<T> a1    = first.cartesian.getAcceleration();
  308.         final FieldRotation<T> r1    = first.angular.getRotation();
  309.         final FieldVector3D<T> o1    = first.angular.getRotationRate();
  310.         final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
  311.         final FieldVector3D<T> p2    = second.cartesian.getPosition();
  312.         final FieldVector3D<T> v2    = second.cartesian.getVelocity();
  313.         final FieldVector3D<T> a2    = second.cartesian.getAcceleration();

  314.         final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o1,    FieldVector3D.crossProduct(o1, p2));
  315.         final FieldVector3D<T> crossV      = FieldVector3D.crossProduct(o1,    v2);
  316.         final FieldVector3D<T> crossDotP   = FieldVector3D.crossProduct(oDot1, p2);

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

  318.     }

  319.     /** Compute a composite rotation rate.
  320.      * @param first first applied transform
  321.      * @param second second applied transform
  322.      * @param <T> the type of the field elements
  323.      * @return rotation rate part of the composite transform
  324.      */
  325.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationRate(final FieldTransform<T> first, final FieldTransform<T> second) {

  326.         final FieldVector3D<T> o1 = first.angular.getRotationRate();
  327.         final FieldRotation<T> r2 = second.angular.getRotation();
  328.         final FieldVector3D<T> o2 = second.angular.getRotationRate();

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

  330.     }

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

  338.         final FieldVector3D<T> o1    = first.angular.getRotationRate();
  339.         final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
  340.         final FieldRotation<T> r2    = second.angular.getRotation();
  341.         final FieldVector3D<T> o2    = second.angular.getRotationRate();
  342.         final FieldVector3D<T> oDot2 = second.angular.getRotationAcceleration();

  343.         return new FieldVector3D<>( 1, oDot2,
  344.                                     1, r2.applyTo(oDot1),
  345.                                    -1, FieldVector3D.crossProduct(o2, r2.applyTo(o1)));

  346.     }

  347.     /** {@inheritDoc} */
  348.     @Override
  349.     public AbsoluteDate getDate() {
  350.         return aDate;
  351.     }

  352.     /** Get the date.
  353.      * @return date attached to the object
  354.      */
  355.     @Override
  356.     public FieldAbsoluteDate<T> getFieldDate() {
  357.         return date;
  358.     }

  359.     /** {@inheritDoc} */
  360.     @Override
  361.     public FieldTransform<T> shiftedBy(final double dt) {
  362.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt),
  363.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  364.     }

  365.     /** Get a time-shifted instance.
  366.      * @param dt time shift in seconds
  367.      * @return a new instance, shifted with respect to instance (which is not changed)
  368.      */
  369.     public FieldTransform<T> shiftedBy(final T dt) {
  370.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt.getReal()),
  371.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  372.     }

  373.     /**
  374.      * Shift the transform in time considering all rates, then return only the
  375.      * translation and rotation portion of the transform.
  376.      *
  377.      * @param dt time shift in seconds.
  378.      * @return shifted transform as a static transform. It is static in the
  379.      * sense that it can only be used to transform directions and positions, but
  380.      * not velocities or accelerations.
  381.      * @see #shiftedBy(double)
  382.      */
  383.     public FieldStaticTransform<T> staticShiftedBy(final T dt) {
  384.         return FieldStaticTransform.of(date.shiftedBy(dt),
  385.                                        cartesian.positionShiftedBy(dt),
  386.                                        angular.rotationShiftedBy(dt));
  387.     }

  388.     /**
  389.      * Create a so-called static transform from the instance.
  390.      *
  391.      * @return static part of the transform. It is static in the
  392.      * sense that it can only be used to transform directions and positions, but
  393.      * not velocities or accelerations.
  394.      * @see FieldStaticTransform
  395.      */
  396.     public FieldStaticTransform<T> toStaticTransform() {
  397.         return FieldStaticTransform.of(date, cartesian.getPosition(), angular.getRotation());
  398.     }

  399.     /** Interpolate a transform from a sample set of existing transforms.
  400.      * <p>
  401.      * Calling this method is equivalent to call {@link #interpolate(FieldAbsoluteDate,
  402.      * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
  403.      * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
  404.      * {@link AngularDerivativesFilter#USE_RRA}
  405.      * set to true.
  406.      * </p>
  407.      * @param interpolationDate interpolation date
  408.      * @param sample sample points on which interpolation should be done
  409.      * @param <T> the type of the field elements
  410.      * @return a new instance, interpolated at specified date
  411.      */
  412.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> interpolationDate,
  413.                                                                                 final Collection<FieldTransform<T>> sample) {
  414.         return interpolate(interpolationDate,
  415.                            CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
  416.                            sample);
  417.     }

  418.     /** Interpolate a transform from a sample set of existing transforms.
  419.      * <p>
  420.      * Note that even if first time derivatives (velocities and rotation rates)
  421.      * from sample can be ignored, the interpolated instance always includes
  422.      * interpolated derivatives. This feature can be used explicitly to
  423.      * compute these derivatives when it would be too complex to compute them
  424.      * from an analytical formula: just compute a few sample points from the
  425.      * explicit formula and set the derivatives to zero in these sample points,
  426.      * then use interpolation to add derivatives consistent with the positions
  427.      * and rotations.
  428.      * </p>
  429.      * <p>
  430.      * As this implementation of interpolation is polynomial, it should be used only
  431.      * with small samples (about 10-20 points) in order to avoid <a
  432.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  433.      * and numerical problems (including NaN appearing).
  434.      * </p>
  435.      * @param date interpolation date
  436.      * @param cFilter filter for derivatives from the sample to use in interpolation
  437.      * @param aFilter filter for derivatives from the sample to use in interpolation
  438.      * @param sample sample points on which interpolation should be done
  439.      * @return a new instance, interpolated at specified date
  440.           * @param <T> the type of the field elements
  441.      */
  442.     public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> date,
  443.                                                                                 final CartesianDerivativesFilter cFilter,
  444.                                                                                 final AngularDerivativesFilter aFilter,
  445.                                                                                 final Collection<FieldTransform<T>> sample) {
  446.         return interpolate(date, cFilter, aFilter, sample.stream());
  447.     }

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

  476.         // Create samples
  477.         final List<TimeStampedFieldPVCoordinates<T>>      datedPV = new ArrayList<>();
  478.         final List<TimeStampedFieldAngularCoordinates<T>> datedAC = new ArrayList<>();
  479.         sample.forEach(t -> {
  480.             datedPV.add(new TimeStampedFieldPVCoordinates<>(t.getDate(), t.getTranslation(), t.getVelocity(), t.getAcceleration()));
  481.             datedAC.add(new TimeStampedFieldAngularCoordinates<>(t.getDate(), t.getRotation(), t.getRotationRate(), t.getRotationAcceleration()));
  482.         });

  483.         // Create interpolators
  484.         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> pvInterpolator =
  485.                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(datedPV.size(), cFilter);

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

  488.         // Interpolate
  489.         final TimeStampedFieldPVCoordinates<T>      interpolatedPV = pvInterpolator.interpolate(date, datedPV);
  490.         final TimeStampedFieldAngularCoordinates<T> interpolatedAC = angularInterpolator.interpolate(date, datedAC);

  491.         return new FieldTransform<>(date, date.toAbsoluteDate(), interpolatedPV, interpolatedAC);
  492.     }

  493.     /** Get the inverse transform of the instance.
  494.      * @return inverse transform of the instance
  495.      */
  496.     @Override
  497.     public FieldTransform<T> getInverse() {

  498.         final FieldRotation<T> r    = angular.getRotation();
  499.         final FieldVector3D<T> o    = angular.getRotationRate();
  500.         final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  501.         final FieldVector3D<T> rp   = r.applyTo(cartesian.getPosition());
  502.         final FieldVector3D<T> rv   = r.applyTo(cartesian.getVelocity());
  503.         final FieldVector3D<T> ra   = r.applyTo(cartesian.getAcceleration());

  504.         final FieldVector3D<T> pInv        = rp.negate();
  505.         final FieldVector3D<T> crossP      = FieldVector3D.crossProduct(o, rp);
  506.         final FieldVector3D<T> vInv        = crossP.subtract(rv);
  507.         final FieldVector3D<T> crossV      = FieldVector3D.crossProduct(o, rv);
  508.         final FieldVector3D<T> crossDotP   = FieldVector3D.crossProduct(oDot, rp);
  509.         final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o, crossP);
  510.         final FieldVector3D<T> aInv        = new FieldVector3D<>(-1, ra,
  511.                                                                   2, crossV,
  512.                                                                   1, crossDotP,
  513.                                                                  -1, crossCrossP);

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

  515.     }

  516.     /** Get a frozen transform.
  517.      * <p>
  518.      * This method creates a copy of the instance but frozen in time,
  519.      * i.e. with velocity, acceleration and rotation rate forced to zero.
  520.      * </p>
  521.      * @return a new transform, without any time-dependent parts
  522.      */
  523.     public FieldTransform<T> freeze() {
  524.         return new FieldTransform<>(date, aDate,
  525.                                     new FieldPVCoordinates<>(cartesian.getPosition(),
  526.                                                              FieldVector3D.getZero(date.getField()),
  527.                                                              FieldVector3D.getZero(date.getField())),
  528.                                     new FieldAngularCoordinates<>(angular.getRotation(),
  529.                                                                   FieldVector3D.getZero(date.getField()),
  530.                                                                   FieldVector3D.getZero(date.getField())));
  531.     }

  532.     /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
  533.      * <p>
  534.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  535.      * consistency between the transform {@link #getDate() date} and the time-stamped
  536.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  537.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  538.      * the input argument, regardless of the instance {@link #getDate() date}.
  539.      * </p>
  540.      * @param pv time-stamped  position-velocity to transform.
  541.      * @return transformed time-stamped position-velocity
  542.      */
  543.     public FieldPVCoordinates<T> transformPVCoordinates(final PVCoordinates pv) {
  544.         return angular.applyTo(new FieldPVCoordinates<>(cartesian.getPosition().add(pv.getPosition()),
  545.                                                         cartesian.getVelocity().add(pv.getVelocity()),
  546.                                                         cartesian.getAcceleration().add(pv.getAcceleration())));
  547.     }

  548.     /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
  549.      * <p>
  550.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  551.      * consistency between the transform {@link #getDate() date} and the time-stamped
  552.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  553.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  554.      * the input argument, regardless of the instance {@link #getDate() date}.
  555.      * </p>
  556.      * @param pv time-stamped  position-velocity to transform.
  557.      * @return transformed time-stamped position-velocity
  558.      */
  559.     public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedPVCoordinates pv) {
  560.         return angular.applyTo(new TimeStampedFieldPVCoordinates<>(pv.getDate(),
  561.                                                                    cartesian.getPosition().add(pv.getPosition()),
  562.                                                                    cartesian.getVelocity().add(pv.getVelocity()),
  563.                                                                    cartesian.getAcceleration().add(pv.getAcceleration())));
  564.     }

  565.     /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
  566.      * <p>
  567.      * BEWARE! This method does explicit computation of velocity and acceleration by combining
  568.      * the transform velocity, acceleration, rotation rate and rotation acceleration with the
  569.      * velocity and acceleration from the argument. This implies that this method should
  570.      * <em>not</em> be used when derivatives are contained in the {@link CalculusFieldElement field
  571.      * elements} (typically when using {@link org.hipparchus.analysis.differentiation.DerivativeStructure
  572.      * DerivativeStructure} elements where time is one of the differentiation parameter), otherwise
  573.      * the time derivatives would be computed twice, once explicitly in this method and once implicitly
  574.      * in the field operations. If time derivatives are contained in the field elements themselves,
  575.      * then rather than this method the {@link #transformPosition(FieldVector3D) transformPosition}
  576.      * method should be used, so the derivatives are computed once, as part of the field. This
  577.      * method is rather expected to be used when the field elements are {@link
  578.      * org.hipparchus.analysis.differentiation.DerivativeStructure DerivativeStructure} instances
  579.      * where the differentiation parameters are not time (they can typically be initial state
  580.      * for computing state transition matrices or force models parameters, or ground stations
  581.      * positions, ...).
  582.      * </p>
  583.      * <p>
  584.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  585.      * consistency between the transform {@link #getDate() date} and the time-stamped
  586.      * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
  587.      * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
  588.      * the input argument, regardless of the instance {@link #getDate() date}.
  589.      * </p>
  590.      * @param pv time-stamped position-velocity to transform.
  591.      * @return transformed time-stamped position-velocity
  592.      */
  593.     public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  594.         return angular.applyTo(new FieldPVCoordinates<>(pv.getPosition().add(cartesian.getPosition()),
  595.                                                         pv.getVelocity().add(cartesian.getVelocity()),
  596.                                                         pv.getAcceleration().add(cartesian.getAcceleration())));
  597.     }

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

  632.     /** Compute the Jacobian of the {@link #transformPVCoordinates(FieldPVCoordinates)}
  633.      * method of the transform.
  634.      * <p>
  635.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  636.      * of the transformed {@link FieldPVCoordinates} with respect to Cartesian coordinate j
  637.      * of the input {@link FieldPVCoordinates} in method {@link #transformPVCoordinates(FieldPVCoordinates)}.
  638.      * </p>
  639.      * <p>
  640.      * This definition implies that if we define position-velocity coordinates
  641.      * <pre>PV₁ = transform.transformPVCoordinates(PV₀)</pre>
  642.      * then their differentials dPV₁ and dPV₀ will obey the following relation
  643.      * where J is the matrix computed by this method:
  644.      * <pre>dPV₁ = J &times; dPV₀</pre>
  645.      *
  646.      * @param selector selector specifying the size of the upper left corner that must be filled
  647.      * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
  648.      * velocities and accelerations)
  649.      * @param jacobian placeholder matrix whose upper-left corner is to be filled with
  650.      * the Jacobian, the rest of the matrix remaining untouched
  651.      */
  652.     public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {

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

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

  656.         // dP1/dP0
  657.         System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  658.         System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  659.         System.arraycopy(mData[2], 0, jacobian[2], 0, 3);

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

  661.             // dP1/dV0
  662.             Arrays.fill(jacobian[0], 3, 6, zero);
  663.             Arrays.fill(jacobian[1], 3, 6, zero);
  664.             Arrays.fill(jacobian[2], 3, 6, zero);

  665.             // dV1/dP0
  666.             final FieldVector3D<T> o = angular.getRotationRate();
  667.             final T ox = o.getX();
  668.             final T oy = o.getY();
  669.             final T oz = o.getZ();
  670.             for (int i = 0; i < 3; ++i) {
  671.                 jacobian[3][i] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i]));
  672.                 jacobian[4][i] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i]));
  673.                 jacobian[5][i] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i]));
  674.             }

  675.             // dV1/dV0
  676.             System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
  677.             System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
  678.             System.arraycopy(mData[2], 0, jacobian[5], 3, 3);

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

  680.                 // dP1/dA0
  681.                 Arrays.fill(jacobian[0], 6, 9, zero);
  682.                 Arrays.fill(jacobian[1], 6, 9, zero);
  683.                 Arrays.fill(jacobian[2], 6, 9, zero);

  684.                 // dV1/dA0
  685.                 Arrays.fill(jacobian[3], 6, 9, zero);
  686.                 Arrays.fill(jacobian[4], 6, 9, zero);
  687.                 Arrays.fill(jacobian[5], 6, 9, zero);

  688.                 // dA1/dP0
  689.                 final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  690.                 final T oDotx  = oDot.getX();
  691.                 final T oDoty  = oDot.getY();
  692.                 final T oDotz  = oDot.getZ();
  693.                 for (int i = 0; i < 3; ++i) {
  694.                     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])));
  695.                     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])));
  696.                     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])));
  697.                 }

  698.                 // dA1/dV0
  699.                 for (int i = 0; i < 3; ++i) {
  700.                     jacobian[6][i + 3] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i])).multiply(2);
  701.                     jacobian[7][i + 3] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i])).multiply(2);
  702.                     jacobian[8][i + 3] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i])).multiply(2);
  703.                 }

  704.                 // dA1/dA0
  705.                 System.arraycopy(mData[0], 0, jacobian[6], 6, 3);
  706.                 System.arraycopy(mData[1], 0, jacobian[7], 6, 3);
  707.                 System.arraycopy(mData[2], 0, jacobian[8], 6, 3);

  708.             }

  709.         }

  710.     }

  711.     /** Get the underlying elementary Cartesian part.
  712.      * <p>A transform can be uniquely represented as an elementary
  713.      * translation followed by an elementary rotation. This method
  714.      * returns this unique elementary translation with its derivative.</p>
  715.      * @return underlying elementary Cartesian part
  716.      * @see #getTranslation()
  717.      * @see #getVelocity()
  718.      */
  719.     public FieldPVCoordinates<T> getCartesian() {
  720.         return cartesian;
  721.     }

  722.     /** Get the underlying elementary translation.
  723.      * <p>A transform can be uniquely represented as an elementary
  724.      * translation followed by an elementary rotation. This method
  725.      * returns this unique elementary translation.</p>
  726.      * @return underlying elementary translation
  727.      * @see #getCartesian()
  728.      * @see #getVelocity()
  729.      * @see #getAcceleration()
  730.      */
  731.     public FieldVector3D<T> getTranslation() {
  732.         return cartesian.getPosition();
  733.     }

  734.     /** Get the first time derivative of the translation.
  735.      * @return first time derivative of the translation
  736.      * @see #getCartesian()
  737.      * @see #getTranslation()
  738.      * @see #getAcceleration()
  739.      */
  740.     public FieldVector3D<T> getVelocity() {
  741.         return cartesian.getVelocity();
  742.     }

  743.     /** Get the second time derivative of the translation.
  744.      * @return second time derivative of the translation
  745.      * @see #getCartesian()
  746.      * @see #getTranslation()
  747.      * @see #getVelocity()
  748.      */
  749.     public FieldVector3D<T> getAcceleration() {
  750.         return cartesian.getAcceleration();
  751.     }

  752.     /** Get the underlying elementary angular part.
  753.      * <p>A transform can be uniquely represented as an elementary
  754.      * translation followed by an elementary rotation. This method
  755.      * returns this unique elementary rotation with its derivative.</p>
  756.      * @return underlying elementary angular part
  757.      * @see #getRotation()
  758.      * @see #getRotationRate()
  759.      * @see #getRotationAcceleration()
  760.      */
  761.     public FieldAngularCoordinates<T> getAngular() {
  762.         return angular;
  763.     }

  764.     /** Get the underlying elementary rotation.
  765.      * <p>A transform can be uniquely represented as an elementary
  766.      * translation followed by an elementary rotation. This method
  767.      * returns this unique elementary rotation.</p>
  768.      * @return underlying elementary rotation
  769.      * @see #getAngular()
  770.      * @see #getRotationRate()
  771.      * @see #getRotationAcceleration()
  772.      */
  773.     public FieldRotation<T> getRotation() {
  774.         return angular.getRotation();
  775.     }

  776.     /** Get the first time derivative of the rotation.
  777.      * <p>The norm represents the angular rate.</p>
  778.      * @return First time derivative of the rotation
  779.      * @see #getAngular()
  780.      * @see #getRotation()
  781.      * @see #getRotationAcceleration()
  782.      */
  783.     public FieldVector3D<T> getRotationRate() {
  784.         return angular.getRotationRate();
  785.     }

  786.     /** Get the second time derivative of the rotation.
  787.      * @return Second time derivative of the rotation
  788.      * @see #getAngular()
  789.      * @see #getRotation()
  790.      * @see #getRotationRate()
  791.      */
  792.     public FieldVector3D<T> getRotationAcceleration() {
  793.         return angular.getRotationAcceleration();
  794.     }

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

  797.         /** Simple constructor.
  798.          * @param field field for the components
  799.          */
  800.         FieldIdentityTransform(final Field<T> field) {
  801.             super(FieldAbsoluteDate.getArbitraryEpoch(field),
  802.                   FieldAbsoluteDate.getArbitraryEpoch(field).toAbsoluteDate(),
  803.                   FieldPVCoordinates.getZero(field),
  804.                   FieldAngularCoordinates.getIdentity(field));
  805.         }

  806.         /** {@inheritDoc} */
  807.         @Override
  808.         public FieldTransform<T> shiftedBy(final double dt) {
  809.             return this;
  810.         }

  811.         /** {@inheritDoc} */
  812.         @Override
  813.         public FieldTransform<T> getInverse() {
  814.             return this;
  815.         }

  816.         /** {@inheritDoc} */
  817.         @Override
  818.         public FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  819.             return position;
  820.         }

  821.         /** {@inheritDoc} */
  822.         @Override
  823.         public FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  824.             return vector;
  825.         }

  826.         /** {@inheritDoc} */
  827.         @Override
  828.         public FieldLine<T> transformLine(final FieldLine<T> line) {
  829.             return line;
  830.         }

  831.         /** {@inheritDoc} */
  832.         @Override
  833.         public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  834.             return pv;
  835.         }

  836.         /** {@inheritDoc} */
  837.         @Override
  838.         public FieldTransform<T> freeze() {
  839.             return this;
  840.         }

  841.         /** {@inheritDoc} */
  842.         @Override
  843.         public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {
  844.             final int n = 3 * (selector.getMaxOrder() + 1);
  845.             for (int i = 0; i < n; ++i) {
  846.                 Arrays.fill(jacobian[i], 0, n, getFieldDate().getField().getZero());
  847.                 jacobian[i][i] = getFieldDate().getField().getOne();
  848.             }
  849.         }

  850.     }

  851. }