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