FieldOrbit.java

  1. /* Copyright 2002-2024 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.  * @param <T> type of the field elements
  68.  */
  69. public abstract class FieldOrbit<T extends CalculusFieldElement<T>>
  70.     implements FieldPVCoordinatesProvider<T>, FieldTimeStamped<T>, FieldTimeShiftable<FieldOrbit<T>, T> {

  72.     /** Frame in which are defined the orbital parameters. */
  73.     private final Frame frame;

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

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

  81.     /** Field-valued one.
  82.      * @since 12.0
  83.      * */
  84.     private final T one;

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

  91.     /** Field used by this class.
  92.      * @since 12.0
  93.      */
  94.     private final Field<T> field;

  96.     /** Computed position.
  97.      * @since 12.0
  98.      */
  99.     private transient FieldVector3D<T> position;

  101.     /** Computed PVCoordinates. */
  102.     private transient TimeStampedFieldPVCoordinates<T> pvCoordinates;

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

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

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

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

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

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

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

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

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

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

  197.         } else {
  198.             final FieldVector3D<T> a = pva.getAcceleration();
  199.             if (a == null) {
  200.                 return false;
  201.             }

  203.             final FieldVector3D<T> p = pva.getPosition();
  204.             final T r2 = p.getNormSq();

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

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

  222.     }

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

  232.     /** Get the orbit type.
  233.      * @return orbit type
  234.      */
  235.     public abstract OrbitType getType();

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

  250.     /** Get the frame in which the orbital parameters are defined.
  251.      * @return frame in which the orbital parameters are defined
  252.      */
  253.     public Frame getFrame() {
  254.         return frame;
  255.     }

  257.     /**Transforms the FieldOrbit instance into an Orbit instance.
  258.      * @return Orbit instance with same properties
  259.      */
  260.     public abstract Orbit toOrbit();

  262.     /** Get the semi-major axis.
  263.      * <p>Note that the semi-major axis is considered negative for hyperbolic orbits.</p>
  264.      * @return semi-major axis (m)
  265.      */
  266.     public abstract T getA();

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

  277.     /** Get the first component of the equinoctial eccentricity vector.
  278.      * @return first component of the equinoctial eccentricity vector
  279.      */
  280.     public abstract T getEquinoctialEx();

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

  290.     /** Get the second component of the equinoctial eccentricity vector.
  291.      * @return second component of the equinoctial eccentricity vector
  292.      */
  293.     public abstract T getEquinoctialEy();

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

  303.     /** Get the first component of the inclination vector.
  304.      * @return first component of the inclination vector
  305.      */
  306.     public abstract T getHx();

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

  316.     /** Get the second component of the inclination vector.
  317.      * @return second component of the inclination vector
  318.      */
  319.     public abstract T getHy();

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

  326.     /** Get the eccentric longitude argument.
  327.      * @return E + ω + Ω eccentric longitude argument (rad)
  328.      */
  329.     public abstract T getLE();

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

  339.     /** Get the true longitude argument.
  340.      * @return v + ω + Ω true longitude argument (rad)
  341.      */
  342.     public abstract T getLv();

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

  352.     /** Get the mean longitude argument.
  353.      * @return M + ω + Ω mean longitude argument (rad)
  354.      */
  355.     public abstract T getLM();

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

  365.     // Additional orbital elements

  367.     /** Get the eccentricity.
  368.      * @return eccentricity
  369.      */
  370.     public abstract T getE();

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

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

  388.     /** Get the inclination derivative.
  389.      * @return inclination derivative (rad/s)
  390.      */
  391.     public abstract T getIDot();

  393.     /** Check if orbit includes derivatives.
  394.      * @return true if orbit includes derivatives
  395.      * @see #getADot()
  396.      * @see #getEquinoctialExDot()
  397.      * @see #getEquinoctialEyDot()
  398.      * @see #getHxDot()
  399.      * @see #getHyDot()
  400.      * @see #getLEDot()
  401.      * @see #getLvDot()
  402.      * @see #getLMDot()
  403.      * @see #getEDot()
  404.      * @see #getIDot()
  405.      * @since 9.0
  406.      */
  407.     public abstract boolean hasDerivatives();

  409.     /** Get the central attraction coefficient used for position and velocity conversions (m³/s²).
  410.      * @return central attraction coefficient used for position and velocity conversions (m³/s²)
  411.      */
  412.     public T getMu() {
  413.         return mu;
  414.     }

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

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

  438.     /** Get the derivative of the mean anomaly with respect to the semi major axis.
  439.      * @return derivative of the mean anomaly with respect to the semi major axis
  440.      */
  441.     public T getMeanAnomalyDotWrtA() {
  442.         return getKeplerianMeanMotion().divide(getA()).multiply(-1.5);
  443.     }

  445.     /** Get the date of orbital parameters.
  446.      * @return date of the orbital parameters
  447.      */
  448.     public FieldAbsoluteDate<T> getDate() {
  449.         return date;
  450.     }

  452.     /** Get the {@link TimeStampedPVCoordinates} in a specified frame.
  453.      * @param outputFrame frame in which the position/velocity coordinates shall be computed
  454.      * @return FieldPVCoordinates in the specified output frame
  455.           * @see #getPVCoordinates()
  456.      */
  457.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final Frame outputFrame) {
  458.         if (pvCoordinates == null) {
  459.             pvCoordinates = initPVCoordinates();
  460.         }

  462.         // If output frame requested is the same as definition frame,
  463.         // PV coordinates are returned directly
  464.         if (outputFrame == frame) {
  465.             return pvCoordinates;
  466.         }

  468.         // Else, PV coordinates are transformed to output frame
  469.         final FieldTransform<T> t = frame.getTransformTo(outputFrame, date);
  470.         return t.transformPVCoordinates(pvCoordinates);
  471.     }

  473.     /** {@inheritDoc} */
  474.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> otherDate, final Frame otherFrame) {
  475.         return shiftedBy(otherDate.durationFrom(getDate())).getPVCoordinates(otherFrame);
  476.     }

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

  484.     /** Get the position in a specified frame.
  485.      * @param outputFrame frame in which the position coordinates shall be computed
  486.      * @return position in the specified output frame
  487.      * @see #getPosition()
  488.      * @since 12.0
  489.      */
  490.     public FieldVector3D<T> getPosition(final Frame outputFrame) {
  491.         if (position == null) {
  492.             position = initPosition();
  493.         }

  495.         // If output frame requested is the same as definition frame,
  496.         // Position vector is returned directly
  497.         if (outputFrame == frame) {
  498.             return position;
  499.         }

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

  505.     }

  507.     /** Get the position in definition frame.
  508.      * @return position in the definition frame
  509.      * @see #getPVCoordinates()
  510.      * @since 12.0
  511.      */
  512.     public FieldVector3D<T> getPosition() {
  513.         if (position == null) {
  514.             position = initPosition();
  515.         }
  516.         return position;
  517.     }

  519.     /** Get the {@link TimeStampedPVCoordinates} in definition frame.
  520.      * @return FieldPVCoordinates in the definition frame
  521.      * @see #getPVCoordinates(Frame)
  522.      */
  523.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates() {
  524.         if (pvCoordinates == null) {
  525.             pvCoordinates = initPVCoordinates();
  526.             position      = pvCoordinates.getPosition();
  527.         }
  528.         return pvCoordinates;
  529.     }

  531.     /** Getter for Field-valued one.
  532.      * @return one
  533.      * */
  534.     protected T getOne() {
  535.         return one;
  536.     }

  538.     /** Getter for Field-valued zero.
  539.      * @return zero
  540.      * */
  541.     protected T getZero() {
  542.         return zero;
  543.     }

  545.     /** Getter for Field.
  546.      * @return CalculusField
  547.      * */
  548.     protected Field<T> getField() {
  549.         return field;
  550.     }

  552.     /** Compute the position coordinates from the canonical parameters.
  553.      * @return computed position coordinates
  554.      * @since 12.0
  555.      */
  556.     protected abstract FieldVector3D<T> initPosition();

  558.     /** Compute the position/velocity coordinates from the canonical parameters.
  559.      * @return computed position/velocity coordinates
  560.      */
  561.     protected abstract TimeStampedFieldPVCoordinates<T> initPVCoordinates();

  563.     /** Get a time-shifted orbit.
  564.      * <p>
  565.      * The orbit can be slightly shifted to close dates. This shift is based on
  566.      * a simple Keplerian model. It is <em>not</em> intended as a replacement
  567.      * for proper orbit and attitude propagation but should be sufficient for
  568.      * small time shifts or coarse accuracy.
  569.      * </p>
  570.      * @param dt time shift in seconds
  571.      * @return a new orbit, shifted with respect to the instance (which is immutable)
  572.      */
  573.     public abstract FieldOrbit<T> shiftedBy(T dt);

  575.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  576.      * <p>
  577.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  578.      * respect to Cartesian coordinate j. This means each row corresponds to one orbital parameter
  579.      * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
  580.      * </p>
  581.      * @param type type of the position angle to use
  582.      * @param jacobian placeholder 6x6 (or larger) matrix to be filled with the Jacobian, if matrix
  583.      * is larger than 6x6, only the 6x6 upper left corner will be modified
  584.      */
  585.     public void getJacobianWrtCartesian(final PositionAngleType type, final T[][] jacobian) {

  587.         final T[][] cachedJacobian;
  588.         synchronized (this) {
  589.             switch (type) {
  590.                 case MEAN :
  591.                     if (jacobianMeanWrtCartesian == null) {
  592.                         // first call, we need to compute the Jacobian and cache it
  593.                         jacobianMeanWrtCartesian = computeJacobianMeanWrtCartesian();
  594.                     }
  595.                     cachedJacobian = jacobianMeanWrtCartesian;
  596.                     break;
  597.                 case ECCENTRIC :
  598.                     if (jacobianEccentricWrtCartesian == null) {
  599.                         // first call, we need to compute the Jacobian and cache it
  600.                         jacobianEccentricWrtCartesian = computeJacobianEccentricWrtCartesian();
  601.                     }
  602.                     cachedJacobian = jacobianEccentricWrtCartesian;
  603.                     break;
  604.                 case TRUE :
  605.                     if (jacobianTrueWrtCartesian == null) {
  606.                         // first call, we need to compute the Jacobian and cache it
  607.                         jacobianTrueWrtCartesian = computeJacobianTrueWrtCartesian();
  608.                     }
  609.                     cachedJacobian = jacobianTrueWrtCartesian;
  610.                     break;
  611.                 default :
  612.                     throw new OrekitInternalError(null);
  613.             }
  614.         }

  616.         // fill the user provided array
  617.         for (int i = 0; i < cachedJacobian.length; ++i) {
  618.             System.arraycopy(cachedJacobian[i], 0, jacobian[i], 0, cachedJacobian[i].length);
  619.         }

  621.     }

  623.     /** Compute the Jacobian of the Cartesian parameters with respect to the orbital parameters.
  624.      * <p>
  625.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i of the orbit with
  626.      * respect to orbital parameter j. This means each row corresponds to one Cartesian coordinate
  627.      * x, y, z, xdot, ydot, zdot whereas columns 0 to 5 correspond to the orbital parameters.
  628.      * </p>
  629.      * @param type type of the position angle to use
  630.      * @param jacobian placeholder 6x6 (or larger) matrix to be filled with the Jacobian, if matrix
  631.      * is larger than 6x6, only the 6x6 upper left corner will be modified
  632.      */
  633.     public void getJacobianWrtParameters(final PositionAngleType type, final T[][] jacobian) {

  635.         final T[][] cachedJacobian;
  636.         synchronized (this) {
  637.             switch (type) {
  638.                 case MEAN :
  639.                     if (jacobianWrtParametersMean == null) {
  640.                         // first call, we need to compute the Jacobian and cache it
  641.                         jacobianWrtParametersMean = createInverseJacobian(type);
  642.                     }
  643.                     cachedJacobian = jacobianWrtParametersMean;
  644.                     break;
  645.                 case ECCENTRIC :
  646.                     if (jacobianWrtParametersEccentric == null) {
  647.                         // first call, we need to compute the Jacobian and cache it
  648.                         jacobianWrtParametersEccentric = createInverseJacobian(type);
  649.                     }
  650.                     cachedJacobian = jacobianWrtParametersEccentric;
  651.                     break;
  652.                 case TRUE :
  653.                     if (jacobianWrtParametersTrue == null) {
  654.                         // first call, we need to compute the Jacobian and cache it
  655.                         jacobianWrtParametersTrue = createInverseJacobian(type);
  656.                     }
  657.                     cachedJacobian = jacobianWrtParametersTrue;
  658.                     break;
  659.                 default :
  660.                     throw new OrekitInternalError(null);
  661.             }
  662.         }

  664.         // fill the user-provided array
  665.         for (int i = 0; i < cachedJacobian.length; ++i) {
  666.             System.arraycopy(cachedJacobian[i], 0, jacobian[i], 0, cachedJacobian[i].length);
  667.         }

  669.     }

  671.     /** Create an inverse Jacobian.
  672.      * @param type type of the position angle to use
  673.      * @return inverse Jacobian
  674.      */
  675.     private T[][] createInverseJacobian(final PositionAngleType type) {

  677.         // get the direct Jacobian
  678.         final T[][] directJacobian = MathArrays.buildArray(getA().getField(), 6, 6);
  679.         getJacobianWrtCartesian(type, directJacobian);

  681.         // invert the direct Jacobian
  682.         final FieldMatrix<T> matrix = MatrixUtils.createFieldMatrix(directJacobian);
  683.         final FieldDecompositionSolver<T> solver = new FieldLUDecomposition<>(matrix).getSolver();
  684.         return solver.getInverse().getData();

  686.     }

  688.     /** Compute the Jacobian of the orbital parameters with mean angle with respect to the Cartesian parameters.
  689.      * <p>
  690.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  691.      * respect to Cartesian coordinate j. This means each row correspond to one orbital parameter
  692.      * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
  693.      * </p>
  694.      * @return 6x6 Jacobian matrix
  695.      * @see #computeJacobianEccentricWrtCartesian()
  696.      * @see #computeJacobianTrueWrtCartesian()
  697.      */
  698.     protected abstract T[][] computeJacobianMeanWrtCartesian();

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

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

  724.     /** Add the contribution of the Keplerian motion to parameters derivatives
  725.      * <p>
  726.      * This method is used by integration-based propagators to evaluate the part of Keplerian
  727.      * motion to evolution of the orbital state.
  728.      * </p>
  729.      * @param type type of the position angle in the state
  730.      * @param gm attraction coefficient to use
  731.      * @param pDot array containing orbital state derivatives to update (the Keplerian
  732.      * part must be <em>added</em> to the array components, as the array may already
  733.      * contain some non-zero elements corresponding to non-Keplerian parts)
  734.      */
  735.     public abstract void addKeplerContribution(PositionAngleType type, T gm, T[] pDot);

  737.         /** Fill a Jacobian half row with a single vector.
  738.      * @param a coefficient of the vector
  739.      * @param v vector
  740.      * @param row Jacobian matrix row
  741.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  742.      */

  744.     protected void fillHalfRow(final T a, final FieldVector3D<T> v, final T[] row, final int j) {
  745.         row[j]     = a.multiply(v.getX());
  746.         row[j + 1] = a.multiply(v.getY());
  747.         row[j + 2] = a.multiply(v.getZ());
  748.     }

  750.     /** Fill a Jacobian half row with a linear combination of vectors.
  751.      * @param a1 coefficient of the first vector
  752.      * @param v1 first vector
  753.      * @param a2 coefficient of the second vector
  754.      * @param v2 second vector
  755.      * @param row Jacobian matrix row
  756.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  757.      */
  758.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1, final T a2, final FieldVector3D<T> v2,
  759.                                       final T[] row, final int j) {
  760.         row[j]     = a1.linearCombination(a1, v1.getX(), a2, v2.getX());
  761.         row[j + 1] = a1.linearCombination(a1, v1.getY(), a2, v2.getY());
  762.         row[j + 2] = a1.linearCombination(a1, v1.getZ(), a2, v2.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 a3 coefficient of the third vector
  772.      * @param v3 third vector
  773.      * @param row Jacobian matrix row
  774.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  775.      */
  776.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1,
  777.                                final T a2, final FieldVector3D<T> v2,
  778.                                final T a3, final FieldVector3D<T> v3,
  779.                                       final T[] row, final int j) {
  780.         row[j]     = a1.linearCombination(a1, v1.getX(), a2, v2.getX(), a3, v3.getX());
  781.         row[j + 1] = a1.linearCombination(a1, v1.getY(), a2, v2.getY(), a3, v3.getY());
  782.         row[j + 2] = a1.linearCombination(a1, v1.getZ(), a2, v2.getZ(), a3, v3.getZ());

  784.     }

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

  805.     }

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

  829.     }

  831.     /** Fill a Jacobian half row with a linear combination of vectors.
  832.      * @param a1 coefficient of the first vector
  833.      * @param v1 first vector
  834.      * @param a2 coefficient of the second vector
  835.      * @param v2 second vector
  836.      * @param a3 coefficient of the third vector
  837.      * @param v3 third vector
  838.      * @param a4 coefficient of the fourth vector
  839.      * @param v4 fourth vector
  840.      * @param a5 coefficient of the fifth vector
  841.      * @param v5 fifth vector
  842.      * @param a6 coefficient of the sixth vector
  843.      * @param v6 sixth vector
  844.      * @param row Jacobian matrix row
  845.      * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
  846.      */
  847.     protected void fillHalfRow(final T a1, final FieldVector3D<T> v1, final T a2, final FieldVector3D<T> v2,
  848.                                       final T a3, final FieldVector3D<T> v3, final T a4, final FieldVector3D<T> v4,
  849.                                       final T a5, final FieldVector3D<T> v5, final T a6, final FieldVector3D<T> v6,
  850.                                       final T[] row, final int j) {
  851.         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()));
  852.         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()));
  853.         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()));

  855.     }


  858. }
Created with JaCoCo 0.8.12.202403310830