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.getNorm2Sq();
277 final double r = FastMath.sqrt(r2);
278 final double V2 = pvV.getNorm2Sq();
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().getNorm2Sq();
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 final PVCoordinates pvCoordinates = shiftNonKeplerian(keplerianShifted.getPVCoordinates(), dtS);
869
870
871 return new EquinoctialOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(), pvCoordinates),
872 keplerianShifted.getFrame(), keplerianShifted.getMu());
873
874 } else {
875
876 return keplerianShifted;
877 }
878
879 }
880
881
882 @Override
883 protected double[][] computeJacobianMeanWrtCartesian() {
884
885 final double[][] jacobian = new double[6][6];
886
887
888 computePVWithoutA();
889 final Vector3D position = partialPV.getPosition();
890 final Vector3D velocity = partialPV.getVelocity();
891 final double r2 = position.getNorm2Sq();
892 final double r = FastMath.sqrt(r2);
893 final double r3 = r * r2;
894
895 final double mu = getMu();
896 final double sqrtMuA = FastMath.sqrt(a * mu);
897 final double a2 = a * a;
898
899 final double e2 = ex * ex + ey * ey;
900 final double oMe2 = 1 - e2;
901 final double epsilon = FastMath.sqrt(oMe2);
902 final double beta = 1 / (1 + epsilon);
903 final double ratio = epsilon * beta;
904
905 final double hx2 = hx * hx;
906 final double hy2 = hy * hy;
907 final double hxhy = hx * hy;
908
909
910 final Vector3D f = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
911 final Vector3D g = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
912 final Vector3D w = Vector3D.crossProduct(position, velocity).normalize();
913
914
915 final double x = Vector3D.dotProduct(position, f);
916 final double y = Vector3D.dotProduct(position, g);
917 final double xDot = Vector3D.dotProduct(velocity, f);
918 final double yDot = Vector3D.dotProduct(velocity, g);
919
920
921 final double c1 = a / (sqrtMuA * epsilon);
922 final double c2 = a * sqrtMuA * beta / r3;
923 final double c3 = sqrtMuA / (r3 * epsilon);
924 final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
925 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);
926
927
928 final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
929 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);
930
931
932 final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
933 final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
934 fillHalfRow(1, vectorAR, jacobian[0], 0);
935 fillHalfRow(1, vectorARDot, jacobian[0], 3);
936
937
938 final double d1 = -a * ratio / r3;
939 final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
940 final double d3 = (hx * y - hy * x) / sqrtMuA;
941 final Vector3D vectorExRDot =
942 new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
943 fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
944 fillHalfRow(1, vectorExRDot, jacobian[1], 3);
945
946
947 final Vector3D vectorEyRDot =
948 new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
949 fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
950 fillHalfRow(1, vectorEyRDot, jacobian[2], 3);
951
952
953 final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
954 fillHalfRow(-h * xDot, w, jacobian[3], 0);
955 fillHalfRow( h * x, w, jacobian[3], 3);
956
957
958 fillHalfRow(-h * yDot, w, jacobian[4], 0);
959 fillHalfRow( h * y, w, jacobian[4], 3);
960
961
962 final double l = -ratio / sqrtMuA;
963 fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
964 fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);
965
966 return jacobian;
967
968 }
969
970
971 @Override
972 protected double[][] computeJacobianEccentricWrtCartesian() {
973
974
975 final double[][] jacobian = computeJacobianMeanWrtCartesian();
976
977
978
979
980
981 final SinCos scLe = FastMath.sinCos(getLE());
982 final double cosLe = scLe.cos();
983 final double sinLe = scLe.sin();
984 final double aOr = 1 / (1 - ex * cosLe - ey * sinLe);
985
986
987 final double[] rowEx = jacobian[1];
988 final double[] rowEy = jacobian[2];
989 final double[] rowL = jacobian[5];
990 for (int j = 0; j < 6; ++j) {
991 rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
992 }
993
994 return jacobian;
995
996 }
997
998
999 @Override
1000 protected double[][] computeJacobianTrueWrtCartesian() {
1001
1002
1003 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017 final SinCos scLe = FastMath.sinCos(getLE());
1018 final double cosLe = scLe.cos();
1019 final double sinLe = scLe.sin();
1020 final double eSinE = ex * sinLe - ey * cosLe;
1021 final double ecosE = ex * cosLe + ey * sinLe;
1022 final double e2 = ex * ex + ey * ey;
1023 final double epsilon = FastMath.sqrt(1 - e2);
1024 final double onePeps = 1 + epsilon;
1025 final double d = onePeps - ecosE;
1026 final double cT = (d * d + eSinE * eSinE) / 2;
1027 final double cE = ecosE * onePeps - e2;
1028 final double cX = ex * eSinE / epsilon - ey + sinLe * onePeps;
1029 final double cY = ey * eSinE / epsilon + ex - cosLe * onePeps;
1030 final double factorLe = (cT + cE) / cT;
1031 final double factorEx = cX / cT;
1032 final double factorEy = cY / cT;
1033
1034
1035 final double[] rowEx = jacobian[1];
1036 final double[] rowEy = jacobian[2];
1037 final double[] rowL = jacobian[5];
1038 for (int j = 0; j < 6; ++j) {
1039 rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
1040 }
1041
1042 return jacobian;
1043
1044 }
1045
1046
1047 @Override
1048 public void addKeplerContribution(final PositionAngleType type, final double gm,
1049 final double[] pDot) {
1050 pDot[5] += computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType);
1051 }
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065 private static double computeKeplerianLDot(final PositionAngleType type, final double a, final double ex,
1066 final double ey, final double mu,
1067 final double l, final PositionAngleType cachedType) {
1068 final double n = FastMath.sqrt(mu / a) / a;
1069 if (type == PositionAngleType.MEAN) {
1070 return n;
1071 }
1072 final double oMe2;
1073 final double ksi;
1074 final SinCos sc;
1075 if (type == PositionAngleType.ECCENTRIC) {
1076 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1077 ksi = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1078 return n * ksi;
1079 } else {
1080 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1081 oMe2 = 1 - ex * ex - ey * ey;
1082 ksi = 1 + ex * sc.cos() + ey * sc.sin();
1083 return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1084 }
1085 }
1086
1087
1088
1089
1090 public String toString() {
1091 return new StringBuilder().append("equinoctial parameters: ").append('{').
1092 append("a: ").append(a).
1093 append("; ex: ").append(ex).append("; ey: ").append(ey).
1094 append("; hx: ").append(hx).append("; hy: ").append(hy).
1095 append("; lv: ").append(FastMath.toDegrees(getLv())).
1096 append(";}").toString();
1097 }
1098
1099
1100 @Override
1101 public PositionAngleType getCachedPositionAngleType() {
1102 return cachedPositionAngleType;
1103 }
1104
1105
1106 @Override
1107 public boolean hasNonKeplerianRates() {
1108 return hasNonKeplerianAcceleration();
1109 }
1110
1111
1112 @Override
1113 public EquinoctialOrbit withKeplerianRates() {
1114 final PositionAngleType positionAngleType = getCachedPositionAngleType();
1115 return new EquinoctialOrbit(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
1116 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1117 }
1118
1119 }