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