Transform.java

  1. /* Copyright 2002-2025 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.frames;

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

  26. import org.hipparchus.CalculusFieldElement;
  27. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  28. import org.hipparchus.geometry.euclidean.threed.Line;
  29. import org.hipparchus.geometry.euclidean.threed.Rotation;
  30. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.time.TimeOffset;
  33. import org.orekit.time.TimeInterpolator;
  34. import org.orekit.time.TimeShiftable;
  35. import org.orekit.utils.AngularCoordinates;
  36. import org.orekit.utils.AngularDerivativesFilter;
  37. import org.orekit.utils.CartesianDerivativesFilter;
  38. import org.orekit.utils.FieldPVCoordinates;
  39. import org.orekit.utils.PVCoordinates;
  40. import org.orekit.utils.TimeStampedAngularCoordinates;
  41. import org.orekit.utils.TimeStampedAngularCoordinatesHermiteInterpolator;
  42. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  43. import org.orekit.utils.TimeStampedPVCoordinates;
  44. import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;


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

  104.     /** Identity transform. */
  105.     public static final Transform IDENTITY = new IdentityTransform();

  107.     /** Date of the transform. */
  108.     private final AbsoluteDate date;

  110.     /** Cartesian coordinates of the target frame with respect to the original frame. */
  111.     private final PVCoordinates cartesian;

  113.     /** Angular coordinates of the target frame with respect to the original frame. */
  114.     private final AngularCoordinates angular;

  116.     /** Build a transform from its primitive operations.
  117.      * @param date date of the transform
  118.      * @param cartesian Cartesian coordinates of the target frame with respect to the original frame
  119.      * @param angular angular coordinates of the target frame with respect to the original frame
  120.      */
  121.     public Transform(final AbsoluteDate date, final PVCoordinates cartesian, final AngularCoordinates angular) {
  122.         this.date      = date;
  123.         this.cartesian = cartesian;
  124.         this.angular   = angular;
  125.     }

  127.     /** Build a translation transform.
  128.      * @param date date of the transform
  129.      * @param translation translation to apply (i.e. coordinates of
  130.      * the transformed origin, or coordinates of the origin of the
  131.      * old frame in the new frame)
  132.      */
  133.     public Transform(final AbsoluteDate date, final Vector3D translation) {
  134.         this(date,
  135.              new PVCoordinates(translation),
  136.              AngularCoordinates.IDENTITY);
  137.     }

  139.     /** Build a rotation transform.
  140.      * @param date date of the transform
  141.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  142.      * coordinates of a vector expressed in the old frame to obtain the
  143.      * same vector expressed in the new frame )
  144.      */
  145.     public Transform(final AbsoluteDate date, final Rotation rotation) {
  146.         this(date,
  147.              PVCoordinates.ZERO,
  148.              new AngularCoordinates(rotation));
  149.     }

  151.     /** Build a combined translation and rotation transform.
  152.      * @param date date of the transform
  153.      * @param translation translation to apply (i.e. coordinates of
  154.      * the transformed origin, or coordinates of the origin of the
  155.      * old frame in the new frame)
  156.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  157.      * coordinates of a vector expressed in the old frame to obtain the
  158.      * same vector expressed in the new frame )
  159.      * @since 12.1
  160.      */
  161.     public Transform(final AbsoluteDate date, final Vector3D translation, final Rotation rotation) {
  162.         this(date, new PVCoordinates(translation), new AngularCoordinates(rotation));
  163.     }

  165.     /** Build a translation transform, with its first time derivative.
  166.      * @param date date of the transform
  167.      * @param translation translation to apply (i.e. coordinates of
  168.      * the transformed origin, or coordinates of the origin of the
  169.      * old frame in the new frame)
  170.      * @param velocity the velocity of the translation (i.e. origin
  171.      * of the old frame velocity in the new frame)
  172.      */
  173.     public Transform(final AbsoluteDate date, final Vector3D translation,
  174.                      final Vector3D velocity) {
  175.         this(date,
  176.              new PVCoordinates(translation, velocity, Vector3D.ZERO),
  177.              AngularCoordinates.IDENTITY);
  178.     }

  180.     /** Build a translation transform, with its first and second time derivatives.
  181.      * @param date date of the transform
  182.      * @param translation translation to apply (i.e. coordinates of
  183.      * the transformed origin, or coordinates of the origin of the
  184.      * old frame in the new frame)
  185.      * @param velocity the velocity of the translation (i.e. origin
  186.      * of the old frame velocity in the new frame)
  187.      * @param acceleration the acceleration of the translation (i.e. origin
  188.      * of the old frame acceleration in the new frame)
  189.      */
  190.     public Transform(final AbsoluteDate date, final Vector3D translation,
  191.                      final Vector3D velocity, final Vector3D acceleration) {
  192.         this(date,
  193.              new PVCoordinates(translation, velocity, acceleration),
  194.              AngularCoordinates.IDENTITY);
  195.     }

  197.     /** Build a translation transform, with its first time derivative.
  198.      * @param date date of the transform
  199.      * @param cartesian Cartesian part of the transformation to apply (i.e. coordinates of
  200.      * the transformed origin, or coordinates of the origin of the
  201.      * old frame in the new frame, with their derivatives)
  202.      */
  203.     public Transform(final AbsoluteDate date, final PVCoordinates cartesian) {
  204.         this(date,
  205.              cartesian,
  206.              AngularCoordinates.IDENTITY);
  207.     }

  209.     /** Build a rotation transform.
  210.      * @param date date of the transform
  211.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  212.      * coordinates of a vector expressed in the old frame to obtain the
  213.      * same vector expressed in the new frame )
  214.      * @param rotationRate the axis of the instant rotation
  215.      * expressed in the new frame. (norm representing angular rate)
  216.      */
  217.     public Transform(final AbsoluteDate date, final Rotation rotation, final Vector3D rotationRate) {
  218.         this(date,
  219.              PVCoordinates.ZERO,
  220.              new AngularCoordinates(rotation, rotationRate, Vector3D.ZERO));
  221.     }

  223.     /** Build a rotation transform.
  224.      * @param date date of the transform
  225.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  226.      * coordinates of a vector expressed in the old frame to obtain the
  227.      * same vector expressed in the new frame )
  228.      * @param rotationRate the axis of the instant rotation
  229.      * @param rotationAcceleration the axis of the instant rotation
  230.      * expressed in the new frame. (norm representing angular rate)
  231.      */
  232.     public Transform(final AbsoluteDate date, final Rotation rotation, final Vector3D rotationRate,
  233.                      final Vector3D rotationAcceleration) {
  234.         this(date,
  235.              PVCoordinates.ZERO,
  236.              new AngularCoordinates(rotation, rotationRate, rotationAcceleration));
  237.     }

  239.     /** Build a rotation transform.
  240.      * @param date date of the transform
  241.      * @param angular angular part of the transformation to apply (i.e. rotation to
  242.      * apply to the coordinates of a vector expressed in the old frame to obtain the
  243.      * same vector expressed in the new frame, with its rotation rate)
  244.      */
  245.     public Transform(final AbsoluteDate date, final AngularCoordinates angular) {
  246.         this(date, PVCoordinates.ZERO, angular);
  247.     }

  249.     /** Build a transform by combining two existing ones.
  250.      * <p>
  251.      * Note that the dates of the two existing transformed are <em>ignored</em>,
  252.      * and the combined transform date is set to the date supplied in this constructor
  253.      * without any attempt to shift the raw transforms. This is a design choice allowing
  254.      * user full control of the combination.
  255.      * </p>
  256.      * @param date date of the transform
  257.      * @param first first transform applied
  258.      * @param second second transform applied
  259.      */
  260.     public Transform(final AbsoluteDate date, final Transform first, final Transform second) {
  261.         this(date,
  262.              new PVCoordinates(StaticTransform.compositeTranslation(first, second),
  263.                                KinematicTransform.compositeVelocity(first, second),
  264.                                compositeAcceleration(first, second)),
  265.              new AngularCoordinates(StaticTransform.compositeRotation(first, second),
  266.                                     KinematicTransform.compositeRotationRate(first, second),
  267.                                     compositeRotationAcceleration(first, second)));
  268.     }

  270.     /** Compute a composite acceleration.
  271.      * @param first first applied transform
  272.      * @param second second applied transform
  273.      * @return acceleration part of the composite transform
  274.      */
  275.     private static Vector3D compositeAcceleration(final Transform first, final Transform second) {

  277.         final Vector3D a1    = first.cartesian.getAcceleration();
  278.         final Rotation r1    = first.angular.getRotation();
  279.         final Vector3D o1    = first.angular.getRotationRate();
  280.         final Vector3D oDot1 = first.angular.getRotationAcceleration();
  281.         final Vector3D p2    = second.cartesian.getPosition();
  282.         final Vector3D v2    = second.cartesian.getVelocity();
  283.         final Vector3D a2    = second.cartesian.getAcceleration();

  285.         final Vector3D crossCrossP = Vector3D.crossProduct(o1,    Vector3D.crossProduct(o1, p2));
  286.         final Vector3D crossV      = Vector3D.crossProduct(o1,    v2);
  287.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot1, p2);

  289.         return a1.add(r1.applyInverseTo(new Vector3D(1, a2, 2, crossV, 1, crossCrossP, 1, crossDotP)));

  291.     }

  293.     /** Compute a composite rotation acceleration.
  294.      * @param first first applied transform
  295.      * @param second second applied transform
  296.      * @return rotation acceleration part of the composite transform
  297.      */
  298.     private static Vector3D compositeRotationAcceleration(final Transform first, final Transform second) {

  300.         final Vector3D o1    = first.angular.getRotationRate();
  301.         final Vector3D oDot1 = first.angular.getRotationAcceleration();
  302.         final Rotation r2    = second.angular.getRotation();
  303.         final Vector3D o2    = second.angular.getRotationRate();
  304.         final Vector3D oDot2 = second.angular.getRotationAcceleration();

  306.         return new Vector3D( 1, oDot2,
  307.                              1, r2.applyTo(oDot1),
  308.                             -1, Vector3D.crossProduct(o2, r2.applyTo(o1)));

  310.     }

  312.     /** {@inheritDoc} */
  313.     public AbsoluteDate getDate() {
  314.         return date;
  315.     }

  317.     /** {@inheritDoc} */
  318.     public Transform shiftedBy(final double dt) {
  319.         return shiftedBy(new TimeOffset(dt));
  320.     }

  322.     /** {@inheritDoc} */
  323.     @Override
  324.     public Transform shiftedBy(final TimeOffset dt) {
  325.         return new Transform(date.shiftedBy(dt), cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  326.     }

  328.     /**
  329.      * Shift the transform in time considering all rates, then return only the
  330.      * translation and rotation portion of the transform.
  331.      *
  332.      * @param dt time shift in seconds.
  333.      * @return shifted transform as a static transform. It is static in the
  334.      * sense that it can only be used to transform directions and positions, but
  335.      * not velocities or accelerations.
  336.      * @see #shiftedBy(double)
  337.      */
  338.     public StaticTransform staticShiftedBy(final double dt) {
  339.         return StaticTransform.of(
  340.                 date.shiftedBy(dt),
  341.                 cartesian.positionShiftedBy(dt),
  342.                 angular.rotationShiftedBy(dt));
  343.     }

  345.     /**
  346.      * Create a so-called static transform from the instance.
  347.      *
  348.      * @return static part of the transform. It is static in the
  349.      * sense that it can only be used to transform directions and positions, but
  350.      * not velocities or accelerations.
  351.      * @see StaticTransform
  352.      */
  353.     public StaticTransform toStaticTransform() {
  354.         return StaticTransform.of(date, cartesian.getPosition(), angular.getRotation());
  355.     }

  357.     /** Interpolate a transform from a sample set of existing transforms.
  358.      * <p>
  359.      * Calling this method is equivalent to call {@link #interpolate(AbsoluteDate,
  360.      * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
  361.      * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
  362.      * {@link AngularDerivativesFilter#USE_RRA}
  363.      * set to true.
  364.      * </p>
  365.      * @param interpolationDate interpolation date
  366.      * @param sample sample points on which interpolation should be done
  367.      * @return a new instance, interpolated at specified date
  368.      */
  369.     public Transform interpolate(final AbsoluteDate interpolationDate, final Stream<Transform> sample) {
  370.         return interpolate(interpolationDate,
  371.                            CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
  372.                            sample.collect(Collectors.toList()));
  373.     }

  375.     /** Interpolate a transform from a sample set of existing transforms.
  376.      * <p>
  377.      * Note that even if first time derivatives (velocities and rotation rates)
  378.      * from sample can be ignored, the interpolated instance always includes
  379.      * interpolated derivatives. This feature can be used explicitly to
  380.      * compute these derivatives when it would be too complex to compute them
  381.      * from an analytical formula: just compute a few sample points from the
  382.      * explicit formula and set the derivatives to zero in these sample points,
  383.      * then use interpolation to add derivatives consistent with the positions
  384.      * and rotations.
  385.      * </p>
  386.      * <p>
  387.      * As this implementation of interpolation is polynomial, it should be used only
  388.      * with small samples (about 10-20 points) in order to avoid <a
  389.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  390.      * and numerical problems (including NaN appearing).
  391.      * </p>
  392.      * @param date interpolation date
  393.      * @param cFilter filter for derivatives from the sample to use in interpolation
  394.      * @param aFilter filter for derivatives from the sample to use in interpolation
  395.      * @param sample sample points on which interpolation should be done
  396.      * @return a new instance, interpolated at specified date
  397.      * @since 7.0
  398.      */
  399.     public static Transform interpolate(final AbsoluteDate date,
  400.                                         final CartesianDerivativesFilter cFilter,
  401.                                         final AngularDerivativesFilter aFilter,
  402.                                         final Collection<Transform> sample) {

  404.         // Create samples
  405.         final List<TimeStampedPVCoordinates>      datedPV = new ArrayList<>(sample.size());
  406.         final List<TimeStampedAngularCoordinates> datedAC = new ArrayList<>(sample.size());
  407.         for (final Transform t : sample) {
  408.             datedPV.add(new TimeStampedPVCoordinates(t.getDate(), t.getTranslation(), t.getVelocity(), t.getAcceleration()));
  409.             datedAC.add(new TimeStampedAngularCoordinates(t.getDate(), t.getRotation(), t.getRotationRate(), t.getRotationAcceleration()));
  410.         }

  412.         // Create interpolators
  413.         final TimeInterpolator<TimeStampedPVCoordinates> pvInterpolator =
  414.                 new TimeStampedPVCoordinatesHermiteInterpolator(datedPV.size(), cFilter);

  416.         final TimeInterpolator<TimeStampedAngularCoordinates> angularInterpolator =
  417.                 new TimeStampedAngularCoordinatesHermiteInterpolator(datedPV.size(), aFilter);

  419.         // Interpolate
  420.         final TimeStampedPVCoordinates      interpolatedPV = pvInterpolator.interpolate(date, datedPV);
  421.         final TimeStampedAngularCoordinates interpolatedAC = angularInterpolator.interpolate(date, datedAC);
  422.         return new Transform(date, interpolatedPV, interpolatedAC);
  423.     }

  425.     /** Get the inverse transform of the instance.
  426.      * @return inverse transform of the instance
  427.      */
  428.     @Override
  429.     public Transform getInverse() {

  431.         final Rotation r    = angular.getRotation();
  432.         final Vector3D o    = angular.getRotationRate();
  433.         final Vector3D oDot = angular.getRotationAcceleration();
  434.         final Vector3D rp   = r.applyTo(cartesian.getPosition());
  435.         final Vector3D rv   = r.applyTo(cartesian.getVelocity());
  436.         final Vector3D ra   = r.applyTo(cartesian.getAcceleration());

  438.         final Vector3D pInv        = rp.negate();
  439.         final Vector3D crossP      = Vector3D.crossProduct(o, rp);
  440.         final Vector3D vInv        = crossP.subtract(rv);
  441.         final Vector3D crossV      = Vector3D.crossProduct(o, rv);
  442.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot, rp);
  443.         final Vector3D crossCrossP = Vector3D.crossProduct(o, crossP);
  444.         final Vector3D aInv        = new Vector3D(-1, ra,
  445.                                                    2, crossV,
  446.                                                    1, crossDotP,
  447.                                                   -1, crossCrossP);

  449.         return new Transform(getDate(), new PVCoordinates(pInv, vInv, aInv), angular.revert());

  451.     }

  453.     /** Get a frozen transform.
  454.      * <p>
  455.      * This method creates a copy of the instance but frozen in time,
  456.      * i.e. with velocity, acceleration and rotation rate forced to zero.
  457.      * </p>
  458.      * @return a new transform, without any time-dependent parts
  459.      */
  460.     public Transform freeze() {
  461.         return new Transform(date,
  462.                              new PVCoordinates(cartesian.getPosition(), Vector3D.ZERO, Vector3D.ZERO),
  463.                              new AngularCoordinates(angular.getRotation(), Vector3D.ZERO, Vector3D.ZERO));
  464.     }

  466.     /** Transform {@link PVCoordinates} including kinematic effects.
  467.      * @param pva the position-velocity-acceleration triplet to transform.
  468.      * @return transformed position-velocity-acceleration
  469.      */
  470.     public PVCoordinates transformPVCoordinates(final PVCoordinates pva) {
  471.         return angular.applyTo(new PVCoordinates(1, pva, 1, cartesian));
  472.     }

  474.     /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
  475.      * <p>
  476.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  477.      * consistency between the transform {@link #getDate() date} and the time-stamped
  478.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  479.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  480.      * the input argument, regardless of the instance {@link #getDate() date}.
  481.      * </p>
  482.      * @param pv time-stamped  position-velocity to transform.
  483.      * @return transformed time-stamped position-velocity
  484.      * @since 7.0
  485.      */
  486.     public TimeStampedPVCoordinates transformPVCoordinates(final TimeStampedPVCoordinates pv) {
  487.         return angular.applyTo(new TimeStampedPVCoordinates(pv.getDate(), 1, pv, 1, cartesian));
  488.     }

  490.     /** Transform {@link FieldPVCoordinates} including kinematic effects.
  491.      * @param pv position-velocity to transform.
  492.      * @param <T> type of the field elements
  493.      * @return transformed position-velocity
  494.      */
  495.     public <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  496.         return angular.applyTo(new FieldPVCoordinates<>(pv.getPosition().add(cartesian.getPosition()),
  497.                                                         pv.getVelocity().add(cartesian.getVelocity()),
  498.                                                         pv.getAcceleration().add(cartesian.getAcceleration())));
  499.     }

  501.     /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
  502.      * <p>
  503.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  504.      * consistency between the transform {@link #getDate() date} and the time-stamped
  505.      * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
  506.      * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
  507.      * the input argument, regardless of the instance {@link #getDate() date}.
  508.      * </p>
  509.      * @param pv time-stamped position-velocity to transform.
  510.      * @param <T> type of the field elements
  511.      * @return transformed time-stamped position-velocity
  512.      * @since 7.0
  513.      */
  514.     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedFieldPVCoordinates<T> pv) {
  515.         return angular.applyTo(new TimeStampedFieldPVCoordinates<>(pv.getDate(),
  516.                                                                    pv.getPosition().add(cartesian.getPosition()),
  517.                                                                    pv.getVelocity().add(cartesian.getVelocity()),
  518.                                                                    pv.getAcceleration().add(cartesian.getAcceleration())));
  519.     }

  521.     /** Compute the Jacobian of the {@link #transformPVCoordinates(PVCoordinates)}
  522.      * method of the transform.
  523.      * <p>
  524.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  525.      * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j
  526.      * of the input {@link PVCoordinates} in method {@link #transformPVCoordinates(PVCoordinates)}.
  527.      * </p>
  528.      * <p>
  529.      * This definition implies that if we define position-velocity coordinates
  530.      * <pre>
  531.      * PV₁ = transform.transformPVCoordinates(PV₀), then
  532.      * </pre>
  533.      * <p> their differentials dPV₁ and dPV₀ will obey the following relation
  534.      * where J is the matrix computed by this method:
  535.      * <pre>
  536.      * dPV₁ = J &times; dPV₀
  537.      * </pre>
  538.      *
  539.      * @param selector selector specifying the size of the upper left corner that must be filled
  540.      * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
  541.      * velocities and accelerations)
  542.      * @param jacobian placeholder matrix whose upper-left corner is to be filled with
  543.      * the Jacobian, the rest of the matrix remaining untouched
  544.      */
  545.     public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {

  547.         if (selector.getMaxOrder() == 0) {
  548.             // elementary matrix for rotation
  549.             final double[][] mData = angular.getRotation().getMatrix();

  551.             // dP1/dP0
  552.             System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  553.             System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  554.             System.arraycopy(mData[2], 0, jacobian[2], 0, 3);
  555.         }

  557.         else if (selector.getMaxOrder() == 1) {
  558.             // use KinematicTransform Jacobian
  559.             final double[][] mData = getPVJacobian();
  560.             for (int i = 0; i < mData.length; i++) {
  561.                 System.arraycopy(mData[i], 0, jacobian[i], 0, mData[i].length);
  562.             }
  563.         }

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

  568.             // dP1/dA0
  569.             Arrays.fill(jacobian[0], 6, 9, 0.0);
  570.             Arrays.fill(jacobian[1], 6, 9, 0.0);
  571.             Arrays.fill(jacobian[2], 6, 9, 0.0);

  573.             // dV1/dA0
  574.             Arrays.fill(jacobian[3], 6, 9, 0.0);
  575.             Arrays.fill(jacobian[4], 6, 9, 0.0);
  576.             Arrays.fill(jacobian[5], 6, 9, 0.0);

  578.             // dA1/dP0
  579.             final Vector3D o = angular.getRotationRate();
  580.             final double ox = o.getX();
  581.             final double oy = o.getY();
  582.             final double oz = o.getZ();
  583.             final Vector3D oDot = angular.getRotationAcceleration();
  584.             final double oDotx  = oDot.getX();
  585.             final double oDoty  = oDot.getY();
  586.             final double oDotz  = oDot.getZ();
  587.             for (int i = 0; i < 3; ++i) {
  588.                 jacobian[6][i] = -(oDoty * jacobian[2][i] - oDotz * jacobian[1][i]) - (oy * jacobian[5][i] - oz * jacobian[4][i]);
  589.                 jacobian[7][i] = -(oDotz * jacobian[0][i] - oDotx * jacobian[2][i]) - (oz * jacobian[3][i] - ox * jacobian[5][i]);
  590.                 jacobian[8][i] = -(oDotx * jacobian[1][i] - oDoty * jacobian[0][i]) - (ox * jacobian[4][i] - oy * jacobian[3][i]);
  591.             }

  593.             // dA1/dV0
  594.             for (int i = 0; i < 3; ++i) {
  595.                 jacobian[6][i + 3] = -2 * (oy * jacobian[2][i] - oz * jacobian[1][i]);
  596.                 jacobian[7][i + 3] = -2 * (oz * jacobian[0][i] - ox * jacobian[2][i]);
  597.                 jacobian[8][i + 3] = -2 * (ox * jacobian[1][i] - oy * jacobian[0][i]);
  598.             }

  600.             // dA1/dA0
  601.             System.arraycopy(jacobian[0], 0, jacobian[6], 6, 3);
  602.             System.arraycopy(jacobian[1], 0, jacobian[7], 6, 3);
  603.             System.arraycopy(jacobian[2], 0, jacobian[8], 6, 3);

  605.         }
  606.     }

  608.     /** Get the underlying elementary Cartesian part.
  609.      * <p>A transform can be uniquely represented as an elementary
  610.      * translation followed by an elementary rotation. This method
  611.      * returns this unique elementary translation with its derivative.</p>
  612.      * @return underlying elementary Cartesian part
  613.      * @see #getTranslation()
  614.      * @see #getVelocity()
  615.      */
  616.     public PVCoordinates getCartesian() {
  617.         return cartesian;
  618.     }

  620.     /** Get the underlying elementary translation.
  621.      * <p>A transform can be uniquely represented as an elementary
  622.      * translation followed by an elementary rotation. This method
  623.      * returns this unique elementary translation.</p>
  624.      * @return underlying elementary translation
  625.      * @see #getCartesian()
  626.      * @see #getVelocity()
  627.      * @see #getAcceleration()
  628.      */
  629.     public Vector3D getTranslation() {
  630.         return cartesian.getPosition();
  631.     }

  633.     /** Get the first time derivative of the translation.
  634.      * @return first time derivative of the translation
  635.      * @see #getCartesian()
  636.      * @see #getTranslation()
  637.      * @see #getAcceleration()
  638.      */
  639.     public Vector3D getVelocity() {
  640.         return cartesian.getVelocity();
  641.     }

  643.     /** Get the second time derivative of the translation.
  644.      * @return second time derivative of the translation
  645.      * @see #getCartesian()
  646.      * @see #getTranslation()
  647.      * @see #getVelocity()
  648.      */
  649.     public Vector3D getAcceleration() {
  650.         return cartesian.getAcceleration();
  651.     }

  653.     /** Get the underlying elementary angular part.
  654.      * <p>A transform can be uniquely represented as an elementary
  655.      * translation followed by an elementary rotation. This method
  656.      * returns this unique elementary rotation with its derivative.</p>
  657.      * @return underlying elementary angular part
  658.      * @see #getRotation()
  659.      * @see #getRotationRate()
  660.      * @see #getRotationAcceleration()
  661.      */
  662.     public AngularCoordinates getAngular() {
  663.         return angular;
  664.     }

  666.     /** Get the underlying elementary rotation.
  667.      * <p>A transform can be uniquely represented as an elementary
  668.      * translation followed by an elementary rotation. This method
  669.      * returns this unique elementary rotation.</p>
  670.      * @return underlying elementary rotation
  671.      * @see #getAngular()
  672.      * @see #getRotationRate()
  673.      * @see #getRotationAcceleration()
  674.      */
  675.     public Rotation getRotation() {
  676.         return angular.getRotation();
  677.     }

  679.     /** Get the first time derivative of the rotation.
  680.      * <p>The norm represents the angular rate.</p>
  681.      * @return First time derivative of the rotation
  682.      * @see #getAngular()
  683.      * @see #getRotation()
  684.      * @see #getRotationAcceleration()
  685.      */
  686.     public Vector3D getRotationRate() {
  687.         return angular.getRotationRate();
  688.     }

  690.     /** Get the second time derivative of the rotation.
  691.      * @return Second time derivative of the rotation
  692.      * @see #getAngular()
  693.      * @see #getRotation()
  694.      * @see #getRotationRate()
  695.      */
  696.     public Vector3D getRotationAcceleration() {
  697.         return angular.getRotationAcceleration();
  698.     }

  700.     /** Specialized class for identity transform. */
  701.     private static class IdentityTransform extends Transform {

  703.         /** Simple constructor. */
  704.         IdentityTransform() {
  705.             super(AbsoluteDate.ARBITRARY_EPOCH, PVCoordinates.ZERO, AngularCoordinates.IDENTITY);
  706.         }

  708.         @Override
  709.         public StaticTransform staticShiftedBy(final double dt) {
  710.             return toStaticTransform();
  711.         }

  713.         /** {@inheritDoc} */
  714.         @Override
  715.         public Transform shiftedBy(final double dt) {
  716.             return this;
  717.         }

  719.         @Override
  720.         public StaticTransform getStaticInverse() {
  721.             return toStaticTransform();
  722.         }

  724.         /** {@inheritDoc} */
  725.         @Override
  726.         public Transform shiftedBy(final TimeOffset dt) {
  727.             return this;
  728.         }

  730.         /** {@inheritDoc} */
  731.         @Override
  732.         public Transform getInverse() {
  733.             return this;
  734.         }

  736.         @Override
  737.         public StaticTransform toStaticTransform() {
  738.             return StaticTransform.getIdentity();
  739.         }

  741.         /** {@inheritDoc} */
  742.         @Override
  743.         public Vector3D transformPosition(final Vector3D position) {
  744.             return position;
  745.         }

  747.         /** {@inheritDoc} */
  748.         @Override
  749.         public Vector3D transformVector(final Vector3D vector) {
  750.             return vector;
  751.         }

  753.         @Override
  754.         public <T extends CalculusFieldElement<T>> FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  755.             return transformVector(position);
  756.         }

  758.         @Override
  759.         public <T extends CalculusFieldElement<T>> FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  760.             return new FieldVector3D<>(vector.getX(), vector.getY(), vector.getZ());
  761.         }

  763.         /** {@inheritDoc} */
  764.         @Override
  765.         public Line transformLine(final Line line) {
  766.             return line;
  767.         }

  769.         /** {@inheritDoc} */
  770.         @Override
  771.         public PVCoordinates transformPVCoordinates(final PVCoordinates pv) {
  772.             return pv;
  773.         }

  775.         @Override
  776.         public PVCoordinates transformOnlyPV(final PVCoordinates pv) {
  777.             return new PVCoordinates(pv.getPosition(), pv.getVelocity());
  778.         }

  780.         @Override
  781.         public TimeStampedPVCoordinates transformOnlyPV(final TimeStampedPVCoordinates pv) {
  782.             return new TimeStampedPVCoordinates(pv.getDate(), pv.getPosition(), pv.getVelocity());
  783.         }

  785.         @Override
  786.         public Transform freeze() {
  787.             return this;
  788.         }

  790.         @Override
  791.         public TimeStampedPVCoordinates transformPVCoordinates(
  792.                 final TimeStampedPVCoordinates pv) {
  793.             return pv;
  794.         }

  796.         @Override
  797.         public <T extends CalculusFieldElement<T>> FieldPVCoordinates<T>
  798.             transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  799.             return pv;
  800.         }

  802.         @Override
  803.         public <T extends CalculusFieldElement<T>>
  804.             TimeStampedFieldPVCoordinates<T> transformPVCoordinates(
  805.                     final TimeStampedFieldPVCoordinates<T> pv) {
  806.             return pv;
  807.         }

  809.         /** {@inheritDoc} */
  810.         @Override
  811.         public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {
  812.             final int n = 3 * (selector.getMaxOrder() + 1);
  813.             for (int i = 0; i < n; ++i) {
  814.                 Arrays.fill(jacobian[i], 0, n, 0.0);
  815.                 jacobian[i][i] = 1.0;
  816.             }
  817.         }

  819.     }

  821. }
Created with JaCoCo 0.8.12.202403310830