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.getNormSq();
323 final double r = FastMath.sqrt(r2);
324 final double V2 = pvV.getNormSq();
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().getNormSq();
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
1097
1098 final Vector3D nonKeplerianAcceleration = nonKeplerianAcceleration();
1099
1100
1101 keplerianShifted.computePVWithoutA();
1102 final Vector3D fixedP = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
1103 0.5 * dtS * dtS, nonKeplerianAcceleration);
1104 final double fixedR2 = fixedP.getNormSq();
1105 final double fixedR = FastMath.sqrt(fixedR2);
1106 final Vector3D fixedV = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
1107 dtS, nonKeplerianAcceleration);
1108 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
1109 1, nonKeplerianAcceleration);
1110
1111
1112 return new CircularOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
1113 fixedP, fixedV, fixedA),
1114 keplerianShifted.getFrame(), keplerianShifted.getMu());
1115
1116 } else {
1117
1118 return keplerianShifted;
1119 }
1120
1121 }
1122
1123
1124 @Override
1125 protected double[][] computeJacobianMeanWrtCartesian() {
1126
1127
1128 final double[][] jacobian = new double[6][6];
1129
1130 computePVWithoutA();
1131 final Vector3D position = partialPV.getPosition();
1132 final Vector3D velocity = partialPV.getVelocity();
1133 final double x = position.getX();
1134 final double y = position.getY();
1135 final double z = position.getZ();
1136 final double vx = velocity.getX();
1137 final double vy = velocity.getY();
1138 final double vz = velocity.getZ();
1139 final double pv = Vector3D.dotProduct(position, velocity);
1140 final double r2 = position.getNormSq();
1141 final double r = FastMath.sqrt(r2);
1142 final double v2 = velocity.getNormSq();
1143
1144 final double mu = getMu();
1145 final double oOsqrtMuA = 1 / FastMath.sqrt(mu * a);
1146 final double rOa = r / a;
1147 final double aOr = a / r;
1148 final double aOr2 = a / r2;
1149 final double a2 = a * a;
1150
1151 final double ex2 = ex * ex;
1152 final double ey2 = ey * ey;
1153 final double e2 = ex2 + ey2;
1154 final double epsilon = FastMath.sqrt(1 - e2);
1155 final double beta = 1 / (1 + epsilon);
1156
1157 final double eCosE = 1 - rOa;
1158 final double eSinE = pv * oOsqrtMuA;
1159
1160 final SinCos scI = FastMath.sinCos(i);
1161 final SinCos scRaan = FastMath.sinCos(raan);
1162 final double cosI = scI.cos();
1163 final double sinI = scI.sin();
1164 final double cosRaan = scRaan.cos();
1165 final double sinRaan = scRaan.sin();
1166
1167
1168 fillHalfRow(2 * aOr * aOr2, position, jacobian[0], 0);
1169 fillHalfRow(2 * a2 / mu, velocity, jacobian[0], 3);
1170
1171
1172 final Vector3D danP = new Vector3D(v2, position, -pv, velocity);
1173 final Vector3D danV = new Vector3D(r2, velocity, -pv, position);
1174 final double recip = 1 / partialPV.getMomentum().getNorm();
1175 final double recip2 = recip * recip;
1176 final Vector3D dwXP = new Vector3D(recip, new Vector3D( 0, vz, -vy), -recip2 * sinRaan * sinI, danP);
1177 final Vector3D dwYP = new Vector3D(recip, new Vector3D(-vz, 0, vx), recip2 * cosRaan * sinI, danP);
1178 final Vector3D dwZP = new Vector3D(recip, new Vector3D( vy, -vx, 0), -recip2 * cosI, danP);
1179 final Vector3D dwXV = new Vector3D(recip, new Vector3D( 0, -z, y), -recip2 * sinRaan * sinI, danV);
1180 final Vector3D dwYV = new Vector3D(recip, new Vector3D( z, 0, -x), recip2 * cosRaan * sinI, danV);
1181 final Vector3D dwZV = new Vector3D(recip, new Vector3D( -y, x, 0), -recip2 * cosI, danV);
1182
1183
1184 fillHalfRow(sinRaan * cosI, dwXP, -cosRaan * cosI, dwYP, -sinI, dwZP, jacobian[3], 0);
1185 fillHalfRow(sinRaan * cosI, dwXV, -cosRaan * cosI, dwYV, -sinI, dwZV, jacobian[3], 3);
1186
1187
1188 fillHalfRow(sinRaan / sinI, dwYP, cosRaan / sinI, dwXP, jacobian[4], 0);
1189 fillHalfRow(sinRaan / sinI, dwYV, cosRaan / sinI, dwXV, jacobian[4], 3);
1190
1191
1192
1193 final double u = x * cosRaan + y * sinRaan;
1194 final double cv = -x * sinRaan + y * cosRaan;
1195 final double v = cv * cosI + z * sinI;
1196
1197
1198 final Vector3D duP = new Vector3D(cv * cosRaan / sinI, dwXP,
1199 cv * sinRaan / sinI, dwYP,
1200 1, new Vector3D(cosRaan, sinRaan, 0));
1201 final Vector3D duV = new Vector3D(cv * cosRaan / sinI, dwXV,
1202 cv * sinRaan / sinI, dwYV);
1203
1204
1205 final Vector3D dvP = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXP,
1206 -u * sinRaan * cosI / sinI - cosRaan * z, dwYP,
1207 cv, dwZP,
1208 1, new Vector3D(-sinRaan * cosI, cosRaan * cosI, sinI));
1209 final Vector3D dvV = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXV,
1210 -u * sinRaan * cosI / sinI - cosRaan * z, dwYV,
1211 cv, dwZV);
1212
1213 final Vector3D dc1P = new Vector3D(aOr2 * (2 * eSinE * eSinE + 1 - eCosE) / r2, position,
1214 -2 * aOr2 * eSinE * oOsqrtMuA, velocity);
1215 final Vector3D dc1V = new Vector3D(-2 * aOr2 * eSinE * oOsqrtMuA, position,
1216 2 / mu, velocity);
1217 final Vector3D dc2P = new Vector3D(aOr2 * eSinE * (eSinE * eSinE - (1 - e2)) / (r2 * epsilon), position,
1218 aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, velocity);
1219 final Vector3D dc2V = new Vector3D(aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, position,
1220 eSinE / (mu * epsilon), velocity);
1221
1222 final double cof1 = aOr2 * (eCosE - e2);
1223 final double cof2 = aOr2 * epsilon * eSinE;
1224 final Vector3D dexP = new Vector3D(u, dc1P, v, dc2P, cof1, duP, cof2, dvP);
1225 final Vector3D dexV = new Vector3D(u, dc1V, v, dc2V, cof1, duV, cof2, dvV);
1226 final Vector3D deyP = new Vector3D(v, dc1P, -u, dc2P, cof1, dvP, -cof2, duP);
1227 final Vector3D deyV = new Vector3D(v, dc1V, -u, dc2V, cof1, dvV, -cof2, duV);
1228 fillHalfRow(1, dexP, jacobian[1], 0);
1229 fillHalfRow(1, dexV, jacobian[1], 3);
1230 fillHalfRow(1, deyP, jacobian[2], 0);
1231 fillHalfRow(1, deyV, jacobian[2], 3);
1232
1233 final double cle = u / a + ex - eSinE * beta * ey;
1234 final double sle = v / a + ey + eSinE * beta * ex;
1235 final double m1 = beta * eCosE;
1236 final double m2 = 1 - m1 * eCosE;
1237 final double m3 = (u * ey - v * ex) + eSinE * beta * (u * ex + v * ey);
1238 final double m4 = -sle + cle * eSinE * beta;
1239 final double m5 = cle + sle * eSinE * beta;
1240 fillHalfRow((2 * m3 / r + aOr * eSinE + m1 * eSinE * (1 + m1 - (1 + aOr) * m2) / epsilon) / r2, position,
1241 (m1 * m2 / epsilon - 1) * oOsqrtMuA, velocity,
1242 m4, dexP, m5, deyP, -sle / a, duP, cle / a, dvP,
1243 jacobian[5], 0);
1244 fillHalfRow((m1 * m2 / epsilon - 1) * oOsqrtMuA, position,
1245 (2 * m3 + eSinE * a + m1 * eSinE * r * (eCosE * beta * 2 - aOr * m2) / epsilon) / mu, velocity,
1246 m4, dexV, m5, deyV, -sle / a, duV, cle / a, dvV,
1247 jacobian[5], 3);
1248
1249 return jacobian;
1250
1251 }
1252
1253
1254 @Override
1255 protected double[][] computeJacobianEccentricWrtCartesian() {
1256
1257
1258 final double[][] jacobian = computeJacobianMeanWrtCartesian();
1259
1260
1261
1262
1263
1264 final double alphaE = getAlphaE();
1265 final SinCos scAe = FastMath.sinCos(alphaE);
1266 final double cosAe = scAe.cos();
1267 final double sinAe = scAe.sin();
1268 final double aOr = 1 / (1 - ex * cosAe - ey * sinAe);
1269
1270
1271 final double[] rowEx = jacobian[1];
1272 final double[] rowEy = jacobian[2];
1273 final double[] rowL = jacobian[5];
1274 for (int j = 0; j < 6; ++j) {
1275 rowL[j] = aOr * (rowL[j] + sinAe * rowEx[j] - cosAe * rowEy[j]);
1276 }
1277
1278 return jacobian;
1279
1280 }
1281
1282
1283 @Override
1284 protected double[][] computeJacobianTrueWrtCartesian() {
1285
1286
1287 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
1299
1300
1301 final double alphaE = getAlphaE();
1302 final SinCos scAe = FastMath.sinCos(alphaE);
1303 final double cosAe = scAe.cos();
1304 final double sinAe = scAe.sin();
1305 final double eSinE = ex * sinAe - ey * cosAe;
1306 final double ecosE = ex * cosAe + ey * sinAe;
1307 final double e2 = ex * ex + ey * ey;
1308 final double epsilon = FastMath.sqrt(1 - e2);
1309 final double onePeps = 1 + epsilon;
1310 final double d = onePeps - ecosE;
1311 final double cT = (d * d + eSinE * eSinE) / 2;
1312 final double cE = ecosE * onePeps - e2;
1313 final double cX = ex * eSinE / epsilon - ey + sinAe * onePeps;
1314 final double cY = ey * eSinE / epsilon + ex - cosAe * onePeps;
1315 final double factorLe = (cT + cE) / cT;
1316 final double factorEx = cX / cT;
1317 final double factorEy = cY / cT;
1318
1319
1320 final double[] rowEx = jacobian[1];
1321 final double[] rowEy = jacobian[2];
1322 final double[] rowA = jacobian[5];
1323 for (int j = 0; j < 6; ++j) {
1324 rowA[j] = factorLe * rowA[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
1325 }
1326
1327 return jacobian;
1328
1329 }
1330
1331
1332 @Override
1333 public void addKeplerContribution(final PositionAngleType type, final double gm,
1334 final double[] pDot) {
1335 pDot[5] += computeKeplerianAlphaDot(type, a, ex, ey, gm, cachedAlpha, cachedPositionAngleType);
1336 }
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350 private static double computeKeplerianAlphaDot(final PositionAngleType type, final double a, final double ex,
1351 final double ey, final double mu,
1352 final double alpha, final PositionAngleType cachedType) {
1353 final double n = FastMath.sqrt(mu / a) / a;
1354 if (type == PositionAngleType.MEAN) {
1355 return n;
1356 }
1357 final double ksi;
1358 final SinCos sc;
1359 if (type == PositionAngleType.ECCENTRIC) {
1360 sc = FastMath.sinCos(CircularLatitudeArgumentUtility.convertAlpha(cachedType, alpha, ex, ey, type));
1361 ksi = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1362 return n * ksi;
1363 } else {
1364 sc = FastMath.sinCos(CircularLatitudeArgumentUtility.convertAlpha(cachedType, alpha, ex, ey, type));
1365 final double oMe2 = 1 - ex * ex - ey * ey;
1366 ksi = 1 + ex * sc.cos() + ey * sc.sin();
1367 return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1368 }
1369 }
1370
1371
1372
1373
1374 public String toString() {
1375 return new StringBuilder().append("circular parameters: ").append('{').
1376 append("a: ").append(a).
1377 append(", ex: ").append(ex).append(", ey: ").append(ey).
1378 append(", i: ").append(FastMath.toDegrees(i)).
1379 append(", raan: ").append(FastMath.toDegrees(raan)).
1380 append(", alphaV: ").append(FastMath.toDegrees(getAlphaV())).
1381 append(";}").toString();
1382 }
1383
1384
1385 @Override
1386 public PositionAngleType getCachedPositionAngleType() {
1387 return cachedPositionAngleType;
1388 }
1389
1390
1391 @Override
1392 public boolean hasNonKeplerianRates() {
1393 return hasNonKeplerianAcceleration();
1394 }
1395
1396
1397 @Override
1398 public CircularOrbit withKeplerianRates() {
1399 final PositionAngleType positionAngleType = getCachedPositionAngleType();
1400 return new CircularOrbit(a, ex, ey, i, raan, cachedAlpha, positionAngleType, positionAngleType,
1401 getFrame(), getDate(), getMu());
1402 }
1403
1404 }