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 lUD = initializeCachedL(l, lDot, type);
221 this.cachedL = lUD.getValue();
222 this.cachedLDot = lUD.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 return EquinoctialLongitudeArgumentUtility.convertL(positionAngleType, l, ex, ey, cachedPositionAngleType);
811 }
812
813
814
815
816
817
818
819 private Vector3D nonKeplerianAcceleration() {
820
821 final double[][] dCdP = new double[6][6];
822 getJacobianWrtParameters(PositionAngleType.MEAN, dCdP);
823
824 final double nonKeplerianMeanMotion = getLMDot() - getKeplerianMeanMotion();
825 final double nonKeplerianAx = dCdP[3][0] * aDot + dCdP[3][1] * exDot + dCdP[3][2] * eyDot +
826 dCdP[3][3] * hxDot + dCdP[3][4] * hyDot + dCdP[3][5] * nonKeplerianMeanMotion;
827 final double nonKeplerianAy = dCdP[4][0] * aDot + dCdP[4][1] * exDot + dCdP[4][2] * eyDot +
828 dCdP[4][3] * hxDot + dCdP[4][4] * hyDot + dCdP[4][5] * nonKeplerianMeanMotion;
829 final double nonKeplerianAz = dCdP[5][0] * aDot + dCdP[5][1] * exDot + dCdP[5][2] * eyDot +
830 dCdP[5][3] * hxDot + dCdP[5][4] * hyDot + dCdP[5][5] * nonKeplerianMeanMotion;
831
832 return new Vector3D(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);
833
834 }
835
836
837 @Override
838 protected Vector3D initPosition() {
839
840
841 final double lE = getLE();
842
843
844 final double hx2 = hx * hx;
845 final double hy2 = hy * hy;
846 final double factH = 1. / (1 + hx2 + hy2);
847
848
849 final double ux = (1 + hx2 - hy2) * factH;
850 final double uy = 2 * hx * hy * factH;
851 final double uz = -2 * hy * factH;
852
853 final double vx = uy;
854 final double vy = (1 - hx2 + hy2) * factH;
855 final double vz = 2 * hx * factH;
856
857
858 final double exey = ex * ey;
859 final double ex2 = ex * ex;
860 final double ey2 = ey * ey;
861 final double e2 = ex2 + ey2;
862 final double eta = 1 + FastMath.sqrt(1 - e2);
863 final double beta = 1. / eta;
864
865
866 final SinCos scLe = FastMath.sinCos(lE);
867 final double cLe = scLe.cos();
868 final double sLe = scLe.sin();
869
870
871 final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
872 final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);
873
874 return new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
875
876 }
877
878
879 @Override
880 protected TimeStampedPVCoordinates initPVCoordinates() {
881
882
883 computePVWithoutA();
884
885
886 final double r2 = partialPV.getPosition().getNormSq();
887 final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
888 final Vector3D acceleration = hasDerivatives() ?
889 keplerianAcceleration.add(nonKeplerianAcceleration()) :
890 keplerianAcceleration;
891
892 return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
893
894 }
895
896
897 @Override
898 public EquinoctialOrbit shiftedBy(final double dt) {
899
900
901 final EquinoctialOrbit keplerianShifted = new EquinoctialOrbit(a, ex, ey, hx, hy,
902 getLM() + getKeplerianMeanMotion() * dt,
903 PositionAngleType.MEAN, cachedPositionAngleType,
904 getFrame(),
905 getDate().shiftedBy(dt), getMu());
906
907 if (hasDerivatives()) {
908
909
910 final Vector3D nonKeplerianAcceleration = nonKeplerianAcceleration();
911
912
913 keplerianShifted.computePVWithoutA();
914 final Vector3D fixedP = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
915 0.5 * dt * dt, nonKeplerianAcceleration);
916 final double fixedR2 = fixedP.getNormSq();
917 final double fixedR = FastMath.sqrt(fixedR2);
918 final Vector3D fixedV = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
919 dt, nonKeplerianAcceleration);
920 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
921 1, nonKeplerianAcceleration);
922
923
924 return new EquinoctialOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
925 fixedP, fixedV, fixedA),
926 keplerianShifted.getFrame(), keplerianShifted.getMu());
927
928 } else {
929
930 return keplerianShifted;
931 }
932
933 }
934
935
936 @Override
937 protected double[][] computeJacobianMeanWrtCartesian() {
938
939 final double[][] jacobian = new double[6][6];
940
941
942 computePVWithoutA();
943 final Vector3D position = partialPV.getPosition();
944 final Vector3D velocity = partialPV.getVelocity();
945 final double r2 = position.getNormSq();
946 final double r = FastMath.sqrt(r2);
947 final double r3 = r * r2;
948
949 final double mu = getMu();
950 final double sqrtMuA = FastMath.sqrt(a * mu);
951 final double a2 = a * a;
952
953 final double e2 = ex * ex + ey * ey;
954 final double oMe2 = 1 - e2;
955 final double epsilon = FastMath.sqrt(oMe2);
956 final double beta = 1 / (1 + epsilon);
957 final double ratio = epsilon * beta;
958
959 final double hx2 = hx * hx;
960 final double hy2 = hy * hy;
961 final double hxhy = hx * hy;
962
963
964 final Vector3D f = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
965 final Vector3D g = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
966 final Vector3D w = Vector3D.crossProduct(position, velocity).normalize();
967
968
969 final double x = Vector3D.dotProduct(position, f);
970 final double y = Vector3D.dotProduct(position, g);
971 final double xDot = Vector3D.dotProduct(velocity, f);
972 final double yDot = Vector3D.dotProduct(velocity, g);
973
974
975 final double c1 = a / (sqrtMuA * epsilon);
976 final double c2 = a * sqrtMuA * beta / r3;
977 final double c3 = sqrtMuA / (r3 * epsilon);
978 final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
979 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);
980
981
982 final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
983 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);
984
985
986 final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
987 final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
988 fillHalfRow(1, vectorAR, jacobian[0], 0);
989 fillHalfRow(1, vectorARDot, jacobian[0], 3);
990
991
992 final double d1 = -a * ratio / r3;
993 final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
994 final double d3 = (hx * y - hy * x) / sqrtMuA;
995 final Vector3D vectorExRDot =
996 new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
997 fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
998 fillHalfRow(1, vectorExRDot, jacobian[1], 3);
999
1000
1001 final Vector3D vectorEyRDot =
1002 new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
1003 fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
1004 fillHalfRow(1, vectorEyRDot, jacobian[2], 3);
1005
1006
1007 final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
1008 fillHalfRow(-h * xDot, w, jacobian[3], 0);
1009 fillHalfRow( h * x, w, jacobian[3], 3);
1010
1011
1012 fillHalfRow(-h * yDot, w, jacobian[4], 0);
1013 fillHalfRow( h * y, w, jacobian[4], 3);
1014
1015
1016 final double l = -ratio / sqrtMuA;
1017 fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
1018 fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);
1019
1020 return jacobian;
1021
1022 }
1023
1024
1025 @Override
1026 protected double[][] computeJacobianEccentricWrtCartesian() {
1027
1028
1029 final double[][] jacobian = computeJacobianMeanWrtCartesian();
1030
1031
1032
1033
1034
1035 final SinCos scLe = FastMath.sinCos(getLE());
1036 final double cosLe = scLe.cos();
1037 final double sinLe = scLe.sin();
1038 final double aOr = 1 / (1 - ex * cosLe - ey * sinLe);
1039
1040
1041 final double[] rowEx = jacobian[1];
1042 final double[] rowEy = jacobian[2];
1043 final double[] rowL = jacobian[5];
1044 for (int j = 0; j < 6; ++j) {
1045 rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
1046 }
1047
1048 return jacobian;
1049
1050 }
1051
1052
1053 @Override
1054 protected double[][] computeJacobianTrueWrtCartesian() {
1055
1056
1057 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071 final SinCos scLe = FastMath.sinCos(getLE());
1072 final double cosLe = scLe.cos();
1073 final double sinLe = scLe.sin();
1074 final double eSinE = ex * sinLe - ey * cosLe;
1075 final double ecosE = ex * cosLe + ey * sinLe;
1076 final double e2 = ex * ex + ey * ey;
1077 final double epsilon = FastMath.sqrt(1 - e2);
1078 final double onePeps = 1 + epsilon;
1079 final double d = onePeps - ecosE;
1080 final double cT = (d * d + eSinE * eSinE) / 2;
1081 final double cE = ecosE * onePeps - e2;
1082 final double cX = ex * eSinE / epsilon - ey + sinLe * onePeps;
1083 final double cY = ey * eSinE / epsilon + ex - cosLe * onePeps;
1084 final double factorLe = (cT + cE) / cT;
1085 final double factorEx = cX / cT;
1086 final double factorEy = cY / cT;
1087
1088
1089 final double[] rowEx = jacobian[1];
1090 final double[] rowEy = jacobian[2];
1091 final double[] rowL = jacobian[5];
1092 for (int j = 0; j < 6; ++j) {
1093 rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
1094 }
1095
1096 return jacobian;
1097
1098 }
1099
1100
1101 @Override
1102 public void addKeplerContribution(final PositionAngleType type, final double gm,
1103 final double[] pDot) {
1104 pDot[5] += computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType);
1105 }
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119 private static double computeKeplerianLDot(final PositionAngleType type, final double a, final double ex,
1120 final double ey, final double mu,
1121 final double l, final PositionAngleType cachedType) {
1122 final double n = FastMath.sqrt(mu / a) / a;
1123 if (type == PositionAngleType.MEAN) {
1124 return n;
1125 }
1126 final double oMe2;
1127 final double ksi;
1128 final SinCos sc;
1129 if (type == PositionAngleType.ECCENTRIC) {
1130 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1131 ksi = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1132 return n * ksi;
1133 } else {
1134 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1135 oMe2 = 1 - ex * ex - ey * ey;
1136 ksi = 1 + ex * sc.cos() + ey * sc.sin();
1137 return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1138 }
1139 }
1140
1141
1142
1143
1144 public String toString() {
1145 return new StringBuilder().append("equinoctial parameters: ").append('{').
1146 append("a: ").append(a).
1147 append("; ex: ").append(ex).append("; ey: ").append(ey).
1148 append("; hx: ").append(hx).append("; hy: ").append(hy).
1149 append("; lv: ").append(FastMath.toDegrees(getLv())).
1150 append(";}").toString();
1151 }
1152
1153
1154 @Override
1155 public PositionAngleType getCachedPositionAngleType() {
1156 return cachedPositionAngleType;
1157 }
1158
1159
1160 @Override
1161 public boolean hasRates() {
1162 return hasDerivatives();
1163 }
1164
1165
1166 @Override
1167 public EquinoctialOrbit removeRates() {
1168 final PositionAngleType positionAngleType = getCachedPositionAngleType();
1169 return new EquinoctialOrbit(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
1170 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1171 }
1172
1173
1174
1175
1176 @DefaultDataContext
1177 private Object writeReplace() {
1178 return new DTO(this);
1179 }
1180
1181
1182 @DefaultDataContext
1183 private static class DTO implements Serializable {
1184
1185
1186 private static final long serialVersionUID = 20231217L;
1187
1188
1189 private final double[] d;
1190
1191
1192 private final Frame frame;
1193
1194
1195 private final PositionAngleType positionAngleType;
1196
1197
1198
1199
1200 private DTO(final EquinoctialOrbit orbit) {
1201
1202 final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();
1203 positionAngleType = orbit.cachedPositionAngleType;
1204
1205
1206 final AbsoluteDate j2000Epoch =
1207 DataContext.getDefault().getTimeScales().getJ2000Epoch();
1208 final double epoch = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
1209 final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));
1210
1211 if (orbit.hasDerivatives()) {
1212
1213 this.d = new double[] {
1214 epoch, offset, orbit.getMu(),
1215 orbit.a, orbit.ex, orbit.ey,
1216 orbit.hx, orbit.hy, orbit.cachedL,
1217 orbit.aDot, orbit.exDot, orbit.eyDot,
1218 orbit.hxDot, orbit.hyDot, orbit.cachedLDot
1219 };
1220 } else {
1221
1222 this.d = new double[] {
1223 epoch, offset, orbit.getMu(),
1224 orbit.a, orbit.ex, orbit.ey,
1225 orbit.hx, orbit.hy, orbit.cachedL
1226 };
1227 }
1228
1229 this.frame = orbit.getFrame();
1230
1231 }
1232
1233
1234
1235
1236 private Object readResolve() {
1237 final AbsoluteDate j2000Epoch =
1238 DataContext.getDefault().getTimeScales().getJ2000Epoch();
1239 if (d.length >= 15) {
1240
1241 return new EquinoctialOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8],
1242 d[ 9], d[10], d[11], d[12], d[13], d[14],
1243 positionAngleType,
1244 frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
1245 d[2]);
1246 } else {
1247
1248 return new EquinoctialOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8], positionAngleType,
1249 frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
1250 d[2]);
1251 }
1252 }
1253
1254 }
1255
1256 }