KeplerianMotionCartesianUtility.java
- /* Copyright 2022-2024 Romain Serra
- * Licensed to CS GROUP (CS) under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership.
- * CS licenses this file to You under the Apache License, Version 2.0
- * (the "License"); you may not use this file except in compliance with
- * the License. You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
- package org.orekit.orbits;
- import org.hipparchus.CalculusFieldElement;
- import org.hipparchus.geometry.euclidean.threed.FieldRotation;
- import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
- import org.hipparchus.geometry.euclidean.threed.Rotation;
- import org.hipparchus.geometry.euclidean.threed.RotationConvention;
- import org.hipparchus.geometry.euclidean.threed.Vector3D;
- import org.hipparchus.util.FastMath;
- import org.hipparchus.util.FieldSinCos;
- import org.hipparchus.util.SinCos;
- import org.orekit.utils.FieldPVCoordinates;
- import org.orekit.utils.PVCoordinates;
- /**
- * Utility class to predict position and velocity under Keplerian motion, using lightweight routines based on Cartesian
- * coordinates. Computations do not require a reference frame or an epoch.
- *
- * @author Andrew Goetz
- * @author Romain Serra
- * @see org.orekit.propagation.analytical.KeplerianPropagator
- * @see org.orekit.propagation.analytical.FieldKeplerianPropagator
- * @see CartesianOrbit
- * @see FieldCartesianOrbit
- * @since 12.1
- */
- public class KeplerianMotionCartesianUtility {
- private KeplerianMotionCartesianUtility() {
- // utility class
- }
- /**
- * Method to propagate position and velocity according to Keplerian dynamics.
- * For long time of flights, it is preferable to use {@link org.orekit.propagation.analytical.KeplerianPropagator}.
- * @param dt time of flight
- * @param position initial position vector
- * @param velocity initial velocity vector
- * @param mu central body gravitational parameter
- * @return predicted position-velocity
- */
- public static PVCoordinates predictPositionVelocity(final double dt, final Vector3D position, final Vector3D velocity,
- final double mu) {
- final double r = position.getNorm();
- final double a = r / (2 - r * velocity.getNormSq() / mu);
- if (a >= 0.) {
- return predictPVElliptic(dt, position, velocity, mu, a, r);
- } else {
- return predictPVHyperbolic(dt, position, velocity, mu, a, r);
- }
- }
- /**
- * Method to propagate position and velocity according to Keplerian dynamics, in the case of an elliptic trajectory.
- * @param dt time of flight
- * @param position initial position vector
- * @param velocity initial velocity vector
- * @param mu central body gravitational parameter
- * @param a semi-major axis
- * @param r initial radius
- * @return predicted position-velocity
- */
- private static PVCoordinates predictPVElliptic(final double dt, final Vector3D position, final Vector3D velocity,
- final double mu, final double a, final double r) {
- // preliminary computation
- final Vector3D pvM = position.crossProduct(velocity);
- final double muA = mu * a;
- // compute mean anomaly
- final double eSE = position.dotProduct(velocity) / FastMath.sqrt(muA);
- final double eCE = 1. - r / a;
- final double E0 = FastMath.atan2(eSE, eCE);
- final double M0 = E0 - eSE;
- final double e = FastMath.sqrt(eCE * eCE + eSE * eSE);
- final double sqrt = FastMath.sqrt((1 + e) / (1 - e));
- // find canonical 2D frame with p pointing to perigee
- final double v0 = 2 * FastMath.atan(sqrt * FastMath.tan(E0 / 2));
- final Rotation rotation = new Rotation(pvM, v0, RotationConvention.FRAME_TRANSFORM);
- final Vector3D p = rotation.applyTo(position).normalize();
- final Vector3D q = pvM.crossProduct(p).normalize();
- // compute shifted eccentric anomaly
- final double sqrtRatio = FastMath.sqrt(mu / a);
- final double meanMotion = sqrtRatio / a;
- final double M1 = M0 + meanMotion * dt;
- final double E1 = KeplerianAnomalyUtility.ellipticMeanToEccentric(e, M1);
- // compute shifted in-plane Cartesian coordinates
- final SinCos scE = FastMath.sinCos(E1);
- final double cE = scE.cos();
- final double sE = scE.sin();
- final double sE2m1 = FastMath.sqrt((1 - e) * (1 + e));
- // coordinates of position and velocity in the orbital plane
- final double x = a * (cE - e);
- final double y = a * sE2m1 * sE;
- final double factor = sqrtRatio / (1 - e * cE);
- final double xDot = -factor * sE;
- final double yDot = factor * sE2m1 * cE;
- final Vector3D predictedPosition = new Vector3D(x, p, y, q);
- final Vector3D predictedVelocity = new Vector3D(xDot, p, yDot, q);
- return new PVCoordinates(predictedPosition, predictedVelocity);
- }
- /**
- * Method to propagate position and velocity according to Keplerian dynamics, in the case of a hyperbolic trajectory.
- * @param dt time of flight
- * @param position initial position vector
- * @param velocity initial velocity vector
- * @param mu central body gravitational parameter
- * @param a semi-major axis
- * @param r initial radius
- * @return predicted position-velocity
- */
- private static PVCoordinates predictPVHyperbolic(final double dt, final Vector3D position, final Vector3D velocity,
- final double mu, final double a, final double r) {
- // preliminary computations
- final Vector3D pvM = position.crossProduct(velocity);
- final double muA = mu * a;
- final double e = FastMath.sqrt(1 - pvM.getNormSq() / muA);
- final double sqrt = FastMath.sqrt((e + 1) / (e - 1));
- // compute mean anomaly
- final double eSH = position.dotProduct(velocity) / FastMath.sqrt(-muA);
- final double eCH = 1. - r / a;
- final double H0 = FastMath.log((eCH + eSH) / (eCH - eSH)) / 2;
- final double M0 = e * FastMath.sinh(H0) - H0;
- // find canonical 2D frame with p pointing to perigee
- final double v0 = 2 * FastMath.atan(sqrt * FastMath.tanh(H0 / 2));
- final Rotation rotation = new Rotation(pvM, v0, RotationConvention.FRAME_TRANSFORM);
- final Vector3D p = rotation.applyTo(position).normalize();
- final Vector3D q = pvM.crossProduct(p).normalize();
- // compute shifted eccentric anomaly
- final double absA = FastMath.abs(a);
- final double sqrtRatio = FastMath.sqrt(mu / absA);
- final double meanMotion = sqrtRatio / absA;
- final double M1 = M0 + meanMotion * dt;
- final double H1 = KeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);
- // compute shifted in-plane Cartesian coordinates
- final double cH = FastMath.cosh(H1);
- final double sH = FastMath.sinh(H1);
- final double sE2m1 = FastMath.sqrt((e - 1) * (e + 1));
- // coordinates of position and velocity in the orbital plane
- final double x = a * (cH - e);
- final double y = -a * sE2m1 * sH;
- final double factor = sqrtRatio / (e * cH - 1);
- final double xDot = -factor * sH;
- final double yDot = factor * sE2m1 * cH;
- final Vector3D predictedPosition = new Vector3D(x, p, y, q);
- final Vector3D predictedVelocity = new Vector3D(xDot, p, yDot, q);
- return new PVCoordinates(predictedPosition, predictedVelocity);
- }
- /**
- * Method to propagate position and velocity according to Keplerian dynamics.
- * For long time of flights, it is preferable to use {@link org.orekit.propagation.analytical.KeplerianPropagator}.
- * @param <T> field type
- * @param dt time of flight
- * @param position initial position vector
- * @param velocity initial velocity vector
- * @param mu central body gravitational parameter
- * @return predicted position-velocity
- */
- public static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> predictPositionVelocity(final T dt,
- final FieldVector3D<T> position,
- final FieldVector3D<T> velocity,
- final T mu) {
- final T r = position.getNorm();
- final T a = r.divide(r.multiply(velocity.getNormSq()).divide(mu).negate().add(2));
- if (a.getReal() >= 0.) {
- return predictPVElliptic(dt, position, velocity, mu, a, r);
- } else {
- return predictPVHyperbolic(dt, position, velocity, mu, a, r);
- }
- }
- /**
- * Method to propagate position and velocity according to Keplerian dynamics, in the case of an elliptic trajectory.
- * @param <T> field type
- * @param dt time of flight
- * @param position initial position vector
- * @param velocity initial velocity vector
- * @param mu central body gravitational parameter
- * @param a semi-major axis
- * @param r initial radius
- * @return predicted position-velocity
- */
- private static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> predictPVElliptic(final T dt,
- final FieldVector3D<T> position,
- final FieldVector3D<T> velocity,
- final T mu, final T a,
- final T r) {
- // preliminary computation0
- final FieldVector3D<T> pvM = position.crossProduct(velocity);
- final T muA = mu.multiply(a);
- // compute mean anomaly
- final T eSE = position.dotProduct(velocity).divide(muA.sqrt());
- final T eCE = r.divide(a).negate().add(1);
- final T E0 = FastMath.atan2(eSE, eCE);
- final T M0 = E0.subtract(eSE);
- final T e = eCE.square().add(eSE.square()).sqrt();
- final T ePlusOne = e.add(1);
- final T oneMinusE = e.negate().add(1);
- final T sqrt = ePlusOne.divide(oneMinusE).sqrt();
- // find canonical 2D frame with p pointing to perigee
- final T v0 = sqrt.multiply((E0.divide(2)).tan()).atan().multiply(2);
- final FieldRotation<T> rotation = new FieldRotation<>(pvM, v0, RotationConvention.FRAME_TRANSFORM);
- final FieldVector3D<T> p = rotation.applyTo(position).normalize();
- final FieldVector3D<T> q = pvM.crossProduct(p).normalize();
- // compute shifted eccentric anomaly
- final T sqrtRatio = (a.reciprocal().multiply(mu)).sqrt();
- final T meanMotion = sqrtRatio.divide(a);
- final T M1 = M0.add(meanMotion.multiply(dt));
- final T E1 = FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, M1);
- // compute shifted in-plane Cartesian coordinates
- final FieldSinCos<T> scE = FastMath.sinCos(E1);
- final T cE = scE.cos();
- final T sE = scE.sin();
- final T sE2m1 = oneMinusE.multiply(ePlusOne).sqrt();
- // coordinates of position and velocity in the orbital plane
- final T x = a.multiply(cE.subtract(e));
- final T y = a.multiply(sE2m1).multiply(sE);
- final T factor = sqrtRatio.divide(e.multiply(cE).negate().add(1));
- final T xDot = factor.multiply(sE).negate();
- final T yDot = factor.multiply(sE2m1).multiply(cE);
- final FieldVector3D<T> predictedPosition = new FieldVector3D<>(x, p, y, q);
- final FieldVector3D<T> predictedVelocity = new FieldVector3D<>(xDot, p, yDot, q);
- return new FieldPVCoordinates<>(predictedPosition, predictedVelocity);
- }
- /**
- * Method to propagate position and velocity according to Keplerian dynamics, in the case of a hyperbolic trajectory.
- * @param <T> field type
- * @param dt time of flight
- * @param position initial position vector
- * @param velocity initial velocity vector
- * @param mu central body gravitational parameter
- * @param a semi-major axis
- * @param r initial radius
- * @return predicted position-velocity
- */
- private static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> predictPVHyperbolic(final T dt,
- final FieldVector3D<T> position,
- final FieldVector3D<T> velocity,
- final T mu, final T a,
- final T r) {
- // preliminary computations
- final FieldVector3D<T> pvM = position.crossProduct(velocity);
- final T muA = a.multiply(mu);
- final T e = a.newInstance(1.).subtract(pvM.getNormSq().divide(muA)).sqrt();
- final T ePlusOne = e.add(1);
- final T eMinusOne = e.subtract(1);
- final T sqrt = ePlusOne.divide(eMinusOne).sqrt();
- // compute mean anomaly
- final T eSH = position.dotProduct(velocity).divide(muA.negate().sqrt());
- final T eCH = r.divide(a).negate().add(1);
- final T H0 = eCH.add(eSH).divide(eCH.subtract(eSH)).log().divide(2);
- final T M0 = e.multiply(H0.sinh()).subtract(H0);
- // find canonical 2D frame with p pointing to perigee
- final T v0 = sqrt.multiply(H0.divide(2).tanh()).atan().multiply(2);
- final FieldRotation<T> rotation = new FieldRotation<>(pvM, v0, RotationConvention.FRAME_TRANSFORM);
- final FieldVector3D<T> p = rotation.applyTo(position).normalize();
- final FieldVector3D<T> q = pvM.crossProduct(p).normalize();
- // compute shifted eccentric anomaly
- final T sqrtRatio = (a.reciprocal().negate().multiply(mu)).sqrt();
- final T meanMotion = sqrtRatio.divide(a).negate();
- final T M1 = M0.add(meanMotion.multiply(dt));
- final T H1 = FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);
- // compute shifted in-plane Cartesian coordinates
- final T cH = H1.cosh();
- final T sH = H1.sinh();
- final T sE2m1 = eMinusOne.multiply(ePlusOne).sqrt();
- // coordinates of position and velocity in the orbital plane
- final T x = a.multiply(cH.subtract(e));
- final T y = a.negate().multiply(sE2m1).multiply(sH);
- final T factor = sqrtRatio.divide(e.multiply(cH).subtract(1));
- final T xDot = factor.negate().multiply(sH);
- final T yDot = factor.multiply(sE2m1).multiply(cH);
- final FieldVector3D<T> predictedPosition = new FieldVector3D<>(x, p, y, q);
- final FieldVector3D<T> predictedVelocity = new FieldVector3D<>(xDot, p, yDot, q);
- return new FieldPVCoordinates<>(predictedPosition, predictedVelocity);
- }
- }