1   /* Copyright 2002-2026 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;
18  
19  import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
20  import org.hipparchus.geometry.euclidean.threed.Vector3D;
21  import org.hipparchus.util.FastMath;
22  import org.hipparchus.util.SinCos;
23  import org.orekit.errors.OrekitIllegalArgumentException;
24  import org.orekit.errors.OrekitInternalError;
25  import org.orekit.errors.OrekitMessages;
26  import org.orekit.frames.Frame;
27  import org.orekit.frames.KinematicTransform;
28  import org.orekit.time.AbsoluteDate;
29  import org.orekit.time.TimeOffset;
30  import org.orekit.utils.PVCoordinates;
31  import org.orekit.utils.TimeStampedPVCoordinates;
32  
33  
34  /**
35   * This class handles equinoctial orbital parameters, which can support both
36   * circular and equatorial orbits.
37   * <p>
38   * The parameters used internally are the equinoctial elements which can be
39   * related to Keplerian elements as follows:
40   *   <pre>
41   *     a
42   *     ex = e cos(ω + Ω)
43   *     ey = e sin(ω + Ω)
44   *     hx = tan(i/2) cos(Ω)
45   *     hy = tan(i/2) sin(Ω)
46   *     lv = v + ω + Ω
47   *   </pre>
48   * where ω stands for the Periapsis Argument and Ω stands for the
49   * Right Ascension of the Ascending Node.
50   *
51   * <p>
52   * The conversion equations from and to Keplerian elements given above hold only
53   * when both sides are unambiguously defined, i.e. when orbit is neither equatorial
54   * nor circular. When orbit is either equatorial or circular, the equinoctial
55   * parameters are still unambiguously defined whereas some Keplerian elements
56   * (more precisely ω and Ω) become ambiguous. For this reason, equinoctial
57   * parameters are the recommended way to represent orbits. Note however than
58   * the present implementation does not handle non-elliptical cases.
59   * </p>
60   * <p>
61   * The instance <code>EquinoctialOrbit</code> is guaranteed to be immutable.
62   * </p>
63   * @see    Orbit
64   * @see    KeplerianOrbit
65   * @see    CircularOrbit
66   * @see    CartesianOrbit
67   * @author Mathieu Rom&eacute;ro
68   * @author Luc Maisonobe
69   * @author Guylaine Prat
70   * @author Fabien Maussion
71   * @author V&eacute;ronique Pommier-Maurussane
72   */
73  public class EquinoctialOrbit extends Orbit implements PositionAngleBased<EquinoctialOrbit> {
74  
75      /** Semi-major axis (m). */
76      private final double a;
77  
78      /** First component of the eccentricity vector. */
79      private final double ex;
80  
81      /** Second component of the eccentricity vector. */
82      private final double ey;
83  
84      /** First component of the inclination vector. */
85      private final double hx;
86  
87      /** Second component of the inclination vector. */
88      private final double hy;
89  
90      /** Cached longitude argument (rad). */
91      private final double cachedL;
92  
93      /** Cache type of position angle (longitude argument). */
94      private final PositionAngleType cachedPositionAngleType;
95  
96      /** Semi-major axis derivative (m/s). */
97      private final double aDot;
98  
99      /** First component of the eccentricity vector derivative. */
100     private final double exDot;
101 
102     /** Second component of the eccentricity vector derivative. */
103     private final double eyDot;
104 
105     /** First component of the inclination vector derivative. */
106     private final double hxDot;
107 
108     /** Second component of the inclination vector derivative. */
109     private final double hyDot;
110 
111     /** Derivative of cached longitude argument (rad/s). */
112     private final double cachedLDot;
113 
114     /** Partial Cartesian coordinates (position and velocity are valid, acceleration may be missing). */
115     private PVCoordinates partialPV;
116 
117     /** Creates a new instance.
118      * @param a  semi-major axis (m)
119      * @param ex e cos(ω + Ω), first component of eccentricity vector
120      * @param ey e sin(ω + Ω), second component of eccentricity vector
121      * @param hx tan(i/2) cos(Ω), first component of inclination vector
122      * @param hy tan(i/2) sin(Ω), second component of inclination vector
123      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
124      * @param type type of longitude argument
125      * @param cachedPositionAngleType type of cached longitude argument
126      * @param frame the frame in which the parameters are defined
127      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
128      * @param date date of the orbital parameters
129      * @param mu central attraction coefficient (m³/s²)
130      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
131      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
132      * @since 12.1
133      */
134     public EquinoctialOrbit(final double a, final double ex, final double ey,
135                             final double hx, final double hy, final double l,
136                             final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
137                             final Frame frame, final AbsoluteDate date, final double mu)
138         throws IllegalArgumentException {
139         this(new EquinoctialParameters(a, ex, ey, hx, hy, l, type).withPositionAngleType(cachedPositionAngleType),
140                 frame, date, mu);
141     }
142 
143     /** Creates a new instance without derivatives and with cached position angle same as value inputted.
144      * @param parameters equinoctial orbital parameters
145      * @param frame the frame in which the parameters are defined
146      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
147      * @param date date of the orbital parameters
148      * @param mu central attraction coefficient (m³/s²)
149      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
150      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
151      * @since 14.0
152      */
153     public EquinoctialOrbit(final EquinoctialParameters parameters, final Frame frame, final AbsoluteDate date,
154                             final double mu)
155             throws IllegalArgumentException {
156         this(parameters.a(), parameters.ex(), parameters.ey(), parameters.hx(), parameters.hy(),
157                 parameters.longitudeArgument(), 0., 0., 0., 0., 0.,
158                 computeKeplerianLDot(parameters.positionAngleType(), parameters.a(), parameters.ex(), parameters.ey(), mu, parameters.longitudeArgument(), parameters.positionAngleType()),
159                 parameters.positionAngleType(), parameters.positionAngleType(), frame, date, mu);
160     }
161 
162     /** Creates a new instance without derivatives and with cached position angle same as value inputted.
163      * @param a  semi-major axis (m)
164      * @param ex e cos(ω + Ω), first component of eccentricity vector
165      * @param ey e sin(ω + Ω), second component of eccentricity vector
166      * @param hx tan(i/2) cos(Ω), first component of inclination vector
167      * @param hy tan(i/2) sin(Ω), second component of inclination vector
168      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
169      * @param type type of longitude argument
170      * @param frame the frame in which the parameters are defined
171      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
172      * @param date date of the orbital parameters
173      * @param mu central attraction coefficient (m³/s²)
174      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
175      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
176      */
177     public EquinoctialOrbit(final double a, final double ex, final double ey,
178                             final double hx, final double hy, final double l,
179                             final PositionAngleType type,
180                             final Frame frame, final AbsoluteDate date, final double mu)
181             throws IllegalArgumentException {
182         this(a, ex, ey, hx, hy, l, type, type, frame, date, mu);
183     }
184 
185     /** Creates a new instance.
186      * @param a  semi-major axis (m)
187      * @param ex e cos(ω + Ω), first component of eccentricity vector
188      * @param ey e sin(ω + Ω), second component of eccentricity vector
189      * @param hx tan(i/2) cos(Ω), first component of inclination vector
190      * @param hy tan(i/2) sin(Ω), second component of inclination vector
191      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
192      * @param aDot  semi-major axis derivative (m/s)
193      * @param exDot d(e cos(ω + Ω))/dt, first component of eccentricity vector derivative
194      * @param eyDot d(e sin(ω + Ω))/dt, second component of eccentricity vector derivative
195      * @param hxDot d(tan(i/2) cos(Ω))/dt, first component of inclination vector derivative
196      * @param hyDot d(tan(i/2) sin(Ω))/dt, second component of inclination vector derivative
197      * @param lDot  d(M or E or v) + ω + Ω)/dr, mean, eccentric or true longitude argument  derivative (rad/s)
198      * @param type type of longitude argument
199      * @param cachedPositionAngleType of cached longitude argument
200      * @param frame the frame in which the parameters are defined
201      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
202      * @param date date of the orbital parameters
203      * @param mu central attraction coefficient (m³/s²)
204      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
205      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
206      * @since 12.1
207      */
208     public EquinoctialOrbit(final double a, final double ex, final double ey,
209                             final double hx, final double hy, final double l,
210                             final double aDot, final double exDot, final double eyDot,
211                             final double hxDot, final double hyDot, final double lDot,
212                             final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
213                             final Frame frame, final AbsoluteDate date, final double mu)
214         throws IllegalArgumentException {
215         super(frame, date, mu);
216         if (ex * ex + ey * ey >= 1.0) {
217             throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
218                                                      getClass().getName());
219         }
220         this.cachedPositionAngleType = cachedPositionAngleType;
221         this.a     = a;
222         this.aDot  = aDot;
223         this.ex    = ex;
224         this.exDot = exDot;
225         this.ey    = ey;
226         this.eyDot = eyDot;
227         this.hx    = hx;
228         this.hxDot = hxDot;
229         this.hy    = hy;
230         this.hyDot = hyDot;
231 
232         final UnivariateDerivative1 lUD = initializeCachedL(l, lDot, type);
233         this.cachedL = lUD.getValue();
234         this.cachedLDot = lUD.getFirstDerivative();
235 
236         this.partialPV = null;
237 
238     }
239 
240     /** Creates a new instance with derivatives and with cached position angle same as value inputted.
241      * @param a  semi-major axis (m)
242      * @param ex e cos(ω + Ω), first component of eccentricity vector
243      * @param ey e sin(ω + Ω), second component of eccentricity vector
244      * @param hx tan(i/2) cos(Ω), first component of inclination vector
245      * @param hy tan(i/2) sin(Ω), second component of inclination vector
246      * @param l  (M or E or v) + ω + Ω, mean, eccentric or true longitude argument (rad)
247      * @param aDot  semi-major axis derivative (m/s)
248      * @param exDot d(e cos(ω + Ω))/dt, first component of eccentricity vector derivative
249      * @param eyDot d(e sin(ω + Ω))/dt, second component of eccentricity vector derivative
250      * @param hxDot d(tan(i/2) cos(Ω))/dt, first component of inclination vector derivative
251      * @param hyDot d(tan(i/2) sin(Ω))/dt, second component of inclination vector derivative
252      * @param lDot  d(M or E or v) + ω + Ω)/dr, mean, eccentric or true longitude argument  derivative (rad/s)
253      * @param type type of longitude argument
254      * @param frame the frame in which the parameters are defined
255      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
256      * @param date date of the orbital parameters
257      * @param mu central attraction coefficient (m³/s²)
258      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
259      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
260      */
261     public EquinoctialOrbit(final double a, final double ex, final double ey,
262                             final double hx, final double hy, final double l,
263                             final double aDot, final double exDot, final double eyDot,
264                             final double hxDot, final double hyDot, final double lDot,
265                             final PositionAngleType type,
266                             final Frame frame, final AbsoluteDate date, final double mu)
267             throws IllegalArgumentException {
268         this(a, ex, ey, hx, hy, l, aDot, exDot, eyDot, hxDot, hyDot, lDot, type, type, frame, date, mu);
269     }
270 
271     /** Constructor from Cartesian parameters.
272      *
273      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
274      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
275      * use {@code mu} and the position to compute the acceleration, including
276      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
277      *
278      * @param pvCoordinates the position, velocity and acceleration
279      * @param frame the frame in which are defined the {@link PVCoordinates}
280      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
281      * @param mu central attraction coefficient (m³/s²)
282      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
283      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
284      */
285     public EquinoctialOrbit(final TimeStampedPVCoordinates pvCoordinates,
286                             final Frame frame, final double mu)
287         throws IllegalArgumentException {
288         super(pvCoordinates, frame, mu);
289 
290         final EquinoctialParametersConverter converter = new EquinoctialParametersConverter(mu);
291         cachedPositionAngleType = PositionAngleType.TRUE;
292         final EquinoctialParameters parameters = converter.toParameters(pvCoordinates, cachedPositionAngleType);
293         a = parameters.a();
294         ex = parameters.ex();
295         ey = parameters.ey();
296         hx = parameters.hx();
297         hy = parameters.hy();
298         cachedL = parameters.longitudeArgument();
299 
300         partialPV = pvCoordinates;
301 
302         if (hasNonKeplerianAcceleration(pvCoordinates, mu)) {
303             // we have a relevant acceleration, we can compute derivatives
304 
305             final double[][] jacobian = new double[6][6];
306             getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
307 
308             final double r3 = FastMath.pow(pvCoordinates.getPosition().getNorm2Sq(), 3. / 2.);
309             final Vector3D keplerianAcceleration    = new Vector3D(-mu / r3, pvCoordinates.getPosition());
310             final Vector3D nonKeplerianAcceleration = pvCoordinates.getAcceleration().subtract(keplerianAcceleration);
311             final double   aX                       = nonKeplerianAcceleration.getX();
312             final double   aY                       = nonKeplerianAcceleration.getY();
313             final double   aZ                       = nonKeplerianAcceleration.getZ();
314             aDot  = jacobian[0][3] * aX + jacobian[0][4] * aY + jacobian[0][5] * aZ;
315             exDot = jacobian[1][3] * aX + jacobian[1][4] * aY + jacobian[1][5] * aZ;
316             eyDot = jacobian[2][3] * aX + jacobian[2][4] * aY + jacobian[2][5] * aZ;
317             hxDot = jacobian[3][3] * aX + jacobian[3][4] * aY + jacobian[3][5] * aZ;
318             hyDot = jacobian[4][3] * aX + jacobian[4][4] * aY + jacobian[4][5] * aZ;
319 
320             // in order to compute true longitude argument derivative, we must compute
321             // mean longitude argument derivative including Keplerian motion and convert to true longitude argument
322             final double lMDot = getKeplerianMeanMotion() +
323                                  jacobian[5][3] * aX + jacobian[5][4] * aY + jacobian[5][5] * aZ;
324             final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
325             final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
326             final UnivariateDerivative1 lMUD = new UnivariateDerivative1(getLM(), lMDot);
327             final UnivariateDerivative1 lvUD = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lMUD);
328             cachedLDot = lvUD.getFirstDerivative();
329 
330         } else {
331             // acceleration is either almost zero or NaN,
332             // we assume acceleration was not known
333             // we don't set up derivatives
334             aDot  = 0.;
335             exDot = 0.;
336             eyDot = 0.;
337             hxDot = 0.;
338             hyDot = 0.;
339             cachedLDot = computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, mu, cachedL, cachedPositionAngleType);
340         }
341 
342     }
343 
344     /** Constructor from Cartesian parameters.
345      *
346      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
347      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
348      * use {@code mu} and the position to compute the acceleration, including
349      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
350      *
351      * @param pvCoordinates the position end velocity
352      * @param frame the frame in which are defined the {@link PVCoordinates}
353      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
354      * @param date date of the orbital parameters
355      * @param mu central attraction coefficient (m³/s²)
356      * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or
357      * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame}
358      */
359     public EquinoctialOrbit(final PVCoordinates pvCoordinates, final Frame frame,
360                             final AbsoluteDate date, final double mu)
361         throws IllegalArgumentException {
362         this(new TimeStampedPVCoordinates(date, pvCoordinates), frame, mu);
363     }
364 
365     /** Constructor from any kind of orbital parameters.
366      * @param op orbital parameters to copy
367      */
368     public EquinoctialOrbit(final Orbit op) {
369         super(op.getFrame(), op.getDate(), op.getMu());
370         a         = op.getA();
371         aDot      = op.getADot();
372         ex        = op.getEquinoctialEx();
373         exDot     = op.getEquinoctialExDot();
374         ey        = op.getEquinoctialEy();
375         eyDot     = op.getEquinoctialEyDot();
376         hx        = op.getHx();
377         hxDot     = op.getHxDot();
378         hy        = op.getHy();
379         hyDot     = op.getHyDot();
380         cachedPositionAngleType = PositionAngleType.TRUE;
381         cachedL   = op.getLv();
382         cachedLDot = op.hasNonKeplerianAcceleration() ? op.getLvDot() :
383                 computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, op.getMu(), cachedL, cachedPositionAngleType);
384         partialPV = null;
385     }
386 
387     /**
388      * Method providing with the equinoctial elements, using the cached type for the argument of longitude.
389      * @return equinoctial elements
390      * @since 14.0
391      */
392     public EquinoctialParameters getEquinoctialParameters() {
393         return new EquinoctialParameters(a, ex, ey, hx, hy, cachedL, cachedPositionAngleType);
394     }
395 
396     /** {@inheritDoc} */
397     @Override
398     public boolean hasNonKeplerianAcceleration() {
399         return aDot != 0. || exDot != 0. || eyDot != 0. || hxDot != 0. || hyDot != 0. ||
400                 FastMath.abs(cachedLDot - computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, getMu(), cachedL, cachedPositionAngleType)) > TOLERANCE_POSITION_ANGLE_RATE;
401     }
402 
403     /** {@inheritDoc} */
404     @Override
405     public OrbitParamsType getType() {
406         return OrbitParamsType.EQUINOCTIAL;
407     }
408 
409     /** {@inheritDoc} */
410     @Override
411     public AbstractOrbitFactory<EquinoctialOrbit> factory(final PositionAngleType positionAngleType,
412                                                           final double positionScale) {
413         return new EquinoctialOrbitFactory(this, positionScale, positionAngleType);
414     }
415 
416     /** {@inheritDoc} */
417     @Override
418     public double getA() {
419         return a;
420     }
421 
422     /** {@inheritDoc} */
423     @Override
424     public double getADot() {
425         return aDot;
426     }
427 
428     /** {@inheritDoc} */
429     @Override
430     public double getEquinoctialEx() {
431         return ex;
432     }
433 
434     /** {@inheritDoc} */
435     @Override
436     public double getEquinoctialExDot() {
437         return exDot;
438     }
439 
440     /** {@inheritDoc} */
441     @Override
442     public double getEquinoctialEy() {
443         return ey;
444     }
445 
446     /** {@inheritDoc} */
447     @Override
448     public double getEquinoctialEyDot() {
449         return eyDot;
450     }
451 
452     /** {@inheritDoc} */
453     @Override
454     public double getHx() {
455         return hx;
456     }
457 
458     /** {@inheritDoc} */
459     @Override
460     public double getHxDot() {
461         return hxDot;
462     }
463 
464     /** {@inheritDoc} */
465     @Override
466     public double getHy() {
467         return hy;
468     }
469 
470     /** {@inheritDoc} */
471     @Override
472     public double getHyDot() {
473         return hyDot;
474     }
475 
476     /** {@inheritDoc} */
477     @Override
478     public double getLv() {
479         return getL(PositionAngleType.TRUE);
480     }
481 
482     /** {@inheritDoc} */
483     @Override
484     public double getLvDot() {
485         switch (cachedPositionAngleType) {
486             case ECCENTRIC:
487                 final UnivariateDerivative1 lEUD = new UnivariateDerivative1(cachedL, cachedLDot);
488                 final UnivariateDerivative1 exUD     = new UnivariateDerivative1(ex,     exDot);
489                 final UnivariateDerivative1 eyUD     = new UnivariateDerivative1(ey,     eyDot);
490                 final UnivariateDerivative1 lvUD = FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
491                         lEUD);
492                 return lvUD.getFirstDerivative();
493 
494             case TRUE:
495                 return cachedLDot;
496 
497             case MEAN:
498                 final UnivariateDerivative1 lMUD = new UnivariateDerivative1(cachedL, cachedLDot);
499                 final UnivariateDerivative1 exUD2    = new UnivariateDerivative1(ex,     exDot);
500                 final UnivariateDerivative1 eyUD2    = new UnivariateDerivative1(ey,     eyDot);
501                 final UnivariateDerivative1 lvUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD2,
502                         eyUD2, lMUD);
503                 return lvUD2.getFirstDerivative();
504 
505             default:
506                 throw new OrekitInternalError(null);
507         }
508     }
509 
510     /** {@inheritDoc} */
511     @Override
512     public double getLE() {
513         return getL(PositionAngleType.ECCENTRIC);
514     }
515 
516     /** {@inheritDoc} */
517     @Override
518     public double getLEDot() {
519         switch (cachedPositionAngleType) {
520             case TRUE:
521                 final UnivariateDerivative1 lvUD = new UnivariateDerivative1(cachedL, cachedLDot);
522                 final UnivariateDerivative1 exUD     = new UnivariateDerivative1(ex,     exDot);
523                 final UnivariateDerivative1 eyUD     = new UnivariateDerivative1(ey,     eyDot);
524                 final UnivariateDerivative1 lEUD = FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD,
525                         lvUD);
526                 return lEUD.getFirstDerivative();
527 
528             case ECCENTRIC:
529                 return cachedLDot;
530 
531             case MEAN:
532                 final UnivariateDerivative1 lMUD = new UnivariateDerivative1(cachedL, cachedLDot);
533                 final UnivariateDerivative1 exUD2    = new UnivariateDerivative1(ex,     exDot);
534                 final UnivariateDerivative1 eyUD2    = new UnivariateDerivative1(ey,     eyDot);
535                 final UnivariateDerivative1 lEUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD2,
536                         eyUD2, lMUD);
537                 return lEUD2.getFirstDerivative();
538 
539             default:
540                 throw new OrekitInternalError(null);
541         }
542     }
543 
544     /** {@inheritDoc} */
545     @Override
546     public double getLM() {
547         return getL(PositionAngleType.MEAN);
548     }
549 
550     /** {@inheritDoc} */
551     @Override
552     public double getLMDot() {
553         switch (cachedPositionAngleType) {
554             case TRUE:
555                 final UnivariateDerivative1 lvUD = new UnivariateDerivative1(cachedL, cachedLDot);
556                 final UnivariateDerivative1 exUD     = new UnivariateDerivative1(ex,     exDot);
557                 final UnivariateDerivative1 eyUD     = new UnivariateDerivative1(ey,     eyDot);
558                 final UnivariateDerivative1 lMUD = FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lvUD);
559                 return lMUD.getFirstDerivative();
560 
561             case MEAN:
562                 return cachedLDot;
563 
564             case ECCENTRIC:
565                 final UnivariateDerivative1 lEUD = new UnivariateDerivative1(cachedL, cachedLDot);
566                 final UnivariateDerivative1 exUD2    = new UnivariateDerivative1(ex,     exDot);
567                 final UnivariateDerivative1 eyUD2    = new UnivariateDerivative1(ey,     eyDot);
568                 final UnivariateDerivative1 lMUD2 = FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD2,
569                         eyUD2, lEUD);
570                 return lMUD2.getFirstDerivative();
571 
572             default:
573                 throw new OrekitInternalError(null);
574         }
575     }
576 
577     /** Get the longitude argument.
578      * @param type type of the angle
579      * @return longitude argument (rad)
580      */
581     public double getL(final PositionAngleType type) {
582         return getEquinoctialParameters().withPositionAngleType(type).longitudeArgument();
583     }
584 
585     /** Get the longitude argument derivative.
586      * @param type type of the angle
587      * @return longitude argument derivative (rad/s)
588      */
589     public double getLDot(final PositionAngleType type) {
590         return switch (type) {
591             case TRUE -> getLvDot();
592             case MEAN -> getLMDot();
593             case ECCENTRIC -> getLEDot();
594         };
595     }
596 
597     /** {@inheritDoc} */
598     @Override
599     public double getE() {
600         return FastMath.sqrt(ex * ex + ey * ey);
601     }
602 
603     /** {@inheritDoc} */
604     @Override
605     public double getEDot() {
606         if (!hasNonKeplerianAcceleration()) {
607             return 0.;
608         }
609         return (ex * exDot + ey * eyDot) / FastMath.sqrt(ex * ex + ey * ey);
610     }
611 
612     /** {@inheritDoc} */
613     @Override
614     public double getI() {
615         return 2 * FastMath.atan(FastMath.sqrt(hx * hx + hy * hy));
616     }
617 
618     /** {@inheritDoc} */
619     @Override
620     public double getIDot() {
621         if (!hasNonKeplerianAcceleration()) {
622             return 0.;
623         }
624         final double h2 = hx * hx + hy * hy;
625         final double h  = FastMath.sqrt(h2);
626         return 2 * (hx * hxDot + hy * hyDot) / (h * (1 + h2));
627     }
628 
629     /** Compute position and velocity but not acceleration.
630      */
631     private void computePVWithoutA() {
632 
633         if (partialPV != null) {
634             // already computed
635             return;
636         }
637 
638         final EquinoctialParametersConverter converter = new EquinoctialParametersConverter(getMu());
639         partialPV = converter.toCartesian(getEquinoctialParameters());
640 
641     }
642 
643     /** Initialize cached argument of longitude with rate.
644      * @param l input argument of longitude
645      * @param lDot rate of input argument of longitude
646      * @param inputType position angle type passed as input
647      * @return argument of longitude to cache with rate
648      * @since 12.1
649      */
650     private UnivariateDerivative1 initializeCachedL(final double l, final double lDot,
651                                                     final PositionAngleType inputType) {
652         if (cachedPositionAngleType == inputType) {
653             return new UnivariateDerivative1(l, lDot);
654 
655         } else {
656             final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
657             final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
658             final UnivariateDerivative1 lUD = new UnivariateDerivative1(l, lDot);
659 
660             switch (cachedPositionAngleType) {
661 
662                 case ECCENTRIC:
663                     if (inputType == PositionAngleType.MEAN) {
664                         return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD, eyUD, lUD);
665                     } else {
666                         return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD, lUD);
667                     }
668 
669                 case TRUE:
670                     if (inputType == PositionAngleType.MEAN) {
671                         return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lUD);
672                     } else {
673                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD, lUD);
674                     }
675 
676                 case MEAN:
677                     if (inputType == PositionAngleType.TRUE) {
678                         return FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lUD);
679                     } else {
680                         return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD, eyUD, lUD);
681                     }
682 
683                 default:
684                     throw new OrekitInternalError(null);
685 
686             }
687 
688         }
689 
690     }
691 
692     /** {@inheritDoc} */
693     @Override
694     protected Vector3D initPosition() {
695 
696         // get equinoctial parameters
697         final double lE = getLE();
698 
699         // inclination-related intermediate parameters
700         final double hx2   = hx * hx;
701         final double hy2   = hy * hy;
702         final double factH = 1. / (1 + hx2 + hy2);
703 
704         // reference axes defining the orbital plane
705         final double ux = (1 + hx2 - hy2) * factH;
706         final double uy =  2 * hx * hy * factH;
707         final double uz = -2 * hy * factH;
708 
709         final double vx = uy;
710         final double vy = (1 - hx2 + hy2) * factH;
711         final double vz =  2 * hx * factH;
712 
713         // eccentricity-related intermediate parameters
714         final double exey = ex * ey;
715         final double ex2  = ex * ex;
716         final double ey2  = ey * ey;
717         final double e2   = ex2 + ey2;
718         final double eta  = 1 + FastMath.sqrt(1 - e2);
719         final double beta = 1. / eta;
720 
721         // eccentric longitude argument
722         final SinCos scLe   = FastMath.sinCos(lE);
723         final double cLe    = scLe.cos();
724         final double sLe    = scLe.sin();
725 
726         // coordinates of position and velocity in the orbital plane
727         final double x      = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
728         final double y      = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);
729 
730         return new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
731 
732     }
733 
734     /** {@inheritDoc} */
735     @Override
736     protected TimeStampedPVCoordinates initPVCoordinates() {
737 
738         // position and velocity
739         computePVWithoutA();
740 
741         // acceleration
742         final double r2 = partialPV.getPosition().getNorm2Sq();
743         final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
744         final Vector3D acceleration = hasNonKeplerianRates() ?
745                                       keplerianAcceleration.add(nonKeplerianAcceleration()) :
746                                       keplerianAcceleration;
747 
748         return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
749 
750     }
751 
752     /** {@inheritDoc} */
753     @Override
754     public EquinoctialOrbit inFrame(final Frame inertialFrame) {
755         final PVCoordinates pvCoordinates;
756         if (hasNonKeplerianAcceleration()) {
757             pvCoordinates = getPVCoordinates(inertialFrame);
758         } else {
759             final KinematicTransform transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
760             pvCoordinates = transform.transformOnlyPV(getPVCoordinates());
761         }
762         final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(pvCoordinates, inertialFrame, getDate(), getMu());
763         if (equinoctialOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
764             return equinoctialOrbit;
765         } else {
766             return equinoctialOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
767         }
768     }
769 
770     /** {@inheritDoc} */
771     @Override
772     public EquinoctialOrbit withCachedPositionAngleType(final PositionAngleType positionAngleType) {
773         return new EquinoctialOrbit(a, ex, ey, hx, hy, getL(positionAngleType), aDot, exDot, eyDot, hxDot, hyDot,
774                 getLDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
775     }
776 
777     /** {@inheritDoc} */
778     @Override
779     public EquinoctialOrbit shiftedBy(final double dt) {
780         return shiftedBy(new TimeOffset(dt));
781     }
782 
783     /** {@inheritDoc} */
784     @Override
785     public EquinoctialOrbit shiftedBy(final TimeOffset dt) {
786         if (dt.isZero()) {  // for performance
787             return new EquinoctialOrbit(a, ex, ey, hx, hy, cachedL, aDot, exDot, eyDot, hxDot, hyDot, cachedLDot,
788                     cachedPositionAngleType, getFrame(), getDate(), getMu());
789         }
790 
791         final double dtS = dt.toDouble();
792 
793         // use Keplerian-only motion
794         final EquinoctialParameters parameters = new EquinoctialParameters(a, ex, ey, hx, hy,
795                 getLM() + dtS * getKeplerianMeanMotion(), PositionAngleType.MEAN);
796         final EquinoctialOrbit keplerianShifted = new EquinoctialOrbit(parameters,
797                                                                        getFrame(), getDate().shiftedBy(dt), getMu());
798 
799         if (hasNonKeplerianRates()) {
800             final PVCoordinates pvCoordinates = shiftPVNonKeplerian(keplerianShifted.getPVCoordinates(), dtS);
801 
802             // build a new orbit, taking non-Keplerian acceleration into account
803             return new EquinoctialOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(), pvCoordinates),
804                                         getFrame(), getMu());
805 
806         } else {
807             // Keplerian-only motion is all we can do
808             return keplerianShifted;
809         }
810 
811     }
812 
813     /** {@inheritDoc} */
814     @Override
815     protected EquinoctialOrbit keplerianShiftedBy(final double dt) {
816         return new EquinoctialOrbit(a, ex, ey, hx, hy, getLM() + dt * getKeplerianMeanMotion(),
817                 PositionAngleType.MEAN, getFrame(), getDate().shiftedBy(dt), getMu());
818     }
819 
820     /** {@inheritDoc} */
821     @Override
822     protected double[][] computeJacobianMeanWrtCartesian() {
823 
824         final double[][] jacobian = new double[6][6];
825 
826         // compute various intermediate parameters
827         computePVWithoutA();
828         final Vector3D position = partialPV.getPosition();
829         final Vector3D velocity = partialPV.getVelocity();
830         final double r2         = position.getNorm2Sq();
831         final double r          = FastMath.sqrt(r2);
832         final double r3         = r * r2;
833 
834         final double mu         = getMu();
835         final double sqrtMuA    = FastMath.sqrt(a * mu);
836         final double a2         = a * a;
837 
838         final double e2         = ex * ex + ey * ey;
839         final double oMe2       = 1 - e2;
840         final double epsilon    = FastMath.sqrt(oMe2);
841         final double beta       = 1 / (1 + epsilon);
842         final double ratio      = epsilon * beta;
843 
844         final double hx2        = hx * hx;
845         final double hy2        = hy * hy;
846         final double hxhy       = hx * hy;
847 
848         // precomputing equinoctial frame unit vectors (f, g, w)
849         final Vector3D f  = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
850         final Vector3D g  = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
851         final Vector3D w  = Vector3D.crossProduct(position, velocity).normalize();
852 
853         // coordinates of the spacecraft in the equinoctial frame
854         final double x    = Vector3D.dotProduct(position, f);
855         final double y    = Vector3D.dotProduct(position, g);
856         final double xDot = Vector3D.dotProduct(velocity, f);
857         final double yDot = Vector3D.dotProduct(velocity, g);
858 
859         // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g
860         final double c1 = a / (sqrtMuA * epsilon);
861         final double c2 = a * sqrtMuA * beta / r3;
862         final double c3 = sqrtMuA / (r3 * epsilon);
863         final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
864                                                 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);
865 
866         // drDot / dEy = dXDot / dEy * f + dYDot / dEy * g
867         final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
868                                                 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);
869 
870         // da
871         final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
872         final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
873         fillHalfRow(1, vectorAR,    jacobian[0], 0);
874         fillHalfRow(1, vectorARDot, jacobian[0], 3);
875 
876         // dEx
877         final double d1 = -a * ratio / r3;
878         final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
879         final double d3 = (hx * y - hy * x) / sqrtMuA;
880         final Vector3D vectorExRDot =
881             new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
882         fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
883         fillHalfRow(1, vectorExRDot, jacobian[1], 3);
884 
885         // dEy
886         final Vector3D vectorEyRDot =
887             new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
888         fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
889         fillHalfRow(1, vectorEyRDot, jacobian[2], 3);
890 
891         // dHx
892         final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
893         fillHalfRow(-h * xDot, w, jacobian[3], 0);
894         fillHalfRow( h * x,    w, jacobian[3], 3);
895 
896         // dHy
897         fillHalfRow(-h * yDot, w, jacobian[4], 0);
898         fillHalfRow( h * y,    w, jacobian[4], 3);
899 
900         // dLambdaM
901         final double l = -ratio / sqrtMuA;
902         fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
903         fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);
904 
905         return jacobian;
906 
907     }
908 
909     /** {@inheritDoc} */
910     @Override
911     protected double[][] computeJacobianEccentricWrtCartesian() {
912 
913         // start by computing the Jacobian with mean angle
914         final double[][] jacobian = computeJacobianMeanWrtCartesian();
915 
916         // Differentiating the Kepler equation lM = lE - ex sin lE + ey cos lE leads to:
917         // dlM = (1 - ex cos lE - ey sin lE) dE - sin lE dex + cos lE dey
918         // which is inverted and rewritten as:
919         // dlE = a/r dlM + sin lE a/r dex - cos lE a/r dey
920         final SinCos scLe  = FastMath.sinCos(getLE());
921         final double cosLe = scLe.cos();
922         final double sinLe = scLe.sin();
923         final double aOr   = 1 / (1 - ex * cosLe - ey * sinLe);
924 
925         // update longitude row
926         final double[] rowEx = jacobian[1];
927         final double[] rowEy = jacobian[2];
928         final double[] rowL  = jacobian[5];
929         for (int j = 0; j < 6; ++j) {
930             rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
931         }
932 
933         return jacobian;
934 
935     }
936 
937     /** {@inheritDoc} */
938     @Override
939     protected double[][] computeJacobianTrueWrtCartesian() {
940 
941         // start by computing the Jacobian with eccentric angle
942         final double[][] jacobian = computeJacobianEccentricWrtCartesian();
943 
944         // Differentiating the eccentric longitude equation
945         // tan((lv - lE)/2) = [ex sin lE - ey cos lE] / [sqrt(1-ex^2-ey^2) + 1 - ex cos lE - ey sin lE]
946         // leads to
947         // cT (dlv - dlE) = cE dlE + cX dex + cY dey
948         // with
949         // cT = [d^2 + (ex sin lE - ey cos lE)^2] / 2
950         // d  = 1 + sqrt(1-ex^2-ey^2) - ex cos lE - ey sin lE
951         // cE = (ex cos lE + ey sin lE) (sqrt(1-ex^2-ey^2) + 1) - ex^2 - ey^2
952         // cX =  sin lE (sqrt(1-ex^2-ey^2) + 1) - ey + ex (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
953         // cY = -cos lE (sqrt(1-ex^2-ey^2) + 1) + ex + ey (ex sin lE - ey cos lE) / sqrt(1-ex^2-ey^2)
954         // which can be solved to find the differential of the true longitude
955         // dlv = (cT + cE) / cT dlE + cX / cT deX + cY / cT deX
956         final SinCos scLe      = FastMath.sinCos(getLE());
957         final double cosLe     = scLe.cos();
958         final double sinLe     = scLe.sin();
959         final double eSinE     = ex * sinLe - ey * cosLe;
960         final double ecosE     = ex * cosLe + ey * sinLe;
961         final double e2        = ex * ex + ey * ey;
962         final double epsilon   = FastMath.sqrt(1 - e2);
963         final double onePeps   = 1 + epsilon;
964         final double d         = onePeps - ecosE;
965         final double cT        = (d * d + eSinE * eSinE) / 2;
966         final double cE        = ecosE * onePeps - e2;
967         final double cX        = ex * eSinE / epsilon - ey + sinLe * onePeps;
968         final double cY        = ey * eSinE / epsilon + ex - cosLe * onePeps;
969         final double factorLe  = (cT + cE) / cT;
970         final double factorEx  = cX / cT;
971         final double factorEy  = cY / cT;
972 
973         // update longitude row
974         final double[] rowEx = jacobian[1];
975         final double[] rowEy = jacobian[2];
976         final double[] rowL = jacobian[5];
977         for (int j = 0; j < 6; ++j) {
978             rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
979         }
980 
981         return jacobian;
982 
983     }
984 
985     /** {@inheritDoc} */
986     @Override
987     public void addKeplerContribution(final PositionAngleType type, final double gm,
988                                       final double[] pDot) {
989         pDot[5] += computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType);
990     }
991 
992     /**
993      * Compute rate of argument of longitude.
994      * @param type position angle type of rate
995      * @param a semi major axis
996      * @param ex ex
997      * @param ey ey
998      * @param mu mu
999      * @param l argument of longitude
1000      * @param cachedType position angle type of passed l
1001      * @return first-order time derivative for l
1002      * @since 12.2
1003      */
1004     private static double computeKeplerianLDot(final PositionAngleType type, final double a, final double ex,
1005                                                final double ey, final double mu,
1006                                                final double l, final PositionAngleType cachedType) {
1007         final double n  = FastMath.sqrt(mu / a) / a;
1008         if (type == PositionAngleType.MEAN) {
1009             return n;
1010         }
1011         final double oMe2;
1012         final double ksi;
1013         final SinCos sc;
1014         if (type == PositionAngleType.ECCENTRIC) {
1015             sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1016             ksi  = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1017             return n * ksi;
1018         } else { // TRUE
1019             sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1020             oMe2 = 1 - ex * ex - ey * ey;
1021             ksi  = 1 + ex * sc.cos() + ey * sc.sin();
1022             return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1023         }
1024     }
1025 
1026     /**  Returns a string representation of this equinoctial parameters object.
1027      * @return a string representation of this object
1028      */
1029     public String toString() {
1030         return new StringBuilder().append("equinoctial parameters: ").append('{').
1031                                   append("a: ").append(a).
1032                                   append("; ex: ").append(ex).append("; ey: ").append(ey).
1033                                   append("; hx: ").append(hx).append("; hy: ").append(hy).
1034                                   append("; lv: ").append(FastMath.toDegrees(getLv())).
1035                                   append(";}").toString();
1036     }
1037 
1038     /** {@inheritDoc} */
1039     @Override
1040     public PositionAngleType getCachedPositionAngleType() {
1041         return cachedPositionAngleType;
1042     }
1043 
1044     /** {@inheritDoc} */
1045     @Override
1046     public boolean hasNonKeplerianRates() {
1047         return hasNonKeplerianAcceleration();
1048     }
1049 
1050     /** {@inheritDoc} */
1051     @Override
1052     public EquinoctialOrbit withKeplerianRates() {
1053         final PositionAngleType positionAngleType = getCachedPositionAngleType();
1054         return new EquinoctialOrbit(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
1055                 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1056     }
1057 
1058 }