1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.util.FastMath;
24 import org.hipparchus.util.FieldSinCos;
25 import org.hipparchus.util.MathArrays;
26 import org.orekit.errors.OrekitIllegalArgumentException;
27 import org.orekit.errors.OrekitInternalError;
28 import org.orekit.errors.OrekitMessages;
29 import org.orekit.frames.FieldKinematicTransform;
30 import org.orekit.frames.Frame;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.time.TimeOffset;
33 import org.orekit.utils.FieldPVCoordinates;
34 import org.orekit.utils.TimeStampedFieldPVCoordinates;
35
36
37
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 public class FieldEquinoctialOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T>
78 implements PositionAngleBased<FieldEquinoctialOrbit<T>> {
79
80
81 private final T a;
82
83
84 private final T ex;
85
86
87 private final T ey;
88
89
90 private final T hx;
91
92
93 private final T hy;
94
95
96 private final T cachedL;
97
98
99 private final PositionAngleType cachedPositionAngleType;
100
101
102 private final T aDot;
103
104
105 private final T exDot;
106
107
108 private final T eyDot;
109
110
111 private final T hxDot;
112
113
114 private final T hyDot;
115
116
117 private final T cachedLDot;
118
119
120 private FieldPVCoordinates<T> partialPV;
121
122
123
124
125
126
127
128
129
130
131
132 public FieldEquinoctialOrbit(final FieldEquinoctialParameters<T> parameters,
133 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
134 throws IllegalArgumentException {
135 this(parameters.a(), parameters.ex(), parameters.ey(), parameters.hx(), parameters.hy(), parameters.longitudeArgument(),
136 parameters.positionAngleType(), frame, date, mu);
137 }
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156 public FieldEquinoctialOrbit(final T a, final T ex, final T ey,
157 final T hx, final T hy, final T l,
158 final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
159 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
160 throws IllegalArgumentException {
161 this(new FieldEquinoctialParameters<>(a, ex, ey, hx, hy, l, type).withPositionAngleType(cachedPositionAngleType),
162 frame, date, mu);
163 }
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180 public FieldEquinoctialOrbit(final T a, final T ex, final T ey,
181 final T hx, final T hy, final T l,
182 final PositionAngleType type,
183 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
184 throws IllegalArgumentException {
185 this(a, ex, ey, hx, hy, l,
186 a.getField().getZero(), a.getField().getZero(), a.getField().getZero(), a.getField().getZero(), a.getField().getZero(),
187 computeKeplerianLDot(type, a, ex, ey, mu, l, type), type, type, frame, date, mu);
188 }
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213 public FieldEquinoctialOrbit(final T a, final T ex, final T ey,
214 final T hx, final T hy, final T l,
215 final T aDot, final T exDot, final T eyDot,
216 final T hxDot, final T hyDot, final T lDot,
217 final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
218 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
219 throws IllegalArgumentException {
220 super(frame, date, mu);
221
222 if (ex.getReal() * ex.getReal() + ey.getReal() * ey.getReal() >= 1.0) {
223 throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
224 getClass().getName());
225 }
226 this.cachedPositionAngleType = cachedPositionAngleType;
227 this.a = a;
228 this.aDot = aDot;
229 this.ex = ex;
230 this.exDot = exDot;
231 this.ey = ey;
232 this.eyDot = eyDot;
233 this.hx = hx;
234 this.hxDot = hxDot;
235 this.hy = hy;
236 this.hyDot = hyDot;
237
238 final FieldUnivariateDerivative1<T> lUD = initializeCachedL(l, lDot, type);
239 this.cachedL = lUD.getValue();
240 this.cachedLDot = lUD.getFirstDerivative();
241
242 this.partialPV = null;
243
244 }
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268 public FieldEquinoctialOrbit(final T a, final T ex, final T ey,
269 final T hx, final T hy, final T l,
270 final T aDot, final T exDot, final T eyDot,
271 final T hxDot, final T hyDot, final T lDot,
272 final PositionAngleType type,
273 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
274 throws IllegalArgumentException {
275 this(a, ex, ey, hx, hy, l, aDot, exDot, eyDot, hxDot, hyDot, lDot, type, type, frame, date, mu);
276 }
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292 public FieldEquinoctialOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
293 final Frame frame, final T mu)
294 throws IllegalArgumentException {
295 super(pvCoordinates, frame, mu);
296
297
298 final FieldEquinoctialParametersConverter<T> converter = new FieldEquinoctialParametersConverter<>(mu);
299 cachedPositionAngleType = PositionAngleType.TRUE;
300 final FieldEquinoctialParameters<T> equinoctialParameters = converter.toParameters(pvCoordinates, cachedPositionAngleType);
301 a = equinoctialParameters.a();
302 ex = equinoctialParameters.ex();
303 ey = equinoctialParameters.ey();
304 hx = equinoctialParameters.hx();
305 hy = equinoctialParameters.hy();
306 cachedL = equinoctialParameters.longitudeArgument();
307
308 partialPV = pvCoordinates;
309
310 if (hasNonKeplerianAcceleration(pvCoordinates, mu)) {
311
312
313 final T[][] jacobian = MathArrays.buildArray(a.getField(), 6, 6);
314 getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
315
316 final FieldVector3D<T> pvP = pvCoordinates.getPosition();
317 final FieldVector3D<T> pvA = pvCoordinates.getAcceleration();
318 final T r2 = pvP.getNorm2Sq();
319 final T r = r2.sqrt();
320 final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
321 final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
322 final T aX = nonKeplerianAcceleration.getX();
323 final T aY = nonKeplerianAcceleration.getY();
324 final T aZ = nonKeplerianAcceleration.getZ();
325 aDot = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
326 exDot = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
327 eyDot = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
328 hxDot = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
329 hyDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));
330
331
332
333 final T lMDot = getKeplerianMeanMotion().
334 add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
335 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
336 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
337 final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(getLM(), lMDot);
338 final FieldUnivariateDerivative1<T> lvUD = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lMUD);
339 cachedLDot = lvUD.getFirstDerivative();
340
341 } else {
342
343
344
345 aDot = getZero();
346 exDot = getZero();
347 eyDot = getZero();
348 hxDot = getZero();
349 hyDot = getZero();
350 cachedLDot = computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, mu, cachedL, cachedPositionAngleType);
351 }
352
353 }
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370 public FieldEquinoctialOrbit(final FieldPVCoordinates<T> pvCoordinates, final Frame frame,
371 final FieldAbsoluteDate<T> date, final T mu)
372 throws IllegalArgumentException {
373 this(new TimeStampedFieldPVCoordinates<>(date, pvCoordinates), frame, mu);
374 }
375
376
377
378
379 public FieldEquinoctialOrbit(final FieldOrbit<T> op) {
380 super(op.getFrame(), op.getDate(), op.getMu());
381
382 a = op.getA();
383 ex = op.getEquinoctialEx();
384 ey = op.getEquinoctialEy();
385 hx = op.getHx();
386 hy = op.getHy();
387 cachedPositionAngleType = PositionAngleType.TRUE;
388 cachedL = op.getLv();
389
390 aDot = op.getADot();
391 exDot = op.getEquinoctialExDot();
392 eyDot = op.getEquinoctialEyDot();
393 hxDot = op.getHxDot();
394 hyDot = op.getHyDot();
395 cachedLDot = op.getLvDot();
396 }
397
398
399
400
401
402
403
404 public FieldEquinoctialOrbit(final Field<T> field, final EquinoctialOrbit op) {
405 super(op.getFrame(), new FieldAbsoluteDate<>(field, op.getDate()), field.getZero().newInstance(op.getMu()));
406
407 a = getZero().newInstance(op.getA());
408 ex = getZero().newInstance(op.getEquinoctialEx());
409 ey = getZero().newInstance(op.getEquinoctialEy());
410 hx = getZero().newInstance(op.getHx());
411 hy = getZero().newInstance(op.getHy());
412 cachedPositionAngleType = op.getCachedPositionAngleType();
413 cachedL = getZero().newInstance(op.getL(cachedPositionAngleType));
414
415 aDot = getZero().newInstance(op.getADot());
416 exDot = getZero().newInstance(op.getEquinoctialExDot());
417 eyDot = getZero().newInstance(op.getEquinoctialEyDot());
418 hxDot = getZero().newInstance(op.getHxDot());
419 hyDot = getZero().newInstance(op.getHyDot());
420 cachedLDot = getZero().newInstance(op.getLDot(cachedPositionAngleType));
421 }
422
423
424
425
426
427
428
429 public FieldEquinoctialOrbit(final Field<T> field, final Orbit op) {
430 this(field, (EquinoctialOrbit) OrbitParamsType.EQUINOCTIAL.convertType(op));
431 }
432
433
434
435
436
437
438 public FieldEquinoctialParameters<T> getEquinoctialParameters() {
439 return new FieldEquinoctialParameters<>(a, ex, ey, hx, hy, cachedL, cachedPositionAngleType);
440 }
441
442
443 @Override
444 public OrbitParamsType getType() {
445 return OrbitParamsType.EQUINOCTIAL;
446 }
447
448
449 @Override
450 public T getA() {
451 return a;
452 }
453
454
455 @Override
456 public T getADot() {
457 return aDot;
458 }
459
460
461 @Override
462 public T getEquinoctialEx() {
463 return ex;
464 }
465
466
467 @Override
468 public T getEquinoctialExDot() {
469 return exDot;
470 }
471
472
473 @Override
474 public T getEquinoctialEy() {
475 return ey;
476 }
477
478
479 @Override
480 public T getEquinoctialEyDot() {
481 return eyDot;
482 }
483
484
485 @Override
486 public T getHx() {
487 return hx;
488 }
489
490
491 @Override
492 public T getHxDot() {
493 return hxDot;
494 }
495
496
497 @Override
498 public T getHy() {
499 return hy;
500 }
501
502
503 @Override
504 public T getHyDot() {
505 return hyDot;
506 }
507
508
509 @Override
510 public T getLv() {
511 return getL(PositionAngleType.TRUE);
512 }
513
514
515 @Override
516 public T getLvDot() {
517 switch (cachedPositionAngleType) {
518 case ECCENTRIC:
519 final FieldUnivariateDerivative1<T> lEUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
520 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
521 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
522 final FieldUnivariateDerivative1<T> lvUD = FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
523 lEUD);
524 return lvUD.getFirstDerivative();
525
526 case TRUE:
527 return cachedLDot;
528
529 case MEAN:
530 final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
531 final FieldUnivariateDerivative1<T> exUD2 = new FieldUnivariateDerivative1<>(ex, exDot);
532 final FieldUnivariateDerivative1<T> eyUD2 = new FieldUnivariateDerivative1<>(ey, eyDot);
533 final FieldUnivariateDerivative1<T> lvUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD2,
534 eyUD2, lMUD);
535 return lvUD2.getFirstDerivative();
536
537 default:
538 throw new OrekitInternalError(null);
539 }
540 }
541
542
543 @Override
544 public T getLE() {
545 return getL(PositionAngleType.ECCENTRIC);
546 }
547
548
549 @Override
550 public T getLEDot() {
551
552 switch (cachedPositionAngleType) {
553 case TRUE:
554 final FieldUnivariateDerivative1<T> lvUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
555 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
556 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
557 final FieldUnivariateDerivative1<T> lEUD = FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD,
558 lvUD);
559 return lEUD.getFirstDerivative();
560
561 case ECCENTRIC:
562 return cachedLDot;
563
564 case MEAN:
565 final FieldUnivariateDerivative1<T> lMUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
566 final FieldUnivariateDerivative1<T> exUD2 = new FieldUnivariateDerivative1<>(ex, exDot);
567 final FieldUnivariateDerivative1<T> eyUD2 = new FieldUnivariateDerivative1<>(ey, eyDot);
568 final FieldUnivariateDerivative1<T> lEUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD2,
569 eyUD2, lMUD);
570 return lEUD2.getFirstDerivative();
571
572 default:
573 throw new OrekitInternalError(null);
574 }
575 }
576
577
578 @Override
579 public T getLM() {
580 return getL(PositionAngleType.MEAN);
581 }
582
583
584 @Override
585 public T getLMDot() {
586
587 switch (cachedPositionAngleType) {
588 case TRUE:
589 final FieldUnivariateDerivative1<T> lvUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
590 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
591 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
592 final FieldUnivariateDerivative1<T> lMUD = FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lvUD);
593 return lMUD.getFirstDerivative();
594
595 case MEAN:
596 return cachedLDot;
597
598 case ECCENTRIC:
599 final FieldUnivariateDerivative1<T> lEUD = new FieldUnivariateDerivative1<>(cachedL, cachedLDot);
600 final FieldUnivariateDerivative1<T> exUD2 = new FieldUnivariateDerivative1<>(ex, exDot);
601 final FieldUnivariateDerivative1<T> eyUD2 = new FieldUnivariateDerivative1<>(ey, eyDot);
602 final FieldUnivariateDerivative1<T> lMUD2 = FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD2,
603 eyUD2, lEUD);
604 return lMUD2.getFirstDerivative();
605
606 default:
607 throw new OrekitInternalError(null);
608 }
609 }
610
611
612
613
614
615 public T getL(final PositionAngleType type) {
616 return getEquinoctialParameters().withPositionAngleType(type).longitudeArgument();
617 }
618
619
620
621
622
623 public T getLDot(final PositionAngleType type) {
624 return switch (type) {
625 case TRUE -> getLvDot();
626 case MEAN -> getLMDot();
627 case ECCENTRIC -> getLEDot();
628 };
629 }
630
631
632 @Override
633 public boolean hasNonKeplerianAcceleration() {
634 return aDot.getReal() != 0. || exDot.getReal() != 0 || hxDot.getReal() != 0. || eyDot.getReal() != 0. || hyDot.getReal() != 0. ||
635 FastMath.abs(cachedLDot.subtract(computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, getMu(), cachedL, cachedPositionAngleType)).getReal()) > TOLERANCE_POSITION_ANGLE_RATE;
636 }
637
638
639 @Override
640 public T getE() {
641 return ex.square().add(ey.square()).sqrt();
642 }
643
644
645 @Override
646 public T getEDot() {
647 if (!hasNonKeplerianRates()) {
648 return getZero();
649 }
650 return ex.multiply(exDot).add(ey.multiply(eyDot)).divide(ex.square().add(ey.square()).sqrt());
651
652 }
653
654
655 @Override
656 public T getI() {
657 return hx.square().add(hy.square()).sqrt().atan().multiply(2);
658 }
659
660
661 @Override
662 public T getIDot() {
663 if (!hasNonKeplerianRates()) {
664 return getZero();
665 }
666 final T h2 = hx.square().add(hy.square());
667 final T h = h2.sqrt();
668 return hx.multiply(hxDot).add(hy.multiply(hyDot)).multiply(2).divide(h.multiply(h2.add(1)));
669
670 }
671
672
673
674 private void computePVWithoutA() {
675
676 if (partialPV != null) {
677
678 return;
679 }
680
681 final FieldEquinoctialParametersConverter<T> converter = new FieldEquinoctialParametersConverter<>(getMu());
682 partialPV = converter.toCartesian(getEquinoctialParameters());
683
684 }
685
686
687
688
689
690
691
692
693 private FieldUnivariateDerivative1<T> initializeCachedL(final T l, final T lDot,
694 final PositionAngleType inputType) {
695 if (cachedPositionAngleType == inputType) {
696 return new FieldUnivariateDerivative1<>(l, lDot);
697
698 } else {
699 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
700 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
701 final FieldUnivariateDerivative1<T> lUD = new FieldUnivariateDerivative1<>(l, lDot);
702
703 switch (cachedPositionAngleType) {
704
705 case ECCENTRIC:
706 if (inputType == PositionAngleType.MEAN) {
707 return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD, eyUD, lUD);
708 } else {
709 return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD, lUD);
710 }
711
712 case TRUE:
713 if (inputType == PositionAngleType.MEAN) {
714 return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lUD);
715 } else {
716 return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD, lUD);
717 }
718
719 case MEAN:
720 if (inputType == PositionAngleType.TRUE) {
721 return FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lUD);
722 } else {
723 return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD, eyUD, lUD);
724 }
725
726 default:
727 throw new OrekitInternalError(null);
728
729 }
730
731 }
732
733 }
734
735
736 @Override
737 protected FieldVector3D<T> initPosition() {
738
739
740 final T lE = getLE();
741
742
743 final T hx2 = hx.square();
744 final T hy2 = hy.square();
745 final T factH = getOne().divide(hx2.add(1.0).add(hy2));
746
747
748 final T ux = hx2.add(1.0).subtract(hy2).multiply(factH);
749 final T uy = hx.multiply(hy).multiply(factH).multiply(2);
750 final T uz = hy.multiply(-2).multiply(factH);
751
752 final T vx = uy;
753 final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
754 final T vz = hx.multiply(factH).multiply(2);
755
756
757 final T ex2 = ex.square();
758 final T exey = ex.multiply(ey);
759 final T ey2 = ey.square();
760 final T e2 = ex2.add(ey2);
761 final T eta = getOne().subtract(e2).sqrt().add(1);
762 final T beta = getOne().divide(eta);
763
764
765 final FieldSinCos<T> scLe = FastMath.sinCos(lE);
766 final T cLe = scLe.cos();
767 final T sLe = scLe.sin();
768
769
770 final T x = a.multiply(getOne().subtract(beta.multiply(ey2)).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(ex));
771 final T y = a.multiply(getOne().subtract(beta.multiply(ex2)).multiply(sLe).add(beta .multiply(exey).multiply(cLe)).subtract(ey));
772
773 return new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
774 x.multiply(uy).add(y.multiply(vy)),
775 x.multiply(uz).add(y.multiply(vz)));
776
777 }
778
779
780 @Override
781 protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {
782
783
784 computePVWithoutA();
785
786
787 final T r2 = partialPV.getPosition().getNorm2Sq();
788 final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(getMu().negate()),
789 partialPV.getPosition());
790 final FieldVector3D<T> acceleration = hasNonKeplerianRates() ?
791 keplerianAcceleration.add(nonKeplerianAcceleration()) :
792 keplerianAcceleration;
793
794 return new TimeStampedFieldPVCoordinates<>(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
795
796 }
797
798
799 @Override
800 public FieldEquinoctialOrbit<T> inFrame(final Frame inertialFrame) {
801 final FieldPVCoordinates<T> fieldPVCoordinates;
802 if (hasNonKeplerianAcceleration()) {
803 fieldPVCoordinates = getPVCoordinates(inertialFrame);
804 } else {
805 final FieldKinematicTransform<T> transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
806 fieldPVCoordinates = transform.transformOnlyPV(getPVCoordinates());
807 }
808 final FieldEquinoctialOrbit<T> fieldOrbit = new FieldEquinoctialOrbit<>(fieldPVCoordinates, inertialFrame, getDate(), getMu());
809 if (fieldOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
810 return fieldOrbit;
811 } else {
812 return fieldOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
813 }
814 }
815
816
817 @Override
818 public FieldEquinoctialOrbit<T> withCachedPositionAngleType(final PositionAngleType positionAngleType) {
819 return new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy, getL(positionAngleType), aDot, exDot, eyDot, hxDot, hyDot,
820 getLDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
821 }
822
823
824 @Override
825 public FieldEquinoctialOrbit<T> shiftedBy(final double dt) {
826 return shiftedBy(getZero().newInstance(dt));
827 }
828
829
830 @Override
831 public FieldEquinoctialOrbit<T> shiftedBy(final T dt) {
832
833
834 final FieldEquinoctialOrbit<T> keplerianShifted = keplerianShiftedBy(dt);
835
836
837 if (!dt.isZero() && hasNonKeplerianRates()) {
838 return new FieldEquinoctialOrbit<>(shiftPVNonKeplerian(keplerianShifted.getPVCoordinates(), dt),
839 getFrame(), getDate().shiftedBy(dt), getMu());
840 }
841
842 else {
843 return keplerianShifted;
844 }
845
846 }
847
848
849
850
851
852
853 @Override
854 public FieldEquinoctialOrbit<T> shiftedBy(final TimeOffset dt) {
855
856
857 final Field<T> field = getField();
858 final T dtValue = field.getOne().newInstance(dt.toDouble());
859
860
861 final FieldEquinoctialOrbit<T> keplerianShifted = keplerianShiftedBy(dt);
862
863
864 if (!dtValue.isZero() && hasNonKeplerianRates()) {
865 return new FieldEquinoctialOrbit<>(shiftPVNonKeplerian(keplerianShifted.getPVCoordinates(), dtValue),
866 getFrame(), getDate().shiftedBy(dt), getMu());
867 }
868
869 else {
870 return keplerianShifted;
871 }
872
873 }
874
875
876
877
878
879
880
881 protected FieldEquinoctialOrbit<T> keplerianShiftedBy(final T dt) {
882 return new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy,
883 getLM().add(getKeplerianMeanMotion().multiply(dt)),
884 PositionAngleType.MEAN,
885 cachedPositionAngleType,
886 getFrame(),
887 getDate().shiftedBy(dt),
888 getMu());
889
890 }
891
892
893
894
895
896
897
898
899 private FieldEquinoctialOrbit<T> keplerianShiftedBy(final TimeOffset dt) {
900 return new FieldEquinoctialOrbit<>(a, ex, ey, hx, hy,
901 getLM().add(getKeplerianMeanMotion().multiply(dt.toDouble())),
902 PositionAngleType.MEAN,
903 cachedPositionAngleType,
904 getFrame(),
905 getDate().shiftedBy(dt),
906 getMu());
907
908 }
909
910
911 @Override
912 protected T[][] computeJacobianMeanWrtCartesian() {
913
914 final T[][] jacobian = MathArrays.buildArray(getField(), 6, 6);
915
916
917 computePVWithoutA();
918 final FieldVector3D<T> position = partialPV.getPosition();
919 final FieldVector3D<T> velocity = partialPV.getVelocity();
920 final T r2 = position.getNorm2Sq();
921 final T r = r2.sqrt();
922 final T r3 = r.multiply(r2);
923
924 final T mu = getMu();
925 final T sqrtMuA = a.multiply(mu).sqrt();
926 final T a2 = a.square();
927
928 final T e2 = ex.square().add(ey.square());
929 final T oMe2 = getOne().subtract(e2);
930 final T epsilon = oMe2.sqrt();
931 final T beta = getOne().divide(epsilon.add(1));
932 final T ratio = epsilon.multiply(beta);
933
934 final T hx2 = hx.square();
935 final T hy2 = hy.square();
936 final T hxhy = hx.multiply(hy);
937
938
939 final FieldVector3D<T> f = new FieldVector3D<>(hx2.subtract(hy2).add(1), hxhy.multiply(2), hy.multiply(-2)).normalize();
940 final FieldVector3D<T> g = new FieldVector3D<>(hxhy.multiply(2), hy2.add(1).subtract(hx2), hx.multiply(2)).normalize();
941 final FieldVector3D<T> w = FieldVector3D.crossProduct(position, velocity).normalize();
942
943
944 final T x = FieldVector3D.dotProduct(position, f);
945 final T y = FieldVector3D.dotProduct(position, g);
946 final T xDot = FieldVector3D.dotProduct(velocity, f);
947 final T yDot = FieldVector3D.dotProduct(velocity, g);
948
949
950 final T c1 = a.divide(sqrtMuA.multiply(epsilon));
951 final T c1N = c1.negate();
952 final T c2 = a.multiply(sqrtMuA).multiply(beta).divide(r3);
953 final T c3 = sqrtMuA.divide(r3.multiply(epsilon));
954 final FieldVector3D<T> drDotSdEx = new FieldVector3D<>(c1.multiply(xDot).multiply(yDot).subtract(c2.multiply(ey).multiply(x)).subtract(c3.multiply(x).multiply(y)), f,
955 c1N.multiply(xDot).multiply(xDot).subtract(c2.multiply(ey).multiply(y)).add(c3.multiply(x).multiply(x)), g);
956
957
958 final FieldVector3D<T> drDotSdEy = new FieldVector3D<>(c1.multiply(yDot).multiply(yDot).add(c2.multiply(ex).multiply(x)).subtract(c3.multiply(y).multiply(y)), f,
959 c1N.multiply(xDot).multiply(yDot).add(c2.multiply(ex).multiply(y)).add(c3.multiply(x).multiply(y)), g);
960
961
962 final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
963 final FieldVector3D<T> vectorARDot = new FieldVector3D<>(a2.multiply(2).divide(mu), velocity);
964 fillHalfRow(getOne(), vectorAR, jacobian[0], 0);
965 fillHalfRow(getOne(), vectorARDot, jacobian[0], 3);
966
967
968 final T d1 = a.negate().multiply(ratio).divide(r3);
969 final T d2 = (hy.multiply(xDot).subtract(hx.multiply(yDot))).divide(sqrtMuA.multiply(epsilon));
970 final T d3 = hx.multiply(y).subtract(hy.multiply(x)).divide(sqrtMuA);
971 final FieldVector3D<T> vectorExRDot =
972 new FieldVector3D<>(x.multiply(2).multiply(yDot).subtract(xDot.multiply(y)).divide(mu), g, y.negate().multiply(yDot).divide(mu), f, ey.negate().multiply(d3).divide(epsilon), w);
973 fillHalfRow(ex.multiply(d1), position, ey.negate().multiply(d2), w, epsilon.divide(sqrtMuA), drDotSdEy, jacobian[1], 0);
974 fillHalfRow(getOne(), vectorExRDot, jacobian[1], 3);
975
976
977 final FieldVector3D<T> vectorEyRDot =
978 new FieldVector3D<>(xDot.multiply(2).multiply(y).subtract(x.multiply(yDot)).divide(mu), f, x.negate().multiply(xDot).divide(mu), g, ex.multiply(d3).divide(epsilon), w);
979 fillHalfRow(ey.multiply(d1), position, ex.multiply(d2), w, epsilon.negate().divide(sqrtMuA), drDotSdEx, jacobian[2], 0);
980 fillHalfRow(getOne(), vectorEyRDot, jacobian[2], 3);
981
982
983 final T h = (hx2.add(1).add(hy2)).divide(sqrtMuA.multiply(2).multiply(epsilon));
984 fillHalfRow( h.negate().multiply(xDot), w, jacobian[3], 0);
985 fillHalfRow( h.multiply(x), w, jacobian[3], 3);
986
987
988 fillHalfRow( h.negate().multiply(yDot), w, jacobian[4], 0);
989 fillHalfRow( h.multiply(y), w, jacobian[4], 3);
990
991
992 final T l = ratio.negate().divide(sqrtMuA);
993 fillHalfRow(getOne().negate().divide(sqrtMuA), velocity, d2, w, l.multiply(ex), drDotSdEx, l.multiply(ey), drDotSdEy, jacobian[5], 0);
994 fillHalfRow(getZero().newInstance(-2).divide(sqrtMuA), position, ex.multiply(beta), vectorEyRDot, ey.negate().multiply(beta), vectorExRDot, d3, w, jacobian[5], 3);
995
996 return jacobian;
997
998 }
999
1000
1001 @Override
1002 protected T[][] computeJacobianEccentricWrtCartesian() {
1003
1004
1005 final T[][] jacobian = computeJacobianMeanWrtCartesian();
1006
1007
1008
1009
1010
1011 final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
1012 final T cosLe = scLe.cos();
1013 final T sinLe = scLe.sin();
1014 final T aOr = getOne().divide(getOne().subtract(ex.multiply(cosLe)).subtract(ey.multiply(sinLe)));
1015
1016
1017 final T[] rowEx = jacobian[1];
1018 final T[] rowEy = jacobian[2];
1019 final T[] rowL = jacobian[5];
1020
1021 for (int j = 0; j < 6; ++j) {
1022 rowL[j] = aOr.multiply(rowL[j].add(sinLe.multiply(rowEx[j])).subtract(cosLe.multiply(rowEy[j])));
1023 }
1024 return jacobian;
1025
1026 }
1027
1028
1029 @Override
1030 protected T[][] computeJacobianTrueWrtCartesian() {
1031
1032
1033 final T[][] jacobian = computeJacobianEccentricWrtCartesian();
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047 final FieldSinCos<T> scLe = FastMath.sinCos(getLE());
1048 final T cosLe = scLe.cos();
1049 final T sinLe = scLe.sin();
1050 final T eSinE = ex.multiply(sinLe).subtract(ey.multiply(cosLe));
1051 final T ecosE = ex.multiply(cosLe).add(ey.multiply(sinLe));
1052 final T e2 = ex.square().add(ey.square());
1053 final T epsilon = getOne().subtract(e2).sqrt();
1054 final T onePeps = epsilon.add(1);
1055 final T d = onePeps.subtract(ecosE);
1056 final T cT = d.multiply(d).add(eSinE.multiply(eSinE)).divide(2);
1057 final T cE = ecosE.multiply(onePeps).subtract(e2);
1058 final T cX = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinLe.multiply(onePeps));
1059 final T cY = ey.multiply(eSinE).divide(epsilon).add( ex).subtract(cosLe.multiply(onePeps));
1060 final T factorLe = cT.add(cE).divide(cT);
1061 final T factorEx = cX.divide(cT);
1062 final T factorEy = cY.divide(cT);
1063
1064
1065 final T[] rowEx = jacobian[1];
1066 final T[] rowEy = jacobian[2];
1067 final T[] rowL = jacobian[5];
1068 for (int j = 0; j < 6; ++j) {
1069 rowL[j] = factorLe.multiply(rowL[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
1070 }
1071
1072 return jacobian;
1073
1074 }
1075
1076
1077 @Override
1078 public void addKeplerContribution(final PositionAngleType type, final T gm,
1079 final T[] pDot) {
1080 pDot[5] = pDot[5].add(computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType));
1081 }
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096 private static <T extends CalculusFieldElement<T>> T computeKeplerianLDot(final PositionAngleType type, final T a, final T ex,
1097 final T ey, final T mu, final T l, final PositionAngleType cachedType) {
1098 final T n = mu.divide(a).sqrt().divide(a);
1099 if (type == PositionAngleType.MEAN) {
1100 return n;
1101 }
1102 final FieldSinCos<T> sc;
1103 final T ksi;
1104 if (type == PositionAngleType.ECCENTRIC) {
1105 sc = FastMath.sinCos(FieldEquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1106 ksi = ((ex.multiply(sc.cos())).add(ey.multiply(sc.sin()))).negate().add(1).reciprocal();
1107 return n.multiply(ksi);
1108 } else {
1109 sc = FastMath.sinCos(FieldEquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1110 final T oMe2 = a.getField().getOne().subtract(ex.square()).subtract(ey.square());
1111 ksi = ex.multiply(sc.cos()).add(1).add(ey.multiply(sc.sin()));
1112 return n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt()));
1113 }
1114 }
1115
1116
1117
1118
1119 public String toString() {
1120 return "equinoctial parameters: " + '{' +
1121 "a: " + a.getReal() +
1122 "; ex: " + ex.getReal() + "; ey: " + ey.getReal() +
1123 "; hx: " + hx.getReal() + "; hy: " + hy.getReal() +
1124 "; lv: " + FastMath.toDegrees(getLv().getReal()) +
1125 ";}";
1126 }
1127
1128
1129 @Override
1130 public PositionAngleType getCachedPositionAngleType() {
1131 return cachedPositionAngleType;
1132 }
1133
1134
1135 @Override
1136 public boolean hasNonKeplerianRates() {
1137 return hasNonKeplerianAcceleration();
1138 }
1139
1140
1141 @Override
1142 public FieldEquinoctialOrbit<T> withKeplerianRates() {
1143 return new FieldEquinoctialOrbit<>(getEquinoctialParameters(), getFrame(), getDate(), getMu());
1144 }
1145
1146
1147 @Override
1148 public EquinoctialOrbit toOrbit() {
1149 final double cachedPositionAngle = cachedL.getReal();
1150 if (hasNonKeplerianRates()) {
1151 return new EquinoctialOrbit(a.getReal(), ex.getReal(), ey.getReal(),
1152 hx.getReal(), hy.getReal(), cachedPositionAngle,
1153 aDot.getReal(), exDot.getReal(), eyDot.getReal(),
1154 hxDot.getReal(), hyDot.getReal(), cachedLDot.getReal(),
1155 cachedPositionAngleType, getFrame(),
1156 getDate().toAbsoluteDate(), getMu().getReal());
1157 } else {
1158 return new EquinoctialOrbit(getEquinoctialParameters().toEquinoctialElements(), getFrame(),
1159 getDate().toAbsoluteDate(), getMu().getReal());
1160 }
1161 }
1162
1163 }