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 EquinoctialOrbit extends Orbit implements PositionAngleBased<EquinoctialOrbit> {
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 hx;
86
87
88 private final double hy;
89
90
91 private final double cachedL;
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 hxDot;
107
108
109 private final double hyDot;
110
111
112 private final double cachedLDot;
113
114
115 private PVCoordinates partialPV;
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134 public EquinoctialOrbit(final double a, final double ex, final double ey,
135 final double hx, final double hy, final double l,
136 final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
137 final Frame frame, final AbsoluteDate date, final double mu)
138 throws IllegalArgumentException {
139 this(a, ex, ey, hx, hy, l,
140 0., 0., 0., 0., 0., computeKeplerianLDot(type, a, ex, ey, mu, l, type),
141 type, cachedPositionAngleType, frame, date, mu);
142 }
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159 public EquinoctialOrbit(final double a, final double ex, final double ey,
160 final double hx, final double hy, final double l,
161 final PositionAngleType type,
162 final Frame frame, final AbsoluteDate date, final double mu)
163 throws IllegalArgumentException {
164 this(a, ex, ey, hx, hy, l, type, type, frame, date, mu);
165 }
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190 public EquinoctialOrbit(final double a, final double ex, final double ey,
191 final double hx, final double hy, final double l,
192 final double aDot, final double exDot, final double eyDot,
193 final double hxDot, final double hyDot, final double lDot,
194 final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
195 final Frame frame, final AbsoluteDate date, final double mu)
196 throws IllegalArgumentException {
197 super(frame, date, mu);
198 if (ex * ex + ey * ey >= 1.0) {
199 throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
200 getClass().getName());
201 }
202 this.cachedPositionAngleType = cachedPositionAngleType;
203 this.a = a;
204 this.aDot = aDot;
205 this.ex = ex;
206 this.exDot = exDot;
207 this.ey = ey;
208 this.eyDot = eyDot;
209 this.hx = hx;
210 this.hxDot = hxDot;
211 this.hy = hy;
212 this.hyDot = hyDot;
213
214 final UnivariateDerivative1 lUD = initializeCachedL(l, lDot, type);
215 this.cachedL = lUD.getValue();
216 this.cachedLDot = lUD.getFirstDerivative();
217
218 this.partialPV = null;
219
220 }
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243 public EquinoctialOrbit(final double a, final double ex, final double ey,
244 final double hx, final double hy, final double l,
245 final double aDot, final double exDot, final double eyDot,
246 final double hxDot, final double hyDot, final double lDot,
247 final PositionAngleType type,
248 final Frame frame, final AbsoluteDate date, final double mu)
249 throws IllegalArgumentException {
250 this(a, ex, ey, hx, hy, l, aDot, exDot, eyDot, hxDot, hyDot, lDot, type, type, frame, date, mu);
251 }
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267 public EquinoctialOrbit(final TimeStampedPVCoordinates pvCoordinates,
268 final Frame frame, final double mu)
269 throws IllegalArgumentException {
270 super(pvCoordinates, frame, mu);
271
272
273 final Vector3D pvP = pvCoordinates.getPosition();
274 final Vector3D pvV = pvCoordinates.getVelocity();
275 final Vector3D pvA = pvCoordinates.getAcceleration();
276 final double r2 = pvP.getNormSq();
277 final double r = FastMath.sqrt(r2);
278 final double V2 = pvV.getNormSq();
279 final double rV2OnMu = r * V2 / mu;
280
281
282 a = r / (2 - rV2OnMu);
283
284 if (!isElliptical()) {
285 throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
286 getClass().getName());
287 }
288
289
290 final Vector3D w = pvCoordinates.getMomentum().normalize();
291 final double d = 1.0 / (1 + w.getZ());
292 hx = -d * w.getY();
293 hy = d * w.getX();
294
295
296 cachedPositionAngleType = PositionAngleType.TRUE;
297 final double cLv = (pvP.getX() - d * pvP.getZ() * w.getX()) / r;
298 final double sLv = (pvP.getY() - d * pvP.getZ() * w.getY()) / r;
299 cachedL = FastMath.atan2(sLv, cLv);
300
301
302 final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a);
303 final double eCE = rV2OnMu - 1;
304 final double e2 = eCE * eCE + eSE * eSE;
305 final double f = eCE - e2;
306 final double g = FastMath.sqrt(1 - e2) * eSE;
307 ex = a * (f * cLv + g * sLv) / r;
308 ey = a * (f * sLv - g * cLv) / r;
309
310 partialPV = pvCoordinates;
311
312 if (hasNonKeplerianAcceleration(pvCoordinates, mu)) {
313
314
315 final double[][] jacobian = new double[6][6];
316 getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
317
318 final Vector3D keplerianAcceleration = new Vector3D(-mu / (r * r2), pvP);
319 final Vector3D nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
320 final double aX = nonKeplerianAcceleration.getX();
321 final double aY = nonKeplerianAcceleration.getY();
322 final double aZ = nonKeplerianAcceleration.getZ();
323 aDot = jacobian[0][3] * aX + jacobian[0][4] * aY + jacobian[0][5] * aZ;
324 exDot = jacobian[1][3] * aX + jacobian[1][4] * aY + jacobian[1][5] * aZ;
325 eyDot = jacobian[2][3] * aX + jacobian[2][4] * aY + jacobian[2][5] * aZ;
326 hxDot = jacobian[3][3] * aX + jacobian[3][4] * aY + jacobian[3][5] * aZ;
327 hyDot = jacobian[4][3] * aX + jacobian[4][4] * aY + jacobian[4][5] * aZ;
328
329
330
331 final double lMDot = getKeplerianMeanMotion() +
332 jacobian[5][3] * aX + jacobian[5][4] * aY + jacobian[5][5] * aZ;
333 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
334 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
335 final UnivariateDerivative1 lMUD = new UnivariateDerivative1(getLM(), lMDot);
336 final UnivariateDerivative1 lvUD = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lMUD);
337 cachedLDot = lvUD.getFirstDerivative();
338
339 } else {
340
341
342
343 aDot = 0.;
344 exDot = 0.;
345 eyDot = 0.;
346 hxDot = 0.;
347 hyDot = 0.;
348 cachedLDot = computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, mu, cachedL, cachedPositionAngleType);
349 }
350
351 }
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368 public EquinoctialOrbit(final PVCoordinates pvCoordinates, final Frame frame,
369 final AbsoluteDate date, final double mu)
370 throws IllegalArgumentException {
371 this(new TimeStampedPVCoordinates(date, pvCoordinates), frame, mu);
372 }
373
374
375
376
377 public EquinoctialOrbit(final Orbit op) {
378 super(op.getFrame(), op.getDate(), op.getMu());
379 a = op.getA();
380 aDot = op.getADot();
381 ex = op.getEquinoctialEx();
382 exDot = op.getEquinoctialExDot();
383 ey = op.getEquinoctialEy();
384 eyDot = op.getEquinoctialEyDot();
385 hx = op.getHx();
386 hxDot = op.getHxDot();
387 hy = op.getHy();
388 hyDot = op.getHyDot();
389 cachedPositionAngleType = PositionAngleType.TRUE;
390 cachedL = op.getLv();
391 cachedLDot = op.hasNonKeplerianAcceleration() ? op.getLvDot() :
392 computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, op.getMu(), cachedL, cachedPositionAngleType);
393 partialPV = null;
394 }
395
396
397 @Override
398 public boolean hasNonKeplerianAcceleration() {
399 return aDot != 0. || exDot != 0. || eyDot != 0. || hxDot != 0. || hyDot != 0. ||
400 FastMath.abs(cachedLDot - computeKeplerianLDot(cachedPositionAngleType, a, ex, ey, getMu(), cachedL, cachedPositionAngleType)) > TOLERANCE_POSITION_ANGLE_RATE;
401 }
402
403
404 @Override
405 public OrbitType getType() {
406 return OrbitType.EQUINOCTIAL;
407 }
408
409
410 @Override
411 public double getA() {
412 return a;
413 }
414
415
416 @Override
417 public double getADot() {
418 return aDot;
419 }
420
421
422 @Override
423 public double getEquinoctialEx() {
424 return ex;
425 }
426
427
428 @Override
429 public double getEquinoctialExDot() {
430 return exDot;
431 }
432
433
434 @Override
435 public double getEquinoctialEy() {
436 return ey;
437 }
438
439
440 @Override
441 public double getEquinoctialEyDot() {
442 return eyDot;
443 }
444
445
446 @Override
447 public double getHx() {
448 return hx;
449 }
450
451
452 @Override
453 public double getHxDot() {
454 return hxDot;
455 }
456
457
458 @Override
459 public double getHy() {
460 return hy;
461 }
462
463
464 @Override
465 public double getHyDot() {
466 return hyDot;
467 }
468
469
470 @Override
471 public double getLv() {
472 switch (cachedPositionAngleType) {
473 case TRUE:
474 return cachedL;
475
476 case ECCENTRIC:
477 return EquinoctialLongitudeArgumentUtility.eccentricToTrue(ex, ey, cachedL);
478
479 case MEAN:
480 return EquinoctialLongitudeArgumentUtility.meanToTrue(ex, ey, cachedL);
481
482 default:
483 throw new OrekitInternalError(null);
484 }
485 }
486
487
488 @Override
489 public double getLvDot() {
490 switch (cachedPositionAngleType) {
491 case ECCENTRIC:
492 final UnivariateDerivative1 lEUD = new UnivariateDerivative1(cachedL, cachedLDot);
493 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
494 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
495 final UnivariateDerivative1 lvUD = FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
496 lEUD);
497 return lvUD.getFirstDerivative();
498
499 case TRUE:
500 return cachedLDot;
501
502 case MEAN:
503 final UnivariateDerivative1 lMUD = new UnivariateDerivative1(cachedL, cachedLDot);
504 final UnivariateDerivative1 exUD2 = new UnivariateDerivative1(ex, exDot);
505 final UnivariateDerivative1 eyUD2 = new UnivariateDerivative1(ey, eyDot);
506 final UnivariateDerivative1 lvUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD2,
507 eyUD2, lMUD);
508 return lvUD2.getFirstDerivative();
509
510 default:
511 throw new OrekitInternalError(null);
512 }
513 }
514
515
516 @Override
517 public double getLE() {
518 switch (cachedPositionAngleType) {
519 case TRUE:
520 return EquinoctialLongitudeArgumentUtility.trueToEccentric(ex, ey, cachedL);
521
522 case ECCENTRIC:
523 return cachedL;
524
525 case MEAN:
526 return EquinoctialLongitudeArgumentUtility.meanToEccentric(ex, ey, cachedL);
527
528 default:
529 throw new OrekitInternalError(null);
530 }
531 }
532
533
534 @Override
535 public double getLEDot() {
536 switch (cachedPositionAngleType) {
537 case TRUE:
538 final UnivariateDerivative1 lvUD = new UnivariateDerivative1(cachedL, cachedLDot);
539 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
540 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
541 final UnivariateDerivative1 lEUD = FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD,
542 lvUD);
543 return lEUD.getFirstDerivative();
544
545 case ECCENTRIC:
546 return cachedLDot;
547
548 case MEAN:
549 final UnivariateDerivative1 lMUD = new UnivariateDerivative1(cachedL, cachedLDot);
550 final UnivariateDerivative1 exUD2 = new UnivariateDerivative1(ex, exDot);
551 final UnivariateDerivative1 eyUD2 = new UnivariateDerivative1(ey, eyDot);
552 final UnivariateDerivative1 lEUD2 = FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD2,
553 eyUD2, lMUD);
554 return lEUD2.getFirstDerivative();
555
556 default:
557 throw new OrekitInternalError(null);
558 }
559 }
560
561
562 @Override
563 public double getLM() {
564 switch (cachedPositionAngleType) {
565 case TRUE:
566 return EquinoctialLongitudeArgumentUtility.trueToMean(ex, ey, cachedL);
567
568 case MEAN:
569 return cachedL;
570
571 case ECCENTRIC:
572 return EquinoctialLongitudeArgumentUtility.eccentricToMean(ex, ey, cachedL);
573
574 default:
575 throw new OrekitInternalError(null);
576 }
577 }
578
579
580 @Override
581 public double getLMDot() {
582 switch (cachedPositionAngleType) {
583 case TRUE:
584 final UnivariateDerivative1 lvUD = new UnivariateDerivative1(cachedL, cachedLDot);
585 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
586 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
587 final UnivariateDerivative1 lMUD = FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lvUD);
588 return lMUD.getFirstDerivative();
589
590 case MEAN:
591 return cachedLDot;
592
593 case ECCENTRIC:
594 final UnivariateDerivative1 lEUD = new UnivariateDerivative1(cachedL, cachedLDot);
595 final UnivariateDerivative1 exUD2 = new UnivariateDerivative1(ex, exDot);
596 final UnivariateDerivative1 eyUD2 = new UnivariateDerivative1(ey, eyDot);
597 final UnivariateDerivative1 lMUD2 = FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD2,
598 eyUD2, lEUD);
599 return lMUD2.getFirstDerivative();
600
601 default:
602 throw new OrekitInternalError(null);
603 }
604 }
605
606
607
608
609
610 public double getL(final PositionAngleType type) {
611 return (type == PositionAngleType.MEAN) ? getLM() :
612 ((type == PositionAngleType.ECCENTRIC) ? getLE() :
613 getLv());
614 }
615
616
617
618
619
620 public double getLDot(final PositionAngleType type) {
621 return (type == PositionAngleType.MEAN) ? getLMDot() :
622 ((type == PositionAngleType.ECCENTRIC) ? getLEDot() :
623 getLvDot());
624 }
625
626
627 @Override
628 public double getE() {
629 return FastMath.sqrt(ex * ex + ey * ey);
630 }
631
632
633 @Override
634 public double getEDot() {
635 if (!hasNonKeplerianAcceleration()) {
636 return 0.;
637 }
638 return (ex * exDot + ey * eyDot) / FastMath.sqrt(ex * ex + ey * ey);
639 }
640
641
642 @Override
643 public double getI() {
644 return 2 * FastMath.atan(FastMath.sqrt(hx * hx + hy * hy));
645 }
646
647
648 @Override
649 public double getIDot() {
650 if (!hasNonKeplerianAcceleration()) {
651 return 0.;
652 }
653 final double h2 = hx * hx + hy * hy;
654 final double h = FastMath.sqrt(h2);
655 return 2 * (hx * hxDot + hy * hyDot) / (h * (1 + h2));
656 }
657
658
659
660 private void computePVWithoutA() {
661
662 if (partialPV != null) {
663
664 return;
665 }
666
667
668 final double lE = getLE();
669
670
671 final double hx2 = hx * hx;
672 final double hy2 = hy * hy;
673 final double factH = 1. / (1 + hx2 + hy2);
674
675
676 final double ux = (1 + hx2 - hy2) * factH;
677 final double uy = 2 * hx * hy * factH;
678 final double uz = -2 * hy * factH;
679
680 final double vx = uy;
681 final double vy = (1 - hx2 + hy2) * factH;
682 final double vz = 2 * hx * factH;
683
684
685 final double exey = ex * ey;
686 final double ex2 = ex * ex;
687 final double ey2 = ey * ey;
688 final double e2 = ex2 + ey2;
689 final double eta = 1 + FastMath.sqrt(1 - e2);
690 final double beta = 1. / eta;
691
692
693 final SinCos scLe = FastMath.sinCos(lE);
694 final double cLe = scLe.cos();
695 final double sLe = scLe.sin();
696 final double exCeyS = ex * cLe + ey * sLe;
697
698
699 final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
700 final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);
701
702 final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
703 final double xdot = factor * (-sLe + beta * ey * exCeyS);
704 final double ydot = factor * ( cLe - beta * ex * exCeyS);
705
706 final Vector3D position =
707 new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
708 final Vector3D velocity =
709 new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz);
710 partialPV = new PVCoordinates(position, velocity);
711
712 }
713
714
715
716
717
718
719
720
721 private UnivariateDerivative1 initializeCachedL(final double l, final double lDot,
722 final PositionAngleType inputType) {
723 if (cachedPositionAngleType == inputType) {
724 return new UnivariateDerivative1(l, lDot);
725
726 } else {
727 final UnivariateDerivative1 exUD = new UnivariateDerivative1(ex, exDot);
728 final UnivariateDerivative1 eyUD = new UnivariateDerivative1(ey, eyDot);
729 final UnivariateDerivative1 lUD = new UnivariateDerivative1(l, lDot);
730
731 switch (cachedPositionAngleType) {
732
733 case ECCENTRIC:
734 if (inputType == PositionAngleType.MEAN) {
735 return FieldEquinoctialLongitudeArgumentUtility.meanToEccentric(exUD, eyUD, lUD);
736 } else {
737 return FieldEquinoctialLongitudeArgumentUtility.trueToEccentric(exUD, eyUD, lUD);
738 }
739
740 case TRUE:
741 if (inputType == PositionAngleType.MEAN) {
742 return FieldEquinoctialLongitudeArgumentUtility.meanToTrue(exUD, eyUD, lUD);
743 } else {
744 return FieldEquinoctialLongitudeArgumentUtility.eccentricToTrue(exUD, eyUD, lUD);
745 }
746
747 case MEAN:
748 if (inputType == PositionAngleType.TRUE) {
749 return FieldEquinoctialLongitudeArgumentUtility.trueToMean(exUD, eyUD, lUD);
750 } else {
751 return FieldEquinoctialLongitudeArgumentUtility.eccentricToMean(exUD, eyUD, lUD);
752 }
753
754 default:
755 throw new OrekitInternalError(null);
756
757 }
758
759 }
760
761 }
762
763
764 @Override
765 protected Vector3D initPosition() {
766
767
768 final double lE = getLE();
769
770
771 final double hx2 = hx * hx;
772 final double hy2 = hy * hy;
773 final double factH = 1. / (1 + hx2 + hy2);
774
775
776 final double ux = (1 + hx2 - hy2) * factH;
777 final double uy = 2 * hx * hy * factH;
778 final double uz = -2 * hy * factH;
779
780 final double vx = uy;
781 final double vy = (1 - hx2 + hy2) * factH;
782 final double vz = 2 * hx * factH;
783
784
785 final double exey = ex * ey;
786 final double ex2 = ex * ex;
787 final double ey2 = ey * ey;
788 final double e2 = ex2 + ey2;
789 final double eta = 1 + FastMath.sqrt(1 - e2);
790 final double beta = 1. / eta;
791
792
793 final SinCos scLe = FastMath.sinCos(lE);
794 final double cLe = scLe.cos();
795 final double sLe = scLe.sin();
796
797
798 final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
799 final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);
800
801 return new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
802
803 }
804
805
806 @Override
807 protected TimeStampedPVCoordinates initPVCoordinates() {
808
809
810 computePVWithoutA();
811
812
813 final double r2 = partialPV.getPosition().getNormSq();
814 final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
815 final Vector3D acceleration = hasNonKeplerianRates() ?
816 keplerianAcceleration.add(nonKeplerianAcceleration()) :
817 keplerianAcceleration;
818
819 return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
820
821 }
822
823
824 @Override
825 public EquinoctialOrbit inFrame(final Frame inertialFrame) {
826 final PVCoordinates pvCoordinates;
827 if (hasNonKeplerianAcceleration()) {
828 pvCoordinates = getPVCoordinates(inertialFrame);
829 } else {
830 final KinematicTransform transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
831 pvCoordinates = transform.transformOnlyPV(getPVCoordinates());
832 }
833 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(pvCoordinates, inertialFrame, getDate(), getMu());
834 if (equinoctialOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
835 return equinoctialOrbit;
836 } else {
837 return equinoctialOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
838 }
839 }
840
841
842 @Override
843 public EquinoctialOrbit withCachedPositionAngleType(final PositionAngleType positionAngleType) {
844 return new EquinoctialOrbit(a, ex, ey, hx, hy, getL(positionAngleType), aDot, exDot, eyDot, hxDot, hyDot,
845 getLDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
846 }
847
848
849 @Override
850 public EquinoctialOrbit shiftedBy(final double dt) {
851 return shiftedBy(new TimeOffset(dt));
852 }
853
854
855 @Override
856 public EquinoctialOrbit shiftedBy(final TimeOffset dt) {
857
858 final double dtS = dt.toDouble();
859
860
861 final EquinoctialOrbit keplerianShifted = new EquinoctialOrbit(a, ex, ey, hx, hy,
862 getLM() + getKeplerianMeanMotion() * dtS,
863 PositionAngleType.MEAN, cachedPositionAngleType,
864 getFrame(),
865 getDate().shiftedBy(dt), getMu());
866
867 if (dtS != 0. && hasNonKeplerianRates()) {
868
869
870 final Vector3D nonKeplerianAcceleration = nonKeplerianAcceleration();
871
872
873 keplerianShifted.computePVWithoutA();
874 final Vector3D fixedP = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
875 0.5 * dtS * dtS, nonKeplerianAcceleration);
876 final double fixedR2 = fixedP.getNormSq();
877 final double fixedR = FastMath.sqrt(fixedR2);
878 final Vector3D fixedV = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
879 dtS, nonKeplerianAcceleration);
880 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
881 1, nonKeplerianAcceleration);
882
883
884 return new EquinoctialOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
885 fixedP, fixedV, fixedA),
886 keplerianShifted.getFrame(), keplerianShifted.getMu());
887
888 } else {
889
890 return keplerianShifted;
891 }
892
893 }
894
895
896 @Override
897 protected double[][] computeJacobianMeanWrtCartesian() {
898
899 final double[][] jacobian = new double[6][6];
900
901
902 computePVWithoutA();
903 final Vector3D position = partialPV.getPosition();
904 final Vector3D velocity = partialPV.getVelocity();
905 final double r2 = position.getNormSq();
906 final double r = FastMath.sqrt(r2);
907 final double r3 = r * r2;
908
909 final double mu = getMu();
910 final double sqrtMuA = FastMath.sqrt(a * mu);
911 final double a2 = a * a;
912
913 final double e2 = ex * ex + ey * ey;
914 final double oMe2 = 1 - e2;
915 final double epsilon = FastMath.sqrt(oMe2);
916 final double beta = 1 / (1 + epsilon);
917 final double ratio = epsilon * beta;
918
919 final double hx2 = hx * hx;
920 final double hy2 = hy * hy;
921 final double hxhy = hx * hy;
922
923
924 final Vector3D f = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
925 final Vector3D g = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
926 final Vector3D w = Vector3D.crossProduct(position, velocity).normalize();
927
928
929 final double x = Vector3D.dotProduct(position, f);
930 final double y = Vector3D.dotProduct(position, g);
931 final double xDot = Vector3D.dotProduct(velocity, f);
932 final double yDot = Vector3D.dotProduct(velocity, g);
933
934
935 final double c1 = a / (sqrtMuA * epsilon);
936 final double c2 = a * sqrtMuA * beta / r3;
937 final double c3 = sqrtMuA / (r3 * epsilon);
938 final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
939 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);
940
941
942 final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
943 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);
944
945
946 final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
947 final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
948 fillHalfRow(1, vectorAR, jacobian[0], 0);
949 fillHalfRow(1, vectorARDot, jacobian[0], 3);
950
951
952 final double d1 = -a * ratio / r3;
953 final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
954 final double d3 = (hx * y - hy * x) / sqrtMuA;
955 final Vector3D vectorExRDot =
956 new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
957 fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
958 fillHalfRow(1, vectorExRDot, jacobian[1], 3);
959
960
961 final Vector3D vectorEyRDot =
962 new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
963 fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
964 fillHalfRow(1, vectorEyRDot, jacobian[2], 3);
965
966
967 final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
968 fillHalfRow(-h * xDot, w, jacobian[3], 0);
969 fillHalfRow( h * x, w, jacobian[3], 3);
970
971
972 fillHalfRow(-h * yDot, w, jacobian[4], 0);
973 fillHalfRow( h * y, w, jacobian[4], 3);
974
975
976 final double l = -ratio / sqrtMuA;
977 fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
978 fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);
979
980 return jacobian;
981
982 }
983
984
985 @Override
986 protected double[][] computeJacobianEccentricWrtCartesian() {
987
988
989 final double[][] jacobian = computeJacobianMeanWrtCartesian();
990
991
992
993
994
995 final SinCos scLe = FastMath.sinCos(getLE());
996 final double cosLe = scLe.cos();
997 final double sinLe = scLe.sin();
998 final double aOr = 1 / (1 - ex * cosLe - ey * sinLe);
999
1000
1001 final double[] rowEx = jacobian[1];
1002 final double[] rowEy = jacobian[2];
1003 final double[] rowL = jacobian[5];
1004 for (int j = 0; j < 6; ++j) {
1005 rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
1006 }
1007
1008 return jacobian;
1009
1010 }
1011
1012
1013 @Override
1014 protected double[][] computeJacobianTrueWrtCartesian() {
1015
1016
1017 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031 final SinCos scLe = FastMath.sinCos(getLE());
1032 final double cosLe = scLe.cos();
1033 final double sinLe = scLe.sin();
1034 final double eSinE = ex * sinLe - ey * cosLe;
1035 final double ecosE = ex * cosLe + ey * sinLe;
1036 final double e2 = ex * ex + ey * ey;
1037 final double epsilon = FastMath.sqrt(1 - e2);
1038 final double onePeps = 1 + epsilon;
1039 final double d = onePeps - ecosE;
1040 final double cT = (d * d + eSinE * eSinE) / 2;
1041 final double cE = ecosE * onePeps - e2;
1042 final double cX = ex * eSinE / epsilon - ey + sinLe * onePeps;
1043 final double cY = ey * eSinE / epsilon + ex - cosLe * onePeps;
1044 final double factorLe = (cT + cE) / cT;
1045 final double factorEx = cX / cT;
1046 final double factorEy = cY / cT;
1047
1048
1049 final double[] rowEx = jacobian[1];
1050 final double[] rowEy = jacobian[2];
1051 final double[] rowL = jacobian[5];
1052 for (int j = 0; j < 6; ++j) {
1053 rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
1054 }
1055
1056 return jacobian;
1057
1058 }
1059
1060
1061 @Override
1062 public void addKeplerContribution(final PositionAngleType type, final double gm,
1063 final double[] pDot) {
1064 pDot[5] += computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType);
1065 }
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079 private static double computeKeplerianLDot(final PositionAngleType type, final double a, final double ex,
1080 final double ey, final double mu,
1081 final double l, final PositionAngleType cachedType) {
1082 final double n = FastMath.sqrt(mu / a) / a;
1083 if (type == PositionAngleType.MEAN) {
1084 return n;
1085 }
1086 final double oMe2;
1087 final double ksi;
1088 final SinCos sc;
1089 if (type == PositionAngleType.ECCENTRIC) {
1090 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1091 ksi = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1092 return n * ksi;
1093 } else {
1094 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1095 oMe2 = 1 - ex * ex - ey * ey;
1096 ksi = 1 + ex * sc.cos() + ey * sc.sin();
1097 return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1098 }
1099 }
1100
1101
1102
1103
1104 public String toString() {
1105 return new StringBuilder().append("equinoctial parameters: ").append('{').
1106 append("a: ").append(a).
1107 append("; ex: ").append(ex).append("; ey: ").append(ey).
1108 append("; hx: ").append(hx).append("; hy: ").append(hy).
1109 append("; lv: ").append(FastMath.toDegrees(getLv())).
1110 append(";}").toString();
1111 }
1112
1113
1114 @Override
1115 public PositionAngleType getCachedPositionAngleType() {
1116 return cachedPositionAngleType;
1117 }
1118
1119
1120 @Override
1121 public boolean hasNonKeplerianRates() {
1122 return hasNonKeplerianAcceleration();
1123 }
1124
1125
1126 @Override
1127 public EquinoctialOrbit withKeplerianRates() {
1128 final PositionAngleType positionAngleType = getCachedPositionAngleType();
1129 return new EquinoctialOrbit(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
1130 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1131 }
1132
1133 }