Transform.java

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

  25. import org.apache.commons.math3.RealFieldElement;
  26. import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
  27. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  28. import org.apache.commons.math3.geometry.euclidean.threed.Line;
  29. import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
  30. import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
  31. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  32. import org.orekit.errors.OrekitException;
  33. import org.orekit.time.AbsoluteDate;
  34. import org.orekit.time.TimeInterpolable;
  35. import org.orekit.time.TimeShiftable;
  36. import org.orekit.time.TimeStamped;
  37. import org.orekit.utils.AngularCoordinates;
  38. import org.orekit.utils.AngularDerivativesFilter;
  39. import org.orekit.utils.CartesianDerivativesFilter;
  40. import org.orekit.utils.FieldPVCoordinates;
  41. import org.orekit.utils.PVCoordinates;
  42. import org.orekit.utils.TimeStampedAngularCoordinates;
  43. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  44. import org.orekit.utils.TimeStampedPVCoordinates;


  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.<p>
  57.  *
  58.  * <p>Instances of this class are guaranteed to be immutable.</p>
  59.  *
  60.  *  <h5> Example </h5>
  61.  *
  62.  * <pre>
  63.  *
  64.  * 1 ) Example of translation from R<sub>A</sub> to R<sub>B</sub>:
  65.  * We want to transform the {@link PVCoordinates} PV<sub>A</sub> to PV<sub>B</sub>.
  66.  *
  67.  * With :  PV<sub>A</sub> = ({1, 0, 0}, {2, 0, 0}, {3, 0, 0});
  68.  * and  :  PV<sub>B</sub> = ({0, 0, 0}, {0, 0, 0}, {0, 0, 0});
  69.  *
  70.  * The transform to apply then is defined as follows :
  71.  *
  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.  * PV<sub>B</sub> = R1toR2.transformPVCoordinates(PV<sub>A</sub>);
  79.  *
  80.  *
  81.  * 2 ) Example of rotation from R<sub>A</sub> to R<sub>B</sub>:
  82.  * We want to transform the {@link PVCoordinates} PV<sub>A</sub> to PV<sub>B</sub>.
  83.  *
  84.  * With :  PV<sub>A</sub> = ({1, 0, 0}, {1, 0, 0});
  85.  * and  :  PV<sub>B</sub> = ({0, 1, 0}, {-2, 1, 0});
  86.  *
  87.  * The transform to apply then is defined as follows :
  88.  *
  89.  * Rotation rotation = new Rotation(Vector3D.PLUS_K, FastMath.PI / 2);
  90.  * Vector3D rotationRate = new Vector3D(0, 0, -2);
  91.  *
  92.  * Transform R1toR2 = new Transform(rotation, rotationRate);
  93.  *
  94.  * PV<sub>B</sub> = R1toR2.transformPVCoordinates(PV<sub>A</sub>);
  95.  *
  96.  * </pre>
  97.  *
  98.  * @author Luc Maisonobe
  99.  * @author Fabien Maussion
  100.  */
  101. public class Transform
  102.     implements TimeStamped, TimeShiftable<Transform>, TimeInterpolable<Transform>, Serializable {

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  267.         final Vector3D p1 = first.cartesian.getPosition();
  268.         final Rotation r1 = first.angular.getRotation();
  269.         final Vector3D p2 = second.cartesian.getPosition();

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

  273.     }

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

  282.         final Vector3D v1 = first.cartesian.getVelocity();
  283.         final Rotation r1 = first.angular.getRotation();
  284.         final Vector3D o1 = first.angular.getRotationRate();
  285.         final Vector3D p2 = second.cartesian.getPosition();
  286.         final Vector3D v2 = second.cartesian.getVelocity();

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

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

  292.     }

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

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

  309.         final Vector3D crossCrossP = Vector3D.crossProduct(o1,    Vector3D.crossProduct(o1, p2));
  310.         final Vector3D crossV      = Vector3D.crossProduct(o1,    v2);
  311.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot1, p2);

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

  315.     }

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

  324.         final Rotation r1 = first.angular.getRotation();
  325.         final Rotation r2 = second.angular.getRotation();

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

  329.     }

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

  338.         final Vector3D o1 = first.angular.getRotationRate();
  339.         final Rotation r2 = second.angular.getRotation();
  340.         final Vector3D o2 = second.angular.getRotationRate();

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

  344.     }

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

  353.         final Vector3D o1    = first.angular.getRotationRate();
  354.         final Vector3D oDot1 = first.angular.getRotationAcceleration();
  355.         final Rotation r2    = second.angular.getRotation();
  356.         final Vector3D o2    = second.angular.getRotationRate();
  357.         final Vector3D oDot2 = second.angular.getRotationAcceleration();

  359.         return new Vector3D( 1, oDot2,
  360.                              1, r2.applyTo(oDot1),
  361.                             -1, Vector3D.crossProduct(o2, r2.applyTo(o1)));

  363.     }

  365.     /** {@inheritDoc} */
  366.     public AbsoluteDate getDate() {
  367.         return date;
  368.     }

  370.     /** {@inheritDoc} */
  371.     public Transform shiftedBy(final double dt) {
  372.         return new Transform(date.shiftedBy(dt), cartesian.shiftedBy(dt), angular.shiftedBy(dt));
  373.     };

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

  392.     /** Interpolate a transform from a sample set of existing transforms.
  393.      * <p>
  394.      * Note that even if first time derivatives (velocities and rotation rates)
  395.      * from sample can be ignored, the interpolated instance always includes
  396.      * interpolated derivatives. This feature can be used explicitly to
  397.      * compute these derivatives when it would be too complex to compute them
  398.      * from an analytical formula: just compute a few sample points from the
  399.      * explicit formula and set the derivatives to zero in these sample points,
  400.      * then use interpolation to add derivatives consistent with the positions
  401.      * and rotations.
  402.      * </p>
  403.      * <p>
  404.      * As this implementation of interpolation is polynomial, it should be used only
  405.      * with small samples (about 10-20 points) in order to avoid <a
  406.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  407.      * and numerical problems (including NaN appearing).
  408.      * </p>
  409.      * @param date interpolation date
  410.      * @param useVelocities if true, use sample transforms velocities,
  411.      * otherwise ignore them and use only positions
  412.      * @param useRotationRates if true, use sample points rotation rates,
  413.      * otherwise ignore them and use only rotations
  414.      * @param sample sample points on which interpolation should be done
  415.      * @return a new instance, interpolated at specified date
  416.      * @exception OrekitException if the number of point is too small for interpolating
  417.      * @deprecated as of 7.0, replaced with {@link #interpolate(AbsoluteDate, CartesianDerivativesFilter, AngularDerivativesFilter, Collection)}
  418.      */
  419.     @Deprecated
  420.     public static Transform interpolate(final AbsoluteDate date,
  421.                                         final boolean useVelocities, final boolean useRotationRates,
  422.                                         final Collection<Transform> sample)
  423.         throws OrekitException {
  424.         return interpolate(date,
  425.                            useVelocities    ? CartesianDerivativesFilter.USE_PV : CartesianDerivativesFilter.USE_P,
  426.                            useRotationRates ? AngularDerivativesFilter.USE_RR   : AngularDerivativesFilter.USE_R,
  427.                            sample);
  428.     }

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

  471.     /** Get the inverse transform of the instance.
  472.      * @return inverse transform of the instance
  473.      */
  474.     public Transform getInverse() {

  476.         final Rotation r    = angular.getRotation();
  477.         final Vector3D o    = angular.getRotationRate();
  478.         final Vector3D oDot = angular.getRotationAcceleration();
  479.         final Vector3D rp   = r.applyTo(cartesian.getPosition());
  480.         final Vector3D rv   = r.applyTo(cartesian.getVelocity());
  481.         final Vector3D ra   = r.applyTo(cartesian.getAcceleration());

  483.         final Vector3D pInv        = rp.negate();
  484.         final Vector3D crossP      = Vector3D.crossProduct(o, rp);
  485.         final Vector3D vInv        = crossP.subtract(rv);
  486.         final Vector3D crossV      = Vector3D.crossProduct(o, rv);
  487.         final Vector3D crossDotP   = Vector3D.crossProduct(oDot, rp);
  488.         final Vector3D crossCrossP = Vector3D.crossProduct(o, crossP);
  489.         final Vector3D aInv        = new Vector3D(-1, ra,
  490.                                                    2, crossV,
  491.                                                    1, crossDotP,
  492.                                                   -1, crossCrossP);

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

  496.     }

  498.     /** Get a frozen transform.
  499.      * <p>
  500.      * This method creates a copy of the instance but frozen in time,
  501.      * i.e. with velocity, acceleration and rotation rate forced to zero.
  502.      * </p>
  503.      * @return a new transform, without any time-dependent parts
  504.      */
  505.     public Transform freeze() {
  506.         return new Transform(date,
  507.                              new PVCoordinates(cartesian.getPosition(), Vector3D.ZERO, Vector3D.ZERO),
  508.                              new AngularCoordinates(angular.getRotation(), Vector3D.ZERO, Vector3D.ZERO));
  509.     }

  511.     /** Transform a position vector (including translation effects).
  512.      * @param position vector to transform
  513.      * @return transformed position
  514.      */
  515.     public Vector3D transformPosition(final Vector3D position) {
  516.         return angular.getRotation().applyTo(cartesian.getPosition().add(position));
  517.     }

  519.     /** Transform a position vector (including translation effects).
  520.      * @param position vector to transform
  521.      * @param <T> the type of the field elements
  522.      * @return transformed position
  523.      */
  524.     public <T extends RealFieldElement<T>> FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
  525.         return FieldRotation.applyTo(angular.getRotation(), position.add(cartesian.getPosition()));
  526.     }

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

  536.     /** Transform a vector (ignoring translation effects).
  537.      * @param vector vector to transform
  538.      * @param <T> the type of the field elements
  539.      * @return transformed vector
  540.      */
  541.     public <T extends RealFieldElement<T>> FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
  542.         return FieldRotation.applyTo(angular.getRotation(), vector);
  543.     }

  545.     /** Transform a line.
  546.      * @param line to transform
  547.      * @return transformed line
  548.      */
  549.     public Line transformLine(final Line line) {
  550.         final Vector3D transformedP0 = transformPosition(line.getOrigin());
  551.         final Vector3D transformedP1 = transformPosition(line.pointAt(1.0e6));
  552.         return new Line(transformedP0, transformedP1, 1.0e-10);
  553.     }

  555.     /** Transform {@link PVCoordinates} including kinematic effects.
  556.      * @param pva the position-velocity-acceleration triplet to transform.
  557.      * @return transformed position-velocity-acceleration
  558.      */
  559.     public PVCoordinates transformPVCoordinates(final PVCoordinates pva) {
  560.         return angular.applyTo(new PVCoordinates(1, pva, 1, cartesian));
  561.     }

  563.     /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
  564.      * <p>
  565.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  566.      * consistency between the transform {@link #getDate() date} and the time-stamped
  567.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  568.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  569.      * the input argument, regardless of the instance {@link #getDate() date}.
  570.      * </p>
  571.      * @param pv time-stamped  position-velocity to transform.
  572.      * @return transformed time-stamped position-velocity
  573.      * @since 7.0
  574.      */
  575.     public TimeStampedPVCoordinates transformPVCoordinates(final TimeStampedPVCoordinates pv) {
  576.         return angular.applyTo(new TimeStampedPVCoordinates(pv.getDate(), 1, pv, 1, cartesian));
  577.     }

  579.     /** Transform {@link FieldPVCoordinates} including kinematic effects.
  580.      * @param pv position-velocity to transform.
  581.      * @param <T> type of the field elements
  582.      * @return transformed position-velocity
  583.      */
  584.     public <T extends RealFieldElement<T>> FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {

  586.         // apply translation
  587.         final FieldVector3D<T> intermediateP = pv.getPosition().add(cartesian.getPosition());
  588.         final FieldVector3D<T> intermediateV = pv.getVelocity().add(cartesian.getVelocity());
  589.         final FieldVector3D<T> intermediateA = pv.getAcceleration().add(cartesian.getAcceleration());

  591.         // apply rotation
  592.         final FieldVector3D<T> transformedP = FieldRotation.applyTo(angular.getRotation(), intermediateP);
  593.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(angular.getRotationRate(), transformedP);
  594.         final FieldVector3D<T> transformedV = FieldRotation.applyTo(angular.getRotation(), intermediateV).subtract(crossP);
  595.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(angular.getRotationRate(), transformedV);
  596.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(angular.getRotationRate(), crossP);
  597.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(angular.getRotationAcceleration(), transformedP);
  598.         final FieldVector3D<T> transformedA =
  599.                 new FieldVector3D<T>( 1, FieldRotation.applyTo(angular.getRotation(), intermediateA),
  600.                                      -2, crossV,
  601.                                      -1, crossCrossP,
  602.                                      -1, crossDotP);

  604.         // build transformed object
  605.         return new FieldPVCoordinates<T>(transformedP, transformedV, transformedA);

  607.     }

  609.     /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
  610.      * <p>
  611.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  612.      * consistency between the transform {@link #getDate() date} and the time-stamped
  613.      * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
  614.      * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
  615.      * the input argument, regardless of the instance {@link #getDate() date}.
  616.      * </p>
  617.      * @param pv time-stamped position-velocity to transform.
  618.      * @param <T> type of the field elements
  619.      * @return transformed time-stamped position-velocity
  620.      * @since 7.0
  621.      */
  622.     public <T extends RealFieldElement<T>> TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedFieldPVCoordinates<T> pv) {

  624.         // apply translation
  625.         final FieldVector3D<T> intermediateP = pv.getPosition().add(cartesian.getPosition());
  626.         final FieldVector3D<T> intermediateV = pv.getVelocity().add(cartesian.getVelocity());
  627.         final FieldVector3D<T> intermediateA = pv.getAcceleration().add(cartesian.getAcceleration());

  629.         // apply rotation
  630.         final FieldVector3D<T> transformedP = FieldRotation.applyTo(angular.getRotation(), intermediateP);
  631.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(angular.getRotationRate(), transformedP);
  632.         final FieldVector3D<T> transformedV = FieldRotation.applyTo(angular.getRotation(), intermediateV).subtract(crossP);
  633.         final FieldVector3D<T> crossV       = FieldVector3D.crossProduct(angular.getRotationRate(), transformedV);
  634.         final FieldVector3D<T> crossCrossP  = FieldVector3D.crossProduct(angular.getRotationRate(), crossP);
  635.         final FieldVector3D<T> crossDotP    = FieldVector3D.crossProduct(angular.getRotationAcceleration(), transformedP);
  636.         final FieldVector3D<T> transformedA =
  637.                 new FieldVector3D<T>( 1, FieldRotation.applyTo(angular.getRotation(), intermediateA),
  638.                                      -2, crossV,
  639.                                      -1, crossCrossP,
  640.                                      -1, crossDotP);

  642.         // build transformed object
  643.         return new TimeStampedFieldPVCoordinates<T>(pv.getDate(), transformedP, transformedV, transformedA);

  645.     }

  647.     /** Compute the Jacobian of the {@link #transformPVCoordinates(PVCoordinates)}
  648.      * method of the transform.
  649.      * <p>
  650.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  651.      * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j
  652.      * of the input {@link PVCoordinates} in method {@link #transformPVCoordinates(PVCoordinates)}.
  653.      * </p>
  654.      * <p>
  655.      * This definition implies that if we define position-velocity coordinates
  656.      * <pre>
  657.      * PV₁ = transform.transformPVCoordinates(PV₀), then
  658.      * </pre>
  659.      * their differentials dPV₁ and dPV₀ will obey the following relation
  660.      * where J is the matrix computed by this method:<br/>
  661.      * <pre>
  662.      * dPV₁ = J &times; dPV₀
  663.      * </pre>
  664.      * </p>
  665.      * @param jacobian placeholder 6x6 (or larger) matrix to be filled with
  666.      * the Jacobian, only the upper left 6x6 corner will be modified
  667.      * @deprecated as of 7.0, replaced with {@link #getJacobian(CartesianDerivativesFilter, double[][])}
  668.      */
  669.     @Deprecated
  670.     public void getJacobian(final double[][] jacobian) {
  671.         getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
  672.     }

  674.     /** Compute the Jacobian of the {@link #transformPVCoordinates(PVCoordinates)}
  675.      * method of the transform.
  676.      * <p>
  677.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  678.      * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j
  679.      * of the input {@link PVCoordinates} in method {@link #transformPVCoordinates(PVCoordinates)}.
  680.      * </p>
  681.      * <p>
  682.      * This definition implies that if we define position-velocity coordinates
  683.      * <pre>
  684.      * PV₁ = transform.transformPVCoordinates(PV₀), then
  685.      * </pre>
  686.      * their differentials dPV₁ and dPV₀ will obey the following relation
  687.      * where J is the matrix computed by this method:<br/>
  688.      * <pre>
  689.      * dPV₁ = J &times; dPV₀
  690.      * </pre>
  691.      * </p>
  692.      * @param selector selector specifying the size of the upper left corner that must be filled
  693.      * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
  694.      * velocities and accelerations)
  695.      * @param jacobian placeholder matrix whose upper-left corner is to be filled with
  696.      * the Jacobian, the rest of the matrix remaining untouched
  697.      */
  698.     public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {

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

  703.         // dP1/dP0
  704.         System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
  705.         System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
  706.         System.arraycopy(mData[2], 0, jacobian[2], 0, 3);

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

  710.             // dP1/dV0
  711.             Arrays.fill(jacobian[0], 3, 6, 0.0);
  712.             Arrays.fill(jacobian[1], 3, 6, 0.0);
  713.             Arrays.fill(jacobian[2], 3, 6, 0.0);

  715.             // dV1/dP0
  716.             final Vector3D o = angular.getRotationRate();
  717.             final double ox = o.getX();
  718.             final double oy = o.getY();
  719.             final double oz = o.getZ();
  720.             for (int i = 0; i < 3; ++i) {
  721.                 jacobian[3][i] = -(oy * mData[2][i] - oz * mData[1][i]);
  722.                 jacobian[4][i] = -(oz * mData[0][i] - ox * mData[2][i]);
  723.                 jacobian[5][i] = -(ox * mData[1][i] - oy * mData[0][i]);
  724.             }

  726.             // dV1/dV0
  727.             System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
  728.             System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
  729.             System.arraycopy(mData[2], 0, jacobian[5], 3, 3);

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

  733.                 // dP1/dA0
  734.                 Arrays.fill(jacobian[0], 6, 9, 0.0);
  735.                 Arrays.fill(jacobian[1], 6, 9, 0.0);
  736.                 Arrays.fill(jacobian[2], 6, 9, 0.0);

  738.                 // dV1/dA0
  739.                 Arrays.fill(jacobian[3], 6, 9, 0.0);
  740.                 Arrays.fill(jacobian[4], 6, 9, 0.0);
  741.                 Arrays.fill(jacobian[5], 6, 9, 0.0);

  743.                 // dA1/dP0
  744.                 final Vector3D oDot = angular.getRotationAcceleration();
  745.                 final double oDotx  = oDot.getX();
  746.                 final double oDoty  = oDot.getY();
  747.                 final double oDotz  = oDot.getZ();
  748.                 for (int i = 0; i < 3; ++i) {
  749.                     jacobian[6][i] = -(oDoty * mData[2][i] - oDotz * mData[1][i]) - (oy * jacobian[5][i] - oz * jacobian[4][i]);
  750.                     jacobian[7][i] = -(oDotz * mData[0][i] - oDotx * mData[2][i]) - (oz * jacobian[3][i] - ox * jacobian[5][i]);
  751.                     jacobian[8][i] = -(oDotx * mData[1][i] - oDoty * mData[0][i]) - (ox * jacobian[4][i] - oy * jacobian[3][i]);
  752.                 }

  754.                 // dA1/dV0
  755.                 for (int i = 0; i < 3; ++i) {
  756.                     jacobian[6][i + 3] = -2 * (oy * mData[2][i] - oz * mData[1][i]);
  757.                     jacobian[7][i + 3] = -2 * (oz * mData[0][i] - ox * mData[2][i]);
  758.                     jacobian[8][i + 3] = -2 * (ox * mData[1][i] - oy * mData[0][i]);
  759.                 }

  761.                 // dA1/dA0
  762.                 System.arraycopy(mData[0], 0, jacobian[6], 6, 3);
  763.                 System.arraycopy(mData[1], 0, jacobian[7], 6, 3);
  764.                 System.arraycopy(mData[2], 0, jacobian[8], 6, 3);

  766.             }

  768.         }

  770.     }

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

  784.     /** Get the underlying elementary translation.
  785.      * <p>A transform can be uniquely represented as an elementary
  786.      * translation followed by an elementary rotation. This method
  787.      * returns this unique elementary translation.</p>
  788.      * @return underlying elementary translation
  789.      * @see #getCartesian()
  790.      * @see #getVelocity()
  791.      * @see #getAcceleration()
  792.      */
  793.     public Vector3D getTranslation() {
  794.         return cartesian.getPosition();
  795.     }

  797.     /** Get the first time derivative of the translation.
  798.      * @return first time derivative of the translation
  799.      * @see #getCartesian()
  800.      * @see #getTranslation()
  801.      * @see #getAcceleration()
  802.      */
  803.     public Vector3D getVelocity() {
  804.         return cartesian.getVelocity();
  805.     }

  807.     /** Get the second time derivative of the translation.
  808.      * @return second time derivative of the translation
  809.      * @see #getCartesian()
  810.      * @see #getTranslation()
  811.      * @see #getVelocity()
  812.      */
  813.     public Vector3D getAcceleration() {
  814.         return cartesian.getAcceleration();
  815.     }

  817.     /** Get the underlying elementary angular part.
  818.      * <p>A transform can be uniquely represented as an elementary
  819.      * translation followed by an elementary rotation. This method
  820.      * returns this unique elementary rotation with its derivative.</p>
  821.      * @return underlying elementary angular part
  822.      * @see #getRotation()
  823.      * @see #getRotationRate()
  824.      * @see #getRotationAcceleration()
  825.      */
  826.     public AngularCoordinates getAngular() {
  827.         return angular;
  828.     }

  830.     /** Get the underlying elementary rotation.
  831.      * <p>A transform can be uniquely represented as an elementary
  832.      * translation followed by an elementary rotation. This method
  833.      * returns this unique elementary rotation.</p>
  834.      * @return underlying elementary rotation
  835.      * @see #getAngular()
  836.      * @see #getRotationRate()
  837.      * @see #getRotationAcceleration()
  838.      */
  839.     public Rotation getRotation() {
  840.         return angular.getRotation();
  841.     }

  843.     /** Get the first time derivative of the rotation.
  844.      * <p>The norm represents the angular rate.</p>
  845.      * @return First time derivative of the rotation
  846.      * @see #getAngular()
  847.      * @see #getRotation()
  848.      * @see #getRotationAcceleration()
  849.      */
  850.     public Vector3D getRotationRate() {
  851.         return angular.getRotationRate();
  852.     }

  854.     /** Get the second time derivative of the rotation.
  855.      * @return Second time derivative of the rotation
  856.      * @see #getAngular()
  857.      * @see #getRotation()
  858.      * @see #getRotationRate()
  859.      */
  860.     public Vector3D getRotationAcceleration() {
  861.         return angular.getRotationAcceleration();
  862.     }

  864.     /** Specialized class for identity transform. */
  865.     private static class IdentityTransform extends Transform {

  867.         /** Serializable UID. */
  868.         private static final long serialVersionUID = -9042082036141830517L;

  870.         /** Simple constructor. */
  871.         IdentityTransform() {
  872.             super(AbsoluteDate.J2000_EPOCH, PVCoordinates.ZERO, AngularCoordinates.IDENTITY);
  873.         }

  875.         /** {@inheritDoc} */
  876.         @Override
  877.         public Transform shiftedBy(final double dt) {
  878.             return this;
  879.         }

  881.         /** {@inheritDoc} */
  882.         @Override
  883.         public Transform getInverse() {
  884.             return this;
  885.         };

  887.         /** {@inheritDoc} */
  888.         @Override
  889.         public Vector3D transformPosition(final Vector3D position) {
  890.             return position;
  891.         }

  893.         /** {@inheritDoc} */
  894.         @Override
  895.         public Vector3D transformVector(final Vector3D vector) {
  896.             return vector;
  897.         }

  899.         /** {@inheritDoc} */
  900.         @Override
  901.         public Line transformLine(final Line line) {
  902.             return line;
  903.         }

  905.         /** {@inheritDoc} */
  906.         @Override
  907.         public PVCoordinates transformPVCoordinates(final PVCoordinates pv) {
  908.             return pv;
  909.         }

  911.         /** {@inheritDoc} */
  912.         @Override
  913.         public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {
  914.             final int n = 3 * (selector.getMaxOrder() + 1);
  915.             for (int i = 0; i < n; ++i) {
  916.                 Arrays.fill(jacobian[i], 0, n, 0.0);
  917.                 jacobian[i][i] = 1.0;
  918.             }
  919.         }

  921.     }

  923. }
Created with JaCoCo 0.7.4.201502262128