PVCoordinates.java

  1. /* Copyright 2002-2023 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 java.io.Serializable;

  21. import org.hipparchus.analysis.differentiation.DSFactory;
  22. import org.hipparchus.analysis.differentiation.Derivative;
  23. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  24. import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
  25. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  26. import org.hipparchus.exception.MathIllegalArgumentException;
  27. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  28. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  29. import org.hipparchus.util.Blendable;
  30. import org.hipparchus.util.FastMath;
  31. import org.orekit.errors.OrekitException;
  32. import org.orekit.errors.OrekitMessages;
  33. import org.orekit.time.TimeShiftable;

  35. /** Simple container for Position/Velocity/Acceleration triplets.
  36.  * <p>
  37.  * The state can be slightly shifted to close dates. This shift is based on
  38.  * a simple quadratic model. It is <em>not</em> intended as a replacement for
  39.  * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  40.  * for either small time shifts or coarse accuracy.
  41.  * </p>
  42.  * <p>
  43.  * This class is the angular counterpart to {@link AngularCoordinates}.
  44.  * </p>
  45.  * <p>Instances of this class are guaranteed to be immutable.</p>
  46.  * @author Fabien Maussion
  47.  * @author Luc Maisonobe
  48.  */
  49. public class PVCoordinates implements TimeShiftable<PVCoordinates>, Blendable<PVCoordinates>, Serializable {

  51.     /** Fixed position/velocity at origin (both p, v and a are zero vectors). */
  52.     public static final PVCoordinates ZERO = new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);

  54.     /** Serializable UID. */
  55.     private static final long serialVersionUID = 20140407L;

  57.     /** The position. */
  58.     private final Vector3D position;

  60.     /** The velocity. */
  61.     private final Vector3D velocity;

  63.     /** The acceleration. */
  64.     private final Vector3D acceleration;

  66.     /** Simple constructor.
  67.      * <p> Set the Coordinates to default : (0 0 0), (0 0 0), (0 0 0).</p>
  68.      */
  69.     public PVCoordinates() {
  70.         position     = Vector3D.ZERO;
  71.         velocity     = Vector3D.ZERO;
  72.         acceleration = Vector3D.ZERO;
  73.     }

  75.     /** Builds a PVCoordinates triplet with zero acceleration.
  76.      * <p>Acceleration is set to zero</p>
  77.      * @param position the position vector (m)
  78.      * @param velocity the velocity vector (m/s)
  79.      */
  80.     public PVCoordinates(final Vector3D position, final Vector3D velocity) {
  81.         this.position     = position;
  82.         this.velocity     = velocity;
  83.         this.acceleration = Vector3D.ZERO;
  84.     }

  86.     /** Builds a PVCoordinates triplet.
  87.      * @param position the position vector (m)
  88.      * @param velocity the velocity vector (m/s)
  89.      * @param acceleration the acceleration vector (m/s²)
  90.      */
  91.     public PVCoordinates(final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
  92.         this.position     = position;
  93.         this.velocity     = velocity;
  94.         this.acceleration = acceleration;
  95.     }

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

  109.     /** Subtractive constructor.
  110.      * <p>Build a relative PVCoordinates from a start and an end position.</p>
  111.      * <p>The PVCoordinates built will be end - start.</p>
  112.      * @param start Starting PVCoordinates
  113.      * @param end ending PVCoordinates
  114.      */
  115.     public PVCoordinates(final PVCoordinates start, final PVCoordinates end) {
  116.         this.position     = end.position.subtract(start.position);
  117.         this.velocity     = end.velocity.subtract(start.velocity);
  118.         this.acceleration = end.acceleration.subtract(start.acceleration);
  119.     }

  121.     /** Linear constructor.
  122.      * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
  123.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
  124.      * @param a1 first scale factor
  125.      * @param pv1 first base (unscaled) PVCoordinates
  126.      * @param a2 second scale factor
  127.      * @param pv2 second base (unscaled) PVCoordinates
  128.      */
  129.     public PVCoordinates(final double a1, final PVCoordinates pv1,
  130.                          final double a2, final PVCoordinates pv2) {
  131.         position     = new Vector3D(a1, pv1.position,     a2, pv2.position);
  132.         velocity     = new Vector3D(a1, pv1.velocity,     a2, pv2.velocity);
  133.         acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration);
  134.     }

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

  154.     /** Linear constructor.
  155.      * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
  156.      * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
  157.      * @param a1 first scale factor
  158.      * @param pv1 first base (unscaled) PVCoordinates
  159.      * @param a2 second scale factor
  160.      * @param pv2 second base (unscaled) PVCoordinates
  161.      * @param a3 third scale factor
  162.      * @param pv3 third base (unscaled) PVCoordinates
  163.      * @param a4 fourth scale factor
  164.      * @param pv4 fourth base (unscaled) PVCoordinates
  165.      */
  166.     public PVCoordinates(final double a1, final PVCoordinates pv1,
  167.                          final double a2, final PVCoordinates pv2,
  168.                          final double a3, final PVCoordinates pv3,
  169.                          final double a4, final PVCoordinates pv4) {
  170.         position     = new Vector3D(a1, pv1.position,     a2, pv2.position,
  171.                                     a3, pv3.position,     a4, pv4.position);
  172.         velocity     = new Vector3D(a1, pv1.velocity,     a2, pv2.velocity,
  173.                                     a3, pv3.velocity,     a4, pv4.velocity);
  174.         acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration,
  175.                                     a3, pv3.acceleration, a4, pv4.acceleration);
  176.     }

  178.     /** Builds a PVCoordinates triplet from  a {@link FieldVector3D}&lt;{@link Derivative}&gt;.
  179.      * <p>
  180.      * The vector components must have time as their only derivation parameter and
  181.      * have consistent derivation orders.
  182.      * </p>
  183.      * @param p vector with time-derivatives embedded within the coordinates
  184.      * @param <U> type of the derivative
  185.      */
  186.     public <U extends Derivative<U>> PVCoordinates(final FieldVector3D<U> p) {
  187.         position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal());
  188.         if (p.getX().getOrder() >= 1) {
  189.             velocity = new Vector3D(p.getX().getPartialDerivative(1),
  190.                                     p.getY().getPartialDerivative(1),
  191.                                     p.getZ().getPartialDerivative(1));
  192.             if (p.getX().getOrder() >= 2) {
  193.                 acceleration = new Vector3D(p.getX().getPartialDerivative(2),
  194.                                             p.getY().getPartialDerivative(2),
  195.                                             p.getZ().getPartialDerivative(2));
  196.             } else {
  197.                 acceleration = Vector3D.ZERO;
  198.             }
  199.         } else {
  200.             velocity     = Vector3D.ZERO;
  201.             acceleration = Vector3D.ZERO;
  202.         }
  203.     }

  205.     /**
  206.      * Builds PV coordinates with the givne position, zero velocity, and zero
  207.      * acceleration.
  208.      *
  209.      * @param position position vector (m)
  210.      */
  211.     public PVCoordinates(final Vector3D position) {
  212.         this(position, Vector3D.ZERO);
  213.     }

  215.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link DerivativeStructure}&gt;.
  216.      * <p>
  217.      * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
  218.      * to the user-specified order.
  219.      * </p>
  220.      * @param order derivation order for the vector components (must be either 0, 1 or 2)
  221.      * @return vector with time-derivatives embedded within the coordinates
  222.      */
  223.     public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order) {

  225.         final DSFactory factory;
  226.         final DerivativeStructure x;
  227.         final DerivativeStructure y;
  228.         final DerivativeStructure z;
  229.         switch (order) {
  230.             case 0 :
  231.                 factory = new DSFactory(1, order);
  232.                 x = factory.build(position.getX());
  233.                 y = factory.build(position.getY());
  234.                 z = factory.build(position.getZ());
  235.                 break;
  236.             case 1 :
  237.                 factory = new DSFactory(1, order);
  238.                 x = factory.build(position.getX(), velocity.getX());
  239.                 y = factory.build(position.getY(), velocity.getY());
  240.                 z = factory.build(position.getZ(), velocity.getZ());
  241.                 break;
  242.             case 2 :
  243.                 factory = new DSFactory(1, order);
  244.                 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
  245.                 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
  246.                 z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
  247.                 break;
  248.             default :
  249.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  250.         }

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

  254.     }

  256.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link UnivariateDerivative1}&gt;.
  257.      * <p>
  258.      * The {@link UnivariateDerivative1} coordinates correspond to time-derivatives up
  259.      * to the order 1.
  260.      * </p>
  261.      * @return vector with time-derivatives embedded within the coordinates
  262.      * @see #toUnivariateDerivative2Vector()
  263.      * @since 10.2
  264.      */
  265.     public FieldVector3D<UnivariateDerivative1> toUnivariateDerivative1Vector() {

  267.         final UnivariateDerivative1 x = new UnivariateDerivative1(position.getX(), velocity.getX());
  268.         final UnivariateDerivative1 y = new UnivariateDerivative1(position.getY(), velocity.getY());
  269.         final UnivariateDerivative1 z = new UnivariateDerivative1(position.getZ(), velocity.getZ());

  271.         return new FieldVector3D<>(x, y, z);
  272.     }

  274.     /** Transform the instance to a {@link FieldVector3D}&lt;{@link UnivariateDerivative2}&gt;.
  275.      * <p>
  276.      * The {@link UnivariateDerivative2} coordinates correspond to time-derivatives up
  277.      * to the order 2.
  278.      * </p>
  279.      * @return vector with time-derivatives embedded within the coordinates
  280.      * @see #toUnivariateDerivative1Vector()
  281.      * @since 10.2
  282.      */
  283.     public FieldVector3D<UnivariateDerivative2> toUnivariateDerivative2Vector() {

  285.         final UnivariateDerivative2 x = new UnivariateDerivative2(position.getX(), velocity.getX(), acceleration.getX());
  286.         final UnivariateDerivative2 y = new UnivariateDerivative2(position.getY(), velocity.getY(), acceleration.getY());
  287.         final UnivariateDerivative2 z = new UnivariateDerivative2(position.getZ(), velocity.getZ(), acceleration.getZ());

  289.         return new FieldVector3D<>(x, y, z);
  290.     }

  292.     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link DerivativeStructure}&gt;.
  293.      * <p>
  294.      * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
  295.      * to the user-specified order. As both the instance components {@link #getPosition() position},
  296.      * {@link #getVelocity() velocity} and {@link #getAcceleration() acceleration} and the
  297.      * {@link DerivativeStructure#getPartialDerivative(int...) derivatives} of the components
  298.      * holds time-derivatives, there are several ways to retrieve these derivatives. If for example
  299.      * the {@code order} is set to 2, then both {@code pv.getPosition().getX().getPartialDerivative(2)},
  300.      * {@code pv.getVelocity().getX().getPartialDerivative(1)} and
  301.      * {@code pv.getAcceleration().getX().getValue()} return the exact same value.
  302.      * </p>
  303.      * <p>
  304.      * If derivation order is 1, the first derivative of acceleration will be computed as a
  305.      * Keplerian-only jerk. If derivation order is 2, the second derivative of velocity (which
  306.      * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
  307.      * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
  308.      * </p>
  309.      * @param order derivation order for the vector components (must be either 0, 1 or 2)
  310.      * @return pv coordinates with time-derivatives embedded within the coordinates
  311.      * @since 9.2
  312.      */
  313.     public FieldPVCoordinates<DerivativeStructure> toDerivativeStructurePV(final int order) {

  315.         final DSFactory factory;
  316.         final DerivativeStructure x0;
  317.         final DerivativeStructure y0;
  318.         final DerivativeStructure z0;
  319.         final DerivativeStructure x1;
  320.         final DerivativeStructure y1;
  321.         final DerivativeStructure z1;
  322.         final DerivativeStructure x2;
  323.         final DerivativeStructure y2;
  324.         final DerivativeStructure z2;
  325.         switch (order) {
  326.             case 0 :
  327.                 factory = new DSFactory(1, order);
  328.                 x0 = factory.build(position.getX());
  329.                 y0 = factory.build(position.getY());
  330.                 z0 = factory.build(position.getZ());
  331.                 x1 = factory.build(velocity.getX());
  332.                 y1 = factory.build(velocity.getY());
  333.                 z1 = factory.build(velocity.getZ());
  334.                 x2 = factory.build(acceleration.getX());
  335.                 y2 = factory.build(acceleration.getY());
  336.                 z2 = factory.build(acceleration.getZ());
  337.                 break;
  338.             case 1 : {
  339.                 factory = new DSFactory(1, order);
  340.                 final double   r2            = position.getNormSq();
  341.                 final double   r             = FastMath.sqrt(r2);
  342.                 final double   pvOr2         = Vector3D.dotProduct(position, velocity) / r2;
  343.                 final double   a             = acceleration.getNorm();
  344.                 final double   aOr           = a / r;
  345.                 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
  346.                 x0 = factory.build(position.getX(),     velocity.getX());
  347.                 y0 = factory.build(position.getY(),     velocity.getY());
  348.                 z0 = factory.build(position.getZ(),     velocity.getZ());
  349.                 x1 = factory.build(velocity.getX(),     acceleration.getX());
  350.                 y1 = factory.build(velocity.getY(),     acceleration.getY());
  351.                 z1 = factory.build(velocity.getZ(),     acceleration.getZ());
  352.                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
  353.                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
  354.                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
  355.                 break;
  356.             }
  357.             case 2 : {
  358.                 factory = new DSFactory(1, order);
  359.                 final double   r2              = position.getNormSq();
  360.                 final double   r               = FastMath.sqrt(r2);
  361.                 final double   pvOr2           = Vector3D.dotProduct(position, velocity) / r2;
  362.                 final double   a               = acceleration.getNorm();
  363.                 final double   aOr             = a / r;
  364.                 final Vector3D keplerianJerk   = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
  365.                 final double   v2              = velocity.getNormSq();
  366.                 final double   pa              = Vector3D.dotProduct(position, acceleration);
  367.                 final double   aj              = Vector3D.dotProduct(acceleration, keplerianJerk);
  368.                 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
  369.                                                               4 * aOr * pvOr2 - aj / (a * r), velocity);
  370.                 x0 = factory.build(position.getX(),     velocity.getX(),      acceleration.getX());
  371.                 y0 = factory.build(position.getY(),     velocity.getY(),      acceleration.getY());
  372.                 z0 = factory.build(position.getZ(),     velocity.getZ(),      acceleration.getZ());
  373.                 x1 = factory.build(velocity.getX(),     acceleration.getX(),  keplerianJerk.getX());
  374.                 y1 = factory.build(velocity.getY(),     acceleration.getY(),  keplerianJerk.getY());
  375.                 z1 = factory.build(velocity.getZ(),     acceleration.getZ(),  keplerianJerk.getZ());
  376.                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
  377.                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
  378.                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
  379.                 break;
  380.             }
  381.             default :
  382.                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
  383.         }

  385.         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
  386.                                         new FieldVector3D<>(x1, y1, z1),
  387.                                         new FieldVector3D<>(x2, y2, z2));

  389.     }

  391.     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link UnivariateDerivative1}&gt;.
  392.      * <p>
  393.      * The {@link UnivariateDerivative1} coordinates correspond to time-derivatives up
  394.      * to the order 1.
  395.      * The first derivative of acceleration will be computed as a Keplerian-only jerk.
  396.      * </p>
  397.      * @return pv coordinates with time-derivatives embedded within the coordinates
  398.      * @since 10.2
  399.      */
  400.     public FieldPVCoordinates<UnivariateDerivative1> toUnivariateDerivative1PV() {

  402.         final double   r2            = position.getNormSq();
  403.         final double   r             = FastMath.sqrt(r2);
  404.         final double   pvOr2         = Vector3D.dotProduct(position, velocity) / r2;
  405.         final double   a             = acceleration.getNorm();
  406.         final double   aOr           = a / r;
  407.         final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);

  409.         final UnivariateDerivative1 x0 = new UnivariateDerivative1(position.getX(),     velocity.getX());
  410.         final UnivariateDerivative1 y0 = new UnivariateDerivative1(position.getY(),     velocity.getY());
  411.         final UnivariateDerivative1 z0 = new UnivariateDerivative1(position.getZ(),     velocity.getZ());
  412.         final UnivariateDerivative1 x1 = new UnivariateDerivative1(velocity.getX(),     acceleration.getX());
  413.         final UnivariateDerivative1 y1 = new UnivariateDerivative1(velocity.getY(),     acceleration.getY());
  414.         final UnivariateDerivative1 z1 = new UnivariateDerivative1(velocity.getZ(),     acceleration.getZ());
  415.         final UnivariateDerivative1 x2 = new UnivariateDerivative1(acceleration.getX(), keplerianJerk.getX());
  416.         final UnivariateDerivative1 y2 = new UnivariateDerivative1(acceleration.getY(), keplerianJerk.getY());
  417.         final UnivariateDerivative1 z2 = new UnivariateDerivative1(acceleration.getZ(), keplerianJerk.getZ());

  419.         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
  420.                                         new FieldVector3D<>(x1, y1, z1),
  421.                                         new FieldVector3D<>(x2, y2, z2));

  423.     }

  425.     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link UnivariateDerivative2}&gt;.
  426.      * <p>
  427.      * The {@link UnivariateDerivative2} coordinates correspond to time-derivatives up
  428.      * to the order 2.
  429.      * As derivation order is 2, the second derivative of velocity (which
  430.      * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
  431.      * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
  432.      * </p>
  433.      * @return pv coordinates with time-derivatives embedded within the coordinates
  434.      * @since 10.2
  435.      */
  436.     public FieldPVCoordinates<UnivariateDerivative2> toUnivariateDerivative2PV() {

  438.         final double   r2              = position.getNormSq();
  439.         final double   r               = FastMath.sqrt(r2);
  440.         final double   pvOr2           = Vector3D.dotProduct(position, velocity) / r2;
  441.         final double   a               = acceleration.getNorm();
  442.         final double   aOr             = a / r;
  443.         final Vector3D keplerianJerk   = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
  444.         final double   v2              = velocity.getNormSq();
  445.         final double   pa              = Vector3D.dotProduct(position, acceleration);
  446.         final double   aj              = Vector3D.dotProduct(acceleration, keplerianJerk);
  447.         final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
  448.                                                       4 * aOr * pvOr2 - aj / (a * r), velocity);

  450.         final UnivariateDerivative2 x0 = new UnivariateDerivative2(position.getX(),     velocity.getX(),      acceleration.getX());
  451.         final UnivariateDerivative2 y0 = new UnivariateDerivative2(position.getY(),     velocity.getY(),      acceleration.getY());
  452.         final UnivariateDerivative2 z0 = new UnivariateDerivative2(position.getZ(),     velocity.getZ(),      acceleration.getZ());
  453.         final UnivariateDerivative2 x1 = new UnivariateDerivative2(velocity.getX(),     acceleration.getX(),  keplerianJerk.getX());
  454.         final UnivariateDerivative2 y1 = new UnivariateDerivative2(velocity.getY(),     acceleration.getY(),  keplerianJerk.getY());
  455.         final UnivariateDerivative2 z1 = new UnivariateDerivative2(velocity.getZ(),     acceleration.getZ(),  keplerianJerk.getZ());
  456.         final UnivariateDerivative2 x2 = new UnivariateDerivative2(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
  457.         final UnivariateDerivative2 y2 = new UnivariateDerivative2(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
  458.         final UnivariateDerivative2 z2 = new UnivariateDerivative2(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());

  460.         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
  461.                                         new FieldVector3D<>(x1, y1, z1),
  462.                                         new FieldVector3D<>(x2, y2, z2));

  464.     }

  466.     /** Estimate velocity between two positions.
  467.      * <p>Estimation is based on a simple fixed velocity translation
  468.      * during the time interval between the two positions.</p>
  469.      * @param start start position
  470.      * @param end end position
  471.      * @param dt time elapsed between the dates of the two positions
  472.      * @return velocity allowing to go from start to end positions
  473.      */
  474.     public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
  475.         final double scale = 1.0 / dt;
  476.         return new Vector3D(scale, end, -scale, start);
  477.     }

  479.     /** Get a time-shifted state.
  480.      * <p>
  481.      * The state can be slightly shifted to close dates. This shift is based on
  482.      * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
  483.      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
  484.      * for either small time shifts or coarse accuracy.
  485.      * </p>
  486.      * @param dt time shift in seconds
  487.      * @return a new state, shifted with respect to the instance (which is immutable)
  488.      */
  489.     public PVCoordinates shiftedBy(final double dt) {
  490.         return new PVCoordinates(positionShiftedBy(dt),
  491.                                  new Vector3D(1, velocity, dt, acceleration),
  492.                                  acceleration);
  493.     }

  495.     /**
  496.      * Get a time-shifted position. Same as {@link #shiftedBy(double)} except
  497.      * that only the sifted position is returned.
  498.      * <p>
  499.      * The state can be slightly shifted to close dates. This shift is based on
  500.      * a simple Taylor expansion. It is <em>not</em> intended as a replacement
  501.      * for proper orbit propagation (it is not even Keplerian!) but should be
  502.      * sufficient for either small time shifts or coarse accuracy.
  503.      * </p>
  504.      *
  505.      * @param dt time shift in seconds
  506.      * @return a new state, shifted with respect to the instance (which is
  507.      * immutable)
  508.      */
  509.     public Vector3D positionShiftedBy(final double dt) {
  510.         return new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration);
  511.     }

  513.     /** Gets the position.
  514.      * @return the position vector (m).
  515.      */
  516.     public Vector3D getPosition() {
  517.         return position;
  518.     }

  520.     /** Gets the velocity.
  521.      * @return the velocity vector (m/s).
  522.      */
  523.     public Vector3D getVelocity() {
  524.         return velocity;
  525.     }

  527.     /** Gets the acceleration.
  528.      * @return the acceleration vector (m/s²).
  529.      */
  530.     public Vector3D getAcceleration() {
  531.         return acceleration;
  532.     }

  534.     /** Gets the momentum.
  535.      * <p>This vector is the p &otimes; v where p is position, v is velocity
  536.      * and &otimes; is cross product. To get the real physical angular momentum
  537.      * you need to multiply this vector by the mass.</p>
  538.      * <p>The returned vector is recomputed each time this method is called, it
  539.      * is not cached.</p>
  540.      * @return a new instance of the momentum vector (m²/s).
  541.      */
  542.     public Vector3D getMomentum() {
  543.         return Vector3D.crossProduct(position, velocity);
  544.     }

  546.     /**
  547.      * Get the angular velocity (spin) of this point as seen from the origin.
  548.      *
  549.      * <p> The angular velocity vector is parallel to the {@link #getMomentum()
  550.      * angular momentum} and is computed by ω = p &times; v / ||p||²
  551.      *
  552.      * @return the angular velocity vector
  553.      * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
  554.      *      Wikipedia</a>
  555.      */
  556.     public Vector3D getAngularVelocity() {
  557.         return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
  558.     }

  560.     /** Get the opposite of the instance.
  561.      * @return a new position-velocity which is opposite to the instance
  562.      */
  563.     public PVCoordinates negate() {
  564.         return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
  565.     }

  567.     /** Normalize the position part of the instance.
  568.      * <p>
  569.      * The computed coordinates first component (position) will be a
  570.      * normalized vector, the second component (velocity) will be the
  571.      * derivative of the first component (hence it will generally not
  572.      * be normalized), and the third component (acceleration) will be the
  573.      * derivative of the second component (hence it will generally not
  574.      * be normalized).
  575.      * </p>
  576.      * @return a new instance, with first component normalized and
  577.      * remaining component computed to have consistent derivatives
  578.      */
  579.     public PVCoordinates normalize() {
  580.         final double   inv     = 1.0 / position.getNorm();
  581.         final Vector3D u       = new Vector3D(inv, position);
  582.         final Vector3D v       = new Vector3D(inv, velocity);
  583.         final Vector3D w       = new Vector3D(inv, acceleration);
  584.         final double   uv      = Vector3D.dotProduct(u, v);
  585.         final double   v2      = Vector3D.dotProduct(v, v);
  586.         final double   uw      = Vector3D.dotProduct(u, w);
  587.         final Vector3D uDot    = new Vector3D(1, v, -uv, u);
  588.         final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
  589.         return new PVCoordinates(u, uDot, uDotDot);
  590.     }

  592.     /** Compute the cross-product of two instances.
  593.      * @param pv1 first instances
  594.      * @param pv2 second instances
  595.      * @return the cross product v1 ^ v2 as a new instance
  596.      */
  597.     public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) {
  598.         final Vector3D p1 = pv1.position;
  599.         final Vector3D v1 = pv1.velocity;
  600.         final Vector3D a1 = pv1.acceleration;
  601.         final Vector3D p2 = pv2.position;
  602.         final Vector3D v2 = pv2.velocity;
  603.         final Vector3D a2 = pv2.acceleration;
  604.         return new PVCoordinates(Vector3D.crossProduct(p1, p2),
  605.                                  new Vector3D(1, Vector3D.crossProduct(p1, v2),
  606.                                               1, Vector3D.crossProduct(v1, p2)),
  607.                                  new Vector3D(1, Vector3D.crossProduct(p1, a2),
  608.                                               2, Vector3D.crossProduct(v1, v2),
  609.                                               1, Vector3D.crossProduct(a1, p2)));
  610.     }

  612.     /** Return a string representation of this position/velocity pair.
  613.      * @return string representation of this position/velocity pair
  614.      */
  615.     public String toString() {
  616.         final String comma = ", ";
  617.         return new StringBuilder().append('{').append("P(").
  618.                 append(position.getX()).append(comma).
  619.                 append(position.getY()).append(comma).
  620.                 append(position.getZ()).append("), V(").
  621.                 append(velocity.getX()).append(comma).
  622.                 append(velocity.getY()).append(comma).
  623.                 append(velocity.getZ()).append("), A(").
  624.                 append(acceleration.getX()).append(comma).
  625.                 append(acceleration.getY()).append(comma).
  626.                 append(acceleration.getZ()).append(")}").toString();
  627.     }

  629.     /** Replace the instance with a data transfer object for serialization.
  630.      * @return data transfer object that will be serialized
  631.      */
  632.     private Object writeReplace() {
  633.         return new DTO(this);
  634.     }

  636.     /** {@inheritDoc} */
  637.     @Override
  638.     public PVCoordinates blendArithmeticallyWith(final PVCoordinates other, final double blendingValue)
  639.             throws MathIllegalArgumentException {
  640.         final Vector3D blendedPosition     = position.blendArithmeticallyWith(other.position, blendingValue);
  641.         final Vector3D blendedVelocity     = velocity.blendArithmeticallyWith(other.velocity, blendingValue);
  642.         final Vector3D blendedAcceleration = acceleration.blendArithmeticallyWith(other.acceleration, blendingValue);

  644.         return new PVCoordinates(blendedPosition, blendedVelocity, blendedAcceleration);
  645.     }

  647.     /** Internal class used only for serialization. */
  648.     private static class DTO implements Serializable {

  650.         /** Serializable UID. */
  651.         private static final long serialVersionUID = 20140723L;

  653.         /** Double values. */
  654.         private double[] d;

  656.         /** Simple constructor.
  657.          * @param pv instance to serialize
  658.          */
  659.         private DTO(final PVCoordinates pv) {
  660.             this.d = new double[] {
  661.                 pv.getPosition().getX(),     pv.getPosition().getY(),     pv.getPosition().getZ(),
  662.                 pv.getVelocity().getX(),     pv.getVelocity().getY(),     pv.getVelocity().getZ(),
  663.                 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
  664.             };
  665.         }

  667.         /** Replace the deserialized data transfer object with a {@link PVCoordinates}.
  668.          * @return replacement {@link PVCoordinates}
  669.          */
  670.         private Object readResolve() {
  671.             return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
  672.                                      new Vector3D(d[3], d[4], d[5]),
  673.                                      new Vector3D(d[6], d[7], d[8]));
  674.         }

  676.     }

  678. }
Created with JaCoCo 0.8.11.202310140853