Transform.java

  1. /* Copyright 2002-2022 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.io.Serializable;
  20. import java.util.ArrayList;
  21. import java.util.Arrays;
  22. import java.util.Collection;
  23. import java.util.List;
  24. import java.util.stream.Collectors;
  25. import java.util.stream.Stream;

  27. import org.hipparchus.CalculusFieldElement;
  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.TimeInterpolable;
  33. import org.orekit.time.TimeShiftable;
  34. import org.orekit.utils.AngularCoordinates;
  35. import org.orekit.utils.AngularDerivativesFilter;
  36. import org.orekit.utils.CartesianDerivativesFilter;
  37. import org.orekit.utils.FieldPVCoordinates;
  38. import org.orekit.utils.PVCoordinates;
  39. import org.orekit.utils.TimeStampedAngularCoordinates;
  40. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  41. import org.orekit.utils.TimeStampedPVCoordinates;


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

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

  108.     /** Serializable UID. */
  109.     private static final long serialVersionUID = 210140410L;

  111.     /** Date of the transform. */
  112.     private final AbsoluteDate date;

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

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

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

  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 Transform(final AbsoluteDate date, final Vector3D translation) {
  139.         this(date,
  140.              new PVCoordinates(translation),
  141.              AngularCoordinates.IDENTITY);
  142.     }

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

  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 Transform(final AbsoluteDate date, final Vector3D translation,
  165.                      final Vector3D velocity) {
  166.         this(date,
  167.              new PVCoordinates(translation, velocity, Vector3D.ZERO),
  168.              AngularCoordinates.IDENTITY);
  169.     }

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

  188.     /** Build a translation transform, with its first time derivative.
  189.      * @param date date of the transform
  190.      * @param cartesian Cartesian part of the transformation to apply (i.e. coordinates of
  191.      * the transformed origin, or coordinates of the origin of the
  192.      * old frame in the new frame, with their derivatives)
  193.      */
  194.     public Transform(final AbsoluteDate date, final PVCoordinates cartesian) {
  195.         this(date,
  196.              cartesian,
  197.              AngularCoordinates.IDENTITY);
  198.     }

  200.     /** Build a rotation transform.
  201.      * @param date date of the transform
  202.      * @param rotation rotation to apply ( i.e. rotation to apply to the
  203.      * coordinates of a vector expressed in the old frame to obtain the
  204.      * same vector expressed in the new frame )
  205.      * @param rotationRate the axis of the instant rotation
  206.      * expressed in the new frame. (norm representing angular rate)
  207.      */
  208.     public Transform(final AbsoluteDate date, final Rotation rotation, final Vector3D rotationRate) {
  209.         this(date,
  210.              PVCoordinates.ZERO,
  211.              new AngularCoordinates(rotation, rotationRate, Vector3D.ZERO));
  212.     }

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

  230.     /** Build a rotation transform.
  231.      * @param date date of the transform
  232.      * @param angular angular part of the transformation to apply (i.e. rotation to
  233.      * apply to the coordinates of a vector expressed in the old frame to obtain the
  234.      * same vector expressed in the new frame, with its rotation rate)
  235.      */
  236.     public Transform(final AbsoluteDate date, final AngularCoordinates angular) {
  237.         this(date, PVCoordinates.ZERO, angular);
  238.     }

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

  261.     /** Compute a composite velocity.
  262.      * @param first first applied transform
  263.      * @param second second applied transform
  264.      * @return velocity part of the composite transform
  265.      */
  266.     private static Vector3D compositeVelocity(final Transform first, final Transform second) {

  268.         final Vector3D v1 = first.cartesian.getVelocity();
  269.         final Rotation r1 = first.angular.getRotation();
  270.         final Vector3D o1 = first.angular.getRotationRate();
  271.         final Vector3D p2 = second.cartesian.getPosition();
  272.         final Vector3D v2 = second.cartesian.getVelocity();

  274.         final Vector3D crossP = Vector3D.crossProduct(o1, p2);

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

  278.     }

  280.     /** Compute a composite acceleration.
  281.      * @param first first applied transform
  282.      * @param second second applied transform
  283.      * @return acceleration part of the composite transform
  284.      */
  285.     private static Vector3D compositeAcceleration(final Transform first, final Transform second) {

  287.         final Vector3D a1    = first.cartesian.getAcceleration();
  288.         final Rotation r1    = first.angular.getRotation();
  289.         final Vector3D o1    = first.angular.getRotationRate();
  290.         final Vector3D oDot1 = first.angular.getRotationAcceleration();
  291.         final Vector3D p2    = second.cartesian.getPosition();
  292.         final Vector3D v2    = second.cartesian.getVelocity();
  293.         final Vector3D a2    = second.cartesian.getAcceleration();

  295.         final Vector3D crossCrossP = Vector3D.crossProduct(o1,    Vector3D.crossProduct(o1, p2));
  296.         final Vector3D crossV      = Vector3D.crossProduct(o1,    v2);
  297.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot1, p2);

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

  301.     }

  303.     /** Compute a composite rotation rate.
  304.      * @param first first applied transform
  305.      * @param second second applied transform
  306.      * @return rotation rate part of the composite transform
  307.      */
  308.     private static Vector3D compositeRotationRate(final Transform first, final Transform second) {

  310.         final Vector3D o1 = first.angular.getRotationRate();
  311.         final Rotation r2 = second.angular.getRotation();
  312.         final Vector3D o2 = second.angular.getRotationRate();

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

  316.     }

  318.     /** Compute a composite rotation acceleration.
  319.      * @param first first applied transform
  320.      * @param second second applied transform
  321.      * @return rotation acceleration part of the composite transform
  322.      */
  323.     private static Vector3D compositeRotationAcceleration(final Transform first, final Transform second) {

  325.         final Vector3D o1    = first.angular.getRotationRate();
  326.         final Vector3D oDot1 = first.angular.getRotationAcceleration();
  327.         final Rotation r2    = second.angular.getRotation();
  328.         final Vector3D o2    = second.angular.getRotationRate();
  329.         final Vector3D oDot2 = second.angular.getRotationAcceleration();

  331.         return new Vector3D( 1, oDot2,
  332.                              1, r2.applyTo(oDot1),
  333.                             -1, Vector3D.crossProduct(o2, r2.applyTo(o1)));

  335.     }

  337.     /** {@inheritDoc} */
  338.     public AbsoluteDate getDate() {
  339.         return date;
  340.     }

  342.     /** {@inheritDoc} */
  343.     public Transform shiftedBy(final double dt) {
  344.         return new Transform(date.shiftedBy(dt), cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  345.     }

  347.     /**
  348.      * Shift the transform in time considering all rates, then return only the
  349.      * translation and rotation portion of the transform.
  350.      *
  351.      * @param dt time shift in seconds.
  352.      * @return shifted transform as a static transform. It is static in the
  353.      * sense that it can only be used to transform directions and positions, but
  354.      * not velocities or accelerations.
  355.      * @see #shiftedBy(double)
  356.      */
  357.     public StaticTransform staticShiftedBy(final double dt) {
  358.         return StaticTransform.of(
  359.                 date.shiftedBy(dt),
  360.                 cartesian.positionShiftedBy(dt),
  361.                 angular.rotationShiftedBy(dt));
  362.     }

  364.     /** {@inheritDoc}
  365.      * <p>
  366.      * Calling this method is equivalent to call {@link #interpolate(AbsoluteDate,
  367.      * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
  368.      * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
  369.      * {@link AngularDerivativesFilter#USE_RRA}
  370.      * set to true.
  371.      * </p>
  372.      */
  373.     public Transform interpolate(final AbsoluteDate interpolationDate, final Stream<Transform> sample) {
  374.         return interpolate(interpolationDate,
  375.                            CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
  376.                            sample.collect(Collectors.toList()));
  377.     }

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

  418.     /** Get the inverse transform of the instance.
  419.      * @return inverse transform of the instance
  420.      */
  421.     @Override
  422.     public Transform getInverse() {

  424.         final Rotation r    = angular.getRotation();
  425.         final Vector3D o    = angular.getRotationRate();
  426.         final Vector3D oDot = angular.getRotationAcceleration();
  427.         final Vector3D rp   = r.applyTo(cartesian.getPosition());
  428.         final Vector3D rv   = r.applyTo(cartesian.getVelocity());
  429.         final Vector3D ra   = r.applyTo(cartesian.getAcceleration());

  431.         final Vector3D pInv        = rp.negate();
  432.         final Vector3D crossP      = Vector3D.crossProduct(o, rp);
  433.         final Vector3D vInv        = crossP.subtract(rv);
  434.         final Vector3D crossV      = Vector3D.crossProduct(o, rv);
  435.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot, rp);
  436.         final Vector3D crossCrossP = Vector3D.crossProduct(o, crossP);
  437.         final Vector3D aInv        = new Vector3D(-1, ra,
  438.                                                    2, crossV,
  439.                                                    1, crossDotP,
  440.                                                   -1, crossCrossP);

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

  444.     }

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

  459.     /** Transform {@link PVCoordinates} including kinematic effects.
  460.      * @param pva the position-velocity-acceleration triplet to transform.
  461.      * @return transformed position-velocity-acceleration
  462.      */
  463.     public PVCoordinates transformPVCoordinates(final PVCoordinates pva) {
  464.         return angular.applyTo(new PVCoordinates(1, pva, 1, cartesian));
  465.     }

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

  483.     /** Transform {@link FieldPVCoordinates} including kinematic effects.
  484.      * @param pv position-velocity to transform.
  485.      * @param <T> type of the field elements
  486.      * @return transformed position-velocity
  487.      */
  488.     public <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  489.         return angular.applyTo(new FieldPVCoordinates<>(pv.getPosition().add(cartesian.getPosition()),
  490.                                                         pv.getVelocity().add(cartesian.getVelocity()),
  491.                                                         pv.getAcceleration().add(cartesian.getAcceleration())));
  492.     }

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

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

  540.         // elementary matrix for rotation
  541.         final double[][] mData = angular.getRotation().getMatrix();

  543.         // dP1/dP0
  544.         System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  545.         System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  546.         System.arraycopy(mData[2], 0, jacobian[2], 0, 3);

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

  550.             // dP1/dV0
  551.             Arrays.fill(jacobian[0], 3, 6, 0.0);
  552.             Arrays.fill(jacobian[1], 3, 6, 0.0);
  553.             Arrays.fill(jacobian[2], 3, 6, 0.0);

  555.             // dV1/dP0
  556.             final Vector3D o = angular.getRotationRate();
  557.             final double ox = o.getX();
  558.             final double oy = o.getY();
  559.             final double oz = o.getZ();
  560.             for (int i = 0; i < 3; ++i) {
  561.                 jacobian[3][i] = -(oy * mData[2][i] - oz * mData[1][i]);
  562.                 jacobian[4][i] = -(oz * mData[0][i] - ox * mData[2][i]);
  563.                 jacobian[5][i] = -(ox * mData[1][i] - oy * mData[0][i]);
  564.             }

  566.             // dV1/dV0
  567.             System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
  568.             System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
  569.             System.arraycopy(mData[2], 0, jacobian[5], 3, 3);

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

  573.                 // dP1/dA0
  574.                 Arrays.fill(jacobian[0], 6, 9, 0.0);
  575.                 Arrays.fill(jacobian[1], 6, 9, 0.0);
  576.                 Arrays.fill(jacobian[2], 6, 9, 0.0);

  578.                 // dV1/dA0
  579.                 Arrays.fill(jacobian[3], 6, 9, 0.0);
  580.                 Arrays.fill(jacobian[4], 6, 9, 0.0);
  581.                 Arrays.fill(jacobian[5], 6, 9, 0.0);

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

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

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

  606.             }

  608.         }

  610.     }

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

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

  637.     /** Get the first time derivative of the translation.
  638.      * @return first time derivative of the translation
  639.      * @see #getCartesian()
  640.      * @see #getTranslation()
  641.      * @see #getAcceleration()
  642.      */
  643.     public Vector3D getVelocity() {
  644.         return cartesian.getVelocity();
  645.     }

  647.     /** Get the second time derivative of the translation.
  648.      * @return second time derivative of the translation
  649.      * @see #getCartesian()
  650.      * @see #getTranslation()
  651.      * @see #getVelocity()
  652.      */
  653.     public Vector3D getAcceleration() {
  654.         return cartesian.getAcceleration();
  655.     }

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

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

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

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

  704.     /** Specialized class for identity transform. */
  705.     private static class IdentityTransform extends Transform {

  707.         /** Serializable UID. */
  708.         private static final long serialVersionUID = -9042082036141830517L;

  710.         /** Simple constructor. */
  711.         IdentityTransform() {
  712.             super(AbsoluteDate.ARBITRARY_EPOCH, PVCoordinates.ZERO, AngularCoordinates.IDENTITY);
  713.         }

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

  721.         /** {@inheritDoc} */
  722.         @Override
  723.         public Transform getInverse() {
  724.             return this;
  725.         }

  727.         /** {@inheritDoc} */
  728.         @Override
  729.         public Vector3D transformPosition(final Vector3D position) {
  730.             return position;
  731.         }

  733.         /** {@inheritDoc} */
  734.         @Override
  735.         public Vector3D transformVector(final Vector3D vector) {
  736.             return vector;
  737.         }

  739.         /** {@inheritDoc} */
  740.         @Override
  741.         public Line transformLine(final Line line) {
  742.             return line;
  743.         }

  745.         /** {@inheritDoc} */
  746.         @Override
  747.         public PVCoordinates transformPVCoordinates(final PVCoordinates pv) {
  748.             return pv;
  749.         }

  751.         @Override
  752.         public Transform freeze() {
  753.             return this;
  754.         }

  756.         @Override
  757.         public TimeStampedPVCoordinates transformPVCoordinates(
  758.                 final TimeStampedPVCoordinates pv) {
  759.             return pv;
  760.         }

  762.         @Override
  763.         public <T extends CalculusFieldElement<T>> FieldPVCoordinates<T>
  764.             transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  765.             return pv;
  766.         }

  768.         @Override
  769.         public <T extends CalculusFieldElement<T>>
  770.             TimeStampedFieldPVCoordinates<T> transformPVCoordinates(
  771.                     final TimeStampedFieldPVCoordinates<T> pv) {
  772.             return pv;
  773.         }

  775.         /** {@inheritDoc} */
  776.         @Override
  777.         public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {
  778.             final int n = 3 * (selector.getMaxOrder() + 1);
  779.             for (int i = 0; i < n; ++i) {
  780.                 Arrays.fill(jacobian[i], 0, n, 0.0);
  781.                 jacobian[i][i] = 1.0;
  782.             }
  783.         }

  785.     }

  787. }
Created with JaCoCo 0.8.8.202204050719