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