Transform.java

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

  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.RealFieldElement;
  28. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  29. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  30. import org.hipparchus.geometry.euclidean.threed.Line;
  31. import org.hipparchus.geometry.euclidean.threed.Rotation;
  32. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  33. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  34. import org.orekit.errors.OrekitException;
  35. import org.orekit.time.AbsoluteDate;
  36. import org.orekit.time.TimeInterpolable;
  37. import org.orekit.time.TimeShiftable;
  38. import org.orekit.time.TimeStamped;
  39. import org.orekit.utils.AngularCoordinates;
  40. import org.orekit.utils.AngularDerivativesFilter;
  41. import org.orekit.utils.CartesianDerivativesFilter;
  42. import org.orekit.utils.FieldPVCoordinates;
  43. import org.orekit.utils.PVCoordinates;
  44. import org.orekit.utils.TimeStampedAngularCoordinates;
  45. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  46. import org.orekit.utils.TimeStampedPVCoordinates;


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

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

  110.     /** Serializable UID. */
  111.     private static final long serialVersionUID = 210140410L;

  113.     /** Date of the transform. */
  114.     private final AbsoluteDate date;

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

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

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

  134.     /** Build a translation transform.
  135.      * @param date date of the transform
  136.      * @param translation translation to apply (i.e. coordinates of
  137.      * the transformed origin, or coordinates of the origin of the
  138.      * old frame in the new frame)
  139.      */
  140.     public Transform(final AbsoluteDate date, final Vector3D translation) {
  141.         this(date,
  142.              new PVCoordinates(translation, Vector3D.ZERO, Vector3D.ZERO),
  143.              AngularCoordinates.IDENTITY);
  144.     }

  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 Transform(final AbsoluteDate date, final Rotation rotation) {
  153.         this(date,
  154.              PVCoordinates.ZERO,
  155.              new AngularCoordinates(rotation, Vector3D.ZERO));
  156.     }

  158.     /** Build a translation transform, with its first time derivative.
  159.      * @param date date of the transform
  160.      * @param translation translation to apply (i.e. coordinates of
  161.      * the transformed origin, or coordinates of the origin of the
  162.      * old frame in the new frame)
  163.      * @param velocity the velocity of the translation (i.e. origin
  164.      * of the old frame velocity in the new frame)
  165.      */
  166.     public Transform(final AbsoluteDate date, final Vector3D translation,
  167.                      final Vector3D velocity) {
  168.         this(date,
  169.              new PVCoordinates(translation, velocity, Vector3D.ZERO),
  170.              AngularCoordinates.IDENTITY);
  171.     }

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

  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 Transform(final AbsoluteDate date, final PVCoordinates cartesian) {
  197.         this(date,
  198.              cartesian,
  199.              AngularCoordinates.IDENTITY);
  200.     }

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

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

  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 Transform(final AbsoluteDate date, final AngularCoordinates angular) {
  239.         this(date, PVCoordinates.ZERO, angular);
  240.     }

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

  263.     /** Compute a composite translation.
  264.      * @param first first applied transform
  265.      * @param second second applied transform
  266.      * @return translation part of the composite transform
  267.      */
  268.     private static Vector3D compositeTranslation(final Transform first, final Transform second) {

  270.         final Vector3D p1 = first.cartesian.getPosition();
  271.         final Rotation r1 = first.angular.getRotation();
  272.         final Vector3D p2 = second.cartesian.getPosition();

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

  276.     }

  278.     /** Compute a composite velocity.
  279.      * @param first first applied transform
  280.      * @param second second applied transform
  281.      * @return velocity part of the composite transform
  282.      */
  283.     private static Vector3D compositeVelocity(final Transform first, final Transform second) {

  285.         final Vector3D v1 = first.cartesian.getVelocity();
  286.         final Rotation r1 = first.angular.getRotation();
  287.         final Vector3D o1 = first.angular.getRotationRate();
  288.         final Vector3D p2 = second.cartesian.getPosition();
  289.         final Vector3D v2 = second.cartesian.getVelocity();

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

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

  295.     }

  297.     /** Compute a composite acceleration.
  298.      * @param first first applied transform
  299.      * @param second second applied transform
  300.      * @return acceleration part of the composite transform
  301.      */
  302.     private static Vector3D compositeAcceleration(final Transform first, final Transform second) {

  304.         final Vector3D a1    = first.cartesian.getAcceleration();
  305.         final Rotation r1    = first.angular.getRotation();
  306.         final Vector3D o1    = first.angular.getRotationRate();
  307.         final Vector3D oDot1 = first.angular.getRotationAcceleration();
  308.         final Vector3D p2    = second.cartesian.getPosition();
  309.         final Vector3D v2    = second.cartesian.getVelocity();
  310.         final Vector3D a2    = second.cartesian.getAcceleration();

  312.         final Vector3D crossCrossP = Vector3D.crossProduct(o1,    Vector3D.crossProduct(o1, p2));
  313.         final Vector3D crossV      = Vector3D.crossProduct(o1,    v2);
  314.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot1, p2);

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

  318.     }

  320.     /** Compute a composite rotation.
  321.      * @param first first applied transform
  322.      * @param second second applied transform
  323.      * @return rotation part of the composite transform
  324.      */
  325.     private static Rotation compositeRotation(final Transform first, final Transform second) {

  327.         final Rotation r1 = first.angular.getRotation();
  328.         final Rotation r2 = second.angular.getRotation();

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

  332.     }

  334.     /** Compute a composite rotation rate.
  335.      * @param first first applied transform
  336.      * @param second second applied transform
  337.      * @return rotation rate part of the composite transform
  338.      */
  339.     private static Vector3D compositeRotationRate(final Transform first, final Transform second) {

  341.         final Vector3D o1 = first.angular.getRotationRate();
  342.         final Rotation r2 = second.angular.getRotation();
  343.         final Vector3D o2 = second.angular.getRotationRate();

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

  347.     }

  349.     /** Compute a composite rotation acceleration.
  350.      * @param first first applied transform
  351.      * @param second second applied transform
  352.      * @return rotation acceleration part of the composite transform
  353.      */
  354.     private static Vector3D compositeRotationAcceleration(final Transform first, final Transform second) {

  356.         final Vector3D o1    = first.angular.getRotationRate();
  357.         final Vector3D oDot1 = first.angular.getRotationAcceleration();
  358.         final Rotation r2    = second.angular.getRotation();
  359.         final Vector3D o2    = second.angular.getRotationRate();
  360.         final Vector3D oDot2 = second.angular.getRotationAcceleration();

  362.         return new Vector3D( 1, oDot2,
  363.                              1, r2.applyTo(oDot1),
  364.                             -1, Vector3D.crossProduct(o2, r2.applyTo(o1)));

  366.     }

  368.     /** {@inheritDoc} */
  369.     public AbsoluteDate getDate() {
  370.         return date;
  371.     }

  373.     /** {@inheritDoc} */
  374.     public Transform shiftedBy(final double dt) {
  375.         return new Transform(date.shiftedBy(dt), cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  376.     }

  378.     /** {@inheritDoc}
  379.      * <p>
  380.      * Calling this method is equivalent to call {@link #interpolate(AbsoluteDate,
  381.      * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
  382.      * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
  383.      * {@link AngularDerivativesFilter#USE_RRA}
  384.      * set to true.
  385.      * </p>
  386.      * @exception OrekitException if the number of point is too small for interpolating
  387.      */
  388.     public Transform interpolate(final AbsoluteDate interpolationDate, final Stream<Transform> sample)
  389.         throws OrekitException {
  390.         return interpolate(interpolationDate,
  391.                            CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
  392.                            sample.collect(Collectors.toList()));
  393.     }

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

  436.     /** Get the inverse transform of the instance.
  437.      * @return inverse transform of the instance
  438.      */
  439.     public Transform getInverse() {

  441.         final Rotation r    = angular.getRotation();
  442.         final Vector3D o    = angular.getRotationRate();
  443.         final Vector3D oDot = angular.getRotationAcceleration();
  444.         final Vector3D rp   = r.applyTo(cartesian.getPosition());
  445.         final Vector3D rv   = r.applyTo(cartesian.getVelocity());
  446.         final Vector3D ra   = r.applyTo(cartesian.getAcceleration());

  448.         final Vector3D pInv        = rp.negate();
  449.         final Vector3D crossP      = Vector3D.crossProduct(o, rp);
  450.         final Vector3D vInv        = crossP.subtract(rv);
  451.         final Vector3D crossV      = Vector3D.crossProduct(o, rv);
  452.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot, rp);
  453.         final Vector3D crossCrossP = Vector3D.crossProduct(o, crossP);
  454.         final Vector3D aInv        = new Vector3D(-1, ra,
  455.                                                    2, crossV,
  456.                                                    1, crossDotP,
  457.                                                   -1, crossCrossP);

  459.         return new Transform(date, new PVCoordinates(pInv, vInv, aInv), angular.revert());

  461.     }

  463.     /** Get a frozen transform.
  464.      * <p>
  465.      * This method creates a copy of the instance but frozen in time,
  466.      * i.e. with velocity, acceleration and rotation rate forced to zero.
  467.      * </p>
  468.      * @return a new transform, without any time-dependent parts
  469.      */
  470.     public Transform freeze() {
  471.         return new Transform(date,
  472.                              new PVCoordinates(cartesian.getPosition(), Vector3D.ZERO, Vector3D.ZERO),
  473.                              new AngularCoordinates(angular.getRotation(), Vector3D.ZERO, Vector3D.ZERO));
  474.     }

  476.     /** Transform a position vector (including translation effects).
  477.      * @param position vector to transform
  478.      * @return transformed position
  479.      */
  480.     public Vector3D transformPosition(final Vector3D position) {
  481.         return angular.getRotation().applyTo(cartesian.getPosition().add(position));
  482.     }

  484.     /** Transform a position vector (including translation effects).
  485.      * @param position vector to transform
  486.      * @param <T> the type of the field elements
  487.      * @return transformed position
  488.      */
  489.     public <T extends RealFieldElement<T>> FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  490.         return FieldRotation.applyTo(angular.getRotation(), position.add(cartesian.getPosition()));
  491.     }

  493.     /** Transform a vector (ignoring translation effects).
  494.      * @param vector vector to transform
  495.      * @return transformed vector
  496.      */
  497.     public Vector3D transformVector(final Vector3D vector) {
  498.         return angular.getRotation().applyTo(vector);
  499.     }

  501.     /** Transform a vector (ignoring translation effects).
  502.      * @param vector vector to transform
  503.      * @param <T> the type of the field elements
  504.      * @return transformed vector
  505.      */
  506.     public <T extends RealFieldElement<T>> FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  507.         return FieldRotation.applyTo(angular.getRotation(), vector);
  508.     }

  510.     /** Transform a line.
  511.      * @param line to transform
  512.      * @return transformed line
  513.      */
  514.     public Line transformLine(final Line line) {
  515.         final Vector3D transformedP0 = transformPosition(line.getOrigin());
  516.         final Vector3D transformedP1 = transformPosition(line.pointAt(1.0e6));
  517.         return new Line(transformedP0, transformedP1, 1.0e-10);
  518.     }

  520.     /** Transform {@link PVCoordinates} including kinematic effects.
  521.      * @param pva the position-velocity-acceleration triplet to transform.
  522.      * @return transformed position-velocity-acceleration
  523.      */
  524.     public PVCoordinates transformPVCoordinates(final PVCoordinates pva) {
  525.         return angular.applyTo(new PVCoordinates(1, pva, 1, cartesian));
  526.     }

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

  544.     /** Transform {@link FieldPVCoordinates} including kinematic effects.
  545.      * @param pv position-velocity to transform.
  546.      * @param <T> type of the field elements
  547.      * @return transformed position-velocity
  548.      */
  549.     public <T extends RealFieldElement<T>> FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
  550.         return angular.applyTo(new FieldPVCoordinates<>(pv.getPosition().add(cartesian.getPosition()),
  551.                                                         pv.getVelocity().add(cartesian.getVelocity()),
  552.                                                         pv.getAcceleration().add(cartesian.getAcceleration())));
  553.     }

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

  575.     /** Compute the Jacobian of the {@link #transformPVCoordinates(PVCoordinates)}
  576.      * method of the transform.
  577.      * <p>
  578.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  579.      * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j
  580.      * of the input {@link PVCoordinates} in method {@link #transformPVCoordinates(PVCoordinates)}.
  581.      * </p>
  582.      * <p>
  583.      * This definition implies that if we define position-velocity coordinates
  584.      * <pre>
  585.      * PV₁ = transform.transformPVCoordinates(PV₀), then
  586.      * </pre>
  587.      * <p> their differentials dPV₁ and dPV₀ will obey the following relation
  588.      * where J is the matrix computed by this method:
  589.      * <pre>
  590.      * dPV₁ = J &times; dPV₀
  591.      * </pre>
  592.      *
  593.      * @param selector selector specifying the size of the upper left corner that must be filled
  594.      * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
  595.      * velocities and accelerations)
  596.      * @param jacobian placeholder matrix whose upper-left corner is to be filled with
  597.      * the Jacobian, the rest of the matrix remaining untouched
  598.      */
  599.     public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {

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

  604.         // dP1/dP0
  605.         System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  606.         System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  607.         System.arraycopy(mData[2], 0, jacobian[2], 0, 3);

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

  611.             // dP1/dV0
  612.             Arrays.fill(jacobian[0], 3, 6, 0.0);
  613.             Arrays.fill(jacobian[1], 3, 6, 0.0);
  614.             Arrays.fill(jacobian[2], 3, 6, 0.0);

  616.             // dV1/dP0
  617.             final Vector3D o = angular.getRotationRate();
  618.             final double ox = o.getX();
  619.             final double oy = o.getY();
  620.             final double oz = o.getZ();
  621.             for (int i = 0; i < 3; ++i) {
  622.                 jacobian[3][i] = -(oy * mData[2][i] - oz * mData[1][i]);
  623.                 jacobian[4][i] = -(oz * mData[0][i] - ox * mData[2][i]);
  624.                 jacobian[5][i] = -(ox * mData[1][i] - oy * mData[0][i]);
  625.             }

  627.             // dV1/dV0
  628.             System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
  629.             System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
  630.             System.arraycopy(mData[2], 0, jacobian[5], 3, 3);

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

  634.                 // dP1/dA0
  635.                 Arrays.fill(jacobian[0], 6, 9, 0.0);
  636.                 Arrays.fill(jacobian[1], 6, 9, 0.0);
  637.                 Arrays.fill(jacobian[2], 6, 9, 0.0);

  639.                 // dV1/dA0
  640.                 Arrays.fill(jacobian[3], 6, 9, 0.0);
  641.                 Arrays.fill(jacobian[4], 6, 9, 0.0);
  642.                 Arrays.fill(jacobian[5], 6, 9, 0.0);

  644.                 // dA1/dP0
  645.                 final Vector3D oDot = angular.getRotationAcceleration();
  646.                 final double oDotx  = oDot.getX();
  647.                 final double oDoty  = oDot.getY();
  648.                 final double oDotz  = oDot.getZ();
  649.                 for (int i = 0; i < 3; ++i) {
  650.                     jacobian[6][i] = -(oDoty * mData[2][i] - oDotz * mData[1][i]) - (oy * jacobian[5][i] - oz * jacobian[4][i]);
  651.                     jacobian[7][i] = -(oDotz * mData[0][i] - oDotx * mData[2][i]) - (oz * jacobian[3][i] - ox * jacobian[5][i]);
  652.                     jacobian[8][i] = -(oDotx * mData[1][i] - oDoty * mData[0][i]) - (ox * jacobian[4][i] - oy * jacobian[3][i]);
  653.                 }

  655.                 // dA1/dV0
  656.                 for (int i = 0; i < 3; ++i) {
  657.                     jacobian[6][i + 3] = -2 * (oy * mData[2][i] - oz * mData[1][i]);
  658.                     jacobian[7][i + 3] = -2 * (oz * mData[0][i] - ox * mData[2][i]);
  659.                     jacobian[8][i + 3] = -2 * (ox * mData[1][i] - oy * mData[0][i]);
  660.                 }

  662.                 // dA1/dA0
  663.                 System.arraycopy(mData[0], 0, jacobian[6], 6, 3);
  664.                 System.arraycopy(mData[1], 0, jacobian[7], 6, 3);
  665.                 System.arraycopy(mData[2], 0, jacobian[8], 6, 3);

  667.             }

  669.         }

  671.     }

  673.     /** Get the underlying elementary Cartesian part.
  674.      * <p>A transform can be uniquely represented as an elementary
  675.      * translation followed by an elementary rotation. This method
  676.      * returns this unique elementary translation with its derivative.</p>
  677.      * @return underlying elementary Cartesian part
  678.      * @see #getTranslation()
  679.      * @see #getVelocity()
  680.      */
  681.     public PVCoordinates getCartesian() {
  682.         return cartesian;
  683.     }

  685.     /** Get the underlying elementary translation.
  686.      * <p>A transform can be uniquely represented as an elementary
  687.      * translation followed by an elementary rotation. This method
  688.      * returns this unique elementary translation.</p>
  689.      * @return underlying elementary translation
  690.      * @see #getCartesian()
  691.      * @see #getVelocity()
  692.      * @see #getAcceleration()
  693.      */
  694.     public Vector3D getTranslation() {
  695.         return cartesian.getPosition();
  696.     }

  698.     /** Get the first time derivative of the translation.
  699.      * @return first time derivative of the translation
  700.      * @see #getCartesian()
  701.      * @see #getTranslation()
  702.      * @see #getAcceleration()
  703.      */
  704.     public Vector3D getVelocity() {
  705.         return cartesian.getVelocity();
  706.     }

  708.     /** Get the second time derivative of the translation.
  709.      * @return second time derivative of the translation
  710.      * @see #getCartesian()
  711.      * @see #getTranslation()
  712.      * @see #getVelocity()
  713.      */
  714.     public Vector3D getAcceleration() {
  715.         return cartesian.getAcceleration();
  716.     }

  718.     /** Get the underlying elementary angular part.
  719.      * <p>A transform can be uniquely represented as an elementary
  720.      * translation followed by an elementary rotation. This method
  721.      * returns this unique elementary rotation with its derivative.</p>
  722.      * @return underlying elementary angular part
  723.      * @see #getRotation()
  724.      * @see #getRotationRate()
  725.      * @see #getRotationAcceleration()
  726.      */
  727.     public AngularCoordinates getAngular() {
  728.         return angular;
  729.     }

  731.     /** Get the underlying elementary rotation.
  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 rotation.</p>
  735.      * @return underlying elementary rotation
  736.      * @see #getAngular()
  737.      * @see #getRotationRate()
  738.      * @see #getRotationAcceleration()
  739.      */
  740.     public Rotation getRotation() {
  741.         return angular.getRotation();
  742.     }

  744.     /** Get the first time derivative of the rotation.
  745.      * <p>The norm represents the angular rate.</p>
  746.      * @return First time derivative of the rotation
  747.      * @see #getAngular()
  748.      * @see #getRotation()
  749.      * @see #getRotationAcceleration()
  750.      */
  751.     public Vector3D getRotationRate() {
  752.         return angular.getRotationRate();
  753.     }

  755.     /** Get the second time derivative of the rotation.
  756.      * @return Second time derivative of the rotation
  757.      * @see #getAngular()
  758.      * @see #getRotation()
  759.      * @see #getRotationRate()
  760.      */
  761.     public Vector3D getRotationAcceleration() {
  762.         return angular.getRotationAcceleration();
  763.     }

  765.     /** Specialized class for identity transform. */
  766.     private static class IdentityTransform extends Transform {

  768.         /** Serializable UID. */
  769.         private static final long serialVersionUID = -9042082036141830517L;

  771.         /** Simple constructor. */
  772.         IdentityTransform() {
  773.             super(AbsoluteDate.J2000_EPOCH, PVCoordinates.ZERO, AngularCoordinates.IDENTITY);
  774.         }

  776.         /** {@inheritDoc} */
  777.         @Override
  778.         public Transform shiftedBy(final double dt) {
  779.             return this;
  780.         }

  782.         /** {@inheritDoc} */
  783.         @Override
  784.         public Transform getInverse() {
  785.             return this;
  786.         }

  788.         /** {@inheritDoc} */
  789.         @Override
  790.         public Vector3D transformPosition(final Vector3D position) {
  791.             return position;
  792.         }

  794.         /** {@inheritDoc} */
  795.         @Override
  796.         public Vector3D transformVector(final Vector3D vector) {
  797.             return vector;
  798.         }

  800.         /** {@inheritDoc} */
  801.         @Override
  802.         public Line transformLine(final Line line) {
  803.             return line;
  804.         }

  806.         /** {@inheritDoc} */
  807.         @Override
  808.         public PVCoordinates transformPVCoordinates(final PVCoordinates pv) {
  809.             return pv;
  810.         }

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

  822.     }

  824. }
Created with JaCoCo 0.7.9.201702052155