FieldPVCoordinates.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.utils;

  19. import org.hipparchus.Field;
  20. import org.hipparchus.RealFieldElement;
  21. import org.hipparchus.analysis.differentiation.FDSFactory;
  22. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.orekit.errors.OrekitException;
  25. import org.orekit.errors.OrekitMessages;
  26. import org.orekit.time.TimeShiftable;

  28. /** Simple container for Position/Velocity pairs, using {@link RealFieldElement}.
  29.  * <p>
  30.  * The state can be slightly shifted to close dates. This shift is based on
  31.  * a simple linear model. It is <em>not</em> intended as a replacement for
  32.  * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  33.  * for either small time shifts or coarse accuracy.
  34.  * </p>
  35.  * <p>
  36.  * This class is the angular counterpart to {@link FieldAngularCoordinates}.
  37.  * </p>
  38.  * <p>Instances of this class are guaranteed to be immutable.</p>
  39.  * @param <T> the type of the field elements
  40.  * @author Luc Maisonobe
  41.  * @since 6.0
  42.  * @see PVCoordinates
  43.  */
  44. public class FieldPVCoordinates<T extends RealFieldElement<T>>
  45.     implements TimeShiftable<FieldPVCoordinates<T>> {

  47.     /** The position. */
  48.     private final FieldVector3D<T> position;

  50.     /** The velocity. */
  51.     private final FieldVector3D<T> velocity;

  53.     /** The acceleration. */
  54.     private final FieldVector3D<T> acceleration;

  56.     /** Builds a FieldPVCoordinates triplet with zero acceleration.
  57.      * @param position the position vector (m)
  58.      * @param velocity the velocity vector (m/s)
  59.      */
  60.     public FieldPVCoordinates(final FieldVector3D<T> position, final FieldVector3D<T> velocity) {
  61.         this.position     = position;
  62.         this.velocity     = velocity;
  63.         final T zero      = position.getX().getField().getZero();
  64.         this.acceleration = new FieldVector3D<>(zero, zero, zero);
  65.     }

  67.     /** Builds a FieldPVCoordinates triplet.
  68.      * @param position the position vector (m)
  69.      * @param velocity the velocity vector (m/s)
  70.      * @param acceleration the acceleration vector (m/s²)
  71.      */
  72.     public FieldPVCoordinates(final FieldVector3D<T> position, final FieldVector3D<T> velocity,
  73.                               final FieldVector3D<T> acceleration) {
  74.         this.position     = position;
  75.         this.velocity     = velocity;
  76.         this.acceleration = acceleration;
  77.     }

  79.     /** Builds a FieldPVCoordinates from a field and a regular PVCoordinates.
  80.      * @param field field for the components
  81.      * @param pv PVCoordinates triplet to convert
  82.      */
  83.     public FieldPVCoordinates(final Field<T> field, final PVCoordinates pv) {
  84.         this.position     = new FieldVector3D<>(field, pv.getPosition());
  85.         this.velocity     = new FieldVector3D<>(field, pv.getVelocity());
  86.         this.acceleration = new FieldVector3D<>(field, pv.getAcceleration());
  87.     }

  89.     /** Multiplicative constructor.
  90.      * <p>Build a PVCoordinates from another one and a scale factor.</p>
  91.      * <p>The PVCoordinates built will be a * pv</p>
  92.      * @param a scale factor
  93.      * @param pv base (unscaled) PVCoordinates
  94.      */
  95.     public FieldPVCoordinates(final double a, final FieldPVCoordinates<T> pv) {
  96.         position     = new FieldVector3D<>(a, pv.position);
  97.         velocity     = new FieldVector3D<>(a, pv.velocity);
  98.         acceleration = new FieldVector3D<>(a, pv.acceleration);
  99.     }

  101.     /** Multiplicative constructor.
  102.      * <p>Build a PVCoordinates from another one and a scale factor.</p>
  103.      * <p>The PVCoordinates built will be a * pv</p>
  104.      * @param a scale factor
  105.      * @param pv base (unscaled) PVCoordinates
  106.      */
  107.     public FieldPVCoordinates(final T a, final FieldPVCoordinates<T> pv) {
  108.         position     = new FieldVector3D<>(a, pv.position);
  109.         velocity     = new FieldVector3D<>(a, pv.velocity);
  110.         acceleration = new FieldVector3D<>(a, pv.acceleration);
  111.     }

  113.     /** Multiplicative constructor.
  114.      * <p>Build a PVCoordinates from another one and a scale factor.</p>
  115.      * <p>The PVCoordinates built will be a * pv</p>
  116.      * @param a scale factor
  117.      * @param pv base (unscaled) PVCoordinates
  118.      */
  119.     public FieldPVCoordinates(final T a, final PVCoordinates pv) {
  120.         position     = new FieldVector3D<>(a, pv.getPosition());
  121.         velocity     = new FieldVector3D<>(a, pv.getVelocity());
  122.         acceleration = new FieldVector3D<>(a, pv.getAcceleration());
  123.     }

  125.     /** Subtractive constructor.
  126.      * <p>Build a relative PVCoordinates from a start and an end position.</p>
  127.      * <p>The PVCoordinates built will be end - start.</p>
  128.      * @param start Starting PVCoordinates
  129.      * @param end ending PVCoordinates
  130.      */
  131.     public FieldPVCoordinates(final FieldPVCoordinates<T> start, final FieldPVCoordinates<T> end) {
  132.         this.position     = end.position.subtract(start.position);
  133.         this.velocity     = end.velocity.subtract(start.velocity);
  134.         this.acceleration = end.acceleration.subtract(start.acceleration);
  135.     }

  137.     /** Linear constructor.
  138.      * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
  139.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
  140.      * @param a1 first scale factor
  141.      * @param pv1 first base (unscaled) PVCoordinates
  142.      * @param a2 second scale factor
  143.      * @param pv2 second base (unscaled) PVCoordinates
  144.      */
  145.     public FieldPVCoordinates(final double a1, final FieldPVCoordinates<T> pv1,
  146.                               final double a2, final FieldPVCoordinates<T> pv2) {
  147.         position     = new FieldVector3D<>(a1, pv1.position, a2, pv2.position);
  148.         velocity     = new FieldVector3D<>(a1, pv1.velocity, a2, pv2.velocity);
  149.         acceleration = new FieldVector3D<>(a1, pv1.acceleration, a2, pv2.acceleration);
  150.     }

  152.     /** Linear constructor.
  153.      * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
  154.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
  155.      * @param a1 first scale factor
  156.      * @param pv1 first base (unscaled) PVCoordinates
  157.      * @param a2 second scale factor
  158.      * @param pv2 second base (unscaled) PVCoordinates
  159.      */
  160.     public FieldPVCoordinates(final T a1, final FieldPVCoordinates<T> pv1,
  161.                               final T a2, final FieldPVCoordinates<T> pv2) {
  162.         position     = new FieldVector3D<>(a1, pv1.position, a2, pv2.position);
  163.         velocity     = new FieldVector3D<>(a1, pv1.velocity, a2, pv2.velocity);
  164.         acceleration = new FieldVector3D<>(a1, pv1.acceleration, a2, pv2.acceleration);
  165.     }

  167.     /** Linear constructor.
  168.      * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
  169.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
  170.      * @param a1 first scale factor
  171.      * @param pv1 first base (unscaled) PVCoordinates
  172.      * @param a2 second scale factor
  173.      * @param pv2 second base (unscaled) PVCoordinates
  174.      */
  175.     public FieldPVCoordinates(final T a1, final PVCoordinates pv1,
  176.                               final T a2, final PVCoordinates pv2) {
  177.         position     = new FieldVector3D<>(a1, pv1.getPosition(), a2, pv2.getPosition());
  178.         velocity     = new FieldVector3D<>(a1, pv1.getVelocity(), a2, pv2.getVelocity());
  179.         acceleration = new FieldVector3D<>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration());
  180.     }

  182.     /** Linear constructor.
  183.      * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
  184.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  185.      * @param a1 first scale factor
  186.      * @param pv1 first base (unscaled) PVCoordinates
  187.      * @param a2 second scale factor
  188.      * @param pv2 second base (unscaled) PVCoordinates
  189.      * @param a3 third scale factor
  190.      * @param pv3 third base (unscaled) PVCoordinates
  191.      */
  192.     public FieldPVCoordinates(final double a1, final FieldPVCoordinates<T> pv1,
  193.                               final double a2, final FieldPVCoordinates<T> pv2,
  194.                               final double a3, final FieldPVCoordinates<T> pv3) {
  195.         position     = new FieldVector3D<>(a1, pv1.position,     a2, pv2.position,     a3, pv3.position);
  196.         velocity     = new FieldVector3D<>(a1, pv1.velocity,     a2, pv2.velocity,     a3, pv3.velocity);
  197.         acceleration = new FieldVector3D<>(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration);
  198.     }

  200.     /** Linear constructor.
  201.      * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
  202.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  203.      * @param a1 first scale factor
  204.      * @param pv1 first base (unscaled) PVCoordinates
  205.      * @param a2 second scale factor
  206.      * @param pv2 second base (unscaled) PVCoordinates
  207.      * @param a3 third scale factor
  208.      * @param pv3 third base (unscaled) PVCoordinates
  209.      */
  210.     public FieldPVCoordinates(final T a1, final FieldPVCoordinates<T> pv1,
  211.                               final T a2, final FieldPVCoordinates<T> pv2,
  212.                               final T a3, final FieldPVCoordinates<T> pv3) {
  213.         position     = new FieldVector3D<>(a1, pv1.position,     a2, pv2.position,     a3, pv3.position);
  214.         velocity     = new FieldVector3D<>(a1, pv1.velocity,     a2, pv2.velocity,     a3, pv3.velocity);
  215.         acceleration = new FieldVector3D<>(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration);
  216.     }

  218.     /** Linear constructor.
  219.      * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
  220.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
  221.      * @param a1 first scale factor
  222.      * @param pv1 first base (unscaled) PVCoordinates
  223.      * @param a2 second scale factor
  224.      * @param pv2 second base (unscaled) PVCoordinates
  225.      * @param a3 third scale factor
  226.      * @param pv3 third base (unscaled) PVCoordinates
  227.      */
  228.     public FieldPVCoordinates(final T a1, final PVCoordinates pv1,
  229.                               final T a2, final PVCoordinates pv2,
  230.                               final T a3, final PVCoordinates pv3) {
  231.         position     = new FieldVector3D<>(a1, pv1.getPosition(),     a2, pv2.getPosition(),     a3, pv3.getPosition());
  232.         velocity     = new FieldVector3D<>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),     a3, pv3.getVelocity());
  233.         acceleration = new FieldVector3D<>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(), a3, pv3.getAcceleration());
  234.     }

  236.     /** Linear constructor.
  237.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  238.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  239.      * @param a1 first scale factor
  240.      * @param pv1 first base (unscaled) PVCoordinates
  241.      * @param a2 second scale factor
  242.      * @param pv2 second base (unscaled) PVCoordinates
  243.      * @param a3 third scale factor
  244.      * @param pv3 third base (unscaled) PVCoordinates
  245.      * @param a4 fourth scale factor
  246.      * @param pv4 fourth base (unscaled) PVCoordinates
  247.      */
  248.     public FieldPVCoordinates(final double a1, final FieldPVCoordinates<T> pv1,
  249.                               final double a2, final FieldPVCoordinates<T> pv2,
  250.                               final double a3, final FieldPVCoordinates<T> pv3,
  251.                               final double a4, final FieldPVCoordinates<T> pv4) {
  252.         position     = new FieldVector3D<>(a1, pv1.position,     a2, pv2.position,     a3, pv3.position,     a4, pv4.position);
  253.         velocity     = new FieldVector3D<>(a1, pv1.velocity,     a2, pv2.velocity,     a3, pv3.velocity,     a4, pv4.velocity);
  254.         acceleration = new FieldVector3D<>(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration, a4, pv4.acceleration);
  255.     }

  257.     /** Linear constructor.
  258.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  259.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  260.      * @param a1 first scale factor
  261.      * @param pv1 first base (unscaled) PVCoordinates
  262.      * @param a2 second scale factor
  263.      * @param pv2 second base (unscaled) PVCoordinates
  264.      * @param a3 third scale factor
  265.      * @param pv3 third base (unscaled) PVCoordinates
  266.      * @param a4 fourth scale factor
  267.      * @param pv4 fourth base (unscaled) PVCoordinates
  268.      */
  269.     public FieldPVCoordinates(final T a1, final FieldPVCoordinates<T> pv1,
  270.                               final T a2, final FieldPVCoordinates<T> pv2,
  271.                               final T a3, final FieldPVCoordinates<T> pv3,
  272.                               final T a4, final FieldPVCoordinates<T> pv4) {
  273.         position     = new FieldVector3D<>(a1, pv1.position,     a2, pv2.position,     a3, pv3.position,     a4, pv4.position);
  274.         velocity     = new FieldVector3D<>(a1, pv1.velocity,     a2, pv2.velocity,     a3, pv3.velocity,     a4, pv4.velocity);
  275.         acceleration = new FieldVector3D<>(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration, a4, pv4.acceleration);
  276.     }

  278.     /** Linear constructor.
  279.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  280.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  281.      * @param a1 first scale factor
  282.      * @param pv1 first base (unscaled) PVCoordinates
  283.      * @param a2 second scale factor
  284.      * @param pv2 second base (unscaled) PVCoordinates
  285.      * @param a3 third scale factor
  286.      * @param pv3 third base (unscaled) PVCoordinates
  287.      * @param a4 fourth scale factor
  288.      * @param pv4 fourth base (unscaled) PVCoordinates
  289.      */
  290.     public FieldPVCoordinates(final T a1, final PVCoordinates pv1,
  291.                               final T a2, final PVCoordinates pv2,
  292.                               final T a3, final PVCoordinates pv3,
  293.                               final T a4, final PVCoordinates pv4) {
  294.         position     = new FieldVector3D<>(a1, pv1.getPosition(),     a2, pv2.getPosition(),
  295.                                            a3, pv3.getPosition(),     a4, pv4.getPosition());
  296.         velocity     = new FieldVector3D<>(a1, pv1.getVelocity(),     a2, pv2.getVelocity(),
  297.                                            a3, pv3.getVelocity(),     a4, pv4.getVelocity());
  298.         acceleration = new FieldVector3D<>(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(),
  299.                                            a3, pv3.getAcceleration(), a4, pv4.getAcceleration());
  300.     }

  302.     /** Builds a FieldPVCoordinates triplet from  a {@link FieldVector3D}&lt;{@link FieldDerivativeStructure}&gt;.
  303.      * <p>
  304.      * The vector components must have time as their only derivation parameter and
  305.      * have consistent derivation orders.
  306.      * </p>
  307.      * @param p vector with time-derivatives embedded within the coordinates
  308.      * @since 9.2
  309.      */
  310.     public FieldPVCoordinates(final FieldVector3D<FieldDerivativeStructure<T>> p) {
  311.         position = new FieldVector3D<>(p.getX().getValue(), p.getY().getValue(), p.getZ().getValue());
  312.         if (p.getX().getOrder() >= 1) {
  313.             velocity = new FieldVector3D<>(p.getX().getPartialDerivative(1),
  314.                                            p.getY().getPartialDerivative(1),
  315.                                            p.getZ().getPartialDerivative(1));
  316.             if (p.getX().getOrder() >= 2) {
  317.                 acceleration = new FieldVector3D<>(p.getX().getPartialDerivative(2),
  318.                                                    p.getY().getPartialDerivative(2),
  319.                                                    p.getZ().getPartialDerivative(2));
  320.             } else {
  321.                 acceleration = FieldVector3D.getZero(position.getX().getField());
  322.             }
  323.         } else {
  324.             final FieldVector3D<T> zero = FieldVector3D.getZero(position.getX().getField());
  325.             velocity     = zero;
  326.             acceleration = zero;
  327.         }
  328.     }

  330.     /** Get fixed position/velocity at origin (both p, v and a are zero vectors).
  331.      * @param field field for the components
  332.      * @param <T> the type of the field elements
  333.      * @return a new fixed position/velocity at origin
  334.      */
  335.     public static <T extends RealFieldElement<T>> FieldPVCoordinates<T> getZero(final Field<T> field) {
  336.         return new FieldPVCoordinates<>(field, PVCoordinates.ZERO);
  337.     }

  339.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link FieldDerivativeStructure}&gt;.
  340.      * <p>
  341.      * The {@link FieldDerivativeStructure} coordinates correspond to time-derivatives up
  342.      * to the user-specified order.
  343.      * </p>
  344.      * @param order derivation order for the vector components (must be either 0, 1 or 2)
  345.      * @return vector with time-derivatives embedded within the coordinates
  346.      * @exception OrekitException if the user specified order is too large
  347.      * @since 9.2
  348.      */
  349.     public FieldVector3D<FieldDerivativeStructure<T>> toDerivativeStructureVector(final int order)
  350.         throws OrekitException {

  352.         final FDSFactory<T> factory;
  353.         final FieldDerivativeStructure<T> x;
  354.         final FieldDerivativeStructure<T> y;
  355.         final FieldDerivativeStructure<T> z;
  356.         switch(order) {
  357.             case 0 :
  358.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  359.                 x = factory.build(position.getX());
  360.                 y = factory.build(position.getY());
  361.                 z = factory.build(position.getZ());
  362.                 break;
  363.             case 1 :
  364.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  365.                 x = factory.build(position.getX(), velocity.getX());
  366.                 y = factory.build(position.getY(), velocity.getY());
  367.                 z = factory.build(position.getZ(), velocity.getZ());
  368.                 break;
  369.             case 2 :
  370.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  371.                 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
  372.                 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
  373.                 z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
  374.                 break;
  375.             default :
  376.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  377.         }

  379.         return new FieldVector3D<>(x, y, z);

  381.     }

  383.     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link FieldDerivativeStructure}&gt;.
  384.      * <p>
  385.      * The {@link FieldDerivativeStructure} coordinates correspond to time-derivatives up
  386.      * to the user-specified order. As both the instance components {@link #getPosition() position},
  387.      * {@link #getVelocity() velocity} and {@link #getAcceleration() acceleration} and the
  388.      * {@link FieldDerivativeStructure#getPartialDerivative(int...) derivatives} of the components
  389.      * holds time-derivatives, there are several ways to retrieve these derivatives. If for example
  390.      * the {@code order} is set to 2, then both {@code pv.getPosition().getX().getPartialDerivative(2)},
  391.      * {@code pv.getVelocity().getX().getPartialDerivative(1)} and
  392.      * {@code pv.getAcceleration().getX().getValue()} return the exact same value.
  393.      * </p>
  394.      * <p>
  395.      * If derivation order is 1, the first derivative of acceleration will be computed as a
  396.      * Keplerian-only jerk. If derivation order is 2, the second derivative of velocity (which
  397.      * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
  398.      * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
  399.      * </p>
  400.      * @param order derivation order for the vector components (must be either 0, 1 or 2)
  401.      * @return pv coordinates with time-derivatives embedded within the coordinates
  402.      * @exception OrekitException if the user specified order is too large
  403.      * @since 9.2
  404.      */
  405.     public FieldPVCoordinates<FieldDerivativeStructure<T>> toDerivativeStructurePV(final int order)
  406.         throws OrekitException {

  408.         final FDSFactory<T> factory;
  409.         final FieldDerivativeStructure<T> x0;
  410.         final FieldDerivativeStructure<T> y0;
  411.         final FieldDerivativeStructure<T> z0;
  412.         final FieldDerivativeStructure<T> x1;
  413.         final FieldDerivativeStructure<T> y1;
  414.         final FieldDerivativeStructure<T> z1;
  415.         final FieldDerivativeStructure<T> x2;
  416.         final FieldDerivativeStructure<T> y2;
  417.         final FieldDerivativeStructure<T> z2;
  418.         switch(order) {
  419.             case 0 :
  420.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  421.                 x0 = factory.build(position.getX());
  422.                 y0 = factory.build(position.getY());
  423.                 z0 = factory.build(position.getZ());
  424.                 x1 = factory.build(velocity.getX());
  425.                 y1 = factory.build(velocity.getY());
  426.                 z1 = factory.build(velocity.getZ());
  427.                 x2 = factory.build(acceleration.getX());
  428.                 y2 = factory.build(acceleration.getY());
  429.                 z2 = factory.build(acceleration.getZ());
  430.                 break;
  431.             case 1 : {
  432.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  433.                 final T                r2            = position.getNormSq();
  434.                 final T                r             = r2.sqrt();
  435.                 final T                pvOr2         = FieldVector3D.dotProduct(position, velocity).divide(r2);
  436.                 final T                a             = acceleration.getNorm();
  437.                 final T                aOr           = a.divide(r);
  438.                 final FieldVector3D<T> keplerianJerk = new FieldVector3D<>(pvOr2.multiply(-3), acceleration,
  439.                                                                            aOr.negate(), velocity);
  440.                 x0 = factory.build(position.getX(),     velocity.getX());
  441.                 y0 = factory.build(position.getY(),     velocity.getY());
  442.                 z0 = factory.build(position.getZ(),     velocity.getZ());
  443.                 x1 = factory.build(velocity.getX(),     acceleration.getX());
  444.                 y1 = factory.build(velocity.getY(),     acceleration.getY());
  445.                 z1 = factory.build(velocity.getZ(),     acceleration.getZ());
  446.                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
  447.                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
  448.                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
  449.                 break;
  450.             }
  451.             case 2 : {
  452.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  453.                 final T                r2              = position.getNormSq();
  454.                 final T                r               = r2.sqrt();
  455.                 final T                pvOr2           = FieldVector3D.dotProduct(position, velocity).divide(r2);
  456.                 final T                a               = acceleration.getNorm();
  457.                 final T                aOr             = a.divide(r);
  458.                 final FieldVector3D<T> keplerianJerk   = new FieldVector3D<>(pvOr2.multiply(-3), acceleration,
  459.                                                                              aOr.negate(), velocity);
  460.                 final T                v2              = velocity.getNormSq();
  461.                 final T                pa              = FieldVector3D.dotProduct(position, acceleration);
  462.                 final T                aj              = FieldVector3D.dotProduct(acceleration, keplerianJerk);
  463.                 final FieldVector3D<T> keplerianJounce = new FieldVector3D<>(v2.add(pa).multiply(-3).divide(r2).add(pvOr2.multiply(pvOr2).multiply(15)).subtract(aOr), acceleration,
  464.                                                                              aOr.multiply(4).multiply(pvOr2).subtract(aj.divide(a.multiply(r))), velocity);
  465.                 x0 = factory.build(position.getX(),     velocity.getX(),      acceleration.getX());
  466.                 y0 = factory.build(position.getY(),     velocity.getY(),      acceleration.getY());
  467.                 z0 = factory.build(position.getZ(),     velocity.getZ(),      acceleration.getZ());
  468.                 x1 = factory.build(velocity.getX(),     acceleration.getX(),  keplerianJerk.getX());
  469.                 y1 = factory.build(velocity.getY(),     acceleration.getY(),  keplerianJerk.getY());
  470.                 z1 = factory.build(velocity.getZ(),     acceleration.getZ(),  keplerianJerk.getZ());
  471.                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
  472.                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
  473.                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
  474.                 break;
  475.             }
  476.             default :
  477.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  478.         }

  480.         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
  481.                                         new FieldVector3D<>(x1, y1, z1),
  482.                                         new FieldVector3D<>(x2, y2, z2));

  484.     }

  486.     /** Estimate velocity between two positions.
  487.      * <p>Estimation is based on a simple fixed velocity translation
  488.      * during the time interval between the two positions.</p>
  489.      * @param start start position
  490.      * @param end end position
  491.      * @param dt time elapsed between the dates of the two positions
  492.      * @param <T> the type of the field elements
  493.      * @return velocity allowing to go from start to end positions
  494.      */
  495.     public static <T extends RealFieldElement<T>> FieldVector3D<T> estimateVelocity(final FieldVector3D<T> start,
  496.                                                                                     final FieldVector3D<T> end,
  497.                                                                                     final double dt) {
  498.         final double scale = 1.0 / dt;
  499.         return new FieldVector3D<>(scale, end, -scale, start);
  500.     }

  502.     /** Get a time-shifted state.
  503.      * <p>
  504.      * The state can be slightly shifted to close dates. This shift is based on
  505.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  506.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  507.      * for either small time shifts or coarse accuracy.
  508.      * </p>
  509.      * @param dt time shift in seconds
  510.      * @return a new state, shifted with respect to the instance (which is immutable)
  511.      */
  512.     public FieldPVCoordinates<T> shiftedBy(final double dt) {
  513.         return new FieldPVCoordinates<>(new FieldVector3D<>(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
  514.                                         new FieldVector3D<>(1, velocity, dt, acceleration),
  515.                                         acceleration);
  516.     }

  518.     /** Get a time-shifted state.
  519.      * <p>
  520.      * The state can be slightly shifted to close dates. This shift is based on
  521.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  522.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  523.      * for either small time shifts or coarse accuracy.
  524.      * </p>
  525.      * @param dt time shift in seconds
  526.      * @return a new state, shifted with respect to the instance (which is immutable)
  527.      */
  528.     public FieldPVCoordinates<T> shiftedBy(final T dt) {
  529.         final T one = dt.getField().getOne();
  530.         return new FieldPVCoordinates<>(new FieldVector3D<>(one, position,
  531.                                                             dt, velocity,
  532.                                                             dt.multiply(dt).multiply(0.5), acceleration),
  533.                                         new FieldVector3D<>(one, velocity,
  534.                                                             dt, acceleration),
  535.                                         acceleration);
  536.     }

  538.     /** Gets the position.
  539.      * @return the position vector (m).
  540.      */
  541.     public FieldVector3D<T> getPosition() {
  542.         return position;
  543.     }

  545.     /** Gets the velocity.
  546.      * @return the velocity vector (m/s).
  547.      */
  548.     public FieldVector3D<T> getVelocity() {
  549.         return velocity;
  550.     }

  552.     /** Gets the acceleration.
  553.      * @return the acceleration vector (m/s²).
  554.      */
  555.     public FieldVector3D<T> getAcceleration() {
  556.         return acceleration;
  557.     }

  559.     /** Gets the momentum.
  560.      * <p>This vector is the p &otimes; v where p is position, v is velocity
  561.      * and &otimes; is cross product. To get the real physical angular momentum
  562.      * you need to multiply this vector by the mass.</p>
  563.      * <p>The returned vector is recomputed each time this method is called, it
  564.      * is not cached.</p>
  565.      * @return a new instance of the momentum vector (m²/s).
  566.      */
  567.     public FieldVector3D<T> getMomentum() {
  568.         return FieldVector3D.crossProduct(position, velocity);
  569.     }

  571.     /**
  572.      * Get the angular velocity (spin) of this point as seen from the origin.
  573.      *
  574.      * <p> The angular velocity vector is parallel to the {@link #getMomentum()
  575.      * angular * momentum} and is computed by ω = p &times; v / ||p||²
  576.      *
  577.      * @return the angular velocity vector
  578.      * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
  579.      *      Wikipedia</a>
  580.      */
  581.     public FieldVector3D<T> getAngularVelocity() {
  582.         return this.getMomentum().scalarMultiply(
  583.                 this.getPosition().getNormSq().reciprocal());
  584.     }

  586.     /** Get the opposite of the instance.
  587.      * @return a new position-velocity which is opposite to the instance
  588.      */
  589.     public FieldPVCoordinates<T> negate() {
  590.         return new FieldPVCoordinates<>(position.negate(), velocity.negate(), acceleration.negate());
  591.     }

  593.     /** Normalize the position part of the instance.
  594.      * <p>
  595.      * The computed coordinates first component (position) will be a
  596.      * normalized vector, the second component (velocity) will be the
  597.      * derivative of the first component (hence it will generally not
  598.      * be normalized), and the third component (acceleration) will be the
  599.      * derivative of the second component (hence it will generally not
  600.      * be normalized).
  601.      * </p>
  602.      * @return a new instance, with first component normalized and
  603.      * remaining component computed to have consistent derivatives
  604.      */
  605.     public FieldPVCoordinates<T> normalize() {
  606.         final T   inv     = position.getNorm().reciprocal();
  607.         final FieldVector3D<T> u       = new FieldVector3D<>(inv, position);
  608.         final FieldVector3D<T> v       = new FieldVector3D<>(inv, velocity);
  609.         final FieldVector3D<T> w       = new FieldVector3D<>(inv, acceleration);
  610.         final T   uv      = FieldVector3D.dotProduct(u, v);
  611.         final T   v2      = FieldVector3D.dotProduct(v, v);
  612.         final T   uw      = FieldVector3D.dotProduct(u, w);
  613.         final FieldVector3D<T> uDot    = new FieldVector3D<>(inv.getField().getOne(), v,
  614.                                                              uv.multiply(-1), u);
  615.         final FieldVector3D<T> uDotDot = new FieldVector3D<>(inv.getField().getOne(), w,
  616.                                                              uv.multiply(-2), v,
  617.                                                              uv.multiply(uv).multiply(3).subtract(v2).subtract(uw), u);
  618.         return new FieldPVCoordinates<>(u, uDot, uDotDot);
  619.     }

  621.     /** Compute the cross-product of two instances.
  622.      * @param pv2 second instances
  623.      * @return the cross product v1 ^ v2 as a new instance
  624.      */
  625.     public FieldPVCoordinates<T> crossProduct(final FieldPVCoordinates<T> pv2) {
  626.         final FieldVector3D<T> p1 = position;
  627.         final FieldVector3D<T> v1 = velocity;
  628.         final FieldVector3D<T> a1 = acceleration;
  629.         final FieldVector3D<T> p2 = pv2.position;
  630.         final FieldVector3D<T> v2 = pv2.velocity;
  631.         final FieldVector3D<T> a2 = pv2.acceleration;
  632.         return new FieldPVCoordinates<>(FieldVector3D.crossProduct(p1, p2),
  633.                                         new FieldVector3D<>(1, FieldVector3D.crossProduct(p1, v2),
  634.                                                             1, FieldVector3D.crossProduct(v1, p2)),
  635.                                         new FieldVector3D<>(1, FieldVector3D.crossProduct(p1, a2),
  636.                                                             2, FieldVector3D.crossProduct(v1, v2),
  637.                                                             1, FieldVector3D.crossProduct(a1, p2)));
  638.     }

  640.     /** Convert to a constant position-velocity.
  641.      * @return a constant position-velocity
  642.      */
  643.     public PVCoordinates toPVCoordinates() {
  644.         return new PVCoordinates(position.toVector3D(), velocity.toVector3D(), acceleration.toVector3D());
  645.     }

  647.     /** Return a string representation of this position/velocity pair.
  648.      * @return string representation of this position/velocity pair
  649.      */
  650.     public String toString() {
  651.         final String comma = ", ";
  652.         return new StringBuffer().append('{').append("P(").
  653.                                   append(position.getX().getReal()).append(comma).
  654.                                   append(position.getY().getReal()).append(comma).
  655.                                   append(position.getZ().getReal()).append("), V(").
  656.                                   append(velocity.getX().getReal()).append(comma).
  657.                                   append(velocity.getY().getReal()).append(comma).
  658.                                   append(velocity.getZ().getReal()).append("), A(").
  659.                                   append(acceleration.getX().getReal()).append(comma).
  660.                                   append(acceleration.getY().getReal()).append(comma).
  661.                                   append(acceleration.getZ().getReal()).append(")}").toString();
  662.     }

  664. }
Created with JaCoCo 0.7.9.201702052155