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 if (dt.isZero()) {
787 return new EquinoctialOrbit(a, ex, ey, hx, hy, cachedL, aDot, exDot, eyDot, hxDot, hyDot, cachedLDot,
788 cachedPositionAngleType, getFrame(), getDate(), getMu());
789 }
790
791 final double dtS = dt.toDouble();
792
793
794 final EquinoctialParameters parameters = new EquinoctialParameters(a, ex, ey, hx, hy,
795 getLM() + dtS * getKeplerianMeanMotion(), PositionAngleType.MEAN);
796 final EquinoctialOrbit keplerianShifted = new EquinoctialOrbit(parameters,
797 getFrame(), getDate().shiftedBy(dt), getMu());
798
799 if (hasNonKeplerianRates()) {
800 final PVCoordinates pvCoordinates = shiftPVNonKeplerian(keplerianShifted.getPVCoordinates(), dtS);
801
802
803 return new EquinoctialOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(), pvCoordinates),
804 getFrame(), getMu());
805
806 } else {
807
808 return keplerianShifted;
809 }
810
811 }
812
813
814 @Override
815 protected EquinoctialOrbit keplerianShiftedBy(final double dt) {
816 return new EquinoctialOrbit(a, ex, ey, hx, hy, getLM() + dt * getKeplerianMeanMotion(),
817 PositionAngleType.MEAN, getFrame(), getDate().shiftedBy(dt), getMu());
818 }
819
820
821 @Override
822 protected double[][] computeJacobianMeanWrtCartesian() {
823
824 final double[][] jacobian = new double[6][6];
825
826
827 computePVWithoutA();
828 final Vector3D position = partialPV.getPosition();
829 final Vector3D velocity = partialPV.getVelocity();
830 final double r2 = position.getNorm2Sq();
831 final double r = FastMath.sqrt(r2);
832 final double r3 = r * r2;
833
834 final double mu = getMu();
835 final double sqrtMuA = FastMath.sqrt(a * mu);
836 final double a2 = a * a;
837
838 final double e2 = ex * ex + ey * ey;
839 final double oMe2 = 1 - e2;
840 final double epsilon = FastMath.sqrt(oMe2);
841 final double beta = 1 / (1 + epsilon);
842 final double ratio = epsilon * beta;
843
844 final double hx2 = hx * hx;
845 final double hy2 = hy * hy;
846 final double hxhy = hx * hy;
847
848
849 final Vector3D f = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize();
850 final Vector3D g = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize();
851 final Vector3D w = Vector3D.crossProduct(position, velocity).normalize();
852
853
854 final double x = Vector3D.dotProduct(position, f);
855 final double y = Vector3D.dotProduct(position, g);
856 final double xDot = Vector3D.dotProduct(velocity, f);
857 final double yDot = Vector3D.dotProduct(velocity, g);
858
859
860 final double c1 = a / (sqrtMuA * epsilon);
861 final double c2 = a * sqrtMuA * beta / r3;
862 final double c3 = sqrtMuA / (r3 * epsilon);
863 final Vector3D drDotSdEx = new Vector3D( c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f,
864 -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g);
865
866
867 final Vector3D drDotSdEy = new Vector3D( c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f,
868 -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g);
869
870
871 final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
872 final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity);
873 fillHalfRow(1, vectorAR, jacobian[0], 0);
874 fillHalfRow(1, vectorARDot, jacobian[0], 3);
875
876
877 final double d1 = -a * ratio / r3;
878 final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon);
879 final double d3 = (hx * y - hy * x) / sqrtMuA;
880 final Vector3D vectorExRDot =
881 new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w);
882 fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0);
883 fillHalfRow(1, vectorExRDot, jacobian[1], 3);
884
885
886 final Vector3D vectorEyRDot =
887 new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w);
888 fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0);
889 fillHalfRow(1, vectorEyRDot, jacobian[2], 3);
890
891
892 final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon);
893 fillHalfRow(-h * xDot, w, jacobian[3], 0);
894 fillHalfRow( h * x, w, jacobian[3], 3);
895
896
897 fillHalfRow(-h * yDot, w, jacobian[4], 0);
898 fillHalfRow( h * y, w, jacobian[4], 3);
899
900
901 final double l = -ratio / sqrtMuA;
902 fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0);
903 fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3);
904
905 return jacobian;
906
907 }
908
909
910 @Override
911 protected double[][] computeJacobianEccentricWrtCartesian() {
912
913
914 final double[][] jacobian = computeJacobianMeanWrtCartesian();
915
916
917
918
919
920 final SinCos scLe = FastMath.sinCos(getLE());
921 final double cosLe = scLe.cos();
922 final double sinLe = scLe.sin();
923 final double aOr = 1 / (1 - ex * cosLe - ey * sinLe);
924
925
926 final double[] rowEx = jacobian[1];
927 final double[] rowEy = jacobian[2];
928 final double[] rowL = jacobian[5];
929 for (int j = 0; j < 6; ++j) {
930 rowL[j] = aOr * (rowL[j] + sinLe * rowEx[j] - cosLe * rowEy[j]);
931 }
932
933 return jacobian;
934
935 }
936
937
938 @Override
939 protected double[][] computeJacobianTrueWrtCartesian() {
940
941
942 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
943
944
945
946
947
948
949
950
951
952
953
954
955
956 final SinCos scLe = FastMath.sinCos(getLE());
957 final double cosLe = scLe.cos();
958 final double sinLe = scLe.sin();
959 final double eSinE = ex * sinLe - ey * cosLe;
960 final double ecosE = ex * cosLe + ey * sinLe;
961 final double e2 = ex * ex + ey * ey;
962 final double epsilon = FastMath.sqrt(1 - e2);
963 final double onePeps = 1 + epsilon;
964 final double d = onePeps - ecosE;
965 final double cT = (d * d + eSinE * eSinE) / 2;
966 final double cE = ecosE * onePeps - e2;
967 final double cX = ex * eSinE / epsilon - ey + sinLe * onePeps;
968 final double cY = ey * eSinE / epsilon + ex - cosLe * onePeps;
969 final double factorLe = (cT + cE) / cT;
970 final double factorEx = cX / cT;
971 final double factorEy = cY / cT;
972
973
974 final double[] rowEx = jacobian[1];
975 final double[] rowEy = jacobian[2];
976 final double[] rowL = jacobian[5];
977 for (int j = 0; j < 6; ++j) {
978 rowL[j] = factorLe * rowL[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
979 }
980
981 return jacobian;
982
983 }
984
985
986 @Override
987 public void addKeplerContribution(final PositionAngleType type, final double gm,
988 final double[] pDot) {
989 pDot[5] += computeKeplerianLDot(type, a, ex, ey, gm, cachedL, cachedPositionAngleType);
990 }
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004 private static double computeKeplerianLDot(final PositionAngleType type, final double a, final double ex,
1005 final double ey, final double mu,
1006 final double l, final PositionAngleType cachedType) {
1007 final double n = FastMath.sqrt(mu / a) / a;
1008 if (type == PositionAngleType.MEAN) {
1009 return n;
1010 }
1011 final double oMe2;
1012 final double ksi;
1013 final SinCos sc;
1014 if (type == PositionAngleType.ECCENTRIC) {
1015 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1016 ksi = 1. / (1 - ex * sc.cos() - ey * sc.sin());
1017 return n * ksi;
1018 } else {
1019 sc = FastMath.sinCos(EquinoctialLongitudeArgumentUtility.convertL(cachedType, l, ex, ey, type));
1020 oMe2 = 1 - ex * ex - ey * ey;
1021 ksi = 1 + ex * sc.cos() + ey * sc.sin();
1022 return n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1023 }
1024 }
1025
1026
1027
1028
1029 public String toString() {
1030 return new StringBuilder().append("equinoctial parameters: ").append('{').
1031 append("a: ").append(a).
1032 append("; ex: ").append(ex).append("; ey: ").append(ey).
1033 append("; hx: ").append(hx).append("; hy: ").append(hy).
1034 append("; lv: ").append(FastMath.toDegrees(getLv())).
1035 append(";}").toString();
1036 }
1037
1038
1039 @Override
1040 public PositionAngleType getCachedPositionAngleType() {
1041 return cachedPositionAngleType;
1042 }
1043
1044
1045 @Override
1046 public boolean hasNonKeplerianRates() {
1047 return hasNonKeplerianAcceleration();
1048 }
1049
1050
1051 @Override
1052 public EquinoctialOrbit withKeplerianRates() {
1053 final PositionAngleType positionAngleType = getCachedPositionAngleType();
1054 return new EquinoctialOrbit(getA(), getEquinoctialEx(), getEquinoctialEy(), getHx(), getHy(),
1055 getL(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1056 }
1057
1058 }