FieldOrbit.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.orbits;



  21. import org.hipparchus.CalculusFieldElement;
  22. import org.hipparchus.Field;
  23. import org.hipparchus.analysis.differentiation.Derivative;
  24. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  25. import org.hipparchus.linear.FieldDecompositionSolver;
  26. import org.hipparchus.linear.FieldLUDecomposition;
  27. import org.hipparchus.linear.FieldMatrix;
  28. import org.hipparchus.linear.MatrixUtils;
  29. import org.hipparchus.util.MathArrays;
  30. import org.orekit.errors.OrekitIllegalArgumentException;
  31. import org.orekit.errors.OrekitInternalError;
  32. import org.orekit.errors.OrekitMessages;
  33. import org.orekit.frames.FieldStaticTransform;
  34. import org.orekit.frames.FieldTransform;
  35. import org.orekit.frames.Frame;
  36. import org.orekit.time.FieldAbsoluteDate;
  37. import org.orekit.time.FieldTimeShiftable;
  38. import org.orekit.time.FieldTimeStamped;
  39. import org.orekit.utils.FieldPVCoordinates;
  40. import org.orekit.utils.FieldPVCoordinatesProvider;
  41. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  42. import org.orekit.utils.TimeStampedPVCoordinates;

  44. /**
  45.  * This class handles orbital parameters.

  47.  * <p>
  48.  * For user convenience, both the Cartesian and the equinoctial elements
  49.  * are provided by this class, regardless of the canonical representation
  50.  * implemented in the derived class (which may be classical Keplerian
  51.  * elements for example).
  52.  * </p>
  53.  * <p>
  54.  * The parameters are defined in a frame specified by the user. It is important
  55.  * to make sure this frame is consistent: it probably is inertial and centered
  56.  * on the central body. This information is used for example by some
  57.  * force models.
  58.  * </p>
  59.  * <p>
  60.  * Instance of this class are guaranteed to be immutable.
  61.  * </p>
  62.  * @author Luc Maisonobe
  63.  * @author Guylaine Prat
  64.  * @author Fabien Maussion
  65.  * @author V&eacute;ronique Pommier-Maurussane
  66.  * @since 9.0
  67.  * @see Orbit
  68.  * @param <T> type of the field elements
  69.  */
  70. public abstract class FieldOrbit<T extends CalculusFieldElement<T>>
  71.     implements FieldPVCoordinatesProvider<T>, FieldTimeStamped<T>, FieldTimeShiftable<FieldOrbit<T>, T> {

  73.     /** Absolute tolerance when checking if the rate of the position angle is Keplerian or not. */
  74.     protected static final double TOLERANCE_POSITION_ANGLE_RATE = 1e-15;

  76.     /** Frame in which are defined the orbital parameters. */
  77.     private final Frame frame;

  79.     /** Date of the orbital parameters. */
  80.     private final FieldAbsoluteDate<T> date;

  82.     /** Value of mu used to compute position and velocity (m³/s²). */
  83.     private final T mu;

  85.     /** Field-valued one.
  86.      * @since 12.0
  87.      * */
  88.     private final T one;

  90.     /** Field-valued zero.
  91.      * @since 12.0
  92.      * */
  93.     private final T zero;

  95.     /** Field used by this class.
  96.      * @since 12.0
  97.      */
  98.     private final Field<T> field;

  100.     /** Computed position.
  101.      * @since 12.0
  102.      */
  103.     private FieldVector3D<T> position;

  105.     /** Computed PVCoordinates. */
  106.     private TimeStampedFieldPVCoordinates<T> pvCoordinates;

  108.     /** Jacobian of the orbital parameters with mean angle with respect to the Cartesian coordinates. */
  109.     private T[][] jacobianMeanWrtCartesian;

  111.     /** Jacobian of the Cartesian coordinates with respect to the orbital parameters with mean angle. */
  112.     private T[][] jacobianWrtParametersMean;

  114.     /** Jacobian of the orbital parameters with eccentric angle with respect to the Cartesian coordinates. */
  115.     private T[][] jacobianEccentricWrtCartesian;

  117.     /** Jacobian of the Cartesian coordinates with respect to the orbital parameters with eccentric angle. */
  118.     private T[][] jacobianWrtParametersEccentric;

  120.     /** Jacobian of the orbital parameters with true angle with respect to the Cartesian coordinates. */
  121.     private T[][] jacobianTrueWrtCartesian;

  123.     /** Jacobian of the Cartesian coordinates with respect to the orbital parameters with true angle. */
  124.     private T[][] jacobianWrtParametersTrue;

  126.     /** Default constructor.
  127.      * Build a new instance with arbitrary default elements.
  128.      * @param frame the frame in which the parameters are defined
  129.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  130.      * @param date date of the orbital parameters
  131.      * @param mu central attraction coefficient (m^3/s^2)
  132.      * @exception IllegalArgumentException if frame is not a {@link
  133.      * Frame#isPseudoInertial pseudo-inertial frame}
  134.      */
  135.     protected FieldOrbit(final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  136.         throws IllegalArgumentException {
  137.         ensurePseudoInertialFrame(frame);
  138.         this.field = mu.getField();
  139.         this.zero = this.field.getZero();
  140.         this.one = this.field.getOne();
  141.         this.date                      = date;
  142.         this.mu                        = mu;
  143.         this.pvCoordinates             = null;
  144.         this.frame                     = frame;
  145.         jacobianMeanWrtCartesian       = null;
  146.         jacobianWrtParametersMean      = null;
  147.         jacobianEccentricWrtCartesian  = null;
  148.         jacobianWrtParametersEccentric = null;
  149.         jacobianTrueWrtCartesian       = null;
  150.         jacobianWrtParametersTrue      = null;
  151.     }

  153.     /** Set the orbit from Cartesian parameters.
  154.      *
  155.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  156.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  157.      * use {@code mu} and the position to compute the acceleration, including
  158.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  159.      *
  160.      * @param fieldPVCoordinates the position and velocity in the inertial frame
  161.      * @param frame the frame in which the {@link TimeStampedPVCoordinates} are defined
  162.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  163.      * @param mu central attraction coefficient (m^3/s^2)
  164.      * @exception IllegalArgumentException if frame is not a {@link
  165.      * Frame#isPseudoInertial pseudo-inertial frame}
  166.      */
  167.     protected FieldOrbit(final TimeStampedFieldPVCoordinates<T> fieldPVCoordinates, final Frame frame, final T mu)
  168.         throws IllegalArgumentException {
  169.         ensurePseudoInertialFrame(frame);
  170.         this.field = mu.getField();
  171.         this.zero = this.field.getZero();
  172.         this.one = this.field.getOne();
  173.         this.date = fieldPVCoordinates.getDate();
  174.         this.mu = mu;
  175.         if (fieldPVCoordinates.getAcceleration().getNormSq().getReal() == 0.0) {
  176.             // the acceleration was not provided,
  177.             // compute it from Newtonian attraction
  178.             final T r2 = fieldPVCoordinates.getPosition().getNormSq();
  179.             final T r3 = r2.multiply(r2.sqrt());
  180.             this.pvCoordinates = new TimeStampedFieldPVCoordinates<>(fieldPVCoordinates.getDate(),
  181.                                                                      fieldPVCoordinates.getPosition(),
  182.                                                                      fieldPVCoordinates.getVelocity(),
  183.                                                                      new FieldVector3D<>(r3.reciprocal().multiply(mu.negate()), fieldPVCoordinates.getPosition()));
  184.         } else {
  185.             this.pvCoordinates = fieldPVCoordinates;
  186.         }
  187.         this.frame = frame;
  188.     }

  190.     /** Check if Cartesian coordinates include non-Keplerian acceleration.
  191.      * @param pva Cartesian coordinates
  192.      * @param mu central attraction coefficient
  193.      * @param <T> type of the field elements
  194.      * @return true if Cartesian coordinates include non-Keplerian acceleration
  195.      */
  196.     protected static <T extends CalculusFieldElement<T>> boolean hasNonKeplerianAcceleration(final FieldPVCoordinates<T> pva, final T mu) {

  198.         if (mu.getField().getZero() instanceof Derivative<?>) {
  199.             return Orbit.hasNonKeplerianAcceleration(pva.toPVCoordinates(), mu.getReal()); // for performance

  201.         } else {
  202.             final FieldVector3D<T> a = pva.getAcceleration();
  203.             if (a == null) {
  204.                 return false;
  205.             }

  207.             final FieldVector3D<T> p = pva.getPosition();
  208.             final T r2 = p.getNormSq();

  210.             // Check if acceleration is relatively close to 0 compared to the Keplerian acceleration
  211.             final double tolerance = mu.getReal() * 1e-9;
  212.             final FieldVector3D<T> aTimesR2 = a.scalarMultiply(r2);
  213.             if (aTimesR2.getNorm().getReal() < tolerance) {
  214.                 return false;
  215.             }

  217.             if ((aTimesR2.add(p.normalize().scalarMultiply(mu))).getNorm().getReal() > tolerance) {
  218.                 // we have a relevant acceleration, we can compute derivatives
  219.                 return true;
  220.             } else {
  221.                 // the provided acceleration is either too small to be reliable (probably even 0), or NaN
  222.                 return false;
  223.             }
  224.         }

  226.     }

  228.     /** Returns true if and only if the orbit is elliptical i.e. has a non-negative semi-major axis.
  229.      * @return true if getA() is strictly greater than 0
  230.      * @since 12.0
  231.      */
  232.     public boolean isElliptical() {
  233.         return getA().getReal() > 0.;
  234.     }

  236.     /** Get the orbit type.
  237.      * @return orbit type
  238.      */
  239.     public abstract OrbitType getType();

  241.     /** Ensure the defining frame is a pseudo-inertial frame.
  242.      * @param frame frame to check
  243.      * @exception IllegalArgumentException if frame is not a {@link
  244.      * Frame#isPseudoInertial pseudo-inertial frame}
  245.      */
  246.     private static void ensurePseudoInertialFrame(final Frame frame)
  247.         throws IllegalArgumentException {
  248.         if (!frame.isPseudoInertial()) {
  249.             throw new OrekitIllegalArgumentException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME,
  250.                                                      frame.getName());
  251.         }
  252.     }

  254.     /** Get the frame in which the orbital parameters are defined.
  255.      * @return frame in which the orbital parameters are defined
  256.      */
  257.     public Frame getFrame() {
  258.         return frame;
  259.     }

  261.     /**Transforms the FieldOrbit instance into an Orbit instance.
  262.      * @return Orbit instance with same properties
  263.      */
  264.     public abstract Orbit toOrbit();

  266.     /** Get the semi-major axis.
  267.      * <p>Note that the semi-major axis is considered negative for hyperbolic orbits.</p>
  268.      * @return semi-major axis (m)
  269.      */
  270.     public abstract T getA();

  272.     /** Get the semi-major axis derivative.
  273.      * <p>Note that the semi-major axis is considered negative for hyperbolic orbits.</p>
  274.      * <p>
  275.      * If the orbit was created without derivatives, the value returned is null.
  276.      * </p>
  277.      * @return semi-major axis  derivative (m/s)
  278.      */
  279.     public abstract T getADot();

  281.     /** Get the first component of the equinoctial eccentricity vector.
  282.      * @return first component of the equinoctial eccentricity vector
  283.      */
  284.     public abstract T getEquinoctialEx();

  286.     /** Get the first component of the equinoctial eccentricity vector derivative.
  287.      * <p>
  288.      * If the orbit was created without derivatives, the value returned is null.
  289.      * </p>
  290.      * @return first component of the equinoctial eccentricity vector derivative
  291.      */
  292.     public abstract T getEquinoctialExDot();

  294.     /** Get the second component of the equinoctial eccentricity vector.
  295.      * @return second component of the equinoctial eccentricity vector
  296.      */
  297.     public abstract T getEquinoctialEy();

  299.     /** Get the second component of the equinoctial eccentricity vector derivative.
  300.      * <p>
  301.      * If the orbit was created without derivatives, the value returned is null.
  302.      * </p>
  303.      * @return second component of the equinoctial eccentricity vector derivative
  304.      */
  305.     public abstract T getEquinoctialEyDot();

  307.     /** Get the first component of the inclination vector.
  308.      * @return first component of the inclination vector
  309.      */
  310.     public abstract T getHx();

  312.     /** Get the first component of the inclination vector derivative.
  313.      * <p>
  314.      * If the orbit was created without derivatives, the value returned is null.
  315.      * </p>
  316.      * @return first component of the inclination vector derivative
  317.      */
  318.     public abstract T getHxDot();

  320.     /** Get the second component of the inclination vector.
  321.      * @return second component of the inclination vector
  322.      */
  323.     public abstract T getHy();

  325.     /** Get the second component of the inclination vector derivative.
  326.      * @return second component of the inclination vector derivative
  327.      */
  328.     public abstract T getHyDot();

  330.     /** Get the eccentric longitude argument.
  331.      * @return E + ω + Ω eccentric longitude argument (rad)
  332.      */
  333.     public abstract T getLE();

  335.     /** Get the eccentric longitude argument derivative.
  336.      * <p>
  337.      * If the orbit was created without derivatives, the value returned is null.
  338.      * </p>
  339.      * @return d(E + ω + Ω)/dt eccentric longitude argument derivative (rad/s)
  340.      */
  341.     public abstract T getLEDot();

  343.     /** Get the true longitude argument.
  344.      * @return v + ω + Ω true longitude argument (rad)
  345.      */
  346.     public abstract T getLv();

  348.     /** Get the true longitude argument derivative.
  349.      * <p>
  350.      * If the orbit was created without derivatives, the value returned is null.
  351.      * </p>
  352.      * @return d(v + ω + Ω)/dt true longitude argument derivative (rad/s)
  353.      */
  354.     public abstract T getLvDot();

  356.     /** Get the mean longitude argument.
  357.      * @return M + ω + Ω mean longitude argument (rad)
  358.      */
  359.     public abstract T getLM();

  361.     /** Get the mean longitude argument derivative.
  362.      * <p>
  363.      * If the orbit was created without derivatives, the value returned is null.
  364.      * </p>
  365.      * @return d(M + ω + Ω)/dt mean longitude argument derivative (rad/s)
  366.      */
  367.     public abstract T getLMDot();

  369.     // Additional orbital elements

  371.     /** Get the eccentricity.
  372.      * @return eccentricity
  373.      */
  374.     public abstract T getE();

  376.     /** Get the eccentricity derivative.
  377.      * <p>
  378.      * If the orbit was created without derivatives, the value returned is null.
  379.      * </p>
  380.      * @return eccentricity derivative
  381.      */
  382.     public abstract T getEDot();

  384.     /** Get the inclination.
  385.      * <p>
  386.      * If the orbit was created without derivatives, the value returned is null.
  387.      * </p>
  388.      * @return inclination (rad)
  389.      */
  390.     public abstract T getI();

  392.     /** Get the inclination derivative.
  393.      * @return inclination derivative (rad/s)
  394.      */
  395.     public abstract T getIDot();

  397.     /** Check if orbit includes non-Keplerian rates.
  398.      * @return true if orbit includes non-Keplerian derivatives
  399.      * @see #getADot()
  400.      * @see #getEquinoctialExDot()
  401.      * @see #getEquinoctialEyDot()
  402.      * @see #getHxDot()
  403.      * @see #getHyDot()
  404.      * @see #getLEDot()
  405.      * @see #getLvDot()
  406.      * @see #getLMDot()
  407.      * @see #getEDot()
  408.      * @see #getIDot()
  409.      * @since 13.0
  410.      */
  411.     public boolean hasNonKeplerianAcceleration() {
  412.         return hasNonKeplerianAcceleration(getPVCoordinates(), getMu());
  413.     }

  415.     /** Get the central attraction coefficient used for position and velocity conversions (m³/s²).
  416.      * @return central attraction coefficient used for position and velocity conversions (m³/s²)
  417.      */
  418.     public T getMu() {
  419.         return mu;
  420.     }

  422.     /** Get the Keplerian period.
  423.      * <p>The Keplerian period is computed directly from semi major axis
  424.      * and central acceleration constant.</p>
  425.      * @return Keplerian period in seconds, or positive infinity for hyperbolic orbits
  426.      */
  427.     public T getKeplerianPeriod() {
  428.         final T a = getA();
  429.         return isElliptical() ?
  430.                a.multiply(a.getPi().multiply(2.0)).multiply(a.divide(mu).sqrt()) :
  431.                zero.add(Double.POSITIVE_INFINITY);
  432.     }

  434.     /** Get the Keplerian mean motion.
  435.      * <p>The Keplerian mean motion is computed directly from semi major axis
  436.      * and central acceleration constant.</p>
  437.      * @return Keplerian mean motion in radians per second
  438.      */
  439.     public T getKeplerianMeanMotion() {
  440.         final T absA = getA().abs();
  441.         return absA.reciprocal().multiply(mu).sqrt().divide(absA);
  442.     }

  444.     /** Get the derivative of the mean anomaly with respect to the semi major axis.
  445.      * @return derivative of the mean anomaly with respect to the semi major axis
  446.      */
  447.     public T getMeanAnomalyDotWrtA() {
  448.         return getKeplerianMeanMotion().divide(getA()).multiply(-1.5);
  449.     }

  451.     /** Get the date of orbital parameters.
  452.      * @return date of the orbital parameters
  453.      */
  454.     public FieldAbsoluteDate<T> getDate() {
  455.         return date;
  456.     }

  458.     /** Get the {@link TimeStampedPVCoordinates} in a specified frame.
  459.      * @param outputFrame frame in which the position/velocity coordinates shall be computed
  460.      * @return FieldPVCoordinates in the specified output frame
  461.           * @see #getPVCoordinates()
  462.      */
  463.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final Frame outputFrame) {
  464.         if (pvCoordinates == null) {
  465.             pvCoordinates = initPVCoordinates();
  466.         }

  468.         // If output frame requested is the same as definition frame,
  469.         // PV coordinates are returned directly
  470.         if (outputFrame == frame) {
  471.             return pvCoordinates;
  472.         }

  474.         // Else, PV coordinates are transformed to output frame
  475.         final FieldTransform<T> t = frame.getTransformTo(outputFrame, date);
  476.         return t.transformPVCoordinates(pvCoordinates);
  477.     }

  479.     /** {@inheritDoc} */
  480.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> otherDate, final Frame otherFrame) {
  481.         return shiftedBy(otherDate.durationFrom(getDate())).getPVCoordinates(otherFrame);
  482.     }

  484.     /** {@inheritDoc} */
  485.     @Override
  486.     public FieldVector3D<T> getPosition(final FieldAbsoluteDate<T> otherDate, final Frame otherFrame) {
  487.         return shiftedBy(otherDate.durationFrom(getDate())).getPosition(otherFrame);
  488.     }

  490.     /** Get the position in a specified frame.
  491.      * @param outputFrame frame in which the position coordinates shall be computed
  492.      * @return position in the specified output frame
  493.      * @see #getPosition()
  494.      * @since 12.0
  495.      */
  496.     public FieldVector3D<T> getPosition(final Frame outputFrame) {
  497.         if (position == null) {
  498.             position = initPosition();
  499.         }

  501.         // If output frame requested is the same as definition frame,
  502.         // Position vector is returned directly
  503.         if (outputFrame == frame) {
  504.             return position;
  505.         }

  507.         // Else, position vector is transformed to output frame
  508.         final FieldStaticTransform<T> t = frame.getStaticTransformTo(outputFrame, date);
  509.         return t.transformPosition(position);

  511.     }

  513.     /** Get the position in definition frame.
  514.      * @return position in the definition frame
  515.      * @see #getPVCoordinates()
  516.      * @since 12.0
  517.      */
  518.     public FieldVector3D<T> getPosition() {
  519.         if (position == null) {
  520.             position = initPosition();
  521.         }
  522.         return position;
  523.     }

  525.     /** Get the {@link TimeStampedPVCoordinates} in definition frame.
  526.      * @return FieldPVCoordinates in the definition frame
  527.      * @see #getPVCoordinates(Frame)
  528.      */
  529.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates() {
  530.         if (pvCoordinates == null) {
  531.             pvCoordinates = initPVCoordinates();
  532.             position      = pvCoordinates.getPosition();
  533.         }
  534.         return pvCoordinates;
  535.     }

  537.     /** Getter for Field-valued one.
  538.      * @return one
  539.      * */
  540.     protected T getOne() {
  541.         return one;
  542.     }

  544.     /** Getter for Field-valued zero.
  545.      * @return zero
  546.      * */
  547.     protected T getZero() {
  548.         return zero;
  549.     }

  551.     /** Getter for Field.
  552.      * @return CalculusField
  553.      * */
  554.     protected Field<T> getField() {
  555.         return field;
  556.     }

  558.     /** Compute the position coordinates from the canonical parameters.
  559.      * @return computed position coordinates
  560.      * @since 12.0
  561.      */
  562.     protected abstract FieldVector3D<T> initPosition();

  564.     /** Compute the position/velocity coordinates from the canonical parameters.
  565.      * @return computed position/velocity coordinates
  566.      */
  567.     protected abstract TimeStampedFieldPVCoordinates<T> initPVCoordinates();

  569.     /**
  570.      * Create a new object representing the same physical orbital state, but attached to a different reference frame.
  571.      * If the new frame is not inertial, an exception will be thrown.
  572.      *
  573.      * @param inertialFrame reference frame of output orbit
  574.      * @return orbit with different frame
  575.      * @since 13.0
  576.      */
  577.     public abstract FieldOrbit<T> inFrame(Frame inertialFrame);

  579.     /** Get a time-shifted orbit.
  580.      * <p>
  581.      * The orbit can be slightly shifted to close dates. This shift is based on
  582.      * a simple Keplerian model. It is <em>not</em> intended as a replacement
  583.      * for proper orbit and attitude propagation but should be sufficient for
  584.      * small time shifts or coarse accuracy.
  585.      * </p>
  586.      * @param dt time shift in seconds
  587.      * @return a new orbit, shifted with respect to the instance (which is immutable)
  588.      */
  589.     public abstract FieldOrbit<T> shiftedBy(T dt);

  591.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  592.      * <p>
  593.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  594.      * respect to Cartesian coordinate j. This means each row corresponds to one orbital parameter
  595.      * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
  596.      * </p>
  597.      * @param type type of the position angle to use
  598.      * @param jacobian placeholder 6x6 (or larger) matrix to be filled with the Jacobian, if matrix
  599.      * is larger than 6x6, only the 6x6 upper left corner will be modified
  600.      */
  601.     public void getJacobianWrtCartesian(final PositionAngleType type, final T[][] jacobian) {

  603.         final T[][] cachedJacobian;
  604.         synchronized (this) {
  605.             switch (type) {
  606.                 case MEAN :
  607.                     if (jacobianMeanWrtCartesian == null) {
  608.                         // first call, we need to compute the Jacobian and cache it
  609.                         jacobianMeanWrtCartesian = computeJacobianMeanWrtCartesian();
  610.                     }
  611.                     cachedJacobian = jacobianMeanWrtCartesian;
  612.                     break;
  613.                 case ECCENTRIC :
  614.                     if (jacobianEccentricWrtCartesian == null) {
  615.                         // first call, we need to compute the Jacobian and cache it
  616.                         jacobianEccentricWrtCartesian = computeJacobianEccentricWrtCartesian();
  617.                     }
  618.                     cachedJacobian = jacobianEccentricWrtCartesian;
  619.                     break;
  620.                 case TRUE :
  621.                     if (jacobianTrueWrtCartesian == null) {
  622.                         // first call, we need to compute the Jacobian and cache it
  623.                         jacobianTrueWrtCartesian = computeJacobianTrueWrtCartesian();
  624.                     }
  625.                     cachedJacobian = jacobianTrueWrtCartesian;
  626.                     break;
  627.                 default :
  628.                     throw new OrekitInternalError(null);
  629.             }
  630.         }

  632.         // fill the user provided array
  633.         for (int i = 0; i < cachedJacobian.length; ++i) {
  634.             System.arraycopy(cachedJacobian[i], 0, jacobian[i], 0, cachedJacobian[i].length);
  635.         }

  637.     }

  639.     /** Compute the Jacobian of the Cartesian parameters with respect to the orbital parameters.
  640.      * <p>
  641.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i of the orbit with
  642.      * respect to orbital parameter j. This means each row corresponds to one Cartesian coordinate
  643.      * x, y, z, xdot, ydot, zdot whereas columns 0 to 5 correspond to the orbital parameters.
  644.      * </p>
  645.      * @param type type of the position angle to use
  646.      * @param jacobian placeholder 6x6 (or larger) matrix to be filled with the Jacobian, if matrix
  647.      * is larger than 6x6, only the 6x6 upper left corner will be modified
  648.      */
  649.     public void getJacobianWrtParameters(final PositionAngleType type, final T[][] jacobian) {

  651.         final T[][] cachedJacobian;
  652.         synchronized (this) {
  653.             switch (type) {
  654.                 case MEAN :
  655.                     if (jacobianWrtParametersMean == null) {
  656.                         // first call, we need to compute the Jacobian and cache it
  657.                         jacobianWrtParametersMean = createInverseJacobian(type);
  658.                     }
  659.                     cachedJacobian = jacobianWrtParametersMean;
  660.                     break;
  661.                 case ECCENTRIC :
  662.                     if (jacobianWrtParametersEccentric == null) {
  663.                         // first call, we need to compute the Jacobian and cache it
  664.                         jacobianWrtParametersEccentric = createInverseJacobian(type);
  665.                     }
  666.                     cachedJacobian = jacobianWrtParametersEccentric;
  667.                     break;
  668.                 case TRUE :
  669.                     if (jacobianWrtParametersTrue == null) {
  670.                         // first call, we need to compute the Jacobian and cache it
  671.                         jacobianWrtParametersTrue = createInverseJacobian(type);
  672.                     }
  673.                     cachedJacobian = jacobianWrtParametersTrue;
  674.                     break;
  675.                 default :
  676.                     throw new OrekitInternalError(null);
  677.             }
  678.         }

  680.         // fill the user-provided array
  681.         for (int i = 0; i < cachedJacobian.length; ++i) {
  682.             System.arraycopy(cachedJacobian[i], 0, jacobian[i], 0, cachedJacobian[i].length);
  683.         }

  685.     }

  687.     /** Create an inverse Jacobian.
  688.      * @param type type of the position angle to use
  689.      * @return inverse Jacobian
  690.      */
  691.     private T[][] createInverseJacobian(final PositionAngleType type) {

  693.         // get the direct Jacobian
  694.         final T[][] directJacobian = MathArrays.buildArray(getA().getField(), 6, 6);
  695.         getJacobianWrtCartesian(type, directJacobian);

  697.         // invert the direct Jacobian
  698.         final FieldMatrix<T> matrix = MatrixUtils.createFieldMatrix(directJacobian);
  699.         final FieldDecompositionSolver<T> solver = new FieldLUDecomposition<>(matrix).getSolver();
  700.         return solver.getInverse().getData();

  702.     }

  704.     /** Compute the Jacobian of the orbital parameters with mean angle with respect to the Cartesian parameters.
  705.      * <p>
  706.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  707.      * respect to Cartesian coordinate j. This means each row correspond to one orbital parameter
  708.      * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
  709.      * </p>
  710.      * @return 6x6 Jacobian matrix
  711.      * @see #computeJacobianEccentricWrtCartesian()
  712.      * @see #computeJacobianTrueWrtCartesian()
  713.      */
  714.     protected abstract T[][] computeJacobianMeanWrtCartesian();

  716.     /** Compute the Jacobian of the orbital parameters with eccentric angle with respect to the Cartesian parameters.
  717.      * <p>
  718.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  719.      * respect to Cartesian coordinate j. This means each row correspond to one orbital parameter
  720.      * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
  721.      * </p>
  722.      * @return 6x6 Jacobian matrix
  723.      * @see #computeJacobianMeanWrtCartesian()
  724.      * @see #computeJacobianTrueWrtCartesian()
  725.      */
  726.     protected abstract T[][] computeJacobianEccentricWrtCartesian();

  728.     /** Compute the Jacobian of the orbital parameters with true angle with respect to the Cartesian parameters.
  729.      * <p>
  730.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  731.      * respect to Cartesian coordinate j. This means each row correspond to one orbital parameter
  732.      * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
  733.      * </p>
  734.      * @return 6x6 Jacobian matrix
  735.      * @see #computeJacobianMeanWrtCartesian()
  736.      * @see #computeJacobianEccentricWrtCartesian()
  737.      */
  738.     protected abstract T[][] computeJacobianTrueWrtCartesian();

  740.     /** Add the contribution of the Keplerian motion to parameters derivatives
  741.      * <p>
  742.      * This method is used by integration-based propagators to evaluate the part of Keplerian
  743.      * motion to evolution of the orbital state.
  744.      * </p>
  745.      * @param type type of the position angle in the state
  746.      * @param gm attraction coefficient to use
  747.      * @param pDot array containing orbital state derivatives to update (the Keplerian
  748.      * part must be <em>added</em> to the array components, as the array may already
  749.      * contain some non-zero elements corresponding to non-Keplerian parts)
  750.      */
  751.     public abstract void addKeplerContribution(PositionAngleType type, T gm, T[] pDot);

  753.         /** Fill a Jacobian half row with a single vector.
  754.      * @param a coefficient of the vector
  755.      * @param v vector
  756.      * @param row Jacobian matrix row
  757.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  758.      */

  760.     protected void fillHalfRow(final T a, final FieldVector3D<T> v, final T[] row, final int j) {
  761.         row[j]     = a.multiply(v.getX());
  762.         row[j + 1] = a.multiply(v.getY());
  763.         row[j + 2] = a.multiply(v.getZ());
  764.     }

  766.     /** Fill a Jacobian half row with a linear combination of vectors.
  767.      * @param a1 coefficient of the first vector
  768.      * @param v1 first vector
  769.      * @param a2 coefficient of the second vector
  770.      * @param v2 second vector
  771.      * @param row Jacobian matrix row
  772.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  773.      */
  774.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1, final T a2, final FieldVector3D<T> v2,
  775.                                       final T[] row, final int j) {
  776.         row[j]     = a1.linearCombination(a1, v1.getX(), a2, v2.getX());
  777.         row[j + 1] = a1.linearCombination(a1, v1.getY(), a2, v2.getY());
  778.         row[j + 2] = a1.linearCombination(a1, v1.getZ(), a2, v2.getZ());

  780.     }

  782.     /** Fill a Jacobian half row with a linear combination of vectors.
  783.      * @param a1 coefficient of the first vector
  784.      * @param v1 first vector
  785.      * @param a2 coefficient of the second vector
  786.      * @param v2 second vector
  787.      * @param a3 coefficient of the third vector
  788.      * @param v3 third vector
  789.      * @param row Jacobian matrix row
  790.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  791.      */
  792.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1,
  793.                                final T a2, final FieldVector3D<T> v2,
  794.                                final T a3, final FieldVector3D<T> v3,
  795.                                       final T[] row, final int j) {
  796.         row[j]     = a1.linearCombination(a1, v1.getX(), a2, v2.getX(), a3, v3.getX());
  797.         row[j + 1] = a1.linearCombination(a1, v1.getY(), a2, v2.getY(), a3, v3.getY());
  798.         row[j + 2] = a1.linearCombination(a1, v1.getZ(), a2, v2.getZ(), a3, v3.getZ());

  800.     }

  802.     /** Fill a Jacobian half row with a linear combination of vectors.
  803.      * @param a1 coefficient of the first vector
  804.      * @param v1 first vector
  805.      * @param a2 coefficient of the second vector
  806.      * @param v2 second vector
  807.      * @param a3 coefficient of the third vector
  808.      * @param v3 third vector
  809.      * @param a4 coefficient of the fourth vector
  810.      * @param v4 fourth vector
  811.      * @param row Jacobian matrix row
  812.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  813.      */
  814.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1, final T a2, final FieldVector3D<T> v2,
  815.                                       final T a3, final FieldVector3D<T> v3, final T a4, final FieldVector3D<T> v4,
  816.                                       final T[] row, final int j) {
  817.         row[j]     = a1.linearCombination(a1, v1.getX(), a2, v2.getX(), a3, v3.getX(), a4, v4.getX());
  818.         row[j + 1] = a1.linearCombination(a1, v1.getY(), a2, v2.getY(), a3, v3.getY(), a4, v4.getY());
  819.         row[j + 2] = a1.linearCombination(a1, v1.getZ(), a2, v2.getZ(), a3, v3.getZ(), a4, v4.getZ());

  821.     }

  823.     /** Fill a Jacobian half row with a linear combination of vectors.
  824.      * @param a1 coefficient of the first vector
  825.      * @param v1 first vector
  826.      * @param a2 coefficient of the second vector
  827.      * @param v2 second vector
  828.      * @param a3 coefficient of the third vector
  829.      * @param v3 third vector
  830.      * @param a4 coefficient of the fourth vector
  831.      * @param v4 fourth vector
  832.      * @param a5 coefficient of the fifth vector
  833.      * @param v5 fifth vector
  834.      * @param row Jacobian matrix row
  835.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  836.      */
  837.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1, final T a2, final FieldVector3D<T> v2,
  838.                                       final T a3, final FieldVector3D<T> v3, final T a4, final FieldVector3D<T> v4,
  839.                                       final T a5, final FieldVector3D<T> v5,
  840.                                       final T[] row, final int j) {
  841.         row[j]     = a1.linearCombination(a1, v1.getX(), a2, v2.getX(), a3, v3.getX(), a4, v4.getX()).add(a5.multiply(v5.getX()));
  842.         row[j + 1] = a1.linearCombination(a1, v1.getY(), a2, v2.getY(), a3, v3.getY(), a4, v4.getY()).add(a5.multiply(v5.getY()));
  843.         row[j + 2] = a1.linearCombination(a1, v1.getZ(), a2, v2.getZ(), a3, v3.getZ(), a4, v4.getZ()).add(a5.multiply(v5.getZ()));

  845.     }

  847.     /** Fill a Jacobian half row with a linear combination of vectors.
  848.      * @param a1 coefficient of the first vector
  849.      * @param v1 first vector
  850.      * @param a2 coefficient of the second vector
  851.      * @param v2 second vector
  852.      * @param a3 coefficient of the third vector
  853.      * @param v3 third vector
  854.      * @param a4 coefficient of the fourth vector
  855.      * @param v4 fourth vector
  856.      * @param a5 coefficient of the fifth vector
  857.      * @param v5 fifth vector
  858.      * @param a6 coefficient of the sixth vector
  859.      * @param v6 sixth vector
  860.      * @param row Jacobian matrix row
  861.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  862.      */
  863.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1, final T a2, final FieldVector3D<T> v2,
  864.                                       final T a3, final FieldVector3D<T> v3, final T a4, final FieldVector3D<T> v4,
  865.                                       final T a5, final FieldVector3D<T> v5, final T a6, final FieldVector3D<T> v6,
  866.                                       final T[] row, final int j) {
  867.         row[j]     = a1.linearCombination(a1, v1.getX(), a2, v2.getX(), a3, v3.getX(), a4, v4.getX()).add(a1.linearCombination(a5, v5.getX(), a6, v6.getX()));
  868.         row[j + 1] = a1.linearCombination(a1, v1.getY(), a2, v2.getY(), a3, v3.getY(), a4, v4.getY()).add(a1.linearCombination(a5, v5.getY(), a6, v6.getY()));
  869.         row[j + 2] = a1.linearCombination(a1, v1.getZ(), a2, v2.getZ(), a3, v3.getZ(), a4, v4.getZ()).add(a1.linearCombination(a5, v5.getZ(), a6, v6.getZ()));

  871.     }


  874. }
Created with JaCoCo 0.8.12.202403310830