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