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