KeplerianMotionCartesianUtility.java

  1. /* Copyright 2022-2024 Romain Serra
  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.orbits;

  18. import org.hipparchus.CalculusFieldElement;
  19. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  20. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  21. import org.hipparchus.geometry.euclidean.threed.Rotation;
  22. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  23. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  24. import org.hipparchus.util.FastMath;
  25. import org.hipparchus.util.FieldSinCos;
  26. import org.hipparchus.util.SinCos;
  27. import org.orekit.utils.FieldPVCoordinates;
  28. import org.orekit.utils.PVCoordinates;

  29. /**
  30.  * Utility class to predict position and velocity under Keplerian motion, using lightweight routines based on Cartesian
  31.  * coordinates. Computations do not require a reference frame or an epoch.
  32.  *
  33.  * @author Andrew Goetz
  34.  * @author Romain Serra
  35.  * @see org.orekit.propagation.analytical.KeplerianPropagator
  36.  * @see org.orekit.propagation.analytical.FieldKeplerianPropagator
  37.  * @see CartesianOrbit
  38.  * @see FieldCartesianOrbit
  39.  * @since 12.1
  40.  */
  41. public class KeplerianMotionCartesianUtility {

  42.     private KeplerianMotionCartesianUtility() {
  43.         // utility class
  44.     }

  45.     /**
  46.      * Method to propagate position and velocity according to Keplerian dynamics.
  47.      * For long time of flights, it is preferable to use {@link org.orekit.propagation.analytical.KeplerianPropagator}.
  48.      * @param dt time of flight
  49.      * @param position initial position vector
  50.      * @param velocity initial velocity vector
  51.      * @param mu central body gravitational parameter
  52.      * @return predicted position-velocity
  53.      */
  54.     public static PVCoordinates predictPositionVelocity(final double dt, final Vector3D position, final Vector3D velocity,
  55.                                                         final double mu) {
  56.         final double r = position.getNorm();
  57.         final double a = r / (2 - r * velocity.getNormSq() / mu);
  58.         if (a >= 0.) {
  59.             return predictPVElliptic(dt, position, velocity, mu, a, r);
  60.         } else {
  61.             return predictPVHyperbolic(dt, position, velocity, mu, a, r);
  62.         }
  63.     }

  64.     /**
  65.      * Method to propagate position and velocity according to Keplerian dynamics, in the case of an elliptic trajectory.
  66.      * @param dt time of flight
  67.      * @param position initial position vector
  68.      * @param velocity initial velocity vector
  69.      * @param mu central body gravitational parameter
  70.      * @param a semi-major axis
  71.      * @param r initial radius
  72.      * @return predicted position-velocity
  73.      */
  74.     private static PVCoordinates predictPVElliptic(final double dt, final Vector3D position, final Vector3D velocity,
  75.                                                    final double mu, final double a, final double r) {
  76.         // preliminary computation
  77.         final Vector3D pvM     = position.crossProduct(velocity);
  78.         final double muA       = mu * a;

  79.         // compute mean anomaly
  80.         final double eSE    = position.dotProduct(velocity) / FastMath.sqrt(muA);
  81.         final double eCE    = 1. - r / a;
  82.         final double E0     = FastMath.atan2(eSE, eCE);
  83.         final double M0     = E0 - eSE;

  84.         final double e         = FastMath.sqrt(eCE * eCE + eSE * eSE);
  85.         final double sqrt      = FastMath.sqrt((1 + e) / (1 - e));

  86.         // find canonical 2D frame with p pointing to perigee
  87.         final double v0     = 2 * FastMath.atan(sqrt * FastMath.tan(E0 / 2));
  88.         final Rotation rotation = new Rotation(pvM, v0, RotationConvention.FRAME_TRANSFORM);
  89.         final Vector3D p    = rotation.applyTo(position).normalize();
  90.         final Vector3D q    = pvM.crossProduct(p).normalize();

  91.         // compute shifted eccentric anomaly
  92.         final double sqrtRatio = FastMath.sqrt(mu / a);
  93.         final double meanMotion = sqrtRatio / a;
  94.         final double M1     = M0 + meanMotion * dt;
  95.         final double E1     = KeplerianAnomalyUtility.ellipticMeanToEccentric(e, M1);

  96.         // compute shifted in-plane Cartesian coordinates
  97.         final SinCos scE    = FastMath.sinCos(E1);
  98.         final double cE     = scE.cos();
  99.         final double sE     = scE.sin();
  100.         final double sE2m1  = FastMath.sqrt((1 - e) * (1 + e));

  101.         // coordinates of position and velocity in the orbital plane
  102.         final double x      = a * (cE - e);
  103.         final double y      = a * sE2m1 * sE;
  104.         final double factor = sqrtRatio / (1 - e * cE);
  105.         final double xDot   = -factor * sE;
  106.         final double yDot   =  factor * sE2m1 * cE;

  107.         final Vector3D predictedPosition = new Vector3D(x, p, y, q);
  108.         final Vector3D predictedVelocity = new Vector3D(xDot, p, yDot, q);
  109.         return new PVCoordinates(predictedPosition, predictedVelocity);
  110.     }

  111.     /**
  112.      * Method to propagate position and velocity according to Keplerian dynamics, in the case of a hyperbolic trajectory.
  113.      * @param dt time of flight
  114.      * @param position initial position vector
  115.      * @param velocity initial velocity vector
  116.      * @param mu central body gravitational parameter
  117.      * @param a semi-major axis
  118.      * @param r initial radius
  119.      * @return predicted position-velocity
  120.      */
  121.     private static PVCoordinates predictPVHyperbolic(final double dt, final Vector3D position, final Vector3D velocity,
  122.                                                      final double mu, final double a, final double r) {
  123.         // preliminary computations
  124.         final Vector3D pvM   = position.crossProduct(velocity);
  125.         final double muA     = mu * a;
  126.         final double e       = FastMath.sqrt(1 - pvM.getNormSq() / muA);
  127.         final double sqrt    = FastMath.sqrt((e + 1) / (e - 1));

  128.         // compute mean anomaly
  129.         final double eSH     = position.dotProduct(velocity) / FastMath.sqrt(-muA);
  130.         final double eCH     = 1. - r / a;
  131.         final double H0      = FastMath.log((eCH + eSH) / (eCH - eSH)) / 2;
  132.         final double M0      = e * FastMath.sinh(H0) - H0;

  133.         // find canonical 2D frame with p pointing to perigee
  134.         final double v0      = 2 * FastMath.atan(sqrt * FastMath.tanh(H0 / 2));
  135.         final Rotation rotation = new Rotation(pvM, v0, RotationConvention.FRAME_TRANSFORM);
  136.         final Vector3D p     = rotation.applyTo(position).normalize();
  137.         final Vector3D q     = pvM.crossProduct(p).normalize();

  138.         // compute shifted eccentric anomaly
  139.         final double absA = FastMath.abs(a);
  140.         final double sqrtRatio = FastMath.sqrt(mu / absA);
  141.         final double meanMotion = sqrtRatio / absA;
  142.         final double M1      = M0 + meanMotion * dt;
  143.         final double H1      = KeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);

  144.         // compute shifted in-plane Cartesian coordinates
  145.         final double cH     = FastMath.cosh(H1);
  146.         final double sH     = FastMath.sinh(H1);
  147.         final double sE2m1  = FastMath.sqrt((e - 1) * (e + 1));

  148.         // coordinates of position and velocity in the orbital plane
  149.         final double x      = a * (cH - e);
  150.         final double y      = -a * sE2m1 * sH;
  151.         final double factor = sqrtRatio / (e * cH - 1);
  152.         final double xDot   = -factor * sH;
  153.         final double yDot   =  factor * sE2m1 * cH;

  154.         final Vector3D predictedPosition = new Vector3D(x, p, y, q);
  155.         final Vector3D predictedVelocity = new Vector3D(xDot, p, yDot, q);
  156.         return new PVCoordinates(predictedPosition, predictedVelocity);
  157.     }

  158.     /**
  159.      * Method to propagate position and velocity according to Keplerian dynamics.
  160.      * For long time of flights, it is preferable to use {@link org.orekit.propagation.analytical.KeplerianPropagator}.
  161.      * @param <T> field type
  162.      * @param dt time of flight
  163.      * @param position initial position vector
  164.      * @param velocity initial velocity vector
  165.      * @param mu central body gravitational parameter
  166.      * @return predicted position-velocity
  167.      */
  168.     public static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> predictPositionVelocity(final T dt,
  169.                                                                                                     final FieldVector3D<T> position,
  170.                                                                                                     final FieldVector3D<T> velocity,
  171.                                                                                                     final T mu) {
  172.         final T r = position.getNorm();
  173.         final T a = r.divide(r.multiply(velocity.getNormSq()).divide(mu).negate().add(2));
  174.         if (a.getReal() >= 0.) {
  175.             return predictPVElliptic(dt, position, velocity, mu, a, r);
  176.         } else {
  177.             return predictPVHyperbolic(dt, position, velocity, mu, a, r);
  178.         }
  179.     }

  180.     /**
  181.      * Method to propagate position and velocity according to Keplerian dynamics, in the case of an elliptic trajectory.
  182.      * @param <T> field type
  183.      * @param dt time of flight
  184.      * @param position initial position vector
  185.      * @param velocity initial velocity vector
  186.      * @param mu central body gravitational parameter
  187.      * @param a semi-major axis
  188.      * @param r initial radius
  189.      * @return predicted position-velocity
  190.      */
  191.     private static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> predictPVElliptic(final T dt,
  192.                                                                                                final FieldVector3D<T> position,
  193.                                                                                                final FieldVector3D<T> velocity,
  194.                                                                                                final T mu, final T a,
  195.                                                                                                final T r) {
  196.         // preliminary computation0
  197.         final FieldVector3D<T>      pvM = position.crossProduct(velocity);
  198.         final T muA                     = mu.multiply(a);

  199.         // compute mean anomaly
  200.         final T eSE              = position.dotProduct(velocity).divide(muA.sqrt());
  201.         final T eCE              = r.divide(a).negate().add(1);
  202.         final T E0               = FastMath.atan2(eSE, eCE);
  203.         final T M0               = E0.subtract(eSE);

  204.         final T e                       = eCE.square().add(eSE.square()).sqrt();
  205.         final T ePlusOne = e.add(1);
  206.         final T oneMinusE = e.negate().add(1);
  207.         final T sqrt                    = ePlusOne.divide(oneMinusE).sqrt();

  208.         // find canonical 2D frame with p pointing to perigee
  209.         final T v0               = sqrt.multiply((E0.divide(2)).tan()).atan().multiply(2);
  210.         final FieldRotation<T> rotation = new FieldRotation<>(pvM, v0, RotationConvention.FRAME_TRANSFORM);
  211.         final FieldVector3D<T> p = rotation.applyTo(position).normalize();
  212.         final FieldVector3D<T> q = pvM.crossProduct(p).normalize();

  213.         // compute shifted eccentric anomaly
  214.         final T sqrtRatio = (a.reciprocal().multiply(mu)).sqrt();
  215.         final T meanMotion = sqrtRatio.divide(a);
  216.         final T M1               = M0.add(meanMotion.multiply(dt));
  217.         final T E1               = FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, M1);

  218.         // compute shifted in-plane Cartesian coordinates
  219.         final FieldSinCos<T> scE = FastMath.sinCos(E1);
  220.         final T               cE = scE.cos();
  221.         final T               sE = scE.sin();
  222.         final T            sE2m1 = oneMinusE.multiply(ePlusOne).sqrt();

  223.         // coordinates of position and velocity in the orbital plane
  224.         final T x        = a.multiply(cE.subtract(e));
  225.         final T y        = a.multiply(sE2m1).multiply(sE);
  226.         final T factor   = sqrtRatio.divide(e.multiply(cE).negate().add(1));
  227.         final T xDot     = factor.multiply(sE).negate();
  228.         final T yDot     = factor.multiply(sE2m1).multiply(cE);

  229.         final FieldVector3D<T> predictedPosition = new FieldVector3D<>(x, p, y, q);
  230.         final FieldVector3D<T> predictedVelocity = new FieldVector3D<>(xDot, p, yDot, q);
  231.         return new FieldPVCoordinates<>(predictedPosition, predictedVelocity);
  232.     }

  233.     /**
  234.      * Method to propagate position and velocity according to Keplerian dynamics, in the case of a hyperbolic trajectory.
  235.      * @param <T> field type
  236.      * @param dt time of flight
  237.      * @param position initial position vector
  238.      * @param velocity initial velocity vector
  239.      * @param mu central body gravitational parameter
  240.      * @param a semi-major axis
  241.      * @param r initial radius
  242.      * @return predicted position-velocity
  243.      */
  244.     private static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> predictPVHyperbolic(final T dt,
  245.                                                                                                  final FieldVector3D<T> position,
  246.                                                                                                  final FieldVector3D<T> velocity,
  247.                                                                                                  final T mu, final T a,
  248.                                                                                                  final T r) {
  249.         // preliminary computations
  250.         final FieldVector3D<T> pvM   = position.crossProduct(velocity);
  251.         final T muA     = a.multiply(mu);
  252.         final T e       = a.newInstance(1.).subtract(pvM.getNormSq().divide(muA)).sqrt();
  253.         final T ePlusOne = e.add(1);
  254.         final T eMinusOne = e.subtract(1);
  255.         final T sqrt    = ePlusOne.divide(eMinusOne).sqrt();

  256.         // compute mean anomaly
  257.         final T eSH     = position.dotProduct(velocity).divide(muA.negate().sqrt());
  258.         final T eCH     = r.divide(a).negate().add(1);
  259.         final T H0      = eCH.add(eSH).divide(eCH.subtract(eSH)).log().divide(2);
  260.         final T M0      = e.multiply(H0.sinh()).subtract(H0);

  261.         // find canonical 2D frame with p pointing to perigee
  262.         final T v0      = sqrt.multiply(H0.divide(2).tanh()).atan().multiply(2);
  263.         final FieldRotation<T> rotation = new FieldRotation<>(pvM, v0, RotationConvention.FRAME_TRANSFORM);
  264.         final FieldVector3D<T> p     = rotation.applyTo(position).normalize();
  265.         final FieldVector3D<T> q     = pvM.crossProduct(p).normalize();

  266.         // compute shifted eccentric anomaly
  267.         final T sqrtRatio = (a.reciprocal().negate().multiply(mu)).sqrt();
  268.         final T meanMotion = sqrtRatio.divide(a).negate();
  269.         final T M1      = M0.add(meanMotion.multiply(dt));
  270.         final T H1      = FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);

  271.         // compute shifted in-plane Cartesian coordinates
  272.         final T cH     = H1.cosh();
  273.         final T sH     = H1.sinh();
  274.         final T sE2m1  = eMinusOne.multiply(ePlusOne).sqrt();

  275.         // coordinates of position and velocity in the orbital plane
  276.         final T x      = a.multiply(cH.subtract(e));
  277.         final T y      = a.negate().multiply(sE2m1).multiply(sH);
  278.         final T factor = sqrtRatio.divide(e.multiply(cH).subtract(1));
  279.         final T xDot   = factor.negate().multiply(sH);
  280.         final T yDot   =  factor.multiply(sE2m1).multiply(cH);

  281.         final FieldVector3D<T> predictedPosition = new FieldVector3D<>(x, p, y, q);
  282.         final FieldVector3D<T> predictedVelocity = new FieldVector3D<>(xDot, p, yDot, q);
  283.         return new FieldPVCoordinates<>(predictedPosition, predictedVelocity);
  284.     }
  285. }