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 transient 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
765
766 private Vector3D nonKeplerianAcceleration() {
767
768 final double[][] dCdP = new double[6][6];
769 getJacobianWrtParameters(PositionAngleType.MEAN, dCdP);
770
771 final double nonKeplerianMeanMotion = getLMDot() - getKeplerianMeanMotion();
772 final double nonKeplerianAx = dCdP[3][0] * aDot + dCdP[3][1] * exDot + dCdP[3][2] * eyDot +
773 dCdP[3][3] * hxDot + dCdP[3][4] * hyDot + dCdP[3][5] * nonKeplerianMeanMotion;
774 final double nonKeplerianAy = dCdP[4][0] * aDot + dCdP[4][1] * exDot + dCdP[4][2] * eyDot +
775 dCdP[4][3] * hxDot + dCdP[4][4] * hyDot + dCdP[4][5] * nonKeplerianMeanMotion;
776 final double nonKeplerianAz = dCdP[5][0] * aDot + dCdP[5][1] * exDot + dCdP[5][2] * eyDot +
777 dCdP[5][3] * hxDot + dCdP[5][4] * hyDot + dCdP[5][5] * nonKeplerianMeanMotion;
778
779 return new Vector3D(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);
780
781 }
782
783
784 @Override
785 protected Vector3D initPosition() {
786
787
788 final double lE = getLE();
789
790
791 final double hx2 = hx * hx;
792 final double hy2 = hy * hy;
793 final double factH = 1. / (1 + hx2 + hy2);
794
795
796 final double ux = (1 + hx2 - hy2) * factH;
797 final double uy = 2 * hx * hy * factH;
798 final double uz = -2 * hy * factH;
799
800 final double vx = uy;
801 final double vy = (1 - hx2 + hy2) * factH;
802 final double vz = 2 * hx * factH;
803
804
805 final double exey = ex * ey;
806 final double ex2 = ex * ex;
807 final double ey2 = ey * ey;
808 final double e2 = ex2 + ey2;
809 final double eta = 1 + FastMath.sqrt(1 - e2);
810 final double beta = 1. / eta;
811
812
813 final SinCos scLe = FastMath.sinCos(lE);
814 final double cLe = scLe.cos();
815 final double sLe = scLe.sin();
816
817
818 final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
819 final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);
820
821 return new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
822
823 }
824
825
826 @Override
827 protected TimeStampedPVCoordinates initPVCoordinates() {
828
829
830 computePVWithoutA();
831
832
833 final double r2 = partialPV.getPosition().getNormSq();
834 final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
835 final Vector3D acceleration = hasNonKeplerianRates() ?
836 keplerianAcceleration.add(nonKeplerianAcceleration()) :
837 keplerianAcceleration;
838
839 return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
840
841 }
842
843
844 @Override
845 public EquinoctialOrbit inFrame(final Frame inertialFrame) {
846 final PVCoordinates pvCoordinates;
847 if (hasNonKeplerianAcceleration()) {
848 pvCoordinates = getPVCoordinates(inertialFrame);
849 } else {
850 final KinematicTransform transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
851 pvCoordinates = transform.transformOnlyPV(getPVCoordinates());
852 }
853 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(pvCoordinates, inertialFrame, getDate(), getMu());
854 if (equinoctialOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
855 return equinoctialOrbit;
856 } else {
857 return equinoctialOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
858 }
859 }
860
861
862 @Override
863 public EquinoctialOrbit withCachedPositionAngleType(final PositionAngleType positionAngleType) {
864 return new EquinoctialOrbit(a, ex, ey, hx, hy, getL(positionAngleType), aDot, exDot, eyDot, hxDot, hyDot,
865 getLDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
866 }
867
868
869 @Override
870 public EquinoctialOrbit shiftedBy(final double dt) {
871 return shiftedBy(new TimeOffset(dt));
872 }
873
874
875 @Override
876 public EquinoctialOrbit shiftedBy(final TimeOffset dt) {
877
878 final double dtS = dt.toDouble();
879
880
881 final EquinoctialOrbit keplerianShifted = new EquinoctialOrbit(a, ex, ey, hx, hy,
882 getLM() + getKeplerianMeanMotion() * dtS,
883 PositionAngleType.MEAN, cachedPositionAngleType,
884 getFrame(),
885 getDate().shiftedBy(dt), getMu());
886
887 if (dtS != 0. && hasNonKeplerianRates()) {
888
889
890 final Vector3D nonKeplerianAcceleration = nonKeplerianAcceleration();
891
892
893 keplerianShifted.computePVWithoutA();
894 final Vector3D fixedP = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
895 0.5 * dtS * dtS, nonKeplerianAcceleration);
896 final double fixedR2 = fixedP.getNormSq();
897 final double fixedR = FastMath.sqrt(fixedR2);
898 final Vector3D fixedV = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
899 dtS, nonKeplerianAcceleration);
900 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
901 1, nonKeplerianAcceleration);
902
903
904 return new EquinoctialOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
905 fixedP, fixedV, fixedA),
906 keplerianShifted.getFrame(), keplerianShifted.getMu());
907
908 } else {
909
910 return keplerianShifted;
911 }
912
913 }
914
915
916 @Override
917 protected double[][] computeJacobianMeanWrtCartesian() {
918
919 final double[][] jacobian = new double[6][6];
920
921
922 computePVWithoutA();
923 final Vector3D position = partialPV.getPosition();
924 final Vector3D velocity = partialPV.getVelocity();
925 final double r2 = position.getNormSq();
926 final double r = FastMath.sqrt(r2);
927 final double r3 = r * r2;
928
929 final double mu = getMu();
930 final double sqrtMuA = FastMath.sqrt(a * mu);
931 final double a2 = a * a;
932
933 final double e2 = ex * ex + ey * ey;
934 final double oMe2 = 1 - e2;
935 final double epsilon = FastMath.sqrt(oMe2);
936 final double beta = 1 / (1 + epsilon);
937 final double ratio = epsilon * beta;
938
939 final double hx2 = hx * hx;
940 final double hy2 = hy * hy;
941 final double hxhy = hx * hy;
942
943
944 final Vector3D f = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
945 final Vector3D g = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
946 final Vector3D w = Vector3D.crossProduct(position, velocity).normalize();
947
948
949 final double x = Vector3D.dotProduct(position, f);
950 final double y = Vector3D.dotProduct(position, g);
951 final double xDot = Vector3D.dotProduct(velocity, f);
952 final double yDot = Vector3D.dotProduct(velocity, g);
953
954
955 final double c1 = a / (sqrtMuA * epsilon);
956 final double c2 = a * sqrtMuA * beta / r3;
957 final double c3 = sqrtMuA / (r3 * epsilon);
958 final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
959 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);
960
961
962 final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
963 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);
964
965
966 final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
967 final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
968 fillHalfRow(1, vectorAR, jacobian[0], 0);
969 fillHalfRow(1, vectorARDot, jacobian[0], 3);
970
971
972 final double d1 = -a * ratio / r3;
973 final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
974 final double d3 = (hx * y - hy * x) / sqrtMuA;
975 final Vector3D vectorExRDot =
976 new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
977 fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
978 fillHalfRow(1, vectorExRDot, jacobian[1], 3);
979
980
981 final Vector3D vectorEyRDot =
982 new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
983 fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
984 fillHalfRow(1, vectorEyRDot, jacobian[2], 3);
985
986
987 final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
988 fillHalfRow(-h * xDot, w, jacobian[3], 0);
989 fillHalfRow( h * x, w, jacobian[3], 3);
990
991
992 fillHalfRow(-h * yDot, w, jacobian[4], 0);
993 fillHalfRow( h * y, w, jacobian[4], 3);
994
995
996 final double l = -ratio / sqrtMuA;
997 fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
998 fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);
999
1000 return jacobian;
1001
1002 }
1003
1004
1005 @Override
1006 protected double[][] computeJacobianEccentricWrtCartesian() {
1007
1008
1009 final double[][] jacobian = computeJacobianMeanWrtCartesian();
1010
1011
1012
1013
1014
1015 final SinCos scLe = FastMath.sinCos(getLE());
1016 final double cosLe = scLe.cos();
1017 final double sinLe = scLe.sin();
1018 final double aOr = 1 / (1 - ex * cosLe - ey * sinLe);
1019
1020
1021 final double[] rowEx = jacobian[1];
1022 final double[] rowEy = jacobian[2];
1023 final double[] rowL = jacobian[5];
1024 for (int j = 0; j < 6; ++j) {
1025 rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
1026 }
1027
1028 return jacobian;
1029
1030 }
1031
1032
1033 @Override
1034 protected double[][] computeJacobianTrueWrtCartesian() {
1035
1036
1037 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051 final SinCos scLe = FastMath.sinCos(getLE());
1052 final double cosLe = scLe.cos();
1053 final double sinLe = scLe.sin();
1054 final double eSinE = ex * sinLe - ey * cosLe;
1055 final double ecosE = ex * cosLe + ey * sinLe;
1056 final double e2 = ex * ex + ey * ey;
1057 final double epsilon = FastMath.sqrt(1 - e2);
1058 final double onePeps = 1 + epsilon;
1059 final double d = onePeps - ecosE;
1060 final double cT = (d * d + eSinE * eSinE) / 2;
1061 final double cE = ecosE * onePeps - e2;
1062 final double cX = ex * eSinE / epsilon - ey + sinLe * onePeps;
1063 final double cY = ey * eSinE / epsilon + ex - cosLe * onePeps;
1064 final double factorLe = (cT + cE) / cT;
1065 final double factorEx = cX / cT;
1066 final double factorEy = cY / cT;
1067
1068
1069 final double[] rowEx = jacobian[1];
1070 final double[] rowEy = jacobian[2];
1071 final double[] rowL = jacobian[5];
1072 for (int j = 0; j < 6; ++j) {
1073 rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
1074 }
1075
1076 return jacobian;
1077
1078 }
1079
1080
1081 @Override
1082 public void addKeplerContribution(final PositionAngleType type, final double gm,
1083 final double[] pDot) {
1084 pDot[5] += computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType);
1085 }
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099 private static double computeKeplerianLDot(final PositionAngleType type, final double a, final double ex,
1100 final double ey, final double mu,
1101 final double l, final PositionAngleType cachedType) {
1102 final double n = FastMath.sqrt(mu / a) / a;
1103 if (type == PositionAngleType.MEAN) {
1104 return n;
1105 }
1106 final double oMe2;
1107 final double ksi;
1108 final SinCos sc;
1109 if (type == PositionAngleType.ECCENTRIC) {
1110 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1111 ksi = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1112 return n * ksi;
1113 } else {
1114 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1115 oMe2 = 1 - ex * ex - ey * ey;
1116 ksi = 1 + ex * sc.cos() + ey * sc.sin();
1117 return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1118 }
1119 }
1120
1121
1122
1123
1124 public String toString() {
1125 return new StringBuilder().append("equinoctial parameters: ").append('{').
1126 append("a: ").append(a).
1127 append("; ex: ").append(ex).append("; ey: ").append(ey).
1128 append("; hx: ").append(hx).append("; hy: ").append(hy).
1129 append("; lv: ").append(FastMath.toDegrees(getLv())).
1130 append(";}").toString();
1131 }
1132
1133
1134 @Override
1135 public PositionAngleType getCachedPositionAngleType() {
1136 return cachedPositionAngleType;
1137 }
1138
1139
1140 @Override
1141 public boolean hasNonKeplerianRates() {
1142 return hasNonKeplerianAcceleration();
1143 }
1144
1145
1146 @Override
1147 public EquinoctialOrbit withKeplerianRates() {
1148 final PositionAngleType positionAngleType = getCachedPositionAngleType();
1149 return new EquinoctialOrbit(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
1150 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1151 }
1152
1153 }