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;
18  
19  
20  import java.lang.reflect.Array;
21  
22  import org.hipparchus.CalculusFieldElement;
23  import org.hipparchus.Field;
24  import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
25  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26  import org.hipparchus.util.FastMath;
27  import org.hipparchus.util.FieldSinCos;
28  import org.hipparchus.util.MathArrays;
29  import org.orekit.errors.OrekitException;
30  import org.orekit.errors.OrekitIllegalArgumentException;
31  import org.orekit.errors.OrekitInternalError;
32  import org.orekit.errors.OrekitMessages;
33  import org.orekit.frames.Frame;
34  import org.orekit.time.FieldAbsoluteDate;
35  import org.orekit.utils.FieldPVCoordinates;
36  import org.orekit.utils.TimeStampedFieldPVCoordinates;
37  
38  
39  /**
40   * This class handles traditional Keplerian orbital parameters.
41  
42   * <p>
43   * The parameters used internally are the classical Keplerian elements:
44   *   <pre>
45   *     a
46   *     e
47   *     i
48   *     ω
49   *     Ω
50   *     v
51   *   </pre>
52   * where ω stands for the Perigee Argument, Ω stands for the
53   * Right Ascension of the Ascending Node and v stands for the true anomaly.
54   * <p>
55   * This class supports hyperbolic orbits, using the convention that semi major
56   * axis is negative for such orbits (and of course eccentricity is greater than 1).
57   * </p>
58   * <p>
59   * When orbit is either equatorial or circular, some Keplerian elements
60   * (more precisely ω and Ω) become ambiguous so this class should not
61   * be used for such orbits. For this reason, {@link EquinoctialOrbit equinoctial
62   * orbits} is the recommended way to represent orbits.
63   * </p>
64  
65   * <p>
66   * The instance <code>KeplerianOrbit</code> is guaranteed to be immutable.
67   * </p>
68   * @see     Orbit
69   * @see    CircularOrbit
70   * @see    CartesianOrbit
71   * @see    EquinoctialOrbit
72   * @author Luc Maisonobe
73   * @author Guylaine Prat
74   * @author Fabien Maussion
75   * @author V&eacute;ronique Pommier-Maurussane
76   * @author Andrea Antolino
77   * @since 9.0
78   * @param <T> type of the field elements
79   */
80  public class FieldKeplerianOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T>
81          implements PositionAngleBased {
82  
83      /** Name of the eccentricity parameter. */
84      private static final String ECCENTRICITY = "eccentricity";
85  
86      /** Semi-major axis (m). */
87      private final T a;
88  
89      /** Eccentricity. */
90      private final T e;
91  
92      /** Inclination (rad). */
93      private final T i;
94  
95      /** Perigee Argument (rad). */
96      private final T pa;
97  
98      /** Right Ascension of Ascending Node (rad). */
99      private final T raan;
100 
101     /** Cached anomaly (rad). */
102     private final T cachedAnomaly;
103 
104     /** Semi-major axis derivative (m/s). */
105     private final T aDot;
106 
107     /** Eccentricity derivative. */
108     private final T eDot;
109 
110     /** Inclination derivative (rad/s). */
111     private final T iDot;
112 
113     /** Perigee Argument derivative (rad/s). */
114     private final T paDot;
115 
116     /** Right Ascension of Ascending Node derivative (rad/s). */
117     private final T raanDot;
118 
119     /** Derivative of cached anomaly (rad/s). */
120     private final T cachedAnomalyDot;
121 
122     /** Cached type of position angle. */
123     private final PositionAngleType cachedPositionAngleType;
124 
125     /** Partial Cartesian coordinates (position and velocity are valid, acceleration may be missing). */
126     private FieldPVCoordinates<T> partialPV;
127 
128     /** PThird Canonical Vector. */
129     private final FieldVector3D<T> PLUS_K;
130 
131     /** Creates a new instance.
132      * @param a  semi-major axis (m), negative for hyperbolic orbits
133      * @param e eccentricity (positive or equal to 0)
134      * @param i inclination (rad)
135      * @param pa perigee argument (ω, rad)
136      * @param raan right ascension of ascending node (Ω, rad)
137      * @param anomaly mean, eccentric or true anomaly (rad)
138      * @param type type of anomaly
139      * @param cachedPositionAngleType type of cached anomaly
140      * @param frame the frame in which the parameters are defined
141      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
142      * @param date date of the orbital parameters
143      * @param mu central attraction coefficient (m³/s²)
144      * @exception IllegalArgumentException if frame is not a {@link
145      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
146      * or v is out of range for hyperbolic orbits
147      * @since 12.1
148      */
149     public FieldKeplerianOrbit(final T a, final T e, final T i,
150                                final T pa, final T raan,
151                                final T anomaly, final PositionAngleType type,
152                                final PositionAngleType cachedPositionAngleType,
153                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
154         throws IllegalArgumentException {
155         this(a, e, i, pa, raan, anomaly,
156              null, null, null, null, null, null,
157              type, cachedPositionAngleType, frame, date, mu);
158     }
159 
160     /** Creates a new instance.
161      * @param a  semi-major axis (m), negative for hyperbolic orbits
162      * @param e eccentricity (positive or equal to 0)
163      * @param i inclination (rad)
164      * @param pa perigee argument (ω, rad)
165      * @param raan right ascension of ascending node (Ω, rad)
166      * @param anomaly mean, eccentric or true anomaly (rad)
167      * @param type type of anomaly
168      * @param frame the frame in which the parameters are defined
169      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
170      * @param date date of the orbital parameters
171      * @param mu central attraction coefficient (m³/s²)
172      * @exception IllegalArgumentException if frame is not a {@link
173      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
174      * or v is out of range for hyperbolic orbits
175      * @since 12.1
176      */
177     public FieldKeplerianOrbit(final T a, final T e, final T i,
178                                final T pa, final T raan,
179                                final T anomaly, final PositionAngleType type,
180                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
181             throws IllegalArgumentException {
182         this(a, e, i, pa, raan, anomaly, type, type, frame, date, mu);
183     }
184 
185     /** Creates a new instance.
186      * @param a  semi-major axis (m), negative for hyperbolic orbits
187      * @param e eccentricity (positive or equal to 0)
188      * @param i inclination (rad)
189      * @param pa perigee argument (ω, rad)
190      * @param raan right ascension of ascending node (Ω, rad)
191      * @param anomaly mean, eccentric or true anomaly (rad)
192      * @param aDot  semi-major axis derivative, null if unknown (m/s)
193      * @param eDot eccentricity derivative, null if unknown
194      * @param iDot inclination derivative, null if unknown (rad/s)
195      * @param paDot perigee argument derivative, null if unknown (rad/s)
196      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
197      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
198      * @param type type of anomaly
199      * @param cachedPositionAngleType type of cached anomaly
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 frame is not a {@link
205      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
206      * or v is out of range for hyperbolic orbits
207      * @since 12.1
208      */
209     public FieldKeplerianOrbit(final T a, final T e, final T i,
210                                final T pa, final T raan, final T anomaly,
211                                final T aDot, final T eDot, final T iDot,
212                                final T paDot, final T raanDot, final T anomalyDot,
213                                final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
214                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
215         throws IllegalArgumentException {
216         super(frame, date, mu);
217         this.cachedPositionAngleType = cachedPositionAngleType;
218 
219         if (a.multiply(e.negate().add(1)).getReal() < 0) {
220             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a.getReal(), e.getReal());
221         }
222 
223         // Checking eccentricity range
224         checkParameterRangeInclusive(ECCENTRICITY, e.getReal(), 0.0, Double.POSITIVE_INFINITY);
225 
226         this.a       =    a;
227         this.aDot    =    aDot;
228         this.e       =    e;
229         this.eDot    =    eDot;
230         this.i       =    i;
231         this.iDot    =    iDot;
232         this.pa      =    pa;
233         this.paDot   =    paDot;
234         this.raan    =    raan;
235         this.raanDot =    raanDot;
236 
237         this.PLUS_K = FieldVector3D.getPlusK(a.getField());  // third canonical vector
238 
239         if (hasDerivatives()) {
240             final FieldUnivariateDerivative1<T> cachedAnomalyUD = initializeCachedAnomaly(anomaly, anomalyDot, type);
241             this.cachedAnomaly = cachedAnomalyUD.getValue();
242             this.cachedAnomalyDot = cachedAnomalyUD.getFirstDerivative();
243         } else {
244             this.cachedAnomaly = initializeCachedAnomaly(anomaly, type);
245             this.cachedAnomalyDot = null;
246         }
247 
248         // check true anomaly range
249         if (!isElliptical()) {
250             final T trueAnomaly = getTrueAnomaly();
251             if (e.multiply(trueAnomaly.cos()).add(1).getReal() <= 0) {
252                 final double vMax = e.reciprocal().negate().acos().getReal();
253                 throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
254                         trueAnomaly.getReal(), e.getReal(), -vMax, vMax);
255             }
256         }
257 
258         this.partialPV = null;
259 
260     }
261 
262     /** Creates a new instance.
263      * @param a  semi-major axis (m), negative for hyperbolic orbits
264      * @param e eccentricity (positive or equal to 0)
265      * @param i inclination (rad)
266      * @param pa perigee argument (ω, rad)
267      * @param raan right ascension of ascending node (Ω, rad)
268      * @param anomaly mean, eccentric or true anomaly (rad)
269      * @param aDot  semi-major axis derivative, null if unknown (m/s)
270      * @param eDot eccentricity derivative, null if unknown
271      * @param iDot inclination derivative, null if unknown (rad/s)
272      * @param paDot perigee argument derivative, null if unknown (rad/s)
273      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
274      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
275      * @param type type of anomaly
276      * @param frame the frame in which the parameters are defined
277      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
278      * @param date date of the orbital parameters
279      * @param mu central attraction coefficient (m³/s²)
280      * @exception IllegalArgumentException if frame is not a {@link
281      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
282      * or v is out of range for hyperbolic orbits
283      */
284     public FieldKeplerianOrbit(final T a, final T e, final T i,
285                                final T pa, final T raan, final T anomaly,
286                                final T aDot, final T eDot, final T iDot,
287                                final T paDot, final T raanDot, final T anomalyDot,
288                                final PositionAngleType type,
289                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
290             throws IllegalArgumentException {
291         this(a, e, i, pa, raan, anomaly, aDot, eDot, iDot, paDot, raanDot, anomalyDot,
292                 type, type, frame, date, mu);
293     }
294 
295     /** Constructor from Cartesian parameters.
296      *
297      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
298      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
299      * use {@code mu} and the position to compute the acceleration, including
300      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
301      *
302      * @param pvCoordinates the PVCoordinates of the satellite
303      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
304      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
305      * @param mu central attraction coefficient (m³/s²)
306      * @exception IllegalArgumentException if frame is not a {@link
307      * Frame#isPseudoInertial pseudo-inertial frame}
308      */
309     public FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
310                                final Frame frame, final T mu)
311         throws IllegalArgumentException {
312         this(pvCoordinates, frame, mu, hasNonKeplerianAcceleration(pvCoordinates, mu));
313     }
314 
315     /** Constructor from Cartesian parameters.
316      *
317      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
318      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
319      * use {@code mu} and the position to compute the acceleration, including
320      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
321      *
322      * @param pvCoordinates the PVCoordinates of the satellite
323      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
324      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
325      * @param mu central attraction coefficient (m³/s²)
326      * @param reliableAcceleration if true, the acceleration is considered to be reliable
327      * @exception IllegalArgumentException if frame is not a {@link
328      * Frame#isPseudoInertial pseudo-inertial frame}
329      */
330     private FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
331                                 final Frame frame, final T mu,
332                                 final boolean reliableAcceleration)
333         throws IllegalArgumentException {
334 
335         super(pvCoordinates, frame, mu);
336 
337         // third canonical vector
338         this.PLUS_K = FieldVector3D.getPlusK(getOne().getField());
339 
340         // compute inclination
341         final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
342         final T m2 = momentum.getNormSq();
343 
344         i = FieldVector3D.angle(momentum, PLUS_K);
345         // compute right ascension of ascending node
346         raan = FieldVector3D.crossProduct(PLUS_K, momentum).getAlpha();
347         // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic)
348         final FieldVector3D<T> pvP     = pvCoordinates.getPosition();
349         final FieldVector3D<T> pvV     = pvCoordinates.getVelocity();
350         final FieldVector3D<T> pvA     = pvCoordinates.getAcceleration();
351 
352         final T   r2      = pvP.getNormSq();
353         final T   r       = r2.sqrt();
354         final T   V2      = pvV.getNormSq();
355         final T   rV2OnMu = r.multiply(V2).divide(mu);
356 
357         // compute semi-major axis (will be negative for hyperbolic orbits)
358         a = r.divide(rV2OnMu.negate().add(2.0));
359         final T muA = a.multiply(mu);
360 
361         // compute true anomaly
362         if (isElliptical()) {
363             // elliptic or circular orbit
364             final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
365             final T eCE = rV2OnMu.subtract(1);
366             e = (eSE.multiply(eSE).add(eCE.multiply(eCE))).sqrt();
367             this.cachedPositionAngleType = PositionAngleType.ECCENTRIC;
368             cachedAnomaly = eSE.atan2(eCE);
369         } else {
370             // hyperbolic orbit
371             final T eSH = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
372             final T eCH = rV2OnMu.subtract(1);
373             e = (m2.negate().divide(muA).add(1)).sqrt();
374             this.cachedPositionAngleType = PositionAngleType.TRUE;
375             cachedAnomaly = FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, (eCH.add(eSH)).divide(eCH.subtract(eSH)).log().divide(2));
376         }
377 
378         // Checking eccentricity range
379         checkParameterRangeInclusive(ECCENTRICITY, e.getReal(), 0.0, Double.POSITIVE_INFINITY);
380 
381         // compute perigee argument
382         final FieldVector3D<T> node = new FieldVector3D<>(raan, getZero());
383         final T px = FieldVector3D.dotProduct(pvP, node);
384         final T py = FieldVector3D.dotProduct(pvP, FieldVector3D.crossProduct(momentum, node)).divide(m2.sqrt());
385         pa = py.atan2(px).subtract(getTrueAnomaly());
386 
387         partialPV = pvCoordinates;
388 
389         if (reliableAcceleration) {
390             // we have a relevant acceleration, we can compute derivatives
391 
392             final T[][] jacobian = MathArrays.buildArray(a.getField(), 6, 6);
393             getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
394 
395             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
396             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
397             final T   aX                       = nonKeplerianAcceleration.getX();
398             final T   aY                       = nonKeplerianAcceleration.getY();
399             final T   aZ                       = nonKeplerianAcceleration.getZ();
400             aDot    = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
401             eDot    = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
402             iDot    = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
403             paDot   = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
404             raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));
405 
406             // in order to compute true anomaly derivative, we must compute
407             // mean anomaly derivative including Keplerian motion and convert to true anomaly
408             final T MDot = getKeplerianMeanMotion().
409                            add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
410             final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
411             final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(getMeanAnomaly(), MDot);
412             if (cachedPositionAngleType == PositionAngleType.ECCENTRIC) {
413                 final FieldUnivariateDerivative1<T> EUD = (a.getReal() < 0) ?
414                          FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD, MUD) :
415                          FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD, MUD);
416                 cachedAnomalyDot = EUD.getFirstDerivative();
417             } else { // TRUE
418                 final FieldUnivariateDerivative1<T> vUD = (a.getReal() < 0) ?
419                          FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, MUD) :
420                          FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, MUD);
421                 cachedAnomalyDot = vUD.getFirstDerivative();
422             }
423 
424         } else {
425             // acceleration is either almost zero or NaN,
426             // we assume acceleration was not known
427             // we don't set up derivatives
428             aDot    = null;
429             eDot    = null;
430             iDot    = null;
431             paDot   = null;
432             raanDot = null;
433             cachedAnomalyDot    = null;
434         }
435 
436     }
437 
438     /** Constructor from Cartesian parameters.
439      *
440      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
441      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
442      * use {@code mu} and the position to compute the acceleration, including
443      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
444      *
445      * @param FieldPVCoordinates the PVCoordinates of the satellite
446      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
447      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
448      * @param date date of the orbital parameters
449      * @param mu central attraction coefficient (m³/s²)
450      * @exception IllegalArgumentException if frame is not a {@link
451      * Frame#isPseudoInertial pseudo-inertial frame}
452      */
453     public FieldKeplerianOrbit(final FieldPVCoordinates<T> FieldPVCoordinates,
454                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
455         throws IllegalArgumentException {
456         this(new TimeStampedFieldPVCoordinates<>(date, FieldPVCoordinates), frame, mu);
457     }
458 
459     /** Constructor from any kind of orbital parameters.
460      * @param op orbital parameters to copy
461      */
462     public FieldKeplerianOrbit(final FieldOrbit<T> op) {
463         this(op.getPVCoordinates(), op.getFrame(), op.getMu(), op.hasDerivatives());
464     }
465 
466     /** Constructor from Field and KeplerianOrbit.
467      * <p>Build a FieldKeplerianOrbit from non-Field KeplerianOrbit.</p>
468      * @param field CalculusField to base object on
469      * @param op non-field orbit with only "constant" terms
470      * @since 12.0
471      */
472     public FieldKeplerianOrbit(final Field<T> field, final KeplerianOrbit op) {
473         this(field.getZero().newInstance(op.getA()), field.getZero().newInstance(op.getE()), field.getZero().newInstance(op.getI()),
474                 field.getZero().newInstance(op.getPerigeeArgument()), field.getZero().newInstance(op.getRightAscensionOfAscendingNode()),
475                 field.getZero().newInstance(op.getAnomaly(op.getCachedPositionAngleType())),
476                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getADot()) : null,
477                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getEDot()) : null,
478                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getIDot()) : null,
479                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getPerigeeArgumentDot()) : null,
480                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getRightAscensionOfAscendingNodeDot()) : null,
481                 (op.hasDerivatives()) ? field.getZero().newInstance(op.getAnomalyDot(op.getCachedPositionAngleType())) : null,
482                 op.getCachedPositionAngleType(), op.getFrame(),
483                 new FieldAbsoluteDate<>(field, op.getDate()), field.getZero().newInstance(op.getMu()));
484     }
485 
486     /** Constructor from Field and Orbit.
487      * <p>Build a FieldKeplerianOrbit from any non-Field Orbit.</p>
488      * @param field CalculusField to base object on
489      * @param op non-field orbit with only "constant" terms
490      * @since 12.0
491      */
492     public FieldKeplerianOrbit(final Field<T> field, final Orbit op) {
493         this(field, (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(op));
494     }
495 
496     /** {@inheritDoc} */
497     @Override
498     public OrbitType getType() {
499         return OrbitType.KEPLERIAN;
500     }
501 
502     /** {@inheritDoc} */
503     @Override
504     public T getA() {
505         return a;
506     }
507 
508     /** {@inheritDoc} */
509     @Override
510     public T getADot() {
511         return aDot;
512     }
513 
514     /** {@inheritDoc} */
515     @Override
516     public T getE() {
517         return e;
518     }
519 
520     /** {@inheritDoc} */
521     @Override
522     public T getEDot() {
523         return eDot;
524     }
525 
526     /** {@inheritDoc} */
527     @Override
528     public T getI() {
529         return i;
530     }
531 
532     /** {@inheritDoc} */
533     @Override
534     public T getIDot() {
535         return iDot;
536     }
537 
538     /** Get the perigee argument.
539      * @return perigee argument (rad)
540      */
541     public T getPerigeeArgument() {
542         return pa;
543     }
544 
545     /** Get the perigee argument derivative.
546      * <p>
547      * If the orbit was created without derivatives, the value returned is null.
548      * </p>
549      * @return perigee argument derivative (rad/s)
550      */
551     public T getPerigeeArgumentDot() {
552         return paDot;
553     }
554 
555     /** Get the right ascension of the ascending node.
556      * @return right ascension of the ascending node (rad)
557      */
558     public T getRightAscensionOfAscendingNode() {
559         return raan;
560     }
561 
562     /** Get the right ascension of the ascending node derivative.
563      * <p>
564      * If the orbit was created without derivatives, the value returned is null.
565      * </p>
566      * @return right ascension of the ascending node derivative (rad/s)
567      */
568     public T getRightAscensionOfAscendingNodeDot() {
569         return raanDot;
570     }
571 
572     /** Get the true anomaly.
573      * @return true anomaly (rad)
574      */
575     public T getTrueAnomaly() {
576         switch (cachedPositionAngleType) {
577             case MEAN: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(e, cachedAnomaly) :
578                     FieldKeplerianAnomalyUtility.ellipticMeanToTrue(e, cachedAnomaly);
579 
580             case TRUE: return cachedAnomaly;
581 
582             case ECCENTRIC: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, cachedAnomaly) :
583                     FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, cachedAnomaly);
584 
585             default: throw new OrekitInternalError(null);
586         }
587     }
588 
589     /** Get the true anomaly derivative.
590      * <p>
591      * If the orbit was created without derivatives, the value returned is null.
592      * </p>
593      * @return true anomaly derivative (rad/s)
594      */
595     public T getTrueAnomalyDot() {
596         if (hasDerivatives()) {
597             switch (cachedPositionAngleType) {
598                 case MEAN:
599                     final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
600                     final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
601                     final FieldUnivariateDerivative1<T> vUD = (a.getReal() < 0) ?
602                              FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, MUD) :
603                              FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, MUD);
604                     return vUD.getFirstDerivative();
605 
606                 case TRUE:
607                     return cachedAnomalyDot;
608 
609                 case ECCENTRIC:
610                     final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
611                     final FieldUnivariateDerivative1<T> EUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
612                     final FieldUnivariateDerivative1<T> vUD2 = (a.getReal() < 0) ?
613                              FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(eUD2, EUD) :
614                              FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(eUD2, EUD);
615                     return vUD2.getFirstDerivative();
616 
617                 default:
618                     throw new OrekitInternalError(null);
619             }
620         } else {
621             return null;
622         }
623     }
624 
625     /** Get the eccentric anomaly.
626      * @return eccentric anomaly (rad)
627      */
628     public T getEccentricAnomaly() {
629         switch (cachedPositionAngleType) {
630             case MEAN:
631                 return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, cachedAnomaly) :
632                     FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, cachedAnomaly);
633 
634             case ECCENTRIC:
635                 return cachedAnomaly;
636 
637             case TRUE:
638                 return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, cachedAnomaly) :
639                     FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, cachedAnomaly);
640 
641             default:
642                 throw new OrekitInternalError(null);
643         }
644     }
645 
646     /** Get the eccentric anomaly derivative.
647      * <p>
648      * If the orbit was created without derivatives, the value returned is null.
649      * </p>
650      * @return eccentric anomaly derivative (rad/s)
651      */
652     public T getEccentricAnomalyDot() {
653         if (hasDerivatives()) {
654             switch (cachedPositionAngleType) {
655                 case ECCENTRIC:
656                     return cachedAnomalyDot;
657 
658                 case TRUE:
659                     final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
660                     final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
661                     final FieldUnivariateDerivative1<T> EUD = (a.getReal() < 0) ?
662                              FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(eUD, vUD) :
663                              FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(eUD, vUD);
664                     return EUD.getFirstDerivative();
665 
666                 case MEAN:
667                     final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
668                     final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
669                     final FieldUnivariateDerivative1<T> EUD2 = (a.getReal() < 0) ?
670                              FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD2, MUD) :
671                              FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD2, MUD);
672                     return EUD2.getFirstDerivative();
673 
674                 default:
675                     throw new OrekitInternalError(null);
676             }
677         } else {
678             return null;
679         }
680     }
681 
682     /** Get the mean anomaly.
683      * @return mean anomaly (rad)
684      */
685     public T getMeanAnomaly() {
686         switch (cachedPositionAngleType) {
687             case ECCENTRIC: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(e, cachedAnomaly) :
688                     FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, cachedAnomaly);
689 
690             case MEAN: return cachedAnomaly;
691 
692             case TRUE: return (a.getReal() < 0) ? FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(e, cachedAnomaly) :
693                     FieldKeplerianAnomalyUtility.ellipticTrueToMean(e, cachedAnomaly);
694 
695             default: throw new OrekitInternalError(null);
696         }
697     }
698 
699     /** Get the mean anomaly derivative.
700      * <p>
701      * If the orbit was created without derivatives, the value returned is null.
702      * </p>
703      * @return mean anomaly derivative (rad/s)
704      */
705     public T getMeanAnomalyDot() {
706         if (hasDerivatives()) {
707             switch (cachedPositionAngleType) {
708                 case MEAN:
709                     return cachedAnomalyDot;
710 
711                 case ECCENTRIC:
712                     final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
713                     final FieldUnivariateDerivative1<T> EUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
714                     final FieldUnivariateDerivative1<T> MUD = (a.getReal() < 0) ?
715                              FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(eUD, EUD) :
716                              FieldKeplerianAnomalyUtility.ellipticEccentricToMean(eUD, EUD);
717                     return MUD.getFirstDerivative();
718 
719                 case TRUE:
720                     final FieldUnivariateDerivative1<T> eUD2 = new FieldUnivariateDerivative1<>(e, eDot);
721                     final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(cachedAnomaly, cachedAnomalyDot);
722                     final FieldUnivariateDerivative1<T> MUD2 = (a.getReal() < 0) ?
723                              FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(eUD2, vUD) :
724                              FieldKeplerianAnomalyUtility.ellipticTrueToMean(eUD2, vUD);
725                     return MUD2.getFirstDerivative();
726 
727                 default:
728                     throw new OrekitInternalError(null);
729             }
730         } else {
731             return null;
732         }
733     }
734 
735     /** Get the anomaly.
736      * @param type type of the angle
737      * @return anomaly (rad)
738      */
739     public T getAnomaly(final PositionAngleType type) {
740         return (type == PositionAngleType.MEAN) ? getMeanAnomaly() :
741                                               ((type == PositionAngleType.ECCENTRIC) ? getEccentricAnomaly() :
742                                                                                    getTrueAnomaly());
743     }
744 
745     /** Get the anomaly derivative.
746      * <p>
747      * If the orbit was created without derivatives, the value returned is null.
748      * </p>
749      * @param type type of the angle
750      * @return anomaly derivative (rad/s)
751      */
752     public T getAnomalyDot(final PositionAngleType type) {
753         return (type == PositionAngleType.MEAN) ? getMeanAnomalyDot() :
754                                               ((type == PositionAngleType.ECCENTRIC) ? getEccentricAnomalyDot() :
755                                                                                    getTrueAnomalyDot());
756     }
757 
758     /** {@inheritDoc} */
759     @Override
760     public boolean hasDerivatives() {
761         return aDot != null;
762     }
763 
764     /** {@inheritDoc} */
765     @Override
766     public T getEquinoctialEx() {
767         return e.multiply(pa.add(raan).cos());
768     }
769 
770     /** {@inheritDoc} */
771     @Override
772     public T getEquinoctialExDot() {
773 
774         if (!hasDerivatives()) {
775             return null;
776         }
777 
778         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
779         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
780         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
781         return eUD.multiply(paUD.add(raanUD).cos()).getFirstDerivative();
782 
783     }
784 
785     /** {@inheritDoc} */
786     @Override
787     public T getEquinoctialEy() {
788         return e.multiply((pa.add(raan)).sin());
789     }
790 
791     /** {@inheritDoc} */
792     @Override
793     public T getEquinoctialEyDot() {
794 
795         if (!hasDerivatives()) {
796             return null;
797         }
798 
799         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
800         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
801         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
802         return eUD.multiply(paUD.add(raanUD).sin()).getFirstDerivative();
803 
804     }
805 
806     /** {@inheritDoc} */
807     @Override
808     public T getHx() {
809         // Check for equatorial retrograde orbit
810         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
811             return getZero().add(Double.NaN);
812         }
813         return raan.cos().multiply(i.divide(2).tan());
814     }
815 
816     /** {@inheritDoc} */
817     @Override
818     public T getHxDot() {
819 
820         if (!hasDerivatives()) {
821             return null;
822         }
823 
824         // Check for equatorial retrograde orbit
825         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
826             return getZero().add(Double.NaN);
827         }
828 
829         final FieldUnivariateDerivative1<T> iUD    = new FieldUnivariateDerivative1<>(i,    iDot);
830         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
831         return raanUD.cos().multiply(iUD.multiply(0.5).tan()).getFirstDerivative();
832 
833     }
834 
835     /** {@inheritDoc} */
836     @Override
837     public T getHy() {
838         // Check for equatorial retrograde orbit
839         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
840             return getZero().add(Double.NaN);
841         }
842         return raan.sin().multiply(i.divide(2).tan());
843     }
844 
845     /** {@inheritDoc} */
846     @Override
847     public T getHyDot() {
848 
849         if (!hasDerivatives()) {
850             return null;
851         }
852 
853         // Check for equatorial retrograde orbit
854         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
855             return getZero().add(Double.NaN);
856         }
857 
858         final FieldUnivariateDerivative1<T> iUD    = new FieldUnivariateDerivative1<>(i,    iDot);
859         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
860         return raanUD.sin().multiply(iUD.multiply(0.5).tan()).getFirstDerivative();
861 
862     }
863 
864     /** {@inheritDoc} */
865     @Override
866     public T getLv() {
867         return pa.add(raan).add(getTrueAnomaly());
868     }
869 
870     /** {@inheritDoc} */
871     @Override
872     public T getLvDot() {
873         return hasDerivatives() ?
874                paDot.add(raanDot).add(getTrueAnomalyDot()) :
875                null;
876     }
877 
878     /** {@inheritDoc} */
879     @Override
880     public T getLE() {
881         return pa.add(raan).add(getEccentricAnomaly());
882     }
883 
884     /** {@inheritDoc} */
885     @Override
886     public T getLEDot() {
887         return hasDerivatives() ?
888                paDot.add(raanDot).add(getEccentricAnomalyDot()) :
889                null;
890     }
891 
892     /** {@inheritDoc} */
893     @Override
894     public T getLM() {
895         return pa.add(raan).add(getMeanAnomaly());
896     }
897 
898     /** {@inheritDoc} */
899     @Override
900     public T getLMDot() {
901         return hasDerivatives() ?
902                paDot.add(raanDot).add(getMeanAnomalyDot()) :
903                null;
904     }
905 
906     /** Initialize cached anomaly with rate.
907      * @param anomaly input anomaly
908      * @param anomalyDot rate of input anomaly
909      * @param inputType position angle type passed as input
910      * @return anomaly to cache with rate
911      * @since 12.1
912      */
913     private FieldUnivariateDerivative1<T> initializeCachedAnomaly(final T anomaly, final T anomalyDot,
914                                                                   final PositionAngleType inputType) {
915         if (cachedPositionAngleType == inputType) {
916             return new FieldUnivariateDerivative1<>(anomaly, anomalyDot);
917 
918         } else {
919             final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
920             final FieldUnivariateDerivative1<T> anomalyUD = new FieldUnivariateDerivative1<>(anomaly, anomalyDot);
921 
922             if (a.getReal() < 0) {
923                 switch (cachedPositionAngleType) {
924                     case MEAN:
925                         if (inputType == PositionAngleType.ECCENTRIC) {
926                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(eUD, anomalyUD);
927                         } else {
928                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(eUD, anomalyUD);
929                         }
930 
931                     case ECCENTRIC:
932                         if (inputType == PositionAngleType.MEAN) {
933                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(eUD, anomalyUD);
934                         } else {
935                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(eUD, anomalyUD);
936                         }
937 
938                     case TRUE:
939                         if (inputType == PositionAngleType.MEAN) {
940                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, anomalyUD);
941                         } else {
942                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(eUD, anomalyUD);
943                         }
944 
945                     default:
946                         break;
947                 }
948 
949             } else {
950                 switch (cachedPositionAngleType) {
951                     case MEAN:
952                         if (inputType == PositionAngleType.ECCENTRIC) {
953                             return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(eUD, anomalyUD);
954                         } else {
955                             return FieldKeplerianAnomalyUtility.ellipticTrueToMean(eUD, anomalyUD);
956                         }
957 
958                     case ECCENTRIC:
959                         if (inputType == PositionAngleType.MEAN) {
960                             return FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(eUD, anomalyUD);
961                         } else {
962                             return FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(eUD, anomalyUD);
963                         }
964 
965                     case TRUE:
966                         if (inputType == PositionAngleType.MEAN) {
967                             return FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, anomalyUD);
968                         } else {
969                             return FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(eUD, anomalyUD);
970                         }
971 
972                     default:
973                         break;
974                 }
975 
976             }
977             throw new OrekitInternalError(null);
978         }
979 
980     }
981 
982     /** Initialize cached anomaly.
983      * @param anomaly input anomaly
984      * @param inputType position angle type passed as input
985      * @return anomaly to cache
986      * @since 12.1
987      */
988     private T initializeCachedAnomaly(final T anomaly, final PositionAngleType inputType) {
989         if (inputType == cachedPositionAngleType) {
990             return anomaly;
991 
992         } else {
993             if (a.getReal() < 0) {
994                 switch (cachedPositionAngleType) {
995                     case MEAN:
996                         if (inputType == PositionAngleType.ECCENTRIC) {
997                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(e, anomaly);
998                         } else {
999                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(e, anomaly);
1000                         }
1001 
1002                     case ECCENTRIC:
1003                         if (inputType == PositionAngleType.MEAN) {
1004                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, anomaly);
1005                         } else {
1006                             return FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, anomaly);
1007                         }
1008 
1009                     case TRUE:
1010                         if (inputType == PositionAngleType.ECCENTRIC) {
1011                             return FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, anomaly);
1012                         } else {
1013                             return FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(e, anomaly);
1014                         }
1015 
1016                     default:
1017                         break;
1018                 }
1019 
1020             } else {
1021                 switch (cachedPositionAngleType) {
1022                     case MEAN:
1023                         if (inputType == PositionAngleType.ECCENTRIC) {
1024                             return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, anomaly);
1025                         } else {
1026                             return FieldKeplerianAnomalyUtility.ellipticTrueToMean(e, anomaly);
1027                         }
1028 
1029                     case ECCENTRIC:
1030                         if (inputType == PositionAngleType.MEAN) {
1031                             return FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, anomaly);
1032                         } else {
1033                             return FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, anomaly);
1034                         }
1035 
1036                     case TRUE:
1037                         if (inputType == PositionAngleType.ECCENTRIC) {
1038                             return FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, anomaly);
1039                         } else {
1040                             return FieldKeplerianAnomalyUtility.ellipticMeanToTrue(e, anomaly);
1041                         }
1042 
1043                     default:
1044                         break;
1045                 }
1046             }
1047             throw new OrekitInternalError(null);
1048         }
1049     }
1050 
1051     /** Compute reference axes.
1052      * @return referecne axes
1053      * @since 12.0
1054      */
1055     private FieldVector3D<T>[] referenceAxes() {
1056 
1057         // preliminary variables
1058         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
1059         final FieldSinCos<T> scPa   = FastMath.sinCos(pa);
1060         final FieldSinCos<T> scI    = FastMath.sinCos(i);
1061         final T cosRaan = scRaan.cos();
1062         final T sinRaan = scRaan.sin();
1063         final T cosPa   = scPa.cos();
1064         final T sinPa   = scPa.sin();
1065         final T cosI    = scI.cos();
1066         final T sinI    = scI.sin();
1067         final T crcp    = cosRaan.multiply(cosPa);
1068         final T crsp    = cosRaan.multiply(sinPa);
1069         final T srcp    = sinRaan.multiply(cosPa);
1070         final T srsp    = sinRaan.multiply(sinPa);
1071 
1072         // reference axes defining the orbital plane
1073         @SuppressWarnings("unchecked")
1074         final FieldVector3D<T>[] axes = (FieldVector3D<T>[]) Array.newInstance(FieldVector3D.class, 2);
1075         axes[0] = new FieldVector3D<>(crcp.subtract(cosI.multiply(srsp)),  srcp.add(cosI.multiply(crsp)), sinI.multiply(sinPa));
1076         axes[1] = new FieldVector3D<>(crsp.add(cosI.multiply(srcp)).negate(), cosI.multiply(crcp).subtract(srsp), sinI.multiply(cosPa));
1077 
1078         return axes;
1079 
1080     }
1081 
1082     /** Compute position and velocity but not acceleration.
1083      */
1084     private void computePVWithoutA() {
1085 
1086         if (partialPV != null) {
1087             // already computed
1088             return;
1089         }
1090 
1091         final FieldVector3D<T>[] axes = referenceAxes();
1092 
1093         if (isElliptical()) {
1094 
1095             // elliptical case
1096 
1097             // elliptic eccentric anomaly
1098             final T uME2             = e.negate().add(1).multiply(e.add(1));
1099             final T s1Me2            = uME2.sqrt();
1100             final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
1101             final T cosE             = scE.cos();
1102             final T sinE             = scE.sin();
1103 
1104             // coordinates of position and velocity in the orbital plane
1105             final T x      = a.multiply(cosE.subtract(e));
1106             final T y      = a.multiply(sinE).multiply(s1Me2);
1107             final T factor = FastMath.sqrt(getMu().divide(a)).divide(e.negate().multiply(cosE).add(1));
1108             final T xDot   = sinE.negate().multiply(factor);
1109             final T yDot   = cosE.multiply(s1Me2).multiply(factor);
1110 
1111             final FieldVector3D<T> position = new FieldVector3D<>(x, axes[0], y, axes[1]);
1112             final FieldVector3D<T> velocity = new FieldVector3D<>(xDot, axes[0], yDot, axes[1]);
1113             partialPV = new FieldPVCoordinates<>(position, velocity);
1114 
1115         } else {
1116 
1117             // hyperbolic case
1118 
1119             // compute position and velocity factors
1120             final FieldSinCos<T> scV = FastMath.sinCos(getTrueAnomaly());
1121             final T sinV             = scV.sin();
1122             final T cosV             = scV.cos();
1123             final T f                = a.multiply(e.square().negate().add(1));
1124             final T posFactor        = f.divide(e.multiply(cosV).add(1));
1125             final T velFactor        = FastMath.sqrt(getMu().divide(f));
1126 
1127             final FieldVector3D<T> position     = new FieldVector3D<>(posFactor.multiply(cosV), axes[0], posFactor.multiply(sinV), axes[1]);
1128             final FieldVector3D<T> velocity     = new FieldVector3D<>(velFactor.multiply(sinV).negate(), axes[0], velFactor.multiply(e.add(cosV)), axes[1]);
1129             partialPV = new FieldPVCoordinates<>(position, velocity);
1130 
1131         }
1132 
1133     }
1134 
1135     /** Compute non-Keplerian part of the acceleration from first time derivatives.
1136      * <p>
1137      * This method should be called only when {@link #hasDerivatives()} returns true.
1138      * </p>
1139      * @return non-Keplerian part of the acceleration
1140      */
1141     private FieldVector3D<T> nonKeplerianAcceleration() {
1142 
1143         final T[][] dCdP = MathArrays.buildArray(a.getField(), 6, 6);
1144         getJacobianWrtParameters(PositionAngleType.MEAN, dCdP);
1145 
1146         final T nonKeplerianMeanMotion = getMeanAnomalyDot().subtract(getKeplerianMeanMotion());
1147         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
1148                                  add(dCdP[3][1].multiply(eDot)).
1149                                  add(dCdP[3][2].multiply(iDot)).
1150                                  add(dCdP[3][3].multiply(paDot)).
1151                                  add(dCdP[3][4].multiply(raanDot)).
1152                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
1153         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
1154                                  add(dCdP[4][1].multiply(eDot)).
1155                                  add(dCdP[4][2].multiply(iDot)).
1156                                  add(dCdP[4][3].multiply(paDot)).
1157                                  add(dCdP[4][4].multiply(raanDot)).
1158                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
1159         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
1160                                  add(dCdP[5][1].multiply(eDot)).
1161                                  add(dCdP[5][2].multiply(iDot)).
1162                                  add(dCdP[5][3].multiply(paDot)).
1163                                  add(dCdP[5][4].multiply(raanDot)).
1164                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));
1165 
1166         return new FieldVector3D<>(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);
1167 
1168     }
1169 
1170     /** {@inheritDoc} */
1171     @Override
1172     protected FieldVector3D<T> initPosition() {
1173         final FieldVector3D<T>[] axes = referenceAxes();
1174 
1175         if (isElliptical()) {
1176 
1177             // elliptical case
1178 
1179             // elliptic eccentric anomaly
1180             final T uME2             = e.negate().add(1).multiply(e.add(1));
1181             final T s1Me2            = uME2.sqrt();
1182             final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
1183             final T cosE             = scE.cos();
1184             final T sinE             = scE.sin();
1185 
1186             return new FieldVector3D<>(a.multiply(cosE.subtract(e)), axes[0], a.multiply(sinE).multiply(s1Me2), axes[1]);
1187 
1188         } else {
1189 
1190             // hyperbolic case
1191 
1192             // compute position and velocity factors
1193             final FieldSinCos<T> scV = FastMath.sinCos(getTrueAnomaly());
1194             final T sinV             = scV.sin();
1195             final T cosV             = scV.cos();
1196             final T f                = a.multiply(e.square().negate().add(1));
1197             final T posFactor        = f.divide(e.multiply(cosV).add(1));
1198 
1199             return new FieldVector3D<>(posFactor.multiply(cosV), axes[0], posFactor.multiply(sinV), axes[1]);
1200 
1201         }
1202 
1203     }
1204 
1205     /** {@inheritDoc} */
1206     @Override
1207     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {
1208 
1209         // position and velocity
1210         computePVWithoutA();
1211 
1212         // acceleration
1213         final T r2 = partialPV.getPosition().getNormSq();
1214         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(FastMath.sqrt(r2)).reciprocal().multiply(getMu().negate()),
1215                                                                            partialPV.getPosition());
1216         final FieldVector3D<T> acceleration = hasDerivatives() ?
1217                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
1218                                               keplerianAcceleration;
1219 
1220         return new TimeStampedFieldPVCoordinates<>(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
1221 
1222     }
1223 
1224     /** {@inheritDoc} */
1225     @Override
1226     public FieldKeplerianOrbit<T> shiftedBy(final double dt) {
1227         return shiftedBy(getZero().newInstance(dt));
1228     }
1229 
1230     /** {@inheritDoc} */
1231     @Override
1232     public FieldKeplerianOrbit<T> shiftedBy(final T dt) {
1233 
1234         // use Keplerian-only motion
1235         final FieldKeplerianOrbit<T> keplerianShifted = new FieldKeplerianOrbit<>(a, e, i, pa, raan,
1236                                                                                   getKeplerianMeanMotion().multiply(dt).add(getMeanAnomaly()),
1237                                                                                   PositionAngleType.MEAN, cachedPositionAngleType, getFrame(), getDate().shiftedBy(dt), getMu());
1238 
1239         if (hasDerivatives()) {
1240 
1241             // extract non-Keplerian acceleration from first time derivatives
1242             final FieldVector3D<T> nonKeplerianAcceleration = nonKeplerianAcceleration();
1243 
1244             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
1245             keplerianShifted.computePVWithoutA();
1246             final FieldVector3D<T> fixedP   = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getPosition(),
1247                                                                   dt.square().multiply(0.5), nonKeplerianAcceleration);
1248             final T   fixedR2 = fixedP.getNormSq();
1249             final T   fixedR  = fixedR2.sqrt();
1250             final FieldVector3D<T> fixedV  = new FieldVector3D<>(getOne(), keplerianShifted.partialPV.getVelocity(),
1251                                                                  dt, nonKeplerianAcceleration);
1252             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
1253                                                                  keplerianShifted.partialPV.getPosition(),
1254                                                                  getOne(), nonKeplerianAcceleration);
1255 
1256             // build a new orbit, taking non-Keplerian acceleration into account
1257             return new FieldKeplerianOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
1258                                                                                  fixedP, fixedV, fixedA),
1259                                              keplerianShifted.getFrame(), keplerianShifted.getMu());
1260 
1261         } else {
1262             // Keplerian-only motion is all we can do
1263             return keplerianShifted;
1264         }
1265 
1266     }
1267 
1268     /** {@inheritDoc} */
1269     @Override
1270     protected T[][] computeJacobianMeanWrtCartesian() {
1271         if (isElliptical()) {
1272             return computeJacobianMeanWrtCartesianElliptical();
1273         } else {
1274             return computeJacobianMeanWrtCartesianHyperbolic();
1275         }
1276     }
1277 
1278     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
1279      * <p>
1280      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
1281      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
1282      * yDot for j=4, zDot for j=5).
1283      * </p>
1284      * @return 6x6 Jacobian matrix
1285      */
1286     private T[][] computeJacobianMeanWrtCartesianElliptical() {
1287 
1288         final T[][] jacobian = MathArrays.buildArray(getA().getField(), 6, 6);
1289 
1290         // compute various intermediate parameters
1291         computePVWithoutA();
1292         final FieldVector3D<T> position = partialPV.getPosition();
1293         final FieldVector3D<T> velocity = partialPV.getVelocity();
1294         final FieldVector3D<T> momentum = partialPV.getMomentum();
1295         final T v2         = velocity.getNormSq();
1296         final T r2         = position.getNormSq();
1297         final T r          = r2.sqrt();
1298         final T r3         = r.multiply(r2);
1299 
1300         final T px         = position.getX();
1301         final T py         = position.getY();
1302         final T pz         = position.getZ();
1303         final T vx         = velocity.getX();
1304         final T vy         = velocity.getY();
1305         final T vz         = velocity.getZ();
1306         final T mx         = momentum.getX();
1307         final T my         = momentum.getY();
1308         final T mz         = momentum.getZ();
1309 
1310         final T mu         = getMu();
1311         final T sqrtMuA    = FastMath.sqrt(a.multiply(mu));
1312         final T sqrtAoMu   = FastMath.sqrt(a.divide(mu));
1313         final T a2         = a.square();
1314         final T twoA       = a.multiply(2);
1315         final T rOnA       = r.divide(a);
1316 
1317         final T oMe2       = e.square().negate().add(1);
1318         final T epsilon    = oMe2.sqrt();
1319         final T sqrtRec    = epsilon.reciprocal();
1320 
1321         final FieldSinCos<T> scI  = FastMath.sinCos(i);
1322         final FieldSinCos<T> scPA = FastMath.sinCos(pa);
1323         final T cosI       = scI.cos();
1324         final T sinI       = scI.sin();
1325         final T cosPA      = scPA.cos();
1326         final T sinPA      = scPA.sin();
1327 
1328         final T pv         = FieldVector3D.dotProduct(position, velocity);
1329         final T cosE       = a.subtract(r).divide(a.multiply(e));
1330         final T sinE       = pv.divide(e.multiply(sqrtMuA));
1331 
1332         // da
1333         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
1334         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(mu.divide(2.).reciprocal()));
1335         fillHalfRow(getOne(), vectorAR,    jacobian[0], 0);
1336         fillHalfRow(getOne(), vectorARDot, jacobian[0], 3);
1337 
1338         // de
1339         final T factorER3 = pv.divide(twoA);
1340         final FieldVector3D<T> vectorER   = new FieldVector3D<>(cosE.multiply(v2).divide(r.multiply(mu)), position,
1341                                                                 sinE.divide(sqrtMuA), velocity,
1342                                                                 factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorAR);
1343         final FieldVector3D<T> vectorERDot = new FieldVector3D<>(sinE.divide(sqrtMuA), position,
1344                                                                  cosE.multiply(mu.divide(2.).reciprocal()).multiply(r), velocity,
1345                                                                  factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorARDot);
1346         fillHalfRow(getOne(), vectorER,    jacobian[1], 0);
1347         fillHalfRow(getOne(), vectorERDot, jacobian[1], 3);
1348 
1349         // dE / dr (Eccentric anomaly)
1350         final T coefE = cosE.divide(e.multiply(sqrtMuA));
1351         final FieldVector3D<T>  vectorEAnR =
1352             new FieldVector3D<>(sinE.negate().multiply(v2).divide(e.multiply(r).multiply(mu)), position, coefE, velocity,
1353                                 factorER3.negate().multiply(coefE), vectorAR);
1354 
1355         // dE / drDot
1356         final FieldVector3D<T>  vectorEAnRDot =
1357             new FieldVector3D<>(sinE.multiply(-2).multiply(r).divide(e.multiply(mu)), velocity, coefE, position,
1358                                 factorER3.negate().multiply(coefE), vectorARDot);
1359 
1360         // precomputing some more factors
1361         final T s1 = sinE.negate().multiply(pz).divide(r).subtract(cosE.multiply(vz).multiply(sqrtAoMu));
1362         final T s2 = cosE.negate().multiply(pz).divide(r3);
1363         final T s3 = sinE.multiply(vz).divide(sqrtMuA.multiply(-2));
1364         final T t1 = sqrtRec.multiply(cosE.multiply(pz).divide(r).subtract(sinE.multiply(vz).multiply(sqrtAoMu)));
1365         final T t2 = sqrtRec.multiply(sinE.negate().multiply(pz).divide(r3));
1366         final T t3 = sqrtRec.multiply(cosE.subtract(e)).multiply(vz).divide(sqrtMuA.multiply(2));
1367         final T t4 = sqrtRec.multiply(e.multiply(sinI).multiply(cosPA).multiply(sqrtRec).subtract(vz.multiply(sqrtAoMu)));
1368         final FieldVector3D<T> s = new FieldVector3D<>(cosE.divide(r), this.PLUS_K,
1369                                                        s1,       vectorEAnR,
1370                                                        s2,       position,
1371                                                        s3,       vectorAR);
1372         final FieldVector3D<T> sDot = new FieldVector3D<>(sinE.negate().multiply(sqrtAoMu), this.PLUS_K,
1373                                                           s1,               vectorEAnRDot,
1374                                                           s3,               vectorARDot);
1375         final FieldVector3D<T> t =
1376             new FieldVector3D<>(sqrtRec.multiply(sinE).divide(r), this.PLUS_K).add(new FieldVector3D<>(t1, vectorEAnR,
1377                                                                                                        t2, position,
1378                                                                                                        t3, vectorAR,
1379                                                                                                        t4, vectorER));
1380         final FieldVector3D<T> tDot = new FieldVector3D<>(sqrtRec.multiply(cosE.subtract(e)).multiply(sqrtAoMu), this.PLUS_K,
1381                                                           t1,                                                    vectorEAnRDot,
1382                                                           t3,                                                    vectorARDot,
1383                                                           t4,                                                    vectorERDot);
1384 
1385         // di
1386         final T factorI1 = sinI.negate().multiply(sqrtRec).divide(sqrtMuA);
1387         final T i1 =  factorI1;
1388         final T i2 =  factorI1.negate().multiply(mz).divide(twoA);
1389         final T i3 =  factorI1.multiply(mz).multiply(e).divide(oMe2);
1390         final T i4 = cosI.multiply(sinPA);
1391         final T i5 = cosI.multiply(cosPA);
1392         fillHalfRow(i1, new FieldVector3D<>(vy, vx.negate(), getZero()), i2, vectorAR, i3, vectorER, i4, s, i5, t,
1393                     jacobian[2], 0);
1394         fillHalfRow(i1, new FieldVector3D<>(py.negate(), px, getZero()), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
1395                     jacobian[2], 3);
1396 
1397         // dpa
1398         fillHalfRow(cosPA.divide(sinI), s,    sinPA.negate().divide(sinI), t,    jacobian[3], 0);
1399         fillHalfRow(cosPA.divide(sinI), sDot, sinPA.negate().divide(sinI), tDot, jacobian[3], 3);
1400 
1401         // dRaan
1402         final T factorRaanR = (a.multiply(mu).multiply(oMe2).multiply(sinI).multiply(sinI)).reciprocal();
1403         fillHalfRow( factorRaanR.negate().multiply(my), new FieldVector3D<>(getZero(), vz, vy.negate()),
1404                      factorRaanR.multiply(mx), new FieldVector3D<>(vz.negate(), getZero(),  vx),
1405                      jacobian[4], 0);
1406         fillHalfRow(factorRaanR.negate().multiply(my), new FieldVector3D<>(getZero(), pz.negate(),  py),
1407                      factorRaanR.multiply(mx), new FieldVector3D<>(pz, getZero(), px.negate()),
1408                      jacobian[4], 3);
1409 
1410         // dM
1411         fillHalfRow(rOnA, vectorEAnR,    sinE.negate(), vectorER,    jacobian[5], 0);
1412         fillHalfRow(rOnA, vectorEAnRDot, sinE.negate(), vectorERDot, jacobian[5], 3);
1413 
1414         return jacobian;
1415 
1416     }
1417 
1418     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
1419      * <p>
1420      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
1421      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
1422      * yDot for j=4, zDot for j=5).
1423      * </p>
1424      * @return 6x6 Jacobian matrix
1425      */
1426     private T[][] computeJacobianMeanWrtCartesianHyperbolic() {
1427 
1428         final T[][] jacobian = MathArrays.buildArray(getA().getField(), 6, 6);
1429 
1430         // compute various intermediate parameters
1431         computePVWithoutA();
1432         final FieldVector3D<T> position = partialPV.getPosition();
1433         final FieldVector3D<T> velocity = partialPV.getVelocity();
1434         final FieldVector3D<T> momentum = partialPV.getMomentum();
1435         final T r2         = position.getNormSq();
1436         final T r          = r2.sqrt();
1437         final T r3         = r.multiply(r2);
1438 
1439         final T x          = position.getX();
1440         final T y          = position.getY();
1441         final T z          = position.getZ();
1442         final T vx         = velocity.getX();
1443         final T vy         = velocity.getY();
1444         final T vz         = velocity.getZ();
1445         final T mx         = momentum.getX();
1446         final T my         = momentum.getY();
1447         final T mz         = momentum.getZ();
1448 
1449         final T mu         = getMu();
1450         final T absA       = a.negate();
1451         final T sqrtMuA    = absA.multiply(mu).sqrt();
1452         final T a2         = a.square();
1453         final T rOa        = r.divide(absA);
1454 
1455         final FieldSinCos<T> scI = FastMath.sinCos(i);
1456         final T cosI       = scI.cos();
1457         final T sinI       = scI.sin();
1458 
1459         final T pv         = FieldVector3D.dotProduct(position, velocity);
1460 
1461         // da
1462         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(-2).divide(r3), position);
1463         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(-2).divide(mu));
1464         fillHalfRow(getOne().negate(), vectorAR,    jacobian[0], 0);
1465         fillHalfRow(getOne().negate(), vectorARDot, jacobian[0], 3);
1466 
1467         // differentials of the momentum
1468         final T m      = momentum.getNorm();
1469         final T oOm    = m.reciprocal();
1470         final FieldVector3D<T> dcXP = new FieldVector3D<>(getZero(), vz, vy.negate());
1471         final FieldVector3D<T> dcYP = new FieldVector3D<>(vz.negate(),   getZero(),  vx);
1472         final FieldVector3D<T> dcZP = new FieldVector3D<>( vy, vx.negate(),   getZero());
1473         final FieldVector3D<T> dcXV = new FieldVector3D<>(  getZero(),  z.negate(),   y);
1474         final FieldVector3D<T> dcYV = new FieldVector3D<>(  z,   getZero(),  x.negate());
1475         final FieldVector3D<T> dcZV = new FieldVector3D<>( y.negate(),   x,   getZero());
1476         final FieldVector3D<T> dCP  = new FieldVector3D<>(mx.multiply(oOm), dcXP, my.multiply(oOm), dcYP, mz.multiply(oOm), dcZP);
1477         final FieldVector3D<T> dCV  = new FieldVector3D<>(mx.multiply(oOm), dcXV, my.multiply(oOm), dcYV, mz.multiply(oOm), dcZV);
1478 
1479         // dp
1480         final T mOMu   = m.divide(mu);
1481         final FieldVector3D<T> dpP  = new FieldVector3D<>(mOMu.multiply(2), dCP);
1482         final FieldVector3D<T> dpV  = new FieldVector3D<>(mOMu.multiply(2), dCV);
1483 
1484         // de
1485         final T p      = m.multiply(mOMu);
1486         final T moO2ae = absA.multiply(2).multiply(e).reciprocal();
1487         final T m2OaMu = p.negate().divide(absA);
1488         fillHalfRow(moO2ae, dpP, m2OaMu.multiply(moO2ae), vectorAR,    jacobian[1], 0);
1489         fillHalfRow(moO2ae, dpV, m2OaMu.multiply(moO2ae), vectorARDot, jacobian[1], 3);
1490 
1491         // di
1492         final T cI1 = m.multiply(sinI).reciprocal();
1493         final T cI2 = cosI.multiply(cI1);
1494         fillHalfRow(cI2, dCP, cI1.negate(), dcZP, jacobian[2], 0);
1495         fillHalfRow(cI2, dCV, cI1.negate(), dcZV, jacobian[2], 3);
1496 
1497 
1498         // dPA
1499         final T cP1     =  y.multiply(oOm);
1500         final T cP2     =  x.negate().multiply(oOm);
1501         final T cP3     =  mx.multiply(cP1).add(my.multiply(cP2)).negate();
1502         final T cP4     =  cP3.multiply(oOm);
1503         final T cP5     =  r2.multiply(sinI).multiply(sinI).negate().reciprocal();
1504         final T cP6     = z.multiply(cP5);
1505         final T cP7     = cP3.multiply(cP5);
1506         final FieldVector3D<T> dacP  = new FieldVector3D<>(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new FieldVector3D<>(my.negate(), mx, getZero()));
1507         final FieldVector3D<T> dacV  = new FieldVector3D<>(cP1, dcXV, cP2, dcYV, cP4, dCV);
1508         final FieldVector3D<T> dpoP  = new FieldVector3D<>(cP6, dacP, cP7, this.PLUS_K);
1509         final FieldVector3D<T> dpoV  = new FieldVector3D<>(cP6, dacV);
1510 
1511         final T re2     = r2.multiply(e.square());
1512         final T recOre2 = p.subtract(r).divide(re2);
1513         final T resOre2 = pv.multiply(mOMu).divide(re2);
1514         final FieldVector3D<T> dreP  = new FieldVector3D<>(mOMu, velocity, pv.divide(mu), dCP);
1515         final FieldVector3D<T> dreV  = new FieldVector3D<>(mOMu, position, pv.divide(mu), dCV);
1516         final FieldVector3D<T> davP  = new FieldVector3D<>(resOre2.negate(), dpP, recOre2, dreP, resOre2.divide(r), position);
1517         final FieldVector3D<T> davV  = new FieldVector3D<>(resOre2.negate(), dpV, recOre2, dreV);
1518         fillHalfRow(getOne(), dpoP, getOne().negate(), davP, jacobian[3], 0);
1519         fillHalfRow(getOne(), dpoV, getOne().negate(), davV, jacobian[3], 3);
1520 
1521         // dRAAN
1522         final T cO0 = cI1.square();
1523         final T cO1 =  mx.multiply(cO0);
1524         final T cO2 =  my.negate().multiply(cO0);
1525         fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
1526         fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);
1527 
1528         // dM
1529         final T s2a    = pv.divide(absA.multiply(2));
1530         final T oObux  = m.square().add(absA.multiply(mu)).sqrt().reciprocal();
1531         final T scasbu = pv.multiply(oObux);
1532         final FieldVector3D<T> dauP = new FieldVector3D<>(sqrtMuA.reciprocal(), velocity, s2a.negate().divide(sqrtMuA), vectorAR);
1533         final FieldVector3D<T> dauV = new FieldVector3D<>(sqrtMuA.reciprocal(), position, s2a.negate().divide(sqrtMuA), vectorARDot);
1534         final FieldVector3D<T> dbuP = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorAR,    m.multiply(oObux), dCP);
1535         final FieldVector3D<T> dbuV = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorARDot, m.multiply(oObux), dCV);
1536         final FieldVector3D<T> dcuP = new FieldVector3D<>(oObux, velocity, scasbu.negate().multiply(oObux), dbuP);
1537         final FieldVector3D<T> dcuV = new FieldVector3D<>(oObux, position, scasbu.negate().multiply(oObux), dbuV);
1538         fillHalfRow(getOne(), dauP, e.negate().divide(rOa.add(1)), dcuP, jacobian[5], 0);
1539         fillHalfRow(getOne(), dauV, e.negate().divide(rOa.add(1)), dcuV, jacobian[5], 3);
1540 
1541         return jacobian;
1542 
1543     }
1544 
1545     /** {@inheritDoc} */
1546     @Override
1547     protected T[][] computeJacobianEccentricWrtCartesian() {
1548         if (isElliptical()) {
1549             return computeJacobianEccentricWrtCartesianElliptical();
1550         } else {
1551             return computeJacobianEccentricWrtCartesianHyperbolic();
1552         }
1553     }
1554 
1555     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
1556      * <p>
1557      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
1558      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
1559      * yDot for j=4, zDot for j=5).
1560      * </p>
1561      * @return 6x6 Jacobian matrix
1562      */
1563     private T[][] computeJacobianEccentricWrtCartesianElliptical() {
1564 
1565         // start by computing the Jacobian with mean angle
1566         final T[][] jacobian = computeJacobianMeanWrtCartesianElliptical();
1567 
1568         // Differentiating the Kepler equation M = E - e sin E leads to:
1569         // dM = (1 - e cos E) dE - sin E de
1570         // which is inverted and rewritten as:
1571         // dE = a/r dM + sin E a/r de
1572         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
1573         final T aOr              = e.negate().multiply(scE.cos()).add(1).reciprocal();
1574 
1575         // update anomaly row
1576         final T[] eRow           = jacobian[1];
1577         final T[] anomalyRow     = jacobian[5];
1578         for (int j = 0; j < anomalyRow.length; ++j) {
1579             anomalyRow[j] = aOr.multiply(anomalyRow[j].add(scE.sin().multiply(eRow[j])));
1580         }
1581 
1582         return jacobian;
1583 
1584     }
1585 
1586     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
1587      * <p>
1588      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
1589      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
1590      * yDot for j=4, zDot for j=5).
1591      * </p>
1592      * @return 6x6 Jacobian matrix
1593      */
1594     private T[][] computeJacobianEccentricWrtCartesianHyperbolic() {
1595 
1596         // start by computing the Jacobian with mean angle
1597         final T[][] jacobian = computeJacobianMeanWrtCartesianHyperbolic();
1598 
1599         // Differentiating the Kepler equation M = e sinh H - H leads to:
1600         // dM = (e cosh H - 1) dH + sinh H de
1601         // which is inverted and rewritten as:
1602         // dH = 1 / (e cosh H - 1) dM - sinh H / (e cosh H - 1) de
1603         final T H      = getEccentricAnomaly();
1604         final T coshH  = H.cosh();
1605         final T sinhH  = H.sinh();
1606         final T absaOr = e.multiply(coshH).subtract(1).reciprocal();
1607         // update anomaly row
1608         final T[] eRow       = jacobian[1];
1609         final T[] anomalyRow = jacobian[5];
1610 
1611         for (int j = 0; j < anomalyRow.length; ++j) {
1612             anomalyRow[j] = absaOr.multiply(anomalyRow[j].subtract(sinhH.multiply(eRow[j])));
1613 
1614         }
1615 
1616         return jacobian;
1617 
1618     }
1619 
1620     /** {@inheritDoc} */
1621     @Override
1622     protected T[][] computeJacobianTrueWrtCartesian() {
1623         if (isElliptical()) {
1624             return computeJacobianTrueWrtCartesianElliptical();
1625         } else {
1626             return computeJacobianTrueWrtCartesianHyperbolic();
1627         }
1628     }
1629 
1630     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
1631      * <p>
1632      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
1633      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
1634      * yDot for j=4, zDot for j=5).
1635      * </p>
1636      * @return 6x6 Jacobian matrix
1637      */
1638     private T[][] computeJacobianTrueWrtCartesianElliptical() {
1639 
1640         // start by computing the Jacobian with eccentric angle
1641         final T[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();
1642         // Differentiating the eccentric anomaly equation sin E = sqrt(1-e^2) sin v / (1 + e cos v)
1643         // and using cos E = (e + cos v) / (1 + e cos v) to get rid of cos E leads to:
1644         // dE = [sqrt (1 - e^2) / (1 + e cos v)] dv - [sin E / (1 - e^2)] de
1645         // which is inverted and rewritten as:
1646         // dv = sqrt (1 - e^2) a/r dE + [sin E / sqrt (1 - e^2)] a/r de
1647         final T e2               = e.square();
1648         final T oMe2             = e2.negate().add(1);
1649         final T epsilon          = oMe2.sqrt();
1650         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
1651         final T aOr              = e.multiply(scE.cos()).negate().add(1).reciprocal();
1652         final T aFactor          = epsilon.multiply(aOr);
1653         final T eFactor          = scE.sin().multiply(aOr).divide(epsilon);
1654 
1655         // update anomaly row
1656         final T[] eRow           = jacobian[1];
1657         final T[] anomalyRow     = jacobian[5];
1658         for (int j = 0; j < anomalyRow.length; ++j) {
1659             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).add(eFactor.multiply(eRow[j]));
1660         }
1661         return jacobian;
1662 
1663     }
1664 
1665     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
1666      * <p>
1667      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
1668      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
1669      * yDot for j=4, zDot for j=5).
1670      * </p>
1671      * @return 6x6 Jacobian matrix
1672      */
1673     private T[][] computeJacobianTrueWrtCartesianHyperbolic() {
1674 
1675         // start by computing the Jacobian with eccentric angle
1676         final T[][] jacobian = computeJacobianEccentricWrtCartesianHyperbolic();
1677 
1678         // Differentiating the eccentric anomaly equation sinh H = sqrt(e^2-1) sin v / (1 + e cos v)
1679         // and using cosh H = (e + cos v) / (1 + e cos v) to get rid of cosh H leads to:
1680         // dH = [sqrt (e^2 - 1) / (1 + e cos v)] dv + [sinh H / (e^2 - 1)] de
1681         // which is inverted and rewritten as:
1682         // dv = sqrt (1 - e^2) a/r dH - [sinh H / sqrt (e^2 - 1)] a/r de
1683         final T e2       = e.square();
1684         final T e2Mo     = e2.subtract(1);
1685         final T epsilon  = e2Mo.sqrt();
1686         final T H        = getEccentricAnomaly();
1687         final T coshH    = H.cosh();
1688         final T sinhH    = H.sinh();
1689         final T aOr      = e.multiply(coshH).subtract(1).reciprocal();
1690         final T aFactor  = epsilon.multiply(aOr);
1691         final T eFactor  = sinhH.multiply(aOr).divide(epsilon);
1692 
1693         // update anomaly row
1694         final T[] eRow           = jacobian[1];
1695         final T[] anomalyRow     = jacobian[5];
1696         for (int j = 0; j < anomalyRow.length; ++j) {
1697             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).subtract(eFactor.multiply(eRow[j]));
1698         }
1699 
1700         return jacobian;
1701 
1702     }
1703 
1704     /** {@inheritDoc} */
1705     @Override
1706     public void addKeplerContribution(final PositionAngleType type, final T gm,
1707                                       final T[] pDot) {
1708         final T oMe2;
1709         final T ksi;
1710         final T absA = a.abs();
1711         final T n    = absA.reciprocal().multiply(gm).sqrt().divide(absA);
1712         switch (type) {
1713             case MEAN :
1714                 pDot[5] = pDot[5].add(n);
1715                 break;
1716             case ECCENTRIC :
1717                 oMe2 = e.square().negate().add(1).abs();
1718                 ksi  = e.multiply(getTrueAnomaly().cos()).add(1);
1719                 pDot[5] = pDot[5].add( n.multiply(ksi).divide(oMe2));
1720                 break;
1721             case TRUE :
1722                 oMe2 = e.square().negate().add(1).abs();
1723                 ksi  = e.multiply(getTrueAnomaly().cos()).add(1);
1724                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
1725                 break;
1726             default :
1727                 throw new OrekitInternalError(null);
1728         }
1729     }
1730 
1731     /**  Returns a string representation of this Keplerian parameters object.
1732      * @return a string representation of this object
1733      */
1734     public String toString() {
1735         return new StringBuilder().append("Keplerian parameters: ").append('{').
1736                                   append("a: ").append(a.getReal()).
1737                                   append("; e: ").append(e.getReal()).
1738                                   append("; i: ").append(FastMath.toDegrees(i.getReal())).
1739                                   append("; pa: ").append(FastMath.toDegrees(pa.getReal())).
1740                                   append("; raan: ").append(FastMath.toDegrees(raan.getReal())).
1741                                   append("; v: ").append(FastMath.toDegrees(getTrueAnomaly().getReal())).
1742                                   append(";}").toString();
1743     }
1744 
1745     /** {@inheritDoc} */
1746     @Override
1747     public PositionAngleType getCachedPositionAngleType() {
1748         return cachedPositionAngleType;
1749     }
1750 
1751     /** {@inheritDoc} */
1752     @Override
1753     public boolean hasRates() {
1754         return hasDerivatives();
1755     }
1756 
1757     /** {@inheritDoc} */
1758     @Override
1759     public FieldKeplerianOrbit<T> removeRates() {
1760         return new FieldKeplerianOrbit<>(getA(), getE(), getI(), getPerigeeArgument(), getRightAscensionOfAscendingNode(),
1761                 cachedAnomaly, cachedPositionAngleType, getFrame(), getDate(), getMu());
1762     }
1763 
1764     /** Check if the given parameter is within an acceptable range.
1765      * The bounds are inclusive: an exception is raised when either of those conditions are met:
1766      * <ul>
1767      *     <li>The parameter is strictly greater than upperBound</li>
1768      *     <li>The parameter is strictly lower than lowerBound</li>
1769      * </ul>
1770      * <p>
1771      * In either of these cases, an OrekitException is raised.
1772      * </p>
1773      * @param parameterName name of the parameter
1774      * @param parameter value of the parameter
1775      * @param lowerBound lower bound of the acceptable range (inclusive)
1776      * @param upperBound upper bound of the acceptable range (inclusive)
1777      */
1778     private void checkParameterRangeInclusive(final String parameterName, final double parameter,
1779                                               final double lowerBound, final double upperBound) {
1780         if (parameter < lowerBound || parameter > upperBound) {
1781             throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, parameterName,
1782                                       parameter, lowerBound, upperBound);
1783         }
1784     }
1785 
1786     /** {@inheritDoc} */
1787     @Override
1788     public KeplerianOrbit toOrbit() {
1789         final double cachedPositionAngle = cachedAnomaly.getReal();
1790         if (hasDerivatives()) {
1791             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
1792                                       pa.getReal(), raan.getReal(), cachedPositionAngle,
1793                                       aDot.getReal(), eDot.getReal(), iDot.getReal(),
1794                                       paDot.getReal(), raanDot.getReal(),
1795                                       cachedAnomalyDot.getReal(),
1796                                       cachedPositionAngleType, cachedPositionAngleType,
1797                                       getFrame(), getDate().toAbsoluteDate(), getMu().getReal());
1798         } else {
1799             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
1800                                       pa.getReal(), raan.getReal(), cachedPositionAngle,
1801                                       cachedPositionAngleType, cachedPositionAngleType,
1802                                       getFrame(), getDate().toAbsoluteDate(), getMu().getReal());
1803         }
1804     }
1805 
1806 
1807 }