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