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