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 switch (cachedPositionAngleType) {
622 case TRUE:
623 return cachedAlpha;
624
625 case ECCENTRIC:
626 return CircularLatitudeArgumentUtility.eccentricToTrue(ex, ey, cachedAlpha);
627
628 case MEAN:
629 return CircularLatitudeArgumentUtility.meanToTrue(ex, ey, cachedAlpha);
630
631 default:
632 throw new OrekitInternalError(null);
633 }
634 }
635
636
637
638
639
640
641
642
643 public double getAlphaVDot() {
644 switch (cachedPositionAngleType) {
645 case ECCENTRIC:
646 final UnivariateDerivative1 alphaEUD = new UnivariateDerivative1(cachedAlpha, cachedAlphaDot);
647 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
648 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
649 final UnivariateDerivative1 alphaVUD = FieldCircularLatitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
650 alphaEUD);
651 return alphaVUD.getFirstDerivative();
652
653 case TRUE:
654 return cachedAlphaDot;
655
656 case MEAN:
657 final UnivariateDerivative1 alphaMUD = new UnivariateDerivative1(cachedAlpha, cachedAlphaDot);
658 final UnivariateDerivative1 exUD2 = new UnivariateDerivative1(ex, exDot);
659 final UnivariateDerivative1 eyUD2 = new UnivariateDerivative1(ey, eyDot);
660 final UnivariateDerivative1 alphaVUD2 = FieldCircularLatitudeArgumentUtility.meanToTrue(exUD2,
661 eyUD2, alphaMUD);
662 return alphaVUD2.getFirstDerivative();
663
664 default:
665 throw new OrekitInternalError(null);
666 }
667 }
668
669
670
671
672 public double getAlphaE() {
673 switch (cachedPositionAngleType) {
674 case TRUE:
675 return CircularLatitudeArgumentUtility.trueToEccentric(ex, ey, cachedAlpha);
676
677 case ECCENTRIC:
678 return cachedAlpha;
679
680 case MEAN:
681 return CircularLatitudeArgumentUtility.meanToEccentric(ex, ey, cachedAlpha);
682
683 default:
684 throw new OrekitInternalError(null);
685 }
686 }
687
688
689
690
691
692
693
694
695 public double getAlphaEDot() {
696 switch (cachedPositionAngleType) {
697 case TRUE:
698 final UnivariateDerivative1 alphaVUD = new UnivariateDerivative1(cachedAlpha, cachedAlphaDot);
699 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
700 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
701 final UnivariateDerivative1 alphaEUD = FieldCircularLatitudeArgumentUtility.trueToEccentric(exUD, eyUD,
702 alphaVUD);
703 return alphaEUD.getFirstDerivative();
704
705 case ECCENTRIC:
706 return cachedAlphaDot;
707
708 case MEAN:
709 final UnivariateDerivative1 alphaMUD = new UnivariateDerivative1(cachedAlpha, cachedAlphaDot);
710 final UnivariateDerivative1 exUD2 = new UnivariateDerivative1(ex, exDot);
711 final UnivariateDerivative1 eyUD2 = new UnivariateDerivative1(ey, eyDot);
712 final UnivariateDerivative1 alphaVUD2 = FieldCircularLatitudeArgumentUtility.meanToEccentric(exUD2,
713 eyUD2, alphaMUD);
714 return alphaVUD2.getFirstDerivative();
715
716 default:
717 throw new OrekitInternalError(null);
718 }
719 }
720
721
722
723
724 public double getAlphaM() {
725 switch (cachedPositionAngleType) {
726 case TRUE:
727 return CircularLatitudeArgumentUtility.trueToMean(ex, ey, cachedAlpha);
728
729 case MEAN:
730 return cachedAlpha;
731
732 case ECCENTRIC:
733 return CircularLatitudeArgumentUtility.eccentricToMean(ex, ey, cachedAlpha);
734
735 default:
736 throw new OrekitInternalError(null);
737 }
738 }
739
740
741
742
743
744
745
746
747 public double getAlphaMDot() {
748 switch (cachedPositionAngleType) {
749 case TRUE:
750 final UnivariateDerivative1 alphaVUD = new UnivariateDerivative1(cachedAlpha, cachedAlphaDot);
751 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
752 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
753 final UnivariateDerivative1 alphaMUD = FieldCircularLatitudeArgumentUtility.trueToMean(exUD, eyUD,
754 alphaVUD);
755 return alphaMUD.getFirstDerivative();
756
757 case MEAN:
758 return cachedAlphaDot;
759
760 case ECCENTRIC:
761 final UnivariateDerivative1 alphaEUD = new UnivariateDerivative1(cachedAlpha, cachedAlphaDot);
762 final UnivariateDerivative1 exUD2 = new UnivariateDerivative1(ex, exDot);
763 final UnivariateDerivative1 eyUD2 = new UnivariateDerivative1(ey, eyDot);
764 final UnivariateDerivative1 alphaMUD2 = FieldCircularLatitudeArgumentUtility.eccentricToMean(exUD2,
765 eyUD2, alphaEUD);
766 return alphaMUD2.getFirstDerivative();
767
768 default:
769 throw new OrekitInternalError(null);
770 }
771 }
772
773
774
775
776
777 public double getAlpha(final PositionAngleType type) {
778 return (type == PositionAngleType.MEAN) ? getAlphaM() :
779 ((type == PositionAngleType.ECCENTRIC) ? getAlphaE() :
780 getAlphaV());
781 }
782
783
784
785
786
787
788
789
790
791 public double getAlphaDot(final PositionAngleType type) {
792 return (type == PositionAngleType.MEAN) ? getAlphaMDot() :
793 ((type == PositionAngleType.ECCENTRIC) ? getAlphaEDot() :
794 getAlphaVDot());
795 }
796
797
798 @Override
799 public double getE() {
800 return FastMath.sqrt(ex * ex + ey * ey);
801 }
802
803
804 @Override
805 public double getEDot() {
806 if (!hasNonKeplerianAcceleration()) {
807 return 0.;
808 }
809 return (ex * exDot + ey * eyDot) / getE();
810 }
811
812
813 @Override
814 public double getI() {
815 return i;
816 }
817
818
819 @Override
820 public double getIDot() {
821 return iDot;
822 }
823
824
825
826
827 public double getRightAscensionOfAscendingNode() {
828 return raan;
829 }
830
831
832
833
834
835
836
837
838 public double getRightAscensionOfAscendingNodeDot() {
839 return raanDot;
840 }
841
842
843 @Override
844 public double getLv() {
845 return getAlphaV() + raan;
846 }
847
848
849 @Override
850 public double getLvDot() {
851 return getAlphaVDot() + raanDot;
852 }
853
854
855 @Override
856 public double getLE() {
857 return getAlphaE() + raan;
858 }
859
860
861 @Override
862 public double getLEDot() {
863 return getAlphaEDot() + raanDot;
864 }
865
866
867 @Override
868 public double getLM() {
869 return getAlphaM() + raan;
870 }
871
872
873 @Override
874 public double getLMDot() {
875 return getAlphaMDot() + raanDot;
876 }
877
878
879
880 private void computePVWithoutA() {
881
882 if (partialPV != null) {
883
884 return;
885 }
886
887
888 final double equEx = getEquinoctialEx();
889 final double equEy = getEquinoctialEy();
890 final double hx = getHx();
891 final double hy = getHy();
892 final double lE = getLE();
893
894
895 final double hx2 = hx * hx;
896 final double hy2 = hy * hy;
897 final double factH = 1. / (1 + hx2 + hy2);
898
899
900 final double ux = (1 + hx2 - hy2) * factH;
901 final double uy = 2 * hx * hy * factH;
902 final double uz = -2 * hy * factH;
903
904 final double vx = uy;
905 final double vy = (1 - hx2 + hy2) * factH;
906 final double vz = 2 * hx * factH;
907
908
909 final double exey = equEx * equEy;
910 final double ex2 = equEx * equEx;
911 final double ey2 = equEy * equEy;
912 final double e2 = ex2 + ey2;
913 final double eta = 1 + FastMath.sqrt(1 - e2);
914 final double beta = 1. / eta;
915
916
917 final SinCos scLe = FastMath.sinCos(lE);
918 final double cLe = scLe.cos();
919 final double sLe = scLe.sin();
920 final double exCeyS = equEx * cLe + equEy * sLe;
921
922
923 final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - equEx);
924 final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - equEy);
925
926 final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
927 final double xdot = factor * (-sLe + beta * equEy * exCeyS);
928 final double ydot = factor * ( cLe - beta * equEx * exCeyS);
929
930 final Vector3D position =
931 new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
932 final Vector3D velocity =
933 new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz);
934
935 partialPV = new PVCoordinates(position, velocity);
936
937 }
938
939
940
941
942
943
944
945
946 private UnivariateDerivative1 initializeCachedAlpha(final double alpha, final double alphaDot,
947 final PositionAngleType inputType) {
948 if (cachedPositionAngleType == inputType) {
949 return new UnivariateDerivative1(alpha, alphaDot);
950
951 } else {
952 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
953 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
954 final UnivariateDerivative1 alphaUD = new UnivariateDerivative1(alpha, alphaDot);
955
956 switch (cachedPositionAngleType) {
957
958 case ECCENTRIC:
959 if (inputType == PositionAngleType.MEAN) {
960 return FieldCircularLatitudeArgumentUtility.meanToEccentric(exUD, eyUD, alphaUD);
961 } else {
962 return FieldCircularLatitudeArgumentUtility.trueToEccentric(exUD, eyUD, alphaUD);
963 }
964
965 case TRUE:
966 if (inputType == PositionAngleType.MEAN) {
967 return FieldCircularLatitudeArgumentUtility.meanToTrue(exUD, eyUD, alphaUD);
968 } else {
969 return FieldCircularLatitudeArgumentUtility.eccentricToTrue(exUD, eyUD, alphaUD);
970 }
971
972 case MEAN:
973 if (inputType == PositionAngleType.TRUE) {
974 return FieldCircularLatitudeArgumentUtility.trueToMean(exUD, eyUD, alphaUD);
975 } else {
976 return FieldCircularLatitudeArgumentUtility.eccentricToMean(exUD, eyUD, alphaUD);
977 }
978
979 default:
980 throw new OrekitInternalError(null);
981
982 }
983
984 }
985
986 }
987
988
989 @Override
990 protected Vector3D initPosition() {
991
992
993 final double equEx = getEquinoctialEx();
994 final double equEy = getEquinoctialEy();
995 final double hx = getHx();
996 final double hy = getHy();
997 final double lE = getLE();
998
999
1000 final double hx2 = hx * hx;
1001 final double hy2 = hy * hy;
1002 final double factH = 1. / (1 + hx2 + hy2);
1003
1004
1005 final double ux = (1 + hx2 - hy2) * factH;
1006 final double uy = 2 * hx * hy * factH;
1007 final double uz = -2 * hy * factH;
1008
1009 final double vx = uy;
1010 final double vy = (1 - hx2 + hy2) * factH;
1011 final double vz = 2 * hx * factH;
1012
1013
1014 final double exey = equEx * equEy;
1015 final double ex2 = equEx * equEx;
1016 final double ey2 = equEy * equEy;
1017 final double e2 = ex2 + ey2;
1018 final double eta = 1 + FastMath.sqrt(1 - e2);
1019 final double beta = 1. / eta;
1020
1021
1022 final SinCos scLe = FastMath.sinCos(lE);
1023 final double cLe = scLe.cos();
1024 final double sLe = scLe.sin();
1025
1026
1027 final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - equEx);
1028 final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - equEy);
1029
1030 return new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
1031
1032 }
1033
1034
1035 @Override
1036 protected TimeStampedPVCoordinates initPVCoordinates() {
1037
1038
1039 computePVWithoutA();
1040
1041
1042 final double r2 = partialPV.getPosition().getNorm2Sq();
1043 final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
1044 final Vector3D acceleration = hasNonKeplerianRates() ?
1045 keplerianAcceleration.add(nonKeplerianAcceleration()) :
1046 keplerianAcceleration;
1047
1048 return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
1049
1050 }
1051
1052
1053 @Override
1054 public CircularOrbit inFrame(final Frame inertialFrame) {
1055 final PVCoordinates pvCoordinates;
1056 if (hasNonKeplerianAcceleration()) {
1057 pvCoordinates = getPVCoordinates(inertialFrame);
1058 } else {
1059 final KinematicTransform transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
1060 pvCoordinates = transform.transformOnlyPV(getPVCoordinates());
1061 }
1062 final CircularOrbit circularOrbit = new CircularOrbit(pvCoordinates, inertialFrame, getDate(), getMu());
1063 if (circularOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
1064 return circularOrbit;
1065 } else {
1066 return circularOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
1067 }
1068 }
1069
1070
1071 @Override
1072 public CircularOrbit withCachedPositionAngleType(final PositionAngleType positionAngleType) {
1073 return new CircularOrbit(a, ex, ey, i, raan, getAlpha(positionAngleType), aDot, exDot, eyDot, iDot, raanDot,
1074 getAlphaDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1075 }
1076
1077
1078 @Override
1079 public CircularOrbit shiftedBy(final double dt) {
1080 return shiftedBy(new TimeOffset(dt));
1081 }
1082
1083
1084 @Override
1085 public CircularOrbit shiftedBy(final TimeOffset dt) {
1086
1087 final double dtS = dt.toDouble();
1088
1089
1090 final CircularOrbit keplerianShifted = new CircularOrbit(a, ex, ey, i, raan,
1091 getAlphaM() + getKeplerianMeanMotion() * dtS,
1092 PositionAngleType.MEAN, cachedPositionAngleType,
1093 getFrame(), getDate().shiftedBy(dt), getMu());
1094
1095 if (dtS != 0. && hasNonKeplerianRates()) {
1096 final PVCoordinates pvCoordinates = shiftNonKeplerian(keplerianShifted.getPVCoordinates(), dtS);
1097
1098
1099 return new CircularOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(), pvCoordinates),
1100 keplerianShifted.getFrame(), keplerianShifted.getMu());
1101
1102 } else {
1103
1104 return keplerianShifted;
1105 }
1106
1107 }
1108
1109
1110 @Override
1111 protected double[][] computeJacobianMeanWrtCartesian() {
1112
1113
1114 final double[][] jacobian = new double[6][6];
1115
1116 computePVWithoutA();
1117 final Vector3D position = partialPV.getPosition();
1118 final Vector3D velocity = partialPV.getVelocity();
1119 final double x = position.getX();
1120 final double y = position.getY();
1121 final double z = position.getZ();
1122 final double vx = velocity.getX();
1123 final double vy = velocity.getY();
1124 final double vz = velocity.getZ();
1125 final double pv = Vector3D.dotProduct(position, velocity);
1126 final double r2 = position.getNorm2Sq();
1127 final double r = FastMath.sqrt(r2);
1128 final double v2 = velocity.getNorm2Sq();
1129
1130 final double mu = getMu();
1131 final double oOsqrtMuA = 1 / FastMath.sqrt(mu * a);
1132 final double rOa = r / a;
1133 final double aOr = a / r;
1134 final double aOr2 = a / r2;
1135 final double a2 = a * a;
1136
1137 final double ex2 = ex * ex;
1138 final double ey2 = ey * ey;
1139 final double e2 = ex2 + ey2;
1140 final double epsilon = FastMath.sqrt(1 - e2);
1141 final double beta = 1 / (1 + epsilon);
1142
1143 final double eCosE = 1 - rOa;
1144 final double eSinE = pv * oOsqrtMuA;
1145
1146 final SinCos scI = FastMath.sinCos(i);
1147 final SinCos scRaan = FastMath.sinCos(raan);
1148 final double cosI = scI.cos();
1149 final double sinI = scI.sin();
1150 final double cosRaan = scRaan.cos();
1151 final double sinRaan = scRaan.sin();
1152
1153
1154 fillHalfRow(2 * aOr * aOr2, position, jacobian[0], 0);
1155 fillHalfRow(2 * a2 / mu, velocity, jacobian[0], 3);
1156
1157
1158 final Vector3D danP = new Vector3D(v2, position, -pv, velocity);
1159 final Vector3D danV = new Vector3D(r2, velocity, -pv, position);
1160 final double recip = 1 / partialPV.getMomentum().getNorm();
1161 final double recip2 = recip * recip;
1162 final Vector3D dwXP = new Vector3D(recip, new Vector3D( 0, vz, -vy), -recip2 * sinRaan * sinI, danP);
1163 final Vector3D dwYP = new Vector3D(recip, new Vector3D(-vz, 0, vx), recip2 * cosRaan * sinI, danP);
1164 final Vector3D dwZP = new Vector3D(recip, new Vector3D( vy, -vx, 0), -recip2 * cosI, danP);
1165 final Vector3D dwXV = new Vector3D(recip, new Vector3D( 0, -z, y), -recip2 * sinRaan * sinI, danV);
1166 final Vector3D dwYV = new Vector3D(recip, new Vector3D( z, 0, -x), recip2 * cosRaan * sinI, danV);
1167 final Vector3D dwZV = new Vector3D(recip, new Vector3D( -y, x, 0), -recip2 * cosI, danV);
1168
1169
1170 fillHalfRow(sinRaan * cosI, dwXP, -cosRaan * cosI, dwYP, -sinI, dwZP, jacobian[3], 0);
1171 fillHalfRow(sinRaan * cosI, dwXV, -cosRaan * cosI, dwYV, -sinI, dwZV, jacobian[3], 3);
1172
1173
1174 fillHalfRow(sinRaan / sinI, dwYP, cosRaan / sinI, dwXP, jacobian[4], 0);
1175 fillHalfRow(sinRaan / sinI, dwYV, cosRaan / sinI, dwXV, jacobian[4], 3);
1176
1177
1178
1179 final double u = x * cosRaan + y * sinRaan;
1180 final double cv = -x * sinRaan + y * cosRaan;
1181 final double v = cv * cosI + z * sinI;
1182
1183
1184 final Vector3D duP = new Vector3D(cv * cosRaan / sinI, dwXP,
1185 cv * sinRaan / sinI, dwYP,
1186 1, new Vector3D(cosRaan, sinRaan, 0));
1187 final Vector3D duV = new Vector3D(cv * cosRaan / sinI, dwXV,
1188 cv * sinRaan / sinI, dwYV);
1189
1190
1191 final Vector3D dvP = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXP,
1192 -u * sinRaan * cosI / sinI - cosRaan * z, dwYP,
1193 cv, dwZP,
1194 1, new Vector3D(-sinRaan * cosI, cosRaan * cosI, sinI));
1195 final Vector3D dvV = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXV,
1196 -u * sinRaan * cosI / sinI - cosRaan * z, dwYV,
1197 cv, dwZV);
1198
1199 final Vector3D dc1P = new Vector3D(aOr2 * (2 * eSinE * eSinE + 1 - eCosE) / r2, position,
1200 -2 * aOr2 * eSinE * oOsqrtMuA, velocity);
1201 final Vector3D dc1V = new Vector3D(-2 * aOr2 * eSinE * oOsqrtMuA, position,
1202 2 / mu, velocity);
1203 final Vector3D dc2P = new Vector3D(aOr2 * eSinE * (eSinE * eSinE - (1 - e2)) / (r2 * epsilon), position,
1204 aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, velocity);
1205 final Vector3D dc2V = new Vector3D(aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, position,
1206 eSinE / (mu * epsilon), velocity);
1207
1208 final double cof1 = aOr2 * (eCosE - e2);
1209 final double cof2 = aOr2 * epsilon * eSinE;
1210 final Vector3D dexP = new Vector3D(u, dc1P, v, dc2P, cof1, duP, cof2, dvP);
1211 final Vector3D dexV = new Vector3D(u, dc1V, v, dc2V, cof1, duV, cof2, dvV);
1212 final Vector3D deyP = new Vector3D(v, dc1P, -u, dc2P, cof1, dvP, -cof2, duP);
1213 final Vector3D deyV = new Vector3D(v, dc1V, -u, dc2V, cof1, dvV, -cof2, duV);
1214 fillHalfRow(1, dexP, jacobian[1], 0);
1215 fillHalfRow(1, dexV, jacobian[1], 3);
1216 fillHalfRow(1, deyP, jacobian[2], 0);
1217 fillHalfRow(1, deyV, jacobian[2], 3);
1218
1219 final double cle = u / a + ex - eSinE * beta * ey;
1220 final double sle = v / a + ey + eSinE * beta * ex;
1221 final double m1 = beta * eCosE;
1222 final double m2 = 1 - m1 * eCosE;
1223 final double m3 = (u * ey - v * ex) + eSinE * beta * (u * ex + v * ey);
1224 final double m4 = -sle + cle * eSinE * beta;
1225 final double m5 = cle + sle * eSinE * beta;
1226 fillHalfRow((2 * m3 / r + aOr * eSinE + m1 * eSinE * (1 + m1 - (1 + aOr) * m2) / epsilon) / r2, position,
1227 (m1 * m2 / epsilon - 1) * oOsqrtMuA, velocity,
1228 m4, dexP, m5, deyP, -sle / a, duP, cle / a, dvP,
1229 jacobian[5], 0);
1230 fillHalfRow((m1 * m2 / epsilon - 1) * oOsqrtMuA, position,
1231 (2 * m3 + eSinE * a + m1 * eSinE * r * (eCosE * beta * 2 - aOr * m2) / epsilon) / mu, velocity,
1232 m4, dexV, m5, deyV, -sle / a, duV, cle / a, dvV,
1233 jacobian[5], 3);
1234
1235 return jacobian;
1236
1237 }
1238
1239
1240 @Override
1241 protected double[][] computeJacobianEccentricWrtCartesian() {
1242
1243
1244 final double[][] jacobian = computeJacobianMeanWrtCartesian();
1245
1246
1247
1248
1249
1250 final double alphaE = getAlphaE();
1251 final SinCos scAe = FastMath.sinCos(alphaE);
1252 final double cosAe = scAe.cos();
1253 final double sinAe = scAe.sin();
1254 final double aOr = 1 / (1 - ex * cosAe - ey * sinAe);
1255
1256
1257 final double[] rowEx = jacobian[1];
1258 final double[] rowEy = jacobian[2];
1259 final double[] rowL = jacobian[5];
1260 for (int j = 0; j < 6; ++j) {
1261 rowL[j] = aOr * (rowL[j] + sinAe * rowEx[j] - cosAe * rowEy[j]);
1262 }
1263
1264 return jacobian;
1265
1266 }
1267
1268
1269 @Override
1270 protected double[][] computeJacobianTrueWrtCartesian() {
1271
1272
1273 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287 final double alphaE = getAlphaE();
1288 final SinCos scAe = FastMath.sinCos(alphaE);
1289 final double cosAe = scAe.cos();
1290 final double sinAe = scAe.sin();
1291 final double eSinE = ex * sinAe - ey * cosAe;
1292 final double ecosE = ex * cosAe + ey * sinAe;
1293 final double e2 = ex * ex + ey * ey;
1294 final double epsilon = FastMath.sqrt(1 - e2);
1295 final double onePeps = 1 + epsilon;
1296 final double d = onePeps - ecosE;
1297 final double cT = (d * d + eSinE * eSinE) / 2;
1298 final double cE = ecosE * onePeps - e2;
1299 final double cX = ex * eSinE / epsilon - ey + sinAe * onePeps;
1300 final double cY = ey * eSinE / epsilon + ex - cosAe * onePeps;
1301 final double factorLe = (cT + cE) / cT;
1302 final double factorEx = cX / cT;
1303 final double factorEy = cY / cT;
1304
1305
1306 final double[] rowEx = jacobian[1];
1307 final double[] rowEy = jacobian[2];
1308 final double[] rowA = jacobian[5];
1309 for (int j = 0; j < 6; ++j) {
1310 rowA[j] = factorLe * rowA[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
1311 }
1312
1313 return jacobian;
1314
1315 }
1316
1317
1318 @Override
1319 public void addKeplerContribution(final PositionAngleType type, final double gm,
1320 final double[] pDot) {
1321 pDot[5] += computeKeplerianAlphaDot(type, a, ex, ey, gm, cachedAlpha, cachedPositionAngleType);
1322 }
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336 private static double computeKeplerianAlphaDot(final PositionAngleType type, final double a, final double ex,
1337 final double ey, final double mu,
1338 final double alpha, final PositionAngleType cachedType) {
1339 final double n = FastMath.sqrt(mu / a) / a;
1340 if (type == PositionAngleType.MEAN) {
1341 return n;
1342 }
1343 final double ksi;
1344 final SinCos sc;
1345 if (type == PositionAngleType.ECCENTRIC) {
1346 sc = FastMath.sinCos(CircularLatitudeArgumentUtility.convertAlpha(cachedType, alpha, ex, ey, type));
1347 ksi = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1348 return n * ksi;
1349 } else {
1350 sc = FastMath.sinCos(CircularLatitudeArgumentUtility.convertAlpha(cachedType, alpha, ex, ey, type));
1351 final double oMe2 = 1 - ex * ex - ey * ey;
1352 ksi = 1 + ex * sc.cos() + ey * sc.sin();
1353 return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1354 }
1355 }
1356
1357
1358
1359
1360 public String toString() {
1361 return new StringBuilder().append("circular parameters: ").append('{').
1362 append("a: ").append(a).
1363 append(", ex: ").append(ex).append(", ey: ").append(ey).
1364 append(", i: ").append(FastMath.toDegrees(i)).
1365 append(", raan: ").append(FastMath.toDegrees(raan)).
1366 append(", alphaV: ").append(FastMath.toDegrees(getAlphaV())).
1367 append(";}").toString();
1368 }
1369
1370
1371 @Override
1372 public PositionAngleType getCachedPositionAngleType() {
1373 return cachedPositionAngleType;
1374 }
1375
1376
1377 @Override
1378 public boolean hasNonKeplerianRates() {
1379 return hasNonKeplerianAcceleration();
1380 }
1381
1382
1383 @Override
1384 public CircularOrbit withKeplerianRates() {
1385 final PositionAngleType positionAngleType = getCachedPositionAngleType();
1386 return new CircularOrbit(a, ex, ey, i, raan, cachedAlpha, positionAngleType, positionAngleType,
1387 getFrame(), getDate(), getMu());
1388 }
1389
1390 }