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