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