FieldTransform.java

  1. /* Copyright 2002-2019 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.Field;
  24. import org.hipparchus.RealFieldElement;
  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.hipparchus.geometry.euclidean.threed.Line;
  29. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  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.TimeShiftable;
  34. import org.orekit.time.TimeStamped;
  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.TimeStampedFieldPVCoordinates;
  42. import org.orekit.utils.TimeStampedPVCoordinates;


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

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

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

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

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

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

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

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

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

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

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

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

  201.     /** Build a rotation transform.
  202.      * @param date date of the transform
  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.      * @param rotationRate the axis of the instant rotation
  207.      * expressed in the new frame. (norm representing angular rate)
  208.      */
  209.     public FieldTransform(final FieldAbsoluteDate<T> date,
  210.                           final FieldRotation<T> rotation,
  211.                           final FieldVector3D<T> rotationRate) {
  212.         this(date,
  213.              new FieldAngularCoordinates<>(rotation,
  214.                                            rotationRate,
  215.                                            FieldVector3D.getZero(date.getField())));
  216.     }

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

  232.     /** Build a rotation transform.
  233.      * @param date date of the transform
  234.      * @param angular angular part of the transformation to apply (i.e. rotation to
  235.      * apply to the coordinates of a vector expressed in the old frame to obtain the
  236.      * same vector expressed in the new frame, with its rotation rate)
  237.      */
  238.     public FieldTransform(final FieldAbsoluteDate<T> date, final FieldAngularCoordinates<T> angular) {
  239.         this(date, date.toAbsoluteDate(),
  240.              FieldPVCoordinates.getZero(date.getField()),
  241.              angular);
  242.     }

  243.     /** Build a transform by combining two existing ones.
  244.      * <p>
  245.      * Note that the dates of the two existing transformed are <em>ignored</em>,
  246.      * and the combined transform date is set to the date supplied in this constructor
  247.      * without any attempt to shift the raw transforms. This is a design choice allowing
  248.      * user full control of the combination.
  249.      * </p>
  250.      * @param date date of the transform
  251.      * @param first first transform applied
  252.      * @param second second transform applied
  253.      */
  254.     public FieldTransform(final FieldAbsoluteDate<T> date,
  255.                           final FieldTransform<T> first,
  256.                           final FieldTransform<T> second) {
  257.         this(date, date.toAbsoluteDate(),
  258.              new FieldPVCoordinates<>(compositeTranslation(first, second),
  259.                                       compositeVelocity(first, second),
  260.                                       compositeAcceleration(first, second)),
  261.              new FieldAngularCoordinates<>(compositeRotation(first, second),
  262.                                            compositeRotationRate(first, second),
  263.                                            compositeRotationAcceleration(first, second)));
  264.     }

  265.     /** Get the identity transform.
  266.      * @param field field for the components
  267.      * @param <T> the type of the field elements
  268.      * @return identity transform
  269.      */
  270.     public static <T extends RealFieldElement<T>> FieldTransform<T> getIdentity(final Field<T> field) {
  271.         return new FieldIdentityTransform<>(field);
  272.     }

  273.     /** Compute a composite translation.
  274.      * @param first first applied transform
  275.      * @param second second applied transform
  276.      * @param <T> the type of the field elements
  277.      * @return translation part of the composite transform
  278.      */
  279.     private static <T extends RealFieldElement<T>> FieldVector3D<T> compositeTranslation(final FieldTransform<T> first, final FieldTransform<T> second) {

  280.         final FieldVector3D<T> p1 = first.cartesian.getPosition();
  281.         final FieldRotation<T> r1 = first.angular.getRotation();
  282.         final FieldVector3D<T> p2 = second.cartesian.getPosition();

  283.         return p1.add(r1.applyInverseTo(p2));

  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 RealFieldElement<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 RealFieldElement<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.
  320.      * @param first first applied transform
  321.      * @param second second applied transform
  322.      * @param <T> the type of the field elements
  323.      * @return rotation part of the composite transform
  324.      */
  325.     private static <T extends RealFieldElement<T>> FieldRotation<T> compositeRotation(final FieldTransform<T> first, final FieldTransform<T> second) {

  326.         final FieldRotation<T> r1 = first.angular.getRotation();
  327.         final FieldRotation<T> r2 = second.angular.getRotation();

  328.         return r1.compose(r2, RotationConvention.FRAME_TRANSFORM);

  329.     }

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

  337.         final FieldVector3D<T> o1 = first.angular.getRotationRate();
  338.         final FieldRotation<T> r2 = second.angular.getRotation();
  339.         final FieldVector3D<T> o2 = second.angular.getRotationRate();

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

  341.     }

  342.     /** Compute a composite rotation acceleration.
  343.      * @param first first applied transform
  344.      * @param second second applied transform
  345.      * @param <T> the type of the field elements
  346.      * @return rotation acceleration part of the composite transform
  347.      */
  348.     private static <T extends RealFieldElement<T>> FieldVector3D<T> compositeRotationAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {

  349.         final FieldVector3D<T> o1    = first.angular.getRotationRate();
  350.         final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
  351.         final FieldRotation<T> r2    = second.angular.getRotation();
  352.         final FieldVector3D<T> o2    = second.angular.getRotationRate();
  353.         final FieldVector3D<T> oDot2 = second.angular.getRotationAcceleration();

  354.         return new FieldVector3D<>( 1, oDot2,
  355.                                     1, r2.applyTo(oDot1),
  356.                                    -1, FieldVector3D.crossProduct(o2, r2.applyTo(o1)));

  357.     }

  358.     /** {@inheritDoc} */
  359.     public AbsoluteDate getDate() {
  360.         return aDate;
  361.     }

  362.     /** Get the date.
  363.      * @return date attached to the object
  364.      */
  365.     public FieldAbsoluteDate<T> getFieldDate() {
  366.         return date;
  367.     }

  368.     /** {@inheritDoc} */
  369.     public FieldTransform<T> shiftedBy(final double dt) {
  370.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt),
  371.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  372.     }

  373.     /** Get a time-shifted instance.
  374.      * @param dt time shift in seconds
  375.      * @return a new instance, shifted with respect to instance (which is not changed)
  376.      */
  377.     public FieldTransform<T> shiftedBy(final T dt) {
  378.         return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt.getReal()),
  379.                                     cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  380.     }

  381.     /** Interpolate a transform from a sample set of existing transforms.
  382.      * <p>
  383.      * Calling this method is equivalent to call {@link #interpolate(FieldAbsoluteDate,
  384.      * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
  385.      * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
  386.      * {@link AngularDerivativesFilter#USE_RRA}
  387.      * set to true.
  388.      * </p>
  389.      * @param interpolationDate interpolation date
  390.      * @param sample sample points on which interpolation should be done
  391.      * @param <T> the type of the field elements
  392.      * @return a new instance, interpolated at specified date
  393.      */
  394.     public static <T extends RealFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> interpolationDate,
  395.                                                                                 final Collection<FieldTransform<T>> sample) {
  396.         return interpolate(interpolationDate,
  397.                            CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
  398.                            sample);
  399.     }

  400.     /** Interpolate a transform from a sample set of existing transforms.
  401.      * <p>
  402.      * Note that even if first time derivatives (velocities and rotation rates)
  403.      * from sample can be ignored, the interpolated instance always includes
  404.      * interpolated derivatives. This feature can be used explicitly to
  405.      * compute these derivatives when it would be too complex to compute them
  406.      * from an analytical formula: just compute a few sample points from the
  407.      * explicit formula and set the derivatives to zero in these sample points,
  408.      * then use interpolation to add derivatives consistent with the positions
  409.      * and rotations.
  410.      * </p>
  411.      * <p>
  412.      * As this implementation of interpolation is polynomial, it should be used only
  413.      * with small samples (about 10-20 points) in order to avoid <a
  414.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  415.      * and numerical problems (including NaN appearing).
  416.      * </p>
  417.      * @param date interpolation date
  418.      * @param cFilter filter for derivatives from the sample to use in interpolation
  419.      * @param aFilter filter for derivatives from the sample to use in interpolation
  420.      * @param sample sample points on which interpolation should be done
  421.      * @return a new instance, interpolated at specified date
  422.           * @param <T> the type of the field elements
  423.      */
  424.     public static <T extends RealFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> date,
  425.                                                                                 final CartesianDerivativesFilter cFilter,
  426.                                                                                 final AngularDerivativesFilter aFilter,
  427.                                                                                 final Collection<FieldTransform<T>> sample) {
  428.         return interpolate(date, cFilter, aFilter, sample.stream());
  429.     }

  430.     /** Interpolate a transform from a sample set of existing transforms.
  431.      * <p>
  432.      * Note that even if first time derivatives (velocities and rotation rates)
  433.      * from sample can be ignored, the interpolated instance always includes
  434.      * interpolated derivatives. This feature can be used explicitly to
  435.      * compute these derivatives when it would be too complex to compute them
  436.      * from an analytical formula: just compute a few sample points from the
  437.      * explicit formula and set the derivatives to zero in these sample points,
  438.      * then use interpolation to add derivatives consistent with the positions
  439.      * and rotations.
  440.      * </p>
  441.      * <p>
  442.      * As this implementation of interpolation is polynomial, it should be used only
  443.      * with small samples (about 10-20 points) in order to avoid <a
  444.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  445.      * and numerical problems (including NaN appearing).
  446.      * </p>
  447.      * @param date interpolation date
  448.      * @param cFilter filter for derivatives from the sample to use in interpolation
  449.      * @param aFilter filter for derivatives from the sample to use in interpolation
  450.      * @param sample sample points on which interpolation should be done
  451.      * @return a new instance, interpolated at specified date
  452.           * @param <T> the type of the field elements
  453.      */
  454.     public static <T extends RealFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> date,
  455.                                                                                 final CartesianDerivativesFilter cFilter,
  456.                                                                                 final AngularDerivativesFilter aFilter,
  457.                                                                                 final Stream<FieldTransform<T>> sample) {
  458.         final List<TimeStampedFieldPVCoordinates<T>>      datedPV = new ArrayList<>();
  459.         final List<TimeStampedFieldAngularCoordinates<T>> datedAC = new ArrayList<>();
  460.         sample.forEach(t -> {
  461.             datedPV.add(new TimeStampedFieldPVCoordinates<>(t.getDate(), t.getTranslation(), t.getVelocity(), t.getAcceleration()));
  462.             datedAC.add(new TimeStampedFieldAngularCoordinates<>(t.getDate(), t.getRotation(), t.getRotationRate(), t.getRotationAcceleration()));
  463.         });
  464.         final TimeStampedFieldPVCoordinates<T>      interpolatedPV = TimeStampedFieldPVCoordinates.interpolate(date, cFilter, datedPV);
  465.         final TimeStampedFieldAngularCoordinates<T> interpolatedAC = TimeStampedFieldAngularCoordinates.interpolate(date, aFilter, datedAC);
  466.         return new FieldTransform<>(date, date.toAbsoluteDate(), interpolatedPV, interpolatedAC);
  467.     }

  468.     /** Get the inverse transform of the instance.
  469.      * @return inverse transform of the instance
  470.      */
  471.     public FieldTransform<T> getInverse() {

  472.         final FieldRotation<T> r    = angular.getRotation();
  473.         final FieldVector3D<T> o    = angular.getRotationRate();
  474.         final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  475.         final FieldVector3D<T> rp   = r.applyTo(cartesian.getPosition());
  476.         final FieldVector3D<T> rv   = r.applyTo(cartesian.getVelocity());
  477.         final FieldVector3D<T> ra   = r.applyTo(cartesian.getAcceleration());

  478.         final FieldVector3D<T> pInv        = rp.negate();
  479.         final FieldVector3D<T> crossP      = FieldVector3D.crossProduct(o, rp);
  480.         final FieldVector3D<T> vInv        = crossP.subtract(rv);
  481.         final FieldVector3D<T> crossV      = FieldVector3D.crossProduct(o, rv);
  482.         final FieldVector3D<T> crossDotP   = FieldVector3D.crossProduct(oDot, rp);
  483.         final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o, crossP);
  484.         final FieldVector3D<T> aInv        = new FieldVector3D<>(-1, ra,
  485.                                                                   2, crossV,
  486.                                                                   1, crossDotP,
  487.                                                                  -1, crossCrossP);

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

  489.     }

  490.     /** Get a frozen transform.
  491.      * <p>
  492.      * This method creates a copy of the instance but frozen in time,
  493.      * i.e. with velocity, acceleration and rotation rate forced to zero.
  494.      * </p>
  495.      * @return a new transform, without any time-dependent parts
  496.      */
  497.     public FieldTransform<T> freeze() {
  498.         return new FieldTransform<>(date, aDate,
  499.                                     new FieldPVCoordinates<>(cartesian.getPosition(),
  500.                                                              FieldVector3D.getZero(date.getField()),
  501.                                                              FieldVector3D.getZero(date.getField())),
  502.                                     new FieldAngularCoordinates<>(angular.getRotation(),
  503.                                                                   FieldVector3D.getZero(date.getField()),
  504.                                                                   FieldVector3D.getZero(date.getField())));
  505.     }

  506.     /** Transform a position vector (including translation effects).
  507.      * @param position vector to transform
  508.      * @return transformed position
  509.      */
  510.     public FieldVector3D<T> transformPosition(final Vector3D position) {
  511.         return angular.getRotation().applyTo(cartesian.getPosition().add(position));
  512.     }

  513.     /** Transform a position vector (including translation effects).
  514.      * @param position vector to transform
  515.      * @return transformed position
  516.      */
  517.     public FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  518.         return angular.getRotation().applyTo(position.add(cartesian.getPosition()));
  519.     }

  520.     /** Transform a vector (ignoring translation effects).
  521.      * @param vector vector to transform
  522.      * @return transformed vector
  523.      */
  524.     public FieldVector3D<T> transformVector(final Vector3D vector) {
  525.         return angular.getRotation().applyTo(vector);
  526.     }

  527.     /** Transform a vector (ignoring translation effects).
  528.      * @param vector vector to transform
  529.      * @return transformed vector
  530.      */
  531.     public FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  532.         return angular.getRotation().applyTo(vector);
  533.     }

  534.     /** Transform a line.
  535.      * @param line to transform
  536.      * @return transformed line
  537.      */
  538.     public FieldLine<T> transformLine(final Line line) {
  539.         final FieldVector3D<T> transformedP0 = transformPosition(line.getOrigin());
  540.         final FieldVector3D<T> transformedP1 = transformPosition(line.pointAt(1.0e6));
  541.         return new FieldLine<>(transformedP0, transformedP1, 1.0e-10);
  542.     }

  543.     /** Transform a line.
  544.      * @param line to transform
  545.      * @return transformed line
  546.      */
  547.     public FieldLine<T> transformLine(final FieldLine<T> line) {
  548.         final FieldVector3D<T> transformedP0 = transformPosition(line.getOrigin());
  549.         final FieldVector3D<T> transformedP1 = transformPosition(line.pointAt(1.0e6));
  550.         return new FieldLine<>(transformedP0, transformedP1, 1.0e-10);
  551.     }

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

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

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

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

  652.     /** Compute the Jacobian of the {@link #transformPVCoordinates(FieldPVCoordinates)}
  653.      * method of the transform.
  654.      * <p>
  655.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  656.      * of the transformed {@link FieldPVCoordinates} with respect to Cartesian coordinate j
  657.      * of the input {@link FieldPVCoordinates} in method {@link #transformPVCoordinates(FieldPVCoordinates)}.
  658.      * </p>
  659.      * <p>
  660.      * This definition implies that if we define position-velocity coordinates
  661.      * <pre>PV₁ = transform.transformPVCoordinates(PV₀)</pre>
  662.      * then their differentials dPV₁ and dPV₀ will obey the following relation
  663.      * where J is the matrix computed by this method:
  664.      * <pre>dPV₁ = J &times; dPV₀</pre>
  665.      *
  666.      * @param selector selector specifying the size of the upper left corner that must be filled
  667.      * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
  668.      * velocities and accelerations)
  669.      * @param jacobian placeholder matrix whose upper-left corner is to be filled with
  670.      * the Jacobian, the rest of the matrix remaining untouched
  671.      */
  672.     public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {

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

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

  676.         // dP1/dP0
  677.         System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  678.         System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  679.         System.arraycopy(mData[2], 0, jacobian[2], 0, 3);

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

  681.             // dP1/dV0
  682.             Arrays.fill(jacobian[0], 3, 6, zero);
  683.             Arrays.fill(jacobian[1], 3, 6, zero);
  684.             Arrays.fill(jacobian[2], 3, 6, zero);

  685.             // dV1/dP0
  686.             final FieldVector3D<T> o = angular.getRotationRate();
  687.             final T ox = o.getX();
  688.             final T oy = o.getY();
  689.             final T oz = o.getZ();
  690.             for (int i = 0; i < 3; ++i) {
  691.                 jacobian[3][i] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i]));
  692.                 jacobian[4][i] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i]));
  693.                 jacobian[5][i] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i]));
  694.             }

  695.             // dV1/dV0
  696.             System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
  697.             System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
  698.             System.arraycopy(mData[2], 0, jacobian[5], 3, 3);

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

  700.                 // dP1/dA0
  701.                 Arrays.fill(jacobian[0], 6, 9, zero);
  702.                 Arrays.fill(jacobian[1], 6, 9, zero);
  703.                 Arrays.fill(jacobian[2], 6, 9, zero);

  704.                 // dV1/dA0
  705.                 Arrays.fill(jacobian[3], 6, 9, zero);
  706.                 Arrays.fill(jacobian[4], 6, 9, zero);
  707.                 Arrays.fill(jacobian[5], 6, 9, zero);

  708.                 // dA1/dP0
  709.                 final FieldVector3D<T> oDot = angular.getRotationAcceleration();
  710.                 final T oDotx  = oDot.getX();
  711.                 final T oDoty  = oDot.getY();
  712.                 final T oDotz  = oDot.getZ();
  713.                 for (int i = 0; i < 3; ++i) {
  714.                     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])));
  715.                     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])));
  716.                     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])));
  717.                 }

  718.                 // dA1/dV0
  719.                 for (int i = 0; i < 3; ++i) {
  720.                     jacobian[6][i + 3] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i])).multiply(2);
  721.                     jacobian[7][i + 3] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i])).multiply(2);
  722.                     jacobian[8][i + 3] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i])).multiply(2);
  723.                 }

  724.                 // dA1/dA0
  725.                 System.arraycopy(mData[0], 0, jacobian[6], 6, 3);
  726.                 System.arraycopy(mData[1], 0, jacobian[7], 6, 3);
  727.                 System.arraycopy(mData[2], 0, jacobian[8], 6, 3);

  728.             }

  729.         }

  730.     }

  731.     /** Get the underlying elementary Cartesian part.
  732.      * <p>A transform can be uniquely represented as an elementary
  733.      * translation followed by an elementary rotation. This method
  734.      * returns this unique elementary translation with its derivative.</p>
  735.      * @return underlying elementary Cartesian part
  736.      * @see #getTranslation()
  737.      * @see #getVelocity()
  738.      */
  739.     public FieldPVCoordinates<T> getCartesian() {
  740.         return cartesian;
  741.     }

  742.     /** Get the underlying elementary translation.
  743.      * <p>A transform can be uniquely represented as an elementary
  744.      * translation followed by an elementary rotation. This method
  745.      * returns this unique elementary translation.</p>
  746.      * @return underlying elementary translation
  747.      * @see #getCartesian()
  748.      * @see #getVelocity()
  749.      * @see #getAcceleration()
  750.      */
  751.     public FieldVector3D<T> getTranslation() {
  752.         return cartesian.getPosition();
  753.     }

  754.     /** Get the first time derivative of the translation.
  755.      * @return first time derivative of the translation
  756.      * @see #getCartesian()
  757.      * @see #getTranslation()
  758.      * @see #getAcceleration()
  759.      */
  760.     public FieldVector3D<T> getVelocity() {
  761.         return cartesian.getVelocity();
  762.     }

  763.     /** Get the second time derivative of the translation.
  764.      * @return second time derivative of the translation
  765.      * @see #getCartesian()
  766.      * @see #getTranslation()
  767.      * @see #getVelocity()
  768.      */
  769.     public FieldVector3D<T> getAcceleration() {
  770.         return cartesian.getAcceleration();
  771.     }

  772.     /** Get the underlying elementary angular part.
  773.      * <p>A transform can be uniquely represented as an elementary
  774.      * translation followed by an elementary rotation. This method
  775.      * returns this unique elementary rotation with its derivative.</p>
  776.      * @return underlying elementary angular part
  777.      * @see #getRotation()
  778.      * @see #getRotationRate()
  779.      * @see #getRotationAcceleration()
  780.      */
  781.     public FieldAngularCoordinates<T> getAngular() {
  782.         return angular;
  783.     }

  784.     /** Get the underlying elementary rotation.
  785.      * <p>A transform can be uniquely represented as an elementary
  786.      * translation followed by an elementary rotation. This method
  787.      * returns this unique elementary rotation.</p>
  788.      * @return underlying elementary rotation
  789.      * @see #getAngular()
  790.      * @see #getRotationRate()
  791.      * @see #getRotationAcceleration()
  792.      */
  793.     public FieldRotation<T> getRotation() {
  794.         return angular.getRotation();
  795.     }

  796.     /** Get the first time derivative of the rotation.
  797.      * <p>The norm represents the angular rate.</p>
  798.      * @return First time derivative of the rotation
  799.      * @see #getAngular()
  800.      * @see #getRotation()
  801.      * @see #getRotationAcceleration()
  802.      */
  803.     public FieldVector3D<T> getRotationRate() {
  804.         return angular.getRotationRate();
  805.     }

  806.     /** Get the second time derivative of the rotation.
  807.      * @return Second time derivative of the rotation
  808.      * @see #getAngular()
  809.      * @see #getRotation()
  810.      * @see #getRotationRate()
  811.      */
  812.     public FieldVector3D<T> getRotationAcceleration() {
  813.         return angular.getRotationAcceleration();
  814.     }

  815.     /** Specialized class for identity transform. */
  816.     private static class FieldIdentityTransform<T extends RealFieldElement<T>> extends FieldTransform<T> {

  817.         /** Simple constructor.
  818.          * @param field field for the components
  819.          */
  820.         FieldIdentityTransform(final Field<T> field) {
  821.             super(FieldAbsoluteDate.getJ2000Epoch(field), AbsoluteDate.J2000_EPOCH,
  822.                   FieldPVCoordinates.getZero(field),
  823.                   FieldAngularCoordinates.getIdentity(field));
  824.         }

  825.         /** {@inheritDoc} */
  826.         @Override
  827.         public FieldTransform<T> shiftedBy(final double dt) {
  828.             return this;
  829.         }

  830.         /** {@inheritDoc} */
  831.         @Override
  832.         public FieldTransform<T> getInverse() {
  833.             return this;
  834.         }

  835.         /** {@inheritDoc} */
  836.         @Override
  837.         public FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  838.             return position;
  839.         }

  840.         /** {@inheritDoc} */
  841.         @Override
  842.         public FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  843.             return vector;
  844.         }

  845.         /** {@inheritDoc} */
  846.         @Override
  847.         public FieldLine<T> transformLine(final FieldLine<T> line) {
  848.             return line;
  849.         }

  850.         /** {@inheritDoc} */
  851.         @Override
  852.         public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  853.             return pv;
  854.         }

  855.         /** {@inheritDoc} */
  856.         @Override
  857.         public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {
  858.             final int n = 3 * (selector.getMaxOrder() + 1);
  859.             for (int i = 0; i < n; ++i) {
  860.                 Arrays.fill(jacobian[i], 0, n, getFieldDate().getField().getZero());
  861.                 jacobian[i][i] = getFieldDate().getField().getOne();
  862.             }
  863.         }

  864.     }

  865. }