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.exception.MathIllegalStateException;
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.hipparchus.util.FastMath;
29 import org.hipparchus.util.MathUtils;
30 import org.hipparchus.util.SinCos;
31 import org.orekit.annotation.DefaultDataContext;
32 import org.orekit.data.DataContext;
33 import org.orekit.errors.OrekitException;
34 import org.orekit.errors.OrekitIllegalArgumentException;
35 import org.orekit.errors.OrekitInternalError;
36 import org.orekit.errors.OrekitMessages;
37 import org.orekit.frames.Frame;
38 import org.orekit.time.AbsoluteDate;
39 import org.orekit.utils.PVCoordinates;
40 import org.orekit.utils.TimeStampedPVCoordinates;
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
81
82 public class KeplerianOrbit extends Orbit {
83
84
85 private static final long serialVersionUID = 20170414L;
86
87
88 private static final String ECCENTRICITY = "eccentricity";
89
90
91 private static final double A;
92
93
94 private static final double B;
95
96 static {
97 final double k1 = 3 * FastMath.PI + 2;
98 final double k2 = FastMath.PI - 1;
99 final double k3 = 6 * FastMath.PI - 1;
100 A = 3 * k2 * k2 / k1;
101 B = k3 * k3 / (6 * k1);
102 }
103
104
105 private final double a;
106
107
108 private final double e;
109
110
111 private final double i;
112
113
114 private final double pa;
115
116
117 private final double raan;
118
119
120 private final double v;
121
122
123 private final double aDot;
124
125
126 private final double eDot;
127
128
129 private final double iDot;
130
131
132 private final double paDot;
133
134
135 private final double raanDot;
136
137
138 private final double vDot;
139
140
141 private transient PVCoordinates partialPV;
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159 public KeplerianOrbit(final double a, final double e, final double i,
160 final double pa, final double raan, final double anomaly,
161 final PositionAngle type,
162 final Frame frame, final AbsoluteDate date, final double mu)
163 throws IllegalArgumentException {
164 this(a, e, i, pa, raan, anomaly,
165 Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN,
166 type, frame, date, mu);
167 }
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192 public KeplerianOrbit(final double a, final double e, final double i,
193 final double pa, final double raan, final double anomaly,
194 final double aDot, final double eDot, final double iDot,
195 final double paDot, final double raanDot, final double anomalyDot,
196 final PositionAngle type,
197 final Frame frame, final AbsoluteDate date, final double mu)
198 throws IllegalArgumentException {
199 super(frame, date, mu);
200
201 if (a * (1 - e) < 0) {
202 throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a, e);
203 }
204
205
206 checkParameterRangeInclusive(ECCENTRICITY, e, 0.0, Double.POSITIVE_INFINITY);
207
208 this.a = a;
209 this.aDot = aDot;
210 this.e = e;
211 this.eDot = eDot;
212 this.i = i;
213 this.iDot = iDot;
214 this.pa = pa;
215 this.paDot = paDot;
216 this.raan = raan;
217 this.raanDot = raanDot;
218
219 if (hasDerivatives()) {
220 final UnivariateDerivative1 eUD = new UnivariateDerivative1(e, eDot);
221 final UnivariateDerivative1 anomalyUD = new UnivariateDerivative1(anomaly, anomalyDot);
222 final UnivariateDerivative1 vUD;
223 switch (type) {
224 case MEAN :
225 vUD = (a < 0) ?
226 FieldKeplerianOrbit.hyperbolicEccentricToTrue(FieldKeplerianOrbit.meanToHyperbolicEccentric(anomalyUD, eUD), eUD) :
227 FieldKeplerianOrbit.ellipticEccentricToTrue(FieldKeplerianOrbit.meanToEllipticEccentric(anomalyUD, eUD), eUD);
228 break;
229 case ECCENTRIC :
230 vUD = (a < 0) ?
231 FieldKeplerianOrbit.hyperbolicEccentricToTrue(anomalyUD, eUD) :
232 FieldKeplerianOrbit.ellipticEccentricToTrue(anomalyUD, eUD);
233 break;
234 case TRUE :
235 vUD = anomalyUD;
236 break;
237 default :
238 throw new OrekitInternalError(null);
239 }
240 this.v = vUD.getValue();
241 this.vDot = vUD.getDerivative(1);
242 } else {
243 switch (type) {
244 case MEAN :
245 this.v = (a < 0) ?
246 hyperbolicEccentricToTrue(meanToHyperbolicEccentric(anomaly, e), e) :
247 ellipticEccentricToTrue(meanToEllipticEccentric(anomaly, e), e);
248 break;
249 case ECCENTRIC :
250 this.v = (a < 0) ?
251 hyperbolicEccentricToTrue(anomaly, e) :
252 ellipticEccentricToTrue(anomaly, e);
253 break;
254 case TRUE :
255 this.v = anomaly;
256 break;
257 default :
258 throw new OrekitInternalError(null);
259 }
260 this.vDot = Double.NaN;
261 }
262
263
264 if (1 + e * FastMath.cos(v) <= 0) {
265 final double vMax = FastMath.acos(-1 / e);
266 throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
267 v, e, -vMax, vMax);
268 }
269
270 this.partialPV = null;
271
272 }
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288 public KeplerianOrbit(final TimeStampedPVCoordinates pvCoordinates,
289 final Frame frame, final double mu)
290 throws IllegalArgumentException {
291 this(pvCoordinates, frame, mu, hasNonKeplerianAcceleration(pvCoordinates, mu));
292 }
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309 private KeplerianOrbit(final TimeStampedPVCoordinates pvCoordinates,
310 final Frame frame, final double mu,
311 final boolean reliableAcceleration)
312 throws IllegalArgumentException {
313 super(pvCoordinates, frame, mu);
314
315
316 final Vector3D momentum = pvCoordinates.getMomentum();
317 final double m2 = momentum.getNormSq();
318 i = Vector3D.angle(momentum, Vector3D.PLUS_K);
319
320
321 raan = Vector3D.crossProduct(Vector3D.PLUS_K, momentum).getAlpha();
322
323
324 final Vector3D pvP = pvCoordinates.getPosition();
325 final Vector3D pvV = pvCoordinates.getVelocity();
326 final Vector3D pvA = pvCoordinates.getAcceleration();
327 final double r2 = pvP.getNormSq();
328 final double r = FastMath.sqrt(r2);
329 final double V2 = pvV.getNormSq();
330 final double rV2OnMu = r * V2 / mu;
331
332
333 a = r / (2 - rV2OnMu);
334 final double muA = mu * a;
335
336
337 if (a > 0) {
338
339 final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
340 final double eCE = rV2OnMu - 1;
341 e = FastMath.sqrt(eSE * eSE + eCE * eCE);
342 v = ellipticEccentricToTrue(FastMath.atan2(eSE, eCE), e);
343 } else {
344
345 final double eSH = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA);
346 final double eCH = rV2OnMu - 1;
347 e = FastMath.sqrt(1 - m2 / muA);
348 v = hyperbolicEccentricToTrue(FastMath.log((eCH + eSH) / (eCH - eSH)) / 2, e);
349 }
350
351
352 checkParameterRangeInclusive(ECCENTRICITY, e, 0.0, Double.POSITIVE_INFINITY);
353
354
355 final Vector3D node = new Vector3D(raan, 0.0);
356 final double px = Vector3D.dotProduct(pvP, node);
357 final double py = Vector3D.dotProduct(pvP, Vector3D.crossProduct(momentum, node)) / FastMath.sqrt(m2);
358 pa = FastMath.atan2(py, px) - v;
359
360 partialPV = pvCoordinates;
361
362 if (reliableAcceleration) {
363
364
365 final double[][] jacobian = new double[6][6];
366 getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);
367
368 final Vector3D keplerianAcceleration = new Vector3D(-mu / (r * r2), pvP);
369 final Vector3D nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
370 final double aX = nonKeplerianAcceleration.getX();
371 final double aY = nonKeplerianAcceleration.getY();
372 final double aZ = nonKeplerianAcceleration.getZ();
373 aDot = jacobian[0][3] * aX + jacobian[0][4] * aY + jacobian[0][5] * aZ;
374 eDot = jacobian[1][3] * aX + jacobian[1][4] * aY + jacobian[1][5] * aZ;
375 iDot = jacobian[2][3] * aX + jacobian[2][4] * aY + jacobian[2][5] * aZ;
376 paDot = jacobian[3][3] * aX + jacobian[3][4] * aY + jacobian[3][5] * aZ;
377 raanDot = jacobian[4][3] * aX + jacobian[4][4] * aY + jacobian[4][5] * aZ;
378
379
380
381 final double MDot = getKeplerianMeanMotion() +
382 jacobian[5][3] * aX + jacobian[5][4] * aY + jacobian[5][5] * aZ;
383 final UnivariateDerivative1 eUD = new UnivariateDerivative1(e, eDot);
384 final UnivariateDerivative1 MUD = new UnivariateDerivative1(getMeanAnomaly(), MDot);
385 final UnivariateDerivative1 vUD = (a < 0) ?
386 FieldKeplerianOrbit.hyperbolicEccentricToTrue(FieldKeplerianOrbit.meanToHyperbolicEccentric(MUD, eUD), eUD) :
387 FieldKeplerianOrbit.ellipticEccentricToTrue(FieldKeplerianOrbit.meanToEllipticEccentric(MUD, eUD), eUD);
388 vDot = vUD.getDerivative(1);
389
390 } else {
391
392
393
394 aDot = Double.NaN;
395 eDot = Double.NaN;
396 iDot = Double.NaN;
397 paDot = Double.NaN;
398 raanDot = Double.NaN;
399 vDot = Double.NaN;
400 }
401
402 }
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419 public KeplerianOrbit(final PVCoordinates pvCoordinates,
420 final Frame frame, final AbsoluteDate date, final double mu)
421 throws IllegalArgumentException {
422 this(new TimeStampedPVCoordinates(date, pvCoordinates), frame, mu);
423 }
424
425
426
427
428 public KeplerianOrbit(final Orbit op) {
429 this(op.getPVCoordinates(), op.getFrame(), op.getMu(), op.hasDerivatives());
430 }
431
432
433 public OrbitType getType() {
434 return OrbitType.KEPLERIAN;
435 }
436
437
438 public double getA() {
439 return a;
440 }
441
442
443 public double getADot() {
444 return aDot;
445 }
446
447
448 public double getE() {
449 return e;
450 }
451
452
453 public double getEDot() {
454 return eDot;
455 }
456
457
458 public double getI() {
459 return i;
460 }
461
462
463 public double getIDot() {
464 return iDot;
465 }
466
467
468
469
470 public double getPerigeeArgument() {
471 return pa;
472 }
473
474
475
476
477
478
479
480
481 public double getPerigeeArgumentDot() {
482 return paDot;
483 }
484
485
486
487
488 public double getRightAscensionOfAscendingNode() {
489 return raan;
490 }
491
492
493
494
495
496
497
498
499 public double getRightAscensionOfAscendingNodeDot() {
500 return raanDot;
501 }
502
503
504
505
506 public double getTrueAnomaly() {
507 return v;
508 }
509
510
511
512
513 public double getTrueAnomalyDot() {
514 return vDot;
515 }
516
517
518
519
520 public double getEccentricAnomaly() {
521 return (a < 0) ? trueToHyperbolicEccentric(v, e) : trueToEllipticEccentric(v, e);
522 }
523
524
525
526
527
528 public double getEccentricAnomalyDot() {
529 final UnivariateDerivative1 eUD = new UnivariateDerivative1(e, eDot);
530 final UnivariateDerivative1 vUD = new UnivariateDerivative1(v, vDot);
531 final UnivariateDerivative1 EUD = (a < 0) ?
532 FieldKeplerianOrbit.trueToHyperbolicEccentric(vUD, eUD) :
533 FieldKeplerianOrbit.trueToEllipticEccentric(vUD, eUD);
534 return EUD.getDerivative(1);
535 }
536
537
538
539
540 public double getMeanAnomaly() {
541 return (a < 0) ?
542 hyperbolicEccentricToMean(trueToHyperbolicEccentric(v, e), e) :
543 ellipticEccentricToMean(trueToEllipticEccentric(v, e), e);
544 }
545
546
547
548
549
550 public double getMeanAnomalyDot() {
551 final UnivariateDerivative1 eUD = new UnivariateDerivative1(e, eDot);
552 final UnivariateDerivative1 vUD = new UnivariateDerivative1(v, vDot);
553 final UnivariateDerivative1 MUD = (a < 0) ?
554 FieldKeplerianOrbit.hyperbolicEccentricToMean(FieldKeplerianOrbit.trueToHyperbolicEccentric(vUD, eUD), eUD) :
555 FieldKeplerianOrbit.ellipticEccentricToMean(FieldKeplerianOrbit.trueToEllipticEccentric(vUD, eUD), eUD);
556 return MUD.getDerivative(1);
557 }
558
559
560
561
562
563 public double getAnomaly(final PositionAngle type) {
564 return (type == PositionAngle.MEAN) ? getMeanAnomaly() :
565 ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomaly() :
566 getTrueAnomaly());
567 }
568
569
570
571
572
573
574 public double getAnomalyDot(final PositionAngle type) {
575 return (type == PositionAngle.MEAN) ? getMeanAnomalyDot() :
576 ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomalyDot() :
577 getTrueAnomalyDot());
578 }
579
580
581
582
583
584
585
586 public static double ellipticEccentricToTrue(final double E, final double e) {
587 final double beta = e / (1 + FastMath.sqrt((1 - e) * (1 + e)));
588 final SinCos scE = FastMath.sinCos(E);
589 return E + 2 * FastMath.atan(beta * scE.sin() / (1 - beta * scE.cos()));
590 }
591
592
593
594
595
596
597
598 public static double trueToEllipticEccentric(final double v, final double e) {
599 final double beta = e / (1 + FastMath.sqrt(1 - e * e));
600 final SinCos scv = FastMath.sinCos(v);
601 return v - 2 * FastMath.atan(beta * scv.sin() / (1 + beta * scv.cos()));
602 }
603
604
605
606
607
608
609 public static double hyperbolicEccentricToTrue(final double H, final double e) {
610 return 2 * FastMath.atan(FastMath.sqrt((e + 1) / (e - 1)) * FastMath.tanh(H / 2));
611 }
612
613
614
615
616
617
618
619 public static double trueToHyperbolicEccentric(final double v, final double e) {
620 final SinCos scv = FastMath.sinCos(v);
621 final double sinhH = FastMath.sqrt(e * e - 1) * scv.sin() / (1 + e * scv.cos());
622 return FastMath.asinh(sinhH);
623 }
624
625
626
627
628
629
630
631
632
633
634
635 public static double meanToEllipticEccentric(final double M, final double e) {
636
637
638 final double reducedM = MathUtils.normalizeAngle(M, 0.0);
639
640
641 double E;
642 if (FastMath.abs(reducedM) < 1.0 / 6.0) {
643 E = reducedM + e * (FastMath.cbrt(6 * reducedM) - reducedM);
644 } else {
645 if (reducedM < 0) {
646 final double w = FastMath.PI + reducedM;
647 E = reducedM + e * (A * w / (B - w) - FastMath.PI - reducedM);
648 } else {
649 final double w = FastMath.PI - reducedM;
650 E = reducedM + e * (FastMath.PI - A * w / (B - w) - reducedM);
651 }
652 }
653
654 final double e1 = 1 - e;
655 final boolean noCancellationRisk = (e1 + E * E / 6) >= 0.1;
656
657
658 for (int j = 0; j < 2; ++j) {
659 final double f;
660 double fd;
661 final SinCos sc = FastMath.sinCos(E);
662 final double fdd = e * sc.sin();
663 final double fddd = e * sc.cos();
664 if (noCancellationRisk) {
665 f = (E - fdd) - reducedM;
666 fd = 1 - fddd;
667 } else {
668 f = eMeSinE(E, e) - reducedM;
669 final double s = FastMath.sin(0.5 * E);
670 fd = e1 + 2 * e * s * s;
671 }
672 final double dee = f * fd / (0.5 * f * fdd - fd * fd);
673
674
675 final double w = fd + 0.5 * dee * (fdd + dee * fddd / 3);
676 fd += dee * (fdd + 0.5 * dee * fddd);
677 E -= (f - dee * (fd - w)) / fd;
678
679 }
680
681
682 E += M - reducedM;
683
684 return E;
685
686 }
687
688
689
690
691
692
693
694
695
696
697 private static double eMeSinE(final double E, final double e) {
698 double x = (1 - e) * FastMath.sin(E);
699 final double mE2 = -E * E;
700 double term = E;
701 double d = 0;
702
703 for (double x0 = Double.NaN; !Double.valueOf(x).equals(Double.valueOf(x0));) {
704 d += 2;
705 term *= mE2 / (d * (d + 1));
706 x0 = x;
707 x = x - term;
708 }
709 return x;
710 }
711
712
713
714
715
716
717
718
719
720
721 public static double meanToHyperbolicEccentric(final double M, final double ecc) {
722
723
724
725
726 double H;
727 if (ecc < 1.6) {
728 if (-FastMath.PI < M && M < 0. || M > FastMath.PI) {
729 H = M - ecc;
730 } else {
731 H = M + ecc;
732 }
733 } else {
734 if (ecc < 3.6 && FastMath.abs(M) > FastMath.PI) {
735 H = M - FastMath.copySign(ecc, M);
736 } else {
737 H = M / (ecc - 1.);
738 }
739 }
740
741
742 int iter = 0;
743 do {
744 final double f3 = ecc * FastMath.cosh(H);
745 final double f2 = ecc * FastMath.sinh(H);
746 final double f1 = f3 - 1.;
747 final double f0 = f2 - H - M;
748 final double f12 = 2. * f1;
749 final double d = f0 / f12;
750 final double fdf = f1 - d * f2;
751 final double ds = f0 / fdf;
752
753 final double shift = f0 / (fdf + ds * ds * f3 / 6.);
754
755 H -= shift;
756
757 if (FastMath.abs(shift) <= 1.0e-12) {
758 return H;
759 }
760
761 } while (++iter < 50);
762
763 throw new MathIllegalStateException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
764 iter);
765 }
766
767
768
769
770
771
772
773 public static double ellipticEccentricToMean(final double E, final double e) {
774 return E - e * FastMath.sin(E);
775 }
776
777
778
779
780
781
782
783 public static double hyperbolicEccentricToMean(final double H, final double e) {
784 return e * FastMath.sinh(H) - H;
785 }
786
787
788 public double getEquinoctialEx() {
789 return e * FastMath.cos(pa + raan);
790 }
791
792
793 public double getEquinoctialExDot() {
794 final double paPraan = pa + raan;
795 final SinCos sc = FastMath.sinCos(paPraan);
796 return eDot * sc.cos() - e * sc.sin() * (paDot + raanDot);
797 }
798
799
800 public double getEquinoctialEy() {
801 return e * FastMath.sin(pa + raan);
802 }
803
804
805 public double getEquinoctialEyDot() {
806 final double paPraan = pa + raan;
807 final SinCos sc = FastMath.sinCos(paPraan);
808 return eDot * sc.sin() + e * sc.cos() * (paDot + raanDot);
809 }
810
811
812 public double getHx() {
813
814 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
815 return Double.NaN;
816 }
817 return FastMath.cos(raan) * FastMath.tan(0.5 * i);
818 }
819
820
821 public double getHxDot() {
822
823 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
824 return Double.NaN;
825 }
826 final SinCos sc = FastMath.sinCos(raan);
827 final double tan = FastMath.tan(0.5 * i);
828 return 0.5 * (1 + tan * tan) * sc.cos() * iDot - tan * sc.sin() * raanDot;
829 }
830
831
832 public double getHy() {
833
834 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
835 return Double.NaN;
836 }
837 return FastMath.sin(raan) * FastMath.tan(0.5 * i);
838 }
839
840
841 public double getHyDot() {
842
843 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
844 return Double.NaN;
845 }
846 final SinCos sc = FastMath.sinCos(raan);
847 final double tan = FastMath.tan(0.5 * i);
848 return 0.5 * (1 + tan * tan) * sc.sin() * iDot + tan * sc.cos() * raanDot;
849 }
850
851
852 public double getLv() {
853 return pa + raan + v;
854 }
855
856
857 public double getLvDot() {
858 return paDot + raanDot + vDot;
859 }
860
861
862 public double getLE() {
863 return pa + raan + getEccentricAnomaly();
864 }
865
866
867 public double getLEDot() {
868 return paDot + raanDot + getEccentricAnomalyDot();
869 }
870
871
872 public double getLM() {
873 return pa + raan + getMeanAnomaly();
874 }
875
876
877 public double getLMDot() {
878 return paDot + raanDot + getMeanAnomalyDot();
879 }
880
881
882
883 private void computePVWithoutA() {
884
885 if (partialPV != null) {
886
887 return;
888 }
889
890
891 final SinCos scRaan = FastMath.sinCos(raan);
892 final SinCos scPa = FastMath.sinCos(pa);
893 final SinCos scI = FastMath.sinCos(i);
894 final double cosRaan = scRaan.cos();
895 final double sinRaan = scRaan.sin();
896 final double cosPa = scPa.cos();
897 final double sinPa = scPa.sin();
898 final double cosI = scI.cos();
899 final double sinI = scI.sin();
900
901 final double crcp = cosRaan * cosPa;
902 final double crsp = cosRaan * sinPa;
903 final double srcp = sinRaan * cosPa;
904 final double srsp = sinRaan * sinPa;
905
906
907 final Vector3D p = new Vector3D( crcp - cosI * srsp, srcp + cosI * crsp, sinI * sinPa);
908 final Vector3D q = new Vector3D(-crsp - cosI * srcp, -srsp + cosI * crcp, sinI * cosPa);
909
910 if (a > 0) {
911
912
913
914
915 final double uME2 = (1 - e) * (1 + e);
916 final double s1Me2 = FastMath.sqrt(uME2);
917 final SinCos scE = FastMath.sinCos(getEccentricAnomaly());
918 final double cosE = scE.cos();
919 final double sinE = scE.sin();
920
921
922 final double x = a * (cosE - e);
923 final double y = a * sinE * s1Me2;
924 final double factor = FastMath.sqrt(getMu() / a) / (1 - e * cosE);
925 final double xDot = -sinE * factor;
926 final double yDot = cosE * s1Me2 * factor;
927
928 final Vector3D position = new Vector3D(x, p, y, q);
929 final Vector3D velocity = new Vector3D(xDot, p, yDot, q);
930 partialPV = new PVCoordinates(position, velocity);
931
932 } else {
933
934
935
936
937 final SinCos scV = FastMath.sinCos(v);
938 final double sinV = scV.sin();
939 final double cosV = scV.cos();
940 final double f = a * (1 - e * e);
941 final double posFactor = f / (1 + e * cosV);
942 final double velFactor = FastMath.sqrt(getMu() / f);
943
944 final double x = posFactor * cosV;
945 final double y = posFactor * sinV;
946 final double xDot = -velFactor * sinV;
947 final double yDot = velFactor * (e + cosV);
948
949 final Vector3D position = new Vector3D(x, p, y, q);
950 final Vector3D velocity = new Vector3D(xDot, p, yDot, q);
951 partialPV = new PVCoordinates(position, velocity);
952
953 }
954
955 }
956
957
958
959
960
961
962
963 private Vector3D nonKeplerianAcceleration() {
964
965 final double[][] dCdP = new double[6][6];
966 getJacobianWrtParameters(PositionAngle.MEAN, dCdP);
967
968 final double nonKeplerianMeanMotion = getMeanAnomalyDot() - getKeplerianMeanMotion();
969 final double nonKeplerianAx = dCdP[3][0] * aDot + dCdP[3][1] * eDot + dCdP[3][2] * iDot +
970 dCdP[3][3] * paDot + dCdP[3][4] * raanDot + dCdP[3][5] * nonKeplerianMeanMotion;
971 final double nonKeplerianAy = dCdP[4][0] * aDot + dCdP[4][1] * eDot + dCdP[4][2] * iDot +
972 dCdP[4][3] * paDot + dCdP[4][4] * raanDot + dCdP[4][5] * nonKeplerianMeanMotion;
973 final double nonKeplerianAz = dCdP[5][0] * aDot + dCdP[5][1] * eDot + dCdP[5][2] * iDot +
974 dCdP[5][3] * paDot + dCdP[5][4] * raanDot + dCdP[5][5] * nonKeplerianMeanMotion;
975
976 return new Vector3D(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);
977
978 }
979
980
981 protected TimeStampedPVCoordinates initPVCoordinates() {
982
983
984 computePVWithoutA();
985
986
987 final double r2 = partialPV.getPosition().getNormSq();
988 final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
989 final Vector3D acceleration = hasDerivatives() ?
990 keplerianAcceleration.add(nonKeplerianAcceleration()) :
991 keplerianAcceleration;
992
993 return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
994
995 }
996
997
998 public KeplerianOrbit shiftedBy(final double dt) {
999
1000
1001 final KeplerianOrbit keplerianShifted = new KeplerianOrbit(a, e, i, pa, raan,
1002 getMeanAnomaly() + getKeplerianMeanMotion() * dt,
1003 PositionAngle.MEAN, getFrame(),
1004 getDate().shiftedBy(dt), getMu());
1005
1006 if (hasDerivatives()) {
1007
1008
1009 final Vector3D nonKeplerianAcceleration = nonKeplerianAcceleration();
1010
1011
1012 keplerianShifted.computePVWithoutA();
1013 final Vector3D fixedP = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
1014 0.5 * dt * dt, nonKeplerianAcceleration);
1015 final double fixedR2 = fixedP.getNormSq();
1016 final double fixedR = FastMath.sqrt(fixedR2);
1017 final Vector3D fixedV = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
1018 dt, nonKeplerianAcceleration);
1019 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
1020 1, nonKeplerianAcceleration);
1021
1022
1023 return new KeplerianOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
1024 fixedP, fixedV, fixedA),
1025 keplerianShifted.getFrame(), keplerianShifted.getMu());
1026
1027 } else {
1028
1029 return keplerianShifted;
1030 }
1031
1032 }
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054 public KeplerianOrbit interpolate(final AbsoluteDate date, final Stream<Orbit> sample) {
1055
1056
1057 final List<Orbit> list = sample.collect(Collectors.toList());
1058 boolean useDerivatives = true;
1059 for (final Orbit orbit : list) {
1060 useDerivatives = useDerivatives && orbit.hasDerivatives();
1061 }
1062
1063
1064 final HermiteInterpolator interpolator = new HermiteInterpolator();
1065
1066
1067 AbsoluteDate previousDate = null;
1068 double previousPA = Double.NaN;
1069 double previousRAAN = Double.NaN;
1070 double previousM = Double.NaN;
1071 for (final Orbit orbit : list) {
1072 final KeplerianOrbit kep = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(orbit);
1073 final double continuousPA;
1074 final double continuousRAAN;
1075 final double continuousM;
1076 if (previousDate == null) {
1077 continuousPA = kep.getPerigeeArgument();
1078 continuousRAAN = kep.getRightAscensionOfAscendingNode();
1079 continuousM = kep.getMeanAnomaly();
1080 } else {
1081 final double dt = kep.getDate().durationFrom(previousDate);
1082 final double keplerM = previousM + kep.getKeplerianMeanMotion() * dt;
1083 continuousPA = MathUtils.normalizeAngle(kep.getPerigeeArgument(), previousPA);
1084 continuousRAAN = MathUtils.normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
1085 continuousM = MathUtils.normalizeAngle(kep.getMeanAnomaly(), keplerM);
1086 }
1087 previousDate = kep.getDate();
1088 previousPA = continuousPA;
1089 previousRAAN = continuousRAAN;
1090 previousM = continuousM;
1091 if (useDerivatives) {
1092 interpolator.addSamplePoint(kep.getDate().durationFrom(date),
1093 new double[] {
1094 kep.getA(),
1095 kep.getE(),
1096 kep.getI(),
1097 continuousPA,
1098 continuousRAAN,
1099 continuousM
1100 }, new double[] {
1101 kep.getADot(),
1102 kep.getEDot(),
1103 kep.getIDot(),
1104 kep.getPerigeeArgumentDot(),
1105 kep.getRightAscensionOfAscendingNodeDot(),
1106 kep.getMeanAnomalyDot()
1107 });
1108 } else {
1109 interpolator.addSamplePoint(kep.getDate().durationFrom(date),
1110 new double[] {
1111 kep.getA(),
1112 kep.getE(),
1113 kep.getI(),
1114 continuousPA,
1115 continuousRAAN,
1116 continuousM
1117 });
1118 }
1119 }
1120
1121
1122 final double[][] interpolated = interpolator.derivatives(0.0, 1);
1123
1124
1125 return new KeplerianOrbit(interpolated[0][0], interpolated[0][1], interpolated[0][2],
1126 interpolated[0][3], interpolated[0][4], interpolated[0][5],
1127 interpolated[1][0], interpolated[1][1], interpolated[1][2],
1128 interpolated[1][3], interpolated[1][4], interpolated[1][5],
1129 PositionAngle.MEAN, getFrame(), date, getMu());
1130
1131 }
1132
1133
1134 protected double[][] computeJacobianMeanWrtCartesian() {
1135 if (a > 0) {
1136 return computeJacobianMeanWrtCartesianElliptical();
1137 } else {
1138 return computeJacobianMeanWrtCartesianHyperbolic();
1139 }
1140 }
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150 private double[][] computeJacobianMeanWrtCartesianElliptical() {
1151
1152 final double[][] jacobian = new double[6][6];
1153
1154
1155 computePVWithoutA();
1156 final Vector3D position = partialPV.getPosition();
1157 final Vector3D velocity = partialPV.getVelocity();
1158 final Vector3D momentum = partialPV.getMomentum();
1159 final double v2 = velocity.getNormSq();
1160 final double r2 = position.getNormSq();
1161 final double r = FastMath.sqrt(r2);
1162 final double r3 = r * r2;
1163
1164 final double px = position.getX();
1165 final double py = position.getY();
1166 final double pz = position.getZ();
1167 final double vx = velocity.getX();
1168 final double vy = velocity.getY();
1169 final double vz = velocity.getZ();
1170 final double mx = momentum.getX();
1171 final double my = momentum.getY();
1172 final double mz = momentum.getZ();
1173
1174 final double mu = getMu();
1175 final double sqrtMuA = FastMath.sqrt(a * mu);
1176 final double sqrtAoMu = FastMath.sqrt(a / mu);
1177 final double a2 = a * a;
1178 final double twoA = 2 * a;
1179 final double rOnA = r / a;
1180
1181 final double oMe2 = 1 - e * e;
1182 final double epsilon = FastMath.sqrt(oMe2);
1183 final double sqrtRec = 1 / epsilon;
1184
1185 final SinCos scI = FastMath.sinCos(i);
1186 final SinCos scPA = FastMath.sinCos(pa);
1187 final double cosI = scI.cos();
1188 final double sinI = scI.sin();
1189 final double cosPA = scPA.cos();
1190 final double sinPA = scPA.sin();
1191
1192 final double pv = Vector3D.dotProduct(position, velocity);
1193 final double cosE = (a - r) / (a * e);
1194 final double sinE = pv / (e * sqrtMuA);
1195
1196
1197 final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
1198 final Vector3D vectorARDot = velocity.scalarMultiply(2 * a2 / mu);
1199 fillHalfRow(1, vectorAR, jacobian[0], 0);
1200 fillHalfRow(1, vectorARDot, jacobian[0], 3);
1201
1202
1203 final double factorER3 = pv / twoA;
1204 final Vector3D vectorER = new Vector3D(cosE * v2 / (r * mu), position,
1205 sinE / sqrtMuA, velocity,
1206 -factorER3 * sinE / sqrtMuA, vectorAR);
1207 final Vector3D vectorERDot = new Vector3D(sinE / sqrtMuA, position,
1208 cosE * 2 * r / mu, velocity,
1209 -factorER3 * sinE / sqrtMuA, vectorARDot);
1210 fillHalfRow(1, vectorER, jacobian[1], 0);
1211 fillHalfRow(1, vectorERDot, jacobian[1], 3);
1212
1213
1214 final double coefE = cosE / (e * sqrtMuA);
1215 final Vector3D vectorEAnR =
1216 new Vector3D(-sinE * v2 / (e * r * mu), position, coefE, velocity,
1217 -factorER3 * coefE, vectorAR);
1218
1219
1220 final Vector3D vectorEAnRDot =
1221 new Vector3D(-sinE * 2 * r / (e * mu), velocity, coefE, position,
1222 -factorER3 * coefE, vectorARDot);
1223
1224
1225 final double s1 = -sinE * pz / r - cosE * vz * sqrtAoMu;
1226 final double s2 = -cosE * pz / r3;
1227 final double s3 = -sinE * vz / (2 * sqrtMuA);
1228 final double t1 = sqrtRec * (cosE * pz / r - sinE * vz * sqrtAoMu);
1229 final double t2 = sqrtRec * (-sinE * pz / r3);
1230 final double t3 = sqrtRec * (cosE - e) * vz / (2 * sqrtMuA);
1231 final double t4 = sqrtRec * (e * sinI * cosPA * sqrtRec - vz * sqrtAoMu);
1232 final Vector3D s = new Vector3D(cosE / r, Vector3D.PLUS_K,
1233 s1, vectorEAnR,
1234 s2, position,
1235 s3, vectorAR);
1236 final Vector3D sDot = new Vector3D(-sinE * sqrtAoMu, Vector3D.PLUS_K,
1237 s1, vectorEAnRDot,
1238 s3, vectorARDot);
1239 final Vector3D t =
1240 new Vector3D(sqrtRec * sinE / r, Vector3D.PLUS_K).add(new Vector3D(t1, vectorEAnR,
1241 t2, position,
1242 t3, vectorAR,
1243 t4, vectorER));
1244 final Vector3D tDot = new Vector3D(sqrtRec * (cosE - e) * sqrtAoMu, Vector3D.PLUS_K,
1245 t1, vectorEAnRDot,
1246 t3, vectorARDot,
1247 t4, vectorERDot);
1248
1249
1250 final double factorI1 = -sinI * sqrtRec / sqrtMuA;
1251 final double i1 = factorI1;
1252 final double i2 = -factorI1 * mz / twoA;
1253 final double i3 = factorI1 * mz * e / oMe2;
1254 final double i4 = cosI * sinPA;
1255 final double i5 = cosI * cosPA;
1256 fillHalfRow(i1, new Vector3D(vy, -vx, 0), i2, vectorAR, i3, vectorER, i4, s, i5, t,
1257 jacobian[2], 0);
1258 fillHalfRow(i1, new Vector3D(-py, px, 0), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
1259 jacobian[2], 3);
1260
1261
1262 fillHalfRow(cosPA / sinI, s, -sinPA / sinI, t, jacobian[3], 0);
1263 fillHalfRow(cosPA / sinI, sDot, -sinPA / sinI, tDot, jacobian[3], 3);
1264
1265
1266 final double factorRaanR = 1 / (mu * a * oMe2 * sinI * sinI);
1267 fillHalfRow(-factorRaanR * my, new Vector3D( 0, vz, -vy),
1268 factorRaanR * mx, new Vector3D(-vz, 0, vx),
1269 jacobian[4], 0);
1270 fillHalfRow(-factorRaanR * my, new Vector3D( 0, -pz, py),
1271 factorRaanR * mx, new Vector3D(pz, 0, -px),
1272 jacobian[4], 3);
1273
1274
1275 fillHalfRow(rOnA, vectorEAnR, -sinE, vectorER, jacobian[5], 0);
1276 fillHalfRow(rOnA, vectorEAnRDot, -sinE, vectorERDot, jacobian[5], 3);
1277
1278 return jacobian;
1279
1280 }
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290 private double[][] computeJacobianMeanWrtCartesianHyperbolic() {
1291
1292 final double[][] jacobian = new double[6][6];
1293
1294
1295 computePVWithoutA();
1296 final Vector3D position = partialPV.getPosition();
1297 final Vector3D velocity = partialPV.getVelocity();
1298 final Vector3D momentum = partialPV.getMomentum();
1299 final double r2 = position.getNormSq();
1300 final double r = FastMath.sqrt(r2);
1301 final double r3 = r * r2;
1302
1303 final double x = position.getX();
1304 final double y = position.getY();
1305 final double z = position.getZ();
1306 final double vx = velocity.getX();
1307 final double vy = velocity.getY();
1308 final double vz = velocity.getZ();
1309 final double mx = momentum.getX();
1310 final double my = momentum.getY();
1311 final double mz = momentum.getZ();
1312
1313 final double mu = getMu();
1314 final double absA = -a;
1315 final double sqrtMuA = FastMath.sqrt(absA * mu);
1316 final double a2 = a * a;
1317 final double rOa = r / absA;
1318
1319 final SinCos scI = FastMath.sinCos(i);
1320 final double cosI = scI.cos();
1321 final double sinI = scI.sin();
1322
1323 final double pv = Vector3D.dotProduct(position, velocity);
1324
1325
1326 final Vector3D vectorAR = new Vector3D(-2 * a2 / r3, position);
1327 final Vector3D vectorARDot = velocity.scalarMultiply(-2 * a2 / mu);
1328 fillHalfRow(-1, vectorAR, jacobian[0], 0);
1329 fillHalfRow(-1, vectorARDot, jacobian[0], 3);
1330
1331
1332 final double m = momentum.getNorm();
1333 final double oOm = 1 / m;
1334 final Vector3D dcXP = new Vector3D( 0, vz, -vy);
1335 final Vector3D dcYP = new Vector3D(-vz, 0, vx);
1336 final Vector3D dcZP = new Vector3D( vy, -vx, 0);
1337 final Vector3D dcXV = new Vector3D( 0, -z, y);
1338 final Vector3D dcYV = new Vector3D( z, 0, -x);
1339 final Vector3D dcZV = new Vector3D( -y, x, 0);
1340 final Vector3D dCP = new Vector3D(mx * oOm, dcXP, my * oOm, dcYP, mz * oOm, dcZP);
1341 final Vector3D dCV = new Vector3D(mx * oOm, dcXV, my * oOm, dcYV, mz * oOm, dcZV);
1342
1343
1344 final double mOMu = m / mu;
1345 final Vector3D dpP = new Vector3D(2 * mOMu, dCP);
1346 final Vector3D dpV = new Vector3D(2 * mOMu, dCV);
1347
1348
1349 final double p = m * mOMu;
1350 final double moO2ae = 1 / (2 * absA * e);
1351 final double m2OaMu = -p / absA;
1352 fillHalfRow(moO2ae, dpP, m2OaMu * moO2ae, vectorAR, jacobian[1], 0);
1353 fillHalfRow(moO2ae, dpV, m2OaMu * moO2ae, vectorARDot, jacobian[1], 3);
1354
1355
1356 final double cI1 = 1 / (m * sinI);
1357 final double cI2 = cosI * cI1;
1358 fillHalfRow(cI2, dCP, -cI1, dcZP, jacobian[2], 0);
1359 fillHalfRow(cI2, dCV, -cI1, dcZV, jacobian[2], 3);
1360
1361
1362 final double cP1 = y * oOm;
1363 final double cP2 = -x * oOm;
1364 final double cP3 = -(mx * cP1 + my * cP2);
1365 final double cP4 = cP3 * oOm;
1366 final double cP5 = -1 / (r2 * sinI * sinI);
1367 final double cP6 = z * cP5;
1368 final double cP7 = cP3 * cP5;
1369 final Vector3D dacP = new Vector3D(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new Vector3D(-my, mx, 0));
1370 final Vector3D dacV = new Vector3D(cP1, dcXV, cP2, dcYV, cP4, dCV);
1371 final Vector3D dpoP = new Vector3D(cP6, dacP, cP7, Vector3D.PLUS_K);
1372 final Vector3D dpoV = new Vector3D(cP6, dacV);
1373
1374 final double re2 = r2 * e * e;
1375 final double recOre2 = (p - r) / re2;
1376 final double resOre2 = (pv * mOMu) / re2;
1377 final Vector3D dreP = new Vector3D(mOMu, velocity, pv / mu, dCP);
1378 final Vector3D dreV = new Vector3D(mOMu, position, pv / mu, dCV);
1379 final Vector3D davP = new Vector3D(-resOre2, dpP, recOre2, dreP, resOre2 / r, position);
1380 final Vector3D davV = new Vector3D(-resOre2, dpV, recOre2, dreV);
1381 fillHalfRow(1, dpoP, -1, davP, jacobian[3], 0);
1382 fillHalfRow(1, dpoV, -1, davV, jacobian[3], 3);
1383
1384
1385 final double cO0 = cI1 * cI1;
1386 final double cO1 = mx * cO0;
1387 final double cO2 = -my * cO0;
1388 fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
1389 fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);
1390
1391
1392 final double s2a = pv / (2 * absA);
1393 final double oObux = 1 / FastMath.sqrt(m * m + mu * absA);
1394 final double scasbu = pv * oObux;
1395 final Vector3D dauP = new Vector3D(1 / sqrtMuA, velocity, -s2a / sqrtMuA, vectorAR);
1396 final Vector3D dauV = new Vector3D(1 / sqrtMuA, position, -s2a / sqrtMuA, vectorARDot);
1397 final Vector3D dbuP = new Vector3D(oObux * mu / 2, vectorAR, m * oObux, dCP);
1398 final Vector3D dbuV = new Vector3D(oObux * mu / 2, vectorARDot, m * oObux, dCV);
1399 final Vector3D dcuP = new Vector3D(oObux, velocity, -scasbu * oObux, dbuP);
1400 final Vector3D dcuV = new Vector3D(oObux, position, -scasbu * oObux, dbuV);
1401 fillHalfRow(1, dauP, -e / (1 + rOa), dcuP, jacobian[5], 0);
1402 fillHalfRow(1, dauV, -e / (1 + rOa), dcuV, jacobian[5], 3);
1403
1404 return jacobian;
1405
1406 }
1407
1408
1409 protected double[][] computeJacobianEccentricWrtCartesian() {
1410 if (a > 0) {
1411 return computeJacobianEccentricWrtCartesianElliptical();
1412 } else {
1413 return computeJacobianEccentricWrtCartesianHyperbolic();
1414 }
1415 }
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425 private double[][] computeJacobianEccentricWrtCartesianElliptical() {
1426
1427
1428 final double[][] jacobian = computeJacobianMeanWrtCartesianElliptical();
1429
1430
1431
1432
1433
1434 final SinCos scE = FastMath.sinCos(getEccentricAnomaly());
1435 final double aOr = 1 / (1 - e * scE.cos());
1436
1437
1438 final double[] eRow = jacobian[1];
1439 final double[] anomalyRow = jacobian[5];
1440 for (int j = 0; j < anomalyRow.length; ++j) {
1441 anomalyRow[j] = aOr * (anomalyRow[j] + scE.sin() * eRow[j]);
1442 }
1443
1444 return jacobian;
1445
1446 }
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456 private double[][] computeJacobianEccentricWrtCartesianHyperbolic() {
1457
1458
1459 final double[][] jacobian = computeJacobianMeanWrtCartesianHyperbolic();
1460
1461
1462
1463
1464
1465 final double H = getEccentricAnomaly();
1466 final double coshH = FastMath.cosh(H);
1467 final double sinhH = FastMath.sinh(H);
1468 final double absaOr = 1 / (e * coshH - 1);
1469
1470
1471 final double[] eRow = jacobian[1];
1472 final double[] anomalyRow = jacobian[5];
1473 for (int j = 0; j < anomalyRow.length; ++j) {
1474 anomalyRow[j] = absaOr * (anomalyRow[j] - sinhH * eRow[j]);
1475 }
1476
1477 return jacobian;
1478
1479 }
1480
1481
1482 protected double[][] computeJacobianTrueWrtCartesian() {
1483 if (a > 0) {
1484 return computeJacobianTrueWrtCartesianElliptical();
1485 } else {
1486 return computeJacobianTrueWrtCartesianHyperbolic();
1487 }
1488 }
1489
1490
1491
1492
1493
1494
1495
1496
1497
1498 private double[][] computeJacobianTrueWrtCartesianElliptical() {
1499
1500
1501 final double[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();
1502
1503
1504
1505
1506
1507
1508 final double e2 = e * e;
1509 final double oMe2 = 1 - e2;
1510 final double epsilon = FastMath.sqrt(oMe2);
1511 final SinCos scE = FastMath.sinCos(getEccentricAnomaly());
1512 final double aOr = 1 / (1 - e * scE.cos());
1513 final double aFactor = epsilon * aOr;
1514 final double eFactor = scE.sin() * aOr / epsilon;
1515
1516
1517 final double[] eRow = jacobian[1];
1518 final double[] anomalyRow = jacobian[5];
1519 for (int j = 0; j < anomalyRow.length; ++j) {
1520 anomalyRow[j] = aFactor * anomalyRow[j] + eFactor * eRow[j];
1521 }
1522
1523 return jacobian;
1524
1525 }
1526
1527
1528
1529
1530
1531
1532
1533
1534
1535 private double[][] computeJacobianTrueWrtCartesianHyperbolic() {
1536
1537
1538 final double[][] jacobian = computeJacobianEccentricWrtCartesianHyperbolic();
1539
1540
1541
1542
1543
1544
1545 final double e2 = e * e;
1546 final double e2Mo = e2 - 1;
1547 final double epsilon = FastMath.sqrt(e2Mo);
1548 final double H = getEccentricAnomaly();
1549 final double coshH = FastMath.cosh(H);
1550 final double sinhH = FastMath.sinh(H);
1551 final double aOr = 1 / (e * coshH - 1);
1552 final double aFactor = epsilon * aOr;
1553 final double eFactor = sinhH * aOr / epsilon;
1554
1555
1556 final double[] eRow = jacobian[1];
1557 final double[] anomalyRow = jacobian[5];
1558 for (int j = 0; j < anomalyRow.length; ++j) {
1559 anomalyRow[j] = aFactor * anomalyRow[j] - eFactor * eRow[j];
1560 }
1561
1562 return jacobian;
1563
1564 }
1565
1566
1567 public void addKeplerContribution(final PositionAngle type, final double gm,
1568 final double[] pDot) {
1569 final double oMe2;
1570 final double ksi;
1571 final double absA = FastMath.abs(a);
1572 final double n = FastMath.sqrt(gm / absA) / absA;
1573 switch (type) {
1574 case MEAN :
1575 pDot[5] += n;
1576 break;
1577 case ECCENTRIC :
1578 oMe2 = FastMath.abs(1 - e * e);
1579 ksi = 1 + e * FastMath.cos(v);
1580 pDot[5] += n * ksi / oMe2;
1581 break;
1582 case TRUE :
1583 oMe2 = FastMath.abs(1 - e * e);
1584 ksi = 1 + e * FastMath.cos(v);
1585 pDot[5] += n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1586 break;
1587 default :
1588 throw new OrekitInternalError(null);
1589 }
1590 }
1591
1592
1593
1594
1595 public String toString() {
1596 return new StringBuffer().append("Keplerian parameters: ").append('{').
1597 append("a: ").append(a).
1598 append("; e: ").append(e).
1599 append("; i: ").append(FastMath.toDegrees(i)).
1600 append("; pa: ").append(FastMath.toDegrees(pa)).
1601 append("; raan: ").append(FastMath.toDegrees(raan)).
1602 append("; v: ").append(FastMath.toDegrees(v)).
1603 append(";}").toString();
1604 }
1605
1606
1607
1608
1609
1610
1611
1612
1613
1614
1615
1616
1617
1618
1619
1620 private void checkParameterRangeInclusive(final String parameterName, final double parameter,
1621 final double lowerBound, final double upperBound) {
1622 if (parameter < lowerBound || parameter > upperBound) {
1623 throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, parameterName,
1624 parameter, lowerBound, upperBound);
1625 }
1626 }
1627
1628
1629
1630
1631 @DefaultDataContext
1632 private Object writeReplace() {
1633 return new DTO(this);
1634 }
1635
1636
1637 @DefaultDataContext
1638 private static class DTO implements Serializable {
1639
1640
1641 private static final long serialVersionUID = 20170414L;
1642
1643
1644 private double[] d;
1645
1646
1647 private final Frame frame;
1648
1649
1650
1651
1652 private DTO(final KeplerianOrbit orbit) {
1653
1654 final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();
1655
1656
1657 final AbsoluteDate j2000Epoch =
1658 DataContext.getDefault().getTimeScales().getJ2000Epoch();
1659 final double epoch = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
1660 final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));
1661
1662 if (orbit.hasDerivatives()) {
1663
1664 this.d = new double[] {
1665 epoch, offset, orbit.getMu(),
1666 orbit.a, orbit.e, orbit.i,
1667 orbit.pa, orbit.raan, orbit.v,
1668 orbit.aDot, orbit.eDot, orbit.iDot,
1669 orbit.paDot, orbit.raanDot, orbit.vDot
1670 };
1671 } else {
1672
1673 this.d = new double[] {
1674 epoch, offset, orbit.getMu(),
1675 orbit.a, orbit.e, orbit.i,
1676 orbit.pa, orbit.raan, orbit.v
1677 };
1678 }
1679
1680 this.frame = orbit.getFrame();
1681
1682 }
1683
1684
1685
1686
1687 private Object readResolve() {
1688 final AbsoluteDate j2000Epoch =
1689 DataContext.getDefault().getTimeScales().getJ2000Epoch();
1690 if (d.length >= 15) {
1691
1692 return new KeplerianOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8],
1693 d[ 9], d[10], d[11], d[12], d[13], d[14],
1694 PositionAngle.TRUE,
1695 frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
1696 d[2]);
1697 } else {
1698
1699 return new KeplerianOrbit(d[3], d[4], d[5], d[6], d[7], d[8], PositionAngle.TRUE,
1700 frame, j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
1701 d[2]);
1702 }
1703 }
1704
1705 }
1706
1707 }