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