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