FieldPVCoordinates.java

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

  19. import org.hipparchus.Field;
  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.analysis.differentiation.FDSFactory;
  22. import org.hipparchus.analysis.differentiation.FieldDerivative;
  23. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  24. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
  25. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  26. import org.hipparchus.exception.MathIllegalArgumentException;
  27. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  28. import org.hipparchus.util.FastMath;
  29. import org.hipparchus.util.FieldBlendable;
  30. import org.orekit.errors.OrekitException;
  31. import org.orekit.errors.OrekitMessages;
  32. import org.orekit.time.FieldTimeShiftable;

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

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

  56.     /** The velocity. */
  57.     private final FieldVector3D<T> velocity;

  59.     /** The acceleration. */
  60.     private final FieldVector3D<T> acceleration;

  62.     /** Builds a FieldPVCoordinates triplet with zero acceleration.
  63.      * @param position the position vector (m)
  64.      * @param velocity the velocity vector (m/s)
  65.      */
  66.     public FieldPVCoordinates(final FieldVector3D<T> position, final FieldVector3D<T> velocity) {
  67.         this.position     = position;
  68.         this.velocity     = velocity;
  69.         final T zero      = position.getX().getField().getZero();
  70.         this.acceleration = new FieldVector3D<>(zero, zero, zero);
  71.     }

  73.     /** Builds a FieldPVCoordinates triplet.
  74.      * @param position the position vector (m)
  75.      * @param velocity the velocity vector (m/s)
  76.      * @param acceleration the acceleration vector (m/s²)
  77.      */
  78.     public FieldPVCoordinates(final FieldVector3D<T> position, final FieldVector3D<T> velocity,
  79.                               final FieldVector3D<T> acceleration) {
  80.         this.position     = position;
  81.         this.velocity     = velocity;
  82.         this.acceleration = acceleration;
  83.     }

  85.     /** Builds a FieldPVCoordinates from a field and a regular PVCoordinates.
  86.      * @param field field for the components
  87.      * @param pv PVCoordinates triplet to convert
  88.      */
  89.     public FieldPVCoordinates(final Field<T> field, final PVCoordinates pv) {
  90.         this.position     = new FieldVector3D<>(field, pv.getPosition());
  91.         this.velocity     = new FieldVector3D<>(field, pv.getVelocity());
  92.         this.acceleration = new FieldVector3D<>(field, pv.getAcceleration());
  93.     }

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

  107.     /** Multiplicative constructor.
  108.      * <p>Build a PVCoordinates from another one and a scale factor.</p>
  109.      * <p>The PVCoordinates built will be a * pv</p>
  110.      * @param a scale factor
  111.      * @param pv base (unscaled) PVCoordinates
  112.      */
  113.     public FieldPVCoordinates(final T a, final FieldPVCoordinates<T> pv) {
  114.         position     = new FieldVector3D<>(a, pv.position);
  115.         velocity     = new FieldVector3D<>(a, pv.velocity);
  116.         acceleration = new FieldVector3D<>(a, pv.acceleration);
  117.     }

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

  131.     /** Subtractive constructor.
  132.      * <p>Build a relative PVCoordinates from a start and an end position.</p>
  133.      * <p>The PVCoordinates built will be end - start.</p>
  134.      * @param start Starting PVCoordinates
  135.      * @param end ending PVCoordinates
  136.      */
  137.     public FieldPVCoordinates(final FieldPVCoordinates<T> start, final FieldPVCoordinates<T> end) {
  138.         this.position     = end.position.subtract(start.position);
  139.         this.velocity     = end.velocity.subtract(start.velocity);
  140.         this.acceleration = end.acceleration.subtract(start.acceleration);
  141.     }

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

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

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

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

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

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

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

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

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

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

  337.     /** Get fixed position/velocity at origin (both p, v and a are zero vectors).
  338.      * @param field field for the components
  339.      * @param <T> the type of the field elements
  340.      * @return a new fixed position/velocity at origin
  341.      */
  342.     public static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> getZero(final Field<T> field) {
  343.         return new FieldPVCoordinates<>(field, PVCoordinates.ZERO);
  344.     }

  346.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link FieldDerivativeStructure}&gt;.
  347.      * <p>
  348.      * The {@link FieldDerivativeStructure} coordinates correspond to time-derivatives up
  349.      * to the user-specified order.
  350.      * </p>
  351.      * @param order derivation order for the vector components (must be either 0, 1 or 2)
  352.      * @return vector with time-derivatives embedded within the coordinates
  353.           * @since 9.2
  354.      */
  355.     public FieldVector3D<FieldDerivativeStructure<T>> toDerivativeStructureVector(final int order) {

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

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

  386.     }

  388.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link FieldUnivariateDerivative1}&gt;.
  389.      * <p>
  390.      * The {@link FieldUnivariateDerivative1} coordinates correspond to time-derivatives up
  391.      * to the order 1.
  392.      * </p>
  393.      * @return vector with time-derivatives embedded within the coordinates
  394.      * @see #toUnivariateDerivative2Vector()
  395.      * @since 10.2
  396.      */
  397.     public FieldVector3D<FieldUnivariateDerivative1<T>> toUnivariateDerivative1Vector() {

  399.         final FieldUnivariateDerivative1<T> x = new FieldUnivariateDerivative1<>(position.getX(), velocity.getX());
  400.         final FieldUnivariateDerivative1<T> y = new FieldUnivariateDerivative1<>(position.getY(), velocity.getY());
  401.         final FieldUnivariateDerivative1<T> z = new FieldUnivariateDerivative1<>(position.getZ(), velocity.getZ());

  403.         return new FieldVector3D<>(x, y, z);
  404.     }

  406.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link FieldUnivariateDerivative2}&gt;.
  407.      * <p>
  408.      * The {@link FieldUnivariateDerivative2} coordinates correspond to time-derivatives up
  409.      * to the order 2.
  410.      * </p>
  411.      * @return vector with time-derivatives embedded within the coordinates
  412.      * @see #toUnivariateDerivative1Vector()
  413.      * @since 10.2
  414.      */
  415.     public FieldVector3D<FieldUnivariateDerivative2<T>> toUnivariateDerivative2Vector() {

  417.         final FieldUnivariateDerivative2<T> x = new FieldUnivariateDerivative2<>(position.getX(), velocity.getX(), acceleration.getX());
  418.         final FieldUnivariateDerivative2<T> y = new FieldUnivariateDerivative2<>(position.getY(), velocity.getY(), acceleration.getY());
  419.         final FieldUnivariateDerivative2<T> z = new FieldUnivariateDerivative2<>(position.getZ(), velocity.getZ(), acceleration.getZ());

  421.         return new FieldVector3D<>(x, y, z);
  422.     }

  424.     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link FieldDerivativeStructure}&gt;.
  425.      * <p>
  426.      * The {@link FieldDerivativeStructure} coordinates correspond to time-derivatives up
  427.      * to the user-specified order. As both the instance components {@link #getPosition() position},
  428.      * {@link #getVelocity() velocity} and {@link #getAcceleration() acceleration} and the
  429.      * {@link FieldDerivativeStructure#getPartialDerivative(int...) derivatives} of the components
  430.      * holds time-derivatives, there are several ways to retrieve these derivatives. If for example
  431.      * the {@code order} is set to 2, then both {@code pv.getPosition().getX().getPartialDerivative(2)},
  432.      * {@code pv.getVelocity().getX().getPartialDerivative(1)} and
  433.      * {@code pv.getAcceleration().getX().getValue()} return the exact same value.
  434.      * </p>
  435.      * <p>
  436.      * If derivation order is 1, the first derivative of acceleration will be computed as a
  437.      * Keplerian-only jerk. If derivation order is 2, the second derivative of velocity (which
  438.      * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
  439.      * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
  440.      * </p>
  441.      * @param order derivation order for the vector components (must be either 0, 1 or 2)
  442.      * @return pv coordinates with time-derivatives embedded within the coordinates
  443.           * @since 9.2
  444.      */
  445.     public FieldPVCoordinates<FieldDerivativeStructure<T>> toDerivativeStructurePV(final int order) {

  447.         final FDSFactory<T> factory;
  448.         final FieldDerivativeStructure<T> x0;
  449.         final FieldDerivativeStructure<T> y0;
  450.         final FieldDerivativeStructure<T> z0;
  451.         final FieldDerivativeStructure<T> x1;
  452.         final FieldDerivativeStructure<T> y1;
  453.         final FieldDerivativeStructure<T> z1;
  454.         final FieldDerivativeStructure<T> x2;
  455.         final FieldDerivativeStructure<T> y2;
  456.         final FieldDerivativeStructure<T> z2;
  457.         switch (order) {
  458.             case 0 :
  459.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  460.                 x0 = factory.build(position.getX());
  461.                 y0 = factory.build(position.getY());
  462.                 z0 = factory.build(position.getZ());
  463.                 x1 = factory.build(velocity.getX());
  464.                 y1 = factory.build(velocity.getY());
  465.                 z1 = factory.build(velocity.getZ());
  466.                 x2 = factory.build(acceleration.getX());
  467.                 y2 = factory.build(acceleration.getY());
  468.                 z2 = factory.build(acceleration.getZ());
  469.                 break;
  470.             case 1 : {
  471.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  472.                 final T                r2            = position.getNormSq();
  473.                 final T                r             = r2.sqrt();
  474.                 final T                pvOr2         = FieldVector3D.dotProduct(position, velocity).divide(r2);
  475.                 final T                a             = acceleration.getNorm();
  476.                 final T                aOr           = a.divide(r);
  477.                 final FieldVector3D<T> keplerianJerk = new FieldVector3D<>(pvOr2.multiply(-3), acceleration,
  478.                                                                            aOr.negate(), velocity);
  479.                 x0 = factory.build(position.getX(),     velocity.getX());
  480.                 y0 = factory.build(position.getY(),     velocity.getY());
  481.                 z0 = factory.build(position.getZ(),     velocity.getZ());
  482.                 x1 = factory.build(velocity.getX(),     acceleration.getX());
  483.                 y1 = factory.build(velocity.getY(),     acceleration.getY());
  484.                 z1 = factory.build(velocity.getZ(),     acceleration.getZ());
  485.                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
  486.                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
  487.                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
  488.                 break;
  489.             }
  490.             case 2 : {
  491.                 factory = new FDSFactory<>(getPosition().getX().getField(), 1, order);
  492.                 final T                r2              = position.getNormSq();
  493.                 final T                r               = r2.sqrt();
  494.                 final T                pvOr2           = FieldVector3D.dotProduct(position, velocity).divide(r2);
  495.                 final T                a               = acceleration.getNorm();
  496.                 final T                aOr             = a.divide(r);
  497.                 final FieldVector3D<T> keplerianJerk   = new FieldVector3D<>(pvOr2.multiply(-3), acceleration,
  498.                                                                              aOr.negate(), velocity);
  499.                 final T                v2              = velocity.getNormSq();
  500.                 final T                pa              = FieldVector3D.dotProduct(position, acceleration);
  501.                 final T                aj              = FieldVector3D.dotProduct(acceleration, keplerianJerk);
  502.                 final FieldVector3D<T> keplerianJounce = new FieldVector3D<>(v2.add(pa).multiply(-3).divide(r2).add(pvOr2.multiply(pvOr2).multiply(15)).subtract(aOr), acceleration,
  503.                                                                              aOr.multiply(4).multiply(pvOr2).subtract(aj.divide(a.multiply(r))), velocity);
  504.                 x0 = factory.build(position.getX(),     velocity.getX(),      acceleration.getX());
  505.                 y0 = factory.build(position.getY(),     velocity.getY(),      acceleration.getY());
  506.                 z0 = factory.build(position.getZ(),     velocity.getZ(),      acceleration.getZ());
  507.                 x1 = factory.build(velocity.getX(),     acceleration.getX(),  keplerianJerk.getX());
  508.                 y1 = factory.build(velocity.getY(),     acceleration.getY(),  keplerianJerk.getY());
  509.                 z1 = factory.build(velocity.getZ(),     acceleration.getZ(),  keplerianJerk.getZ());
  510.                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
  511.                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
  512.                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
  513.                 break;
  514.             }
  515.             default :
  516.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  517.         }

  519.         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
  520.                                         new FieldVector3D<>(x1, y1, z1),
  521.                                         new FieldVector3D<>(x2, y2, z2));

  523.     }


  526.     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link FieldUnivariateDerivative1}&gt;.
  527.      * <p>
  528.      * The {@link FieldUnivariateDerivative1} coordinates correspond to time-derivatives up
  529.      * to the order 1.
  530.      * The first derivative of acceleration will be computed as a Keplerian-only jerk.
  531.      * </p>
  532.      * @return pv coordinates with time-derivatives embedded within the coordinates
  533.      * @since 10.2
  534.      */
  535.     public FieldPVCoordinates<FieldUnivariateDerivative1<T>> toUnivariateDerivative1PV() {

  537.         final T   r2            = position.getNormSq();
  538.         final T   r             = FastMath.sqrt(r2);
  539.         final T   pvOr2         = FieldVector3D.dotProduct(position, velocity).divide(r2);
  540.         final T   a             = acceleration.getNorm();
  541.         final T   aOr           = a.divide(r);
  542.         final FieldVector3D<T> keplerianJerk   = new FieldVector3D<>(pvOr2.multiply(-3), acceleration,
  543.                                                                      aOr.negate(), velocity);

  545.         final FieldUnivariateDerivative1<T> x0 = new FieldUnivariateDerivative1<>(position.getX(),     velocity.getX());
  546.         final FieldUnivariateDerivative1<T> y0 = new FieldUnivariateDerivative1<>(position.getY(),     velocity.getY());
  547.         final FieldUnivariateDerivative1<T> z0 = new FieldUnivariateDerivative1<>(position.getZ(),     velocity.getZ());
  548.         final FieldUnivariateDerivative1<T> x1 = new FieldUnivariateDerivative1<>(velocity.getX(),     acceleration.getX());
  549.         final FieldUnivariateDerivative1<T> y1 = new FieldUnivariateDerivative1<>(velocity.getY(),     acceleration.getY());
  550.         final FieldUnivariateDerivative1<T> z1 = new FieldUnivariateDerivative1<>(velocity.getZ(),     acceleration.getZ());
  551.         final FieldUnivariateDerivative1<T> x2 = new FieldUnivariateDerivative1<>(acceleration.getX(), keplerianJerk.getX());
  552.         final FieldUnivariateDerivative1<T> y2 = new FieldUnivariateDerivative1<>(acceleration.getY(), keplerianJerk.getY());
  553.         final FieldUnivariateDerivative1<T> z2 = new FieldUnivariateDerivative1<>(acceleration.getZ(), keplerianJerk.getZ());

  555.         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
  556.                                         new FieldVector3D<>(x1, y1, z1),
  557.                                         new FieldVector3D<>(x2, y2, z2));

  559.     }

  561.     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link FieldUnivariateDerivative2}&gt;.
  562.      * <p>
  563.      * The {@link FieldUnivariateDerivative2} coordinates correspond to time-derivatives up
  564.      * to the order 2.
  565.      * As derivation order is 2, the second derivative of velocity (which
  566.      * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
  567.      * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
  568.      * </p>
  569.      * @return pv coordinates with time-derivatives embedded within the coordinates
  570.      * @since 10.2
  571.      */
  572.     public FieldPVCoordinates<FieldUnivariateDerivative2<T>> toUnivariateDerivative2PV() {

  574.         final T                r2              = position.getNormSq();
  575.         final T                r               = r2.sqrt();
  576.         final T                pvOr2           = FieldVector3D.dotProduct(position, velocity).divide(r2);
  577.         final T                a               = acceleration.getNorm();
  578.         final T                aOr             = a.divide(r);
  579.         final FieldVector3D<T> keplerianJerk   = new FieldVector3D<>(pvOr2.multiply(-3), acceleration,
  580.                                                                      aOr.negate(), velocity);
  581.         final T                v2              = velocity.getNormSq();
  582.         final T                pa              = FieldVector3D.dotProduct(position, acceleration);
  583.         final T                aj              = FieldVector3D.dotProduct(acceleration, keplerianJerk);
  584.         final FieldVector3D<T> keplerianJounce = new FieldVector3D<>(v2.add(pa).multiply(-3).divide(r2).add(pvOr2.multiply(pvOr2).multiply(15)).subtract(aOr), acceleration,
  585.                                                                      aOr.multiply(4).multiply(pvOr2).subtract(aj.divide(a.multiply(r))), velocity);

  587.         final FieldUnivariateDerivative2<T> x0 = new FieldUnivariateDerivative2<>(position.getX(),     velocity.getX(),      acceleration.getX());
  588.         final FieldUnivariateDerivative2<T> y0 = new FieldUnivariateDerivative2<>(position.getY(),     velocity.getY(),      acceleration.getY());
  589.         final FieldUnivariateDerivative2<T> z0 = new FieldUnivariateDerivative2<>(position.getZ(),     velocity.getZ(),      acceleration.getZ());
  590.         final FieldUnivariateDerivative2<T> x1 = new FieldUnivariateDerivative2<>(velocity.getX(),     acceleration.getX(),  keplerianJerk.getX());
  591.         final FieldUnivariateDerivative2<T> y1 = new FieldUnivariateDerivative2<>(velocity.getY(),     acceleration.getY(),  keplerianJerk.getY());
  592.         final FieldUnivariateDerivative2<T> z1 = new FieldUnivariateDerivative2<>(velocity.getZ(),     acceleration.getZ(),  keplerianJerk.getZ());
  593.         final FieldUnivariateDerivative2<T> x2 = new FieldUnivariateDerivative2<>(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
  594.         final FieldUnivariateDerivative2<T> y2 = new FieldUnivariateDerivative2<>(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
  595.         final FieldUnivariateDerivative2<T> z2 = new FieldUnivariateDerivative2<>(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());

  597.         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
  598.                                         new FieldVector3D<>(x1, y1, z1),
  599.                                         new FieldVector3D<>(x2, y2, z2));

  601.     }

  603.     /** Estimate velocity between two positions.
  604.      * <p>Estimation is based on a simple fixed velocity translation
  605.      * during the time interval between the two positions.</p>
  606.      * @param start start position
  607.      * @param end end position
  608.      * @param dt time elapsed between the dates of the two positions
  609.      * @param <T> the type of the field elements
  610.      * @return velocity allowing to go from start to end positions
  611.      */
  612.     public static <T extends CalculusFieldElement<T>> FieldVector3D<T> estimateVelocity(final FieldVector3D<T> start,
  613.                                                                                     final FieldVector3D<T> end,
  614.                                                                                     final double dt) {
  615.         final double scale = 1.0 / dt;
  616.         return new FieldVector3D<>(scale, end, -scale, start);
  617.     }

  619.     /** Get a time-shifted state.
  620.      * <p>
  621.      * The state can be slightly shifted to close dates. This shift is based on
  622.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  623.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  624.      * for either small time shifts or coarse accuracy.
  625.      * </p>
  626.      * @param dt time shift in seconds
  627.      * @return a new state, shifted with respect to the instance (which is immutable)
  628.      */
  629.     @Override
  630.     public FieldPVCoordinates<T> shiftedBy(final double dt) {
  631.         return new FieldPVCoordinates<>(new FieldVector3D<>(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
  632.                                         new FieldVector3D<>(1, velocity, dt, acceleration),
  633.                                         acceleration);
  634.     }

  636.     /** Get a time-shifted state.
  637.      * <p>
  638.      * The state can be slightly shifted to close dates. This shift is based on
  639.      * a simple quadratic model. It is <em>not</em> intended as a replacement for
  640.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  641.      * for either small time shifts or coarse accuracy.
  642.      * </p>
  643.      * @param dt time shift in seconds
  644.      * @return a new state, shifted with respect to the instance (which is immutable)
  645.      */
  646.     @Override
  647.     public FieldPVCoordinates<T> shiftedBy(final T dt) {
  648.         final T one = dt.getField().getOne();
  649.         return new FieldPVCoordinates<>(positionShiftedBy(dt),
  650.                                         new FieldVector3D<>(one, velocity, dt, acceleration),
  651.                                         acceleration);
  652.     }

  654.     /**
  655.      * Get a time-shifted position. Same as {@link #shiftedBy(CalculusFieldElement)} except
  656.      * that only the sifted position is returned.
  657.      * <p>
  658.      * The state can be slightly shifted to close dates. This shift is based on
  659.      * a simple Taylor expansion. It is <em>not</em> intended as a replacement
  660.      * for proper orbit propagation (it is not even Keplerian!) but should be
  661.      * sufficient for either small time shifts or coarse accuracy.
  662.      * </p>
  663.      *
  664.      * @param dt time shift in seconds
  665.      * @return a new state, shifted with respect to the instance (which is
  666.      * immutable)
  667.      * @since 11.2
  668.      */
  669.     public FieldVector3D<T> positionShiftedBy(final T dt) {
  670.         final T one = dt.getField().getOne();
  671.         return new FieldVector3D<>(one, position, dt, velocity, dt.square().multiply(0.5), acceleration);
  672.     }

  674.     /** Gets the position.
  675.      * @return the position vector (m).
  676.      */
  677.     public FieldVector3D<T> getPosition() {
  678.         return position;
  679.     }

  681.     /** Gets the velocity.
  682.      * @return the velocity vector (m/s).
  683.      */
  684.     public FieldVector3D<T> getVelocity() {
  685.         return velocity;
  686.     }

  688.     /** Gets the acceleration.
  689.      * @return the acceleration vector (m/s²).
  690.      */
  691.     public FieldVector3D<T> getAcceleration() {
  692.         return acceleration;
  693.     }

  695.     /** Gets the momentum.
  696.      * <p>This vector is the p &otimes; v where p is position, v is velocity
  697.      * and &otimes; is cross product. To get the real physical angular momentum
  698.      * you need to multiply this vector by the mass.</p>
  699.      * <p>The returned vector is recomputed each time this method is called, it
  700.      * is not cached.</p>
  701.      * @return a new instance of the momentum vector (m²/s).
  702.      */
  703.     public FieldVector3D<T> getMomentum() {
  704.         return FieldVector3D.crossProduct(position, velocity);
  705.     }

  707.     /**
  708.      * Get the angular velocity (spin) of this point as seen from the origin.
  709.      *
  710.      * <p> The angular velocity vector is parallel to the {@link #getMomentum()
  711.      * angular * momentum} and is computed by ω = p &times; v / ||p||²
  712.      *
  713.      * @return the angular velocity vector
  714.      * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
  715.      *      Wikipedia</a>
  716.      */
  717.     public FieldVector3D<T> getAngularVelocity() {
  718.         return this.getMomentum().scalarMultiply(
  719.                 this.getPosition().getNormSq().reciprocal());
  720.     }

  722.     /** Get the opposite of the instance.
  723.      * @return a new position-velocity which is opposite to the instance
  724.      */
  725.     public FieldPVCoordinates<T> negate() {
  726.         return new FieldPVCoordinates<>(position.negate(), velocity.negate(), acceleration.negate());
  727.     }

  729.     /** Normalize the position part of the instance.
  730.      * <p>
  731.      * The computed coordinates first component (position) will be a
  732.      * normalized vector, the second component (velocity) will be the
  733.      * derivative of the first component (hence it will generally not
  734.      * be normalized), and the third component (acceleration) will be the
  735.      * derivative of the second component (hence it will generally not
  736.      * be normalized).
  737.      * </p>
  738.      * @return a new instance, with first component normalized and
  739.      * remaining component computed to have consistent derivatives
  740.      */
  741.     public FieldPVCoordinates<T> normalize() {
  742.         final T   inv     = position.getNorm().reciprocal();
  743.         final FieldVector3D<T> u       = new FieldVector3D<>(inv, position);
  744.         final FieldVector3D<T> v       = new FieldVector3D<>(inv, velocity);
  745.         final FieldVector3D<T> w       = new FieldVector3D<>(inv, acceleration);
  746.         final T   uv      = FieldVector3D.dotProduct(u, v);
  747.         final T   v2      = FieldVector3D.dotProduct(v, v);
  748.         final T   uw      = FieldVector3D.dotProduct(u, w);
  749.         final FieldVector3D<T> uDot    = new FieldVector3D<>(inv.getField().getOne(), v,
  750.                                                              uv.multiply(-1), u);
  751.         final FieldVector3D<T> uDotDot = new FieldVector3D<>(inv.getField().getOne(), w,
  752.                                                              uv.multiply(-2), v,
  753.                                                              uv.multiply(uv).multiply(3).subtract(v2).subtract(uw), u);
  754.         return new FieldPVCoordinates<>(u, uDot, uDotDot);
  755.     }

  757.     /** Compute the cross-product of two instances.
  758.      * @param pv2 second instances
  759.      * @return the cross product v1 ^ v2 as a new instance
  760.      */
  761.     public FieldPVCoordinates<T> crossProduct(final FieldPVCoordinates<T> pv2) {
  762.         final FieldVector3D<T> p1 = position;
  763.         final FieldVector3D<T> v1 = velocity;
  764.         final FieldVector3D<T> a1 = acceleration;
  765.         final FieldVector3D<T> p2 = pv2.position;
  766.         final FieldVector3D<T> v2 = pv2.velocity;
  767.         final FieldVector3D<T> a2 = pv2.acceleration;
  768.         return new FieldPVCoordinates<>(FieldVector3D.crossProduct(p1, p2),
  769.                                         new FieldVector3D<>(1, FieldVector3D.crossProduct(p1, v2),
  770.                                                             1, FieldVector3D.crossProduct(v1, p2)),
  771.                                         new FieldVector3D<>(1, FieldVector3D.crossProduct(p1, a2),
  772.                                                             2, FieldVector3D.crossProduct(v1, v2),
  773.                                                             1, FieldVector3D.crossProduct(a1, p2)));
  774.     }

  776.     /** Convert to a constant position-velocity.
  777.      * @return a constant position-velocity
  778.      */
  779.     public PVCoordinates toPVCoordinates() {
  780.         return new PVCoordinates(position.toVector3D(), velocity.toVector3D(), acceleration.toVector3D());
  781.     }

  783.     /** Return a string representation of this position/velocity pair.
  784.      * @return string representation of this position/velocity pair
  785.      */
  786.     public String toString() {
  787.         final String comma = ", ";
  788.         return new StringBuilder().append('{').append("P(").
  789.                                   append(position.getX().getReal()).append(comma).
  790.                                   append(position.getY().getReal()).append(comma).
  791.                                   append(position.getZ().getReal()).append("), V(").
  792.                                   append(velocity.getX().getReal()).append(comma).
  793.                                   append(velocity.getY().getReal()).append(comma).
  794.                                   append(velocity.getZ().getReal()).append("), A(").
  795.                                   append(acceleration.getX().getReal()).append(comma).
  796.                                   append(acceleration.getY().getReal()).append(comma).
  797.                                   append(acceleration.getZ().getReal()).append(")}").toString();
  798.     }

  800.     /** {@inheritDoc} */
  801.     @Override
  802.     public FieldPVCoordinates<T> blendArithmeticallyWith(final FieldPVCoordinates<T> other,
  803.                                                          final T blendingValue)
  804.             throws MathIllegalArgumentException {
  805.         final FieldVector3D<T> blendedPosition     = position.blendArithmeticallyWith(other.getPosition(), blendingValue);
  806.         final FieldVector3D<T> blendedVelocity     = velocity.blendArithmeticallyWith(other.getVelocity(), blendingValue);
  807.         final FieldVector3D<T> blendedAcceleration = acceleration.blendArithmeticallyWith(other.getAcceleration(), blendingValue);

  809.         return new FieldPVCoordinates<>(blendedPosition, blendedVelocity, blendedAcceleration);
  810.     }
  811. }
Created with JaCoCo 0.8.12.202403310830