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.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.util.FastMath;
24 import org.hipparchus.util.FieldSinCos;
25 import org.hipparchus.util.MathArrays;
26 import org.orekit.errors.OrekitIllegalArgumentException;
27 import org.orekit.errors.OrekitInternalError;
28 import org.orekit.errors.OrekitMessages;
29 import org.orekit.frames.FieldKinematicTransform;
30 import org.orekit.frames.Frame;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.time.TimeOffset;
33 import org.orekit.utils.FieldPVCoordinates;
34 import org.orekit.utils.TimeStampedFieldPVCoordinates;
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
77 public class FieldCircularOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T>
78 implements PositionAngleBased<FieldCircularOrbit<T>> {
79
80
81 private final T a;
82
83
84 private final T ex;
85
86
87 private final T ey;
88
89
90 private final T i;
91
92
93 private final T raan;
94
95
96 private final T cachedAlpha;
97
98
99 private final PositionAngleType cachedPositionAngleType;
100
101
102 private final T aDot;
103
104
105 private final T exDot;
106
107
108 private final T eyDot;
109
110
111 private final T iDot;
112
113
114 private final T raanDot;
115
116
117 private final T cachedAlphaDot;
118
119
120 private FieldPVCoordinates<T> partialPV;
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139 public FieldCircularOrbit(final T a, final T ex, final T ey, final T i, final T raan,
140 final T alpha, final PositionAngleType type,
141 final PositionAngleType cachedPositionAngleType,
142 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
143 throws IllegalArgumentException {
144 this(a, ex, ey, i, raan, alpha,
145 a.getField().getZero(), a.getField().getZero(), a.getField().getZero(), a.getField().getZero(), a.getField().getZero(),
146 computeKeplerianAlphaDot(type, a, ex, ey, mu, alpha, type), type, cachedPositionAngleType, frame, date, mu);
147 }
148
149
150
151
152
153
154
155
156
157
158 public FieldCircularOrbit(final FieldCircularParameters<T> parameters,
159 final Frame frame, final FieldAbsoluteDate<T> date, final T mu) {
160 this(parameters.a(), parameters.ex(), parameters.ey(), parameters.i(), parameters.raan(),
161 parameters.latitudeArgument(), parameters.positionAngleType(), parameters.positionAngleType(), frame, date, mu);
162 }
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179 public FieldCircularOrbit(final T a, final T ex, final T ey, final T i, final T raan,
180 final T alpha, final PositionAngleType type,
181 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
182 throws IllegalArgumentException {
183 this(new FieldCircularParameters<>(a, ex, ey, i, raan, alpha, type), frame, date, mu);
184 }
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207 public FieldCircularOrbit(final T a, final T ex, final T ey,
208 final T i, final T raan, final T alpha,
209 final T aDot, final T exDot, final T eyDot,
210 final T iDot, final T raanDot, final T alphaDot, final PositionAngleType type,
211 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
212 throws IllegalArgumentException {
213 this(a, ex, ey, i, raan, alpha, aDot, exDot, eyDot, iDot, raanDot, alphaDot, type, type, frame, date, mu);
214 }
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239 public FieldCircularOrbit(final T a, final T ex, final T ey,
240 final T i, final T raan, final T alpha,
241 final T aDot, final T exDot, final T eyDot,
242 final T iDot, final T raanDot, final T alphaDot,
243 final PositionAngleType type, final PositionAngleType cachedPositionAngleType,
244 final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
245 throws IllegalArgumentException {
246 super(frame, date, mu);
247 if (ex.getReal() * ex.getReal() + ey.getReal() * ey.getReal() >= 1.0) {
248 throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
249 getClass().getName());
250 }
251
252 this.a = a;
253 this.aDot = aDot;
254 this.ex = ex;
255 this.exDot = exDot;
256 this.ey = ey;
257 this.eyDot = eyDot;
258 this.i = i;
259 this.iDot = iDot;
260 this.raan = raan;
261 this.raanDot = raanDot;
262 this.cachedPositionAngleType = cachedPositionAngleType;
263
264 final FieldUnivariateDerivative1<T> alphaUD = initializeCachedAlpha(alpha, alphaDot, type);
265 this.cachedAlpha = alphaUD.getValue();
266 this.cachedAlphaDot = alphaUD.getFirstDerivative();
267
268 partialPV = null;
269
270 }
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286 public FieldCircularOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
287 final Frame frame, final T mu)
288 throws IllegalArgumentException {
289 super(pvCoordinates, frame, mu);
290 this.cachedPositionAngleType = PositionAngleType.TRUE;
291
292
293 final FieldVector3D<T> pvP = pvCoordinates.getPosition();
294 final FieldVector3D<T> pvV = pvCoordinates.getVelocity();
295 final FieldVector3D<T> pvA = pvCoordinates.getAcceleration();
296 final T r2 = pvP.getNorm2Sq();
297 final T r = r2.sqrt();
298 final T V2 = pvV.getNorm2Sq();
299 final T rV2OnMu = r.multiply(V2).divide(mu);
300
301 a = r.divide(rV2OnMu.negate().add(2));
302
303 if (!isElliptical()) {
304 throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
305 getClass().getName());
306 }
307
308
309 final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
310 final FieldVector3D<T> plusK = FieldVector3D.getPlusK(r.getField());
311 i = FieldVector3D.angle(momentum, plusK);
312
313
314 final FieldVector3D<T> node = FieldVector3D.crossProduct(plusK, momentum);
315 raan = node.getY().atan2(node.getX());
316
317
318 final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
319 final FieldSinCos<T> scI = FastMath.sinCos(i);
320 final T xP = pvP.getX();
321 final T yP = pvP.getY();
322 final T zP = pvP.getZ();
323 final T x2 = (xP.multiply(scRaan.cos()).add(yP .multiply(scRaan.sin()))).divide(a);
324 final T y2 = (yP.multiply(scRaan.cos()).subtract(xP.multiply(scRaan.sin()))).multiply(scI.cos()).add(zP.multiply(scI.sin())).divide(a);
325
326
327 final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(mu).sqrt());
328 final T eCE = rV2OnMu.subtract(1);
329 final T e2 = eCE.multiply(eCE).add(eSE.multiply(eSE));
330 final T f = eCE.subtract(e2);
331 final T g = eSE.multiply(e2.negate().add(1).sqrt());
332 final T aOnR = a.divide(r);
333 final T a2OnR2 = aOnR.square();
334 ex = a2OnR2.multiply(f.multiply(x2).add(g.multiply(y2)));
335 ey = a2OnR2.multiply(f.multiply(y2).subtract(g.multiply(x2)));
336
337
338 final T beta = (ex.multiply(ex).add(ey.multiply(ey)).negate().add(1)).sqrt().add(1).reciprocal();
339 cachedAlpha = FieldCircularLatitudeArgumentUtility.eccentricToTrue(ex, ey, y2.add(ey).add(eSE.multiply(beta).multiply(ex)).atan2(x2.add(ex).subtract(eSE.multiply(beta).multiply(ey)))
340 );
341
342 partialPV = pvCoordinates;
343
344 if (hasNonKeplerianAcceleration(pvCoordinates, mu)) {
345
346
347 final T[][] jacobian = MathArrays.buildArray(a.getField(), 6, 6);
348 getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
349
350 final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
351 final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
352 final T aX = nonKeplerianAcceleration.getX();
353 final T aY = nonKeplerianAcceleration.getY();
354 final T aZ = nonKeplerianAcceleration.getZ();
355 aDot = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
356 exDot = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
357 eyDot = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
358 iDot = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
359 raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));
360
361
362
363 final T alphaMDot = getKeplerianMeanMotion().
364 add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
365 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
366 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
367 final FieldUnivariateDerivative1<T> alphaMUD = new FieldUnivariateDerivative1<>(getAlphaM(), alphaMDot);
368 final FieldUnivariateDerivative1<T> alphavUD = FieldCircularLatitudeArgumentUtility.meanToTrue(exUD, eyUD, alphaMUD);
369 cachedAlphaDot = alphavUD.getFirstDerivative();
370
371 } else {
372
373
374
375 aDot = getZero();
376 exDot = getZero();
377 eyDot = getZero();
378 iDot = getZero();
379 raanDot = getZero();
380 cachedAlphaDot = computeKeplerianAlphaDot(cachedPositionAngleType, a, ex, ey, mu, cachedAlpha, cachedPositionAngleType);
381 }
382
383 }
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400 public FieldCircularOrbit(final FieldPVCoordinates<T> pvCoordinates, final Frame frame,
401 final FieldAbsoluteDate<T> date, final T mu)
402 throws IllegalArgumentException {
403 this(new TimeStampedFieldPVCoordinates<>(date, pvCoordinates), frame, mu);
404 }
405
406
407
408
409 public FieldCircularOrbit(final FieldOrbit<T> op) {
410 super(op.getFrame(), op.getDate(), op.getMu());
411 a = op.getA();
412 i = op.getI();
413 final T hx = op.getHx();
414 final T hy = op.getHy();
415 final T h2 = hx.square().add(hy.square());
416 final T h = h2.sqrt();
417 raan = hy.atan2(hx);
418 final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
419 final T cosRaan = h.getReal() == 0 ? scRaan.cos() : hx.divide(h);
420 final T sinRaan = h.getReal() == 0 ? scRaan.sin() : hy.divide(h);
421 final T equiEx = op.getEquinoctialEx();
422 final T equiEy = op.getEquinoctialEy();
423 ex = equiEx.multiply(cosRaan).add(equiEy.multiply(sinRaan));
424 ey = equiEy.multiply(cosRaan).subtract(equiEx.multiply(sinRaan));
425 cachedPositionAngleType = PositionAngleType.TRUE;
426 cachedAlpha = op.getLv().subtract(raan);
427
428 if (op.hasNonKeplerianAcceleration()) {
429 aDot = op.getADot();
430 final T hxDot = op.getHxDot();
431 final T hyDot = op.getHyDot();
432 iDot = cosRaan.multiply(hxDot).add(sinRaan.multiply(hyDot)).multiply(2).divide(h2.add(1));
433 raanDot = hx.multiply(hyDot).subtract(hy.multiply(hxDot)).divide(h2);
434 final T equiExDot = op.getEquinoctialExDot();
435 final T equiEyDot = op.getEquinoctialEyDot();
436 exDot = equiExDot.add(equiEy.multiply(raanDot)).multiply(cosRaan).
437 add(equiEyDot.subtract(equiEx.multiply(raanDot)).multiply(sinRaan));
438 eyDot = equiEyDot.subtract(equiEx.multiply(raanDot)).multiply(cosRaan).
439 subtract(equiExDot.add(equiEy.multiply(raanDot)).multiply(sinRaan));
440 cachedAlphaDot = op.getLvDot().subtract(raanDot);
441 } else {
442 aDot = getZero();
443 exDot = getZero();
444 eyDot = getZero();
445 iDot = getZero();
446 raanDot = getZero();
447 cachedAlphaDot = computeKeplerianAlphaDot(cachedPositionAngleType, a, ex, ey, op.getMu(), cachedAlpha, cachedPositionAngleType);
448 }
449
450 partialPV = null;
451
452 }
453
454
455
456
457
458
459
460 public FieldCircularOrbit(final Field<T> field, final CircularOrbit op) {
461 super(op.getFrame(), new FieldAbsoluteDate<>(field, op.getDate()), field.getZero().newInstance(op.getMu()));
462
463 a = getZero().newInstance(op.getA());
464 i = getZero().newInstance(op.getI());
465 raan = getZero().newInstance(op.getRightAscensionOfAscendingNode());
466 ex = getZero().newInstance(op.getCircularEx());
467 ey = getZero().newInstance(op.getCircularEy());
468 cachedPositionAngleType = op.getCachedPositionAngleType();
469 cachedAlpha = getZero().newInstance(op.getAlpha(cachedPositionAngleType));
470
471 aDot = getZero().newInstance(op.getADot());
472 iDot = getZero().newInstance(op.getIDot());
473 raanDot = getZero().newInstance(op.getRightAscensionOfAscendingNodeDot());
474 exDot = getZero().newInstance(op.getCircularExDot());
475 eyDot = getZero().newInstance(op.getCircularEyDot());
476 cachedAlphaDot = getZero().newInstance(op.getAlphaDot(cachedPositionAngleType));
477
478 partialPV = null;
479
480 }
481
482
483
484
485
486
487
488 public FieldCircularOrbit(final Field<T> field, final Orbit op) {
489 this(field, (CircularOrbit) OrbitParamsType.CIRCULAR.convertType(op));
490 }
491
492
493
494
495
496
497 public FieldCircularParameters<T> getCircularParameters() {
498 return new FieldCircularParameters<>(a, ex, ey, i, raan, cachedAlpha, cachedPositionAngleType);
499 }
500
501
502 @Override
503 public OrbitParamsType getType() {
504 return OrbitParamsType.CIRCULAR;
505 }
506
507
508 @Override
509 public T getA() {
510 return a;
511 }
512
513
514 @Override
515 public T getADot() {
516 return aDot;
517 }
518
519
520 @Override
521 public T getEquinoctialEx() {
522 final FieldSinCos<T> sc = FastMath.sinCos(raan);
523 return ex.multiply(sc.cos()).subtract(ey.multiply(sc.sin()));
524 }
525
526
527 @Override
528 public T getEquinoctialExDot() {
529 if (!hasNonKeplerianRates()) {
530 return getZero();
531 }
532 final FieldSinCos<T> sc = FastMath.sinCos(raan);
533 return exDot.subtract(ey.multiply(raanDot)).multiply(sc.cos()).
534 subtract(eyDot.add(ex.multiply(raanDot)).multiply(sc.sin()));
535
536 }
537
538
539 @Override
540 public T getEquinoctialEy() {
541 final FieldSinCos<T> sc = FastMath.sinCos(raan);
542 return ey.multiply(sc.cos()).add(ex.multiply(sc.sin()));
543 }
544
545
546 @Override
547 public T getEquinoctialEyDot() {
548 if (!hasNonKeplerianRates()) {
549 return getZero();
550 }
551 final FieldSinCos<T> sc = FastMath.sinCos(raan);
552 return eyDot.add(ex.multiply(raanDot)).multiply(sc.cos()).
553 add(exDot.subtract(ey.multiply(raanDot)).multiply(sc.sin()));
554
555 }
556
557
558
559
560 public T getCircularEx() {
561 return ex;
562 }
563
564
565
566
567 public T getCircularExDot() {
568 return exDot;
569 }
570
571
572
573
574 public T getCircularEy() {
575 return ey;
576 }
577
578
579
580
581 public T getCircularEyDot() {
582 return eyDot;
583 }
584
585
586 @Override
587 public T getHx() {
588
589 if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
590 return getZero().add(Double.NaN);
591 }
592 return raan.cos().multiply(i.divide(2).tan());
593 }
594
595
596 @Override
597 public T getHxDot() {
598
599
600 if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
601 return getZero().add(Double.NaN);
602 }
603 if (!hasNonKeplerianRates()) {
604 return getZero();
605 }
606
607 final FieldSinCos<T> sc = FastMath.sinCos(raan);
608 final T tan = i.multiply(0.5).tan();
609 return sc.cos().multiply(0.5).multiply(tan.multiply(tan).add(1)).multiply(iDot).
610 subtract(sc.sin().multiply(tan).multiply(raanDot));
611
612 }
613
614
615 @Override
616 public T getHy() {
617
618 if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
619 return getZero().add(Double.NaN);
620 }
621 return raan.sin().multiply(i.divide(2).tan());
622 }
623
624
625 @Override
626 public T getHyDot() {
627
628
629 if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
630 return getZero().add(Double.NaN);
631 }
632 if (!hasNonKeplerianRates()) {
633 return getZero();
634 }
635
636 final FieldSinCos<T> sc = FastMath.sinCos(raan);
637 final T tan = i.multiply(0.5).tan();
638 return sc.sin().multiply(0.5).multiply(tan.multiply(tan).add(1)).multiply(iDot).
639 add(sc.cos().multiply(tan).multiply(raanDot));
640
641 }
642
643
644
645
646 public T getAlphaV() {
647 return getAlpha(PositionAngleType.TRUE);
648 }
649
650
651
652
653 public T getAlphaVDot() {
654
655 switch (cachedPositionAngleType) {
656 case ECCENTRIC:
657 final FieldUnivariateDerivative1<T> alphaEUD = new FieldUnivariateDerivative1<>(cachedAlpha, cachedAlphaDot);
658 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
659 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
660 final FieldUnivariateDerivative1<T> alphaVUD = FieldCircularLatitudeArgumentUtility.eccentricToTrue(exUD, eyUD,
661 alphaEUD);
662 return alphaVUD.getFirstDerivative();
663
664 case TRUE:
665 return cachedAlphaDot;
666
667 case MEAN:
668 final FieldUnivariateDerivative1<T> alphaMUD = new FieldUnivariateDerivative1<>(cachedAlpha, cachedAlphaDot);
669 final FieldUnivariateDerivative1<T> exUD2 = new FieldUnivariateDerivative1<>(ex, exDot);
670 final FieldUnivariateDerivative1<T> eyUD2 = new FieldUnivariateDerivative1<>(ey, eyDot);
671 final FieldUnivariateDerivative1<T> alphaVUD2 = FieldCircularLatitudeArgumentUtility.meanToTrue(exUD2,
672 eyUD2, alphaMUD);
673 return alphaVUD2.getFirstDerivative();
674
675 default:
676 throw new OrekitInternalError(null);
677 }
678 }
679
680
681
682
683 public T getAlphaE() {
684 return getAlpha(PositionAngleType.ECCENTRIC);
685 }
686
687
688
689
690 public T getAlphaEDot() {
691
692 switch (cachedPositionAngleType) {
693 case TRUE:
694 final FieldUnivariateDerivative1<T> alphaVUD = new FieldUnivariateDerivative1<>(cachedAlpha, cachedAlphaDot);
695 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
696 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
697 final FieldUnivariateDerivative1<T> alphaEUD = FieldCircularLatitudeArgumentUtility.trueToEccentric(exUD, eyUD,
698 alphaVUD);
699 return alphaEUD.getFirstDerivative();
700
701 case ECCENTRIC:
702 return cachedAlphaDot;
703
704 case MEAN:
705 final FieldUnivariateDerivative1<T> alphaMUD = new FieldUnivariateDerivative1<>(cachedAlpha, cachedAlphaDot);
706 final FieldUnivariateDerivative1<T> exUD2 = new FieldUnivariateDerivative1<>(ex, exDot);
707 final FieldUnivariateDerivative1<T> eyUD2 = new FieldUnivariateDerivative1<>(ey, eyDot);
708 final FieldUnivariateDerivative1<T> alphaVUD2 = FieldCircularLatitudeArgumentUtility.meanToEccentric(exUD2,
709 eyUD2, alphaMUD);
710 return alphaVUD2.getFirstDerivative();
711
712 default:
713 throw new OrekitInternalError(null);
714 }
715
716 }
717
718
719
720
721 public T getAlphaM() {
722 return getAlpha(PositionAngleType.MEAN);
723 }
724
725
726
727
728 public T getAlphaMDot() {
729
730 switch (cachedPositionAngleType) {
731 case TRUE:
732 final FieldUnivariateDerivative1<T> alphaVUD = new FieldUnivariateDerivative1<>(cachedAlpha, cachedAlphaDot);
733 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
734 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
735 final FieldUnivariateDerivative1<T> alphaMUD = FieldCircularLatitudeArgumentUtility.trueToMean(exUD, eyUD,
736 alphaVUD);
737 return alphaMUD.getFirstDerivative();
738
739 case MEAN:
740 return cachedAlphaDot;
741
742 case ECCENTRIC:
743 final FieldUnivariateDerivative1<T> alphaEUD = new FieldUnivariateDerivative1<>(cachedAlpha, cachedAlphaDot);
744 final FieldUnivariateDerivative1<T> exUD2 = new FieldUnivariateDerivative1<>(ex, exDot);
745 final FieldUnivariateDerivative1<T> eyUD2 = new FieldUnivariateDerivative1<>(ey, eyDot);
746 final FieldUnivariateDerivative1<T> alphaMUD2 = FieldCircularLatitudeArgumentUtility.eccentricToMean(exUD2,
747 eyUD2, alphaEUD);
748 return alphaMUD2.getFirstDerivative();
749
750 default:
751 throw new OrekitInternalError(null);
752 }
753 }
754
755
756
757
758
759 public T getAlpha(final PositionAngleType type) {
760 return getCircularParameters().withPositionAngleType(type).latitudeArgument();
761 }
762
763
764
765
766
767 public T getAlphaDot(final PositionAngleType type) {
768 return switch (type) {
769 case TRUE -> getAlphaVDot();
770 case MEAN -> getAlphaMDot();
771 case ECCENTRIC -> getAlphaEDot();
772 };
773 }
774
775
776 @Override
777 public T getE() {
778 return ex.square().add(ey.square()).sqrt();
779 }
780
781
782 @Override
783 public T getEDot() {
784
785 return ex.multiply(exDot).add(ey.multiply(eyDot)).divide(getE());
786
787 }
788
789
790 @Override
791 public T getI() {
792 return i;
793 }
794
795
796 @Override
797 public T getIDot() {
798 return iDot;
799 }
800
801
802
803
804 public T getRightAscensionOfAscendingNode() {
805 return raan;
806 }
807
808
809
810
811 public T getRightAscensionOfAscendingNodeDot() {
812 return raanDot;
813 }
814
815
816 @Override
817 public T getLv() {
818 return getAlphaV().add(raan);
819 }
820
821
822 @Override
823 public T getLvDot() {
824 return getAlphaVDot().add(raanDot);
825 }
826
827
828 @Override
829 public T getLE() {
830 return getAlphaE().add(raan);
831 }
832
833
834 @Override
835 public T getLEDot() {
836 return getAlphaEDot().add(raanDot);
837 }
838
839
840 @Override
841 public T getLM() {
842 return getAlphaM().add(raan);
843 }
844
845
846 @Override
847 public T getLMDot() {
848 return getAlphaMDot().add(raanDot);
849 }
850
851
852 @Override
853 public boolean hasNonKeplerianAcceleration() {
854 return aDot.getReal() != 0. || exDot.getReal() != 0 || iDot.getReal() != 0. || eyDot.getReal() != 0. || raanDot.getReal() != 0. ||
855 FastMath.abs(cachedAlphaDot.subtract(computeKeplerianAlphaDot(cachedPositionAngleType, a, ex, ey, getMu(), cachedAlpha, cachedPositionAngleType)).getReal()) > TOLERANCE_POSITION_ANGLE_RATE;
856 }
857
858
859
860 private void computePVWithoutA() {
861
862 if (partialPV != null) {
863
864 return;
865 }
866
867
868 final T equEx = getEquinoctialEx();
869 final T equEy = getEquinoctialEy();
870 final T hx = getHx();
871 final T hy = getHy();
872 final T lE = getLE();
873
874 final T hx2 = hx.multiply(hx);
875 final T hy2 = hy.multiply(hy);
876 final T factH = (hx2.add(1).add(hy2)).reciprocal();
877
878
879 final T ux = (hx2.add(1).subtract(hy2)).multiply(factH);
880 final T uy = hx.multiply(2).multiply(hy).multiply(factH);
881 final T uz = hy.multiply(-2).multiply(factH);
882
883 final T vx = uy;
884 final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
885 final T vz = hx.multiply(factH).multiply(2);
886
887
888 final T exey = equEx.multiply(equEy);
889 final T ex2 = equEx.square();
890 final T ey2 = equEy.square();
891 final T e2 = ex2.add(ey2);
892 final T eta = e2.negate().add(1).sqrt().add(1);
893 final T beta = eta.reciprocal();
894
895
896 final FieldSinCos<T> scLe = FastMath.sinCos(lE);
897 final T cLe = scLe.cos();
898 final T sLe = scLe.sin();
899 final T exCeyS = equEx.multiply(cLe).add(equEy.multiply(sLe));
900
901 final T x = a.multiply(beta.negate().multiply(ey2).add(1).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(equEx));
902 final T y = a.multiply(beta.negate().multiply(ex2).add(1).multiply(sLe).add(beta.multiply(exey).multiply(cLe)).subtract(equEy));
903
904 final T factor = (getMu().divide(a).sqrt()).divide(exCeyS.negate().add(1));
905 final T xdot = factor.multiply( beta.multiply(equEy).multiply(exCeyS).subtract(sLe ));
906 final T ydot = factor.multiply( cLe.subtract(beta.multiply(equEx).multiply(exCeyS)));
907
908 final FieldVector3D<T> position = new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
909 x.multiply(uy).add(y.multiply(vy)),
910 x.multiply(uz).add(y.multiply(vz)));
911 final FieldVector3D<T> velocity = new FieldVector3D<>(xdot.multiply(ux).add(ydot.multiply(vx)),
912 xdot.multiply(uy).add(ydot.multiply(vy)),
913 xdot.multiply(uz).add(ydot.multiply(vz)));
914
915 partialPV = new FieldPVCoordinates<>(position, velocity);
916
917 }
918
919
920
921
922
923
924
925
926
927 private FieldUnivariateDerivative1<T> initializeCachedAlpha(final T alpha, final T alphaDot,
928 final PositionAngleType inputType) {
929 if (cachedPositionAngleType == inputType) {
930 return new FieldUnivariateDerivative1<>(alpha, alphaDot);
931
932 } else {
933 final FieldUnivariateDerivative1<T> exUD = new FieldUnivariateDerivative1<>(ex, exDot);
934 final FieldUnivariateDerivative1<T> eyUD = new FieldUnivariateDerivative1<>(ey, eyDot);
935 final FieldUnivariateDerivative1<T> alphaUD = new FieldUnivariateDerivative1<>(alpha, alphaDot);
936
937 switch (cachedPositionAngleType) {
938
939 case ECCENTRIC:
940 if (inputType == PositionAngleType.MEAN) {
941 return FieldCircularLatitudeArgumentUtility.meanToEccentric(exUD, eyUD, alphaUD);
942 } else {
943 return FieldCircularLatitudeArgumentUtility.trueToEccentric(exUD, eyUD, alphaUD);
944 }
945
946 case TRUE:
947 if (inputType == PositionAngleType.MEAN) {
948 return FieldCircularLatitudeArgumentUtility.meanToTrue(exUD, eyUD, alphaUD);
949 } else {
950 return FieldCircularLatitudeArgumentUtility.eccentricToTrue(exUD, eyUD, alphaUD);
951 }
952
953 case MEAN:
954 if (inputType == PositionAngleType.TRUE) {
955 return FieldCircularLatitudeArgumentUtility.trueToMean(exUD, eyUD, alphaUD);
956 } else {
957 return FieldCircularLatitudeArgumentUtility.eccentricToMean(exUD, eyUD, alphaUD);
958 }
959
960 default:
961 throw new OrekitInternalError(null);
962
963 }
964
965 }
966
967 }
968
969
970 @Override
971 protected FieldVector3D<T> initPosition() {
972
973 final T equEx = getEquinoctialEx();
974 final T equEy = getEquinoctialEy();
975 final T hx = getHx();
976 final T hy = getHy();
977 final T lE = getLE();
978
979 final T hx2 = hx.multiply(hx);
980 final T hy2 = hy.multiply(hy);
981 final T factH = (hx2.add(1).add(hy2)).reciprocal();
982
983
984 final T ux = (hx2.add(1).subtract(hy2)).multiply(factH);
985 final T uy = hx.multiply(2).multiply(hy).multiply(factH);
986 final T uz = hy.multiply(-2).multiply(factH);
987
988 final T vx = uy;
989 final T vy = (hy2.subtract(hx2).add(1)).multiply(factH);
990 final T vz = hx.multiply(factH).multiply(2);
991
992
993 final T exey = equEx.multiply(equEy);
994 final T ex2 = equEx.square();
995 final T ey2 = equEy.square();
996 final T e2 = ex2.add(ey2);
997 final T eta = e2.negate().add(1).sqrt().add(1);
998 final T beta = eta.reciprocal();
999
1000
1001 final FieldSinCos<T> scLe = FastMath.sinCos(lE);
1002 final T cLe = scLe.cos();
1003 final T sLe = scLe.sin();
1004
1005
1006 final T x = a.multiply(beta.negate().multiply(ey2).add(1).multiply(cLe).add(beta.multiply(exey).multiply(sLe)).subtract(equEx));
1007 final T y = a.multiply(beta.negate().multiply(ex2).add(1).multiply(sLe).add(beta.multiply(exey).multiply(cLe)).subtract(equEy));
1008
1009 return new FieldVector3D<>(x.multiply(ux).add(y.multiply(vx)),
1010 x.multiply(uy).add(y.multiply(vy)),
1011 x.multiply(uz).add(y.multiply(vz)));
1012
1013 }
1014
1015
1016 @Override
1017 protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {
1018
1019
1020 computePVWithoutA();
1021
1022
1023 final T r2 = partialPV.getPosition().getNorm2Sq();
1024 final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(r2.sqrt()).reciprocal().multiply(getMu().negate()),
1025 partialPV.getPosition());
1026 final FieldVector3D<T> acceleration = hasNonKeplerianRates() ?
1027 keplerianAcceleration.add(nonKeplerianAcceleration()) :
1028 keplerianAcceleration;
1029
1030 return new TimeStampedFieldPVCoordinates<>(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
1031
1032 }
1033
1034
1035 @Override
1036 public FieldCircularOrbit<T> inFrame(final Frame inertialFrame) {
1037 final FieldPVCoordinates<T> fieldPVCoordinates;
1038 if (hasNonKeplerianAcceleration()) {
1039 fieldPVCoordinates = getPVCoordinates(inertialFrame);
1040 } else {
1041 final FieldKinematicTransform<T> transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
1042 fieldPVCoordinates = transform.transformOnlyPV(getPVCoordinates());
1043 }
1044 final FieldCircularOrbit<T> fieldOrbit = new FieldCircularOrbit<>(fieldPVCoordinates, inertialFrame, getDate(), getMu());
1045 if (fieldOrbit.getCachedPositionAngleType() == getCachedPositionAngleType()) {
1046 return fieldOrbit;
1047 } else {
1048 return fieldOrbit.withCachedPositionAngleType(getCachedPositionAngleType());
1049 }
1050 }
1051
1052
1053 @Override
1054 public FieldCircularOrbit<T> withCachedPositionAngleType(final PositionAngleType positionAngleType) {
1055 return new FieldCircularOrbit<>(a, ex, ey, i, raan, getAlpha(positionAngleType), aDot, exDot, eyDot, iDot, raanDot,
1056 getAlphaDot(positionAngleType), positionAngleType, getFrame(), getDate(), getMu());
1057 }
1058
1059
1060 @Override
1061 public FieldCircularOrbit<T> shiftedBy(final double dt) {
1062 return shiftedBy(getZero().newInstance(dt));
1063 }
1064
1065
1066 @Override
1067 public FieldCircularOrbit<T> shiftedBy(final T dt) {
1068
1069
1070 final FieldCircularOrbit<T> keplerianShifted = this.keplerianShiftedBy(dt);
1071
1072
1073 if (!dt.isZero() && hasNonKeplerianRates()) {
1074 return new FieldCircularOrbit<>(shiftPVNonKeplerian(keplerianShifted.getPVCoordinates(), dt),
1075 getFrame(), getDate().shiftedBy(dt), getMu());
1076 }
1077
1078 else {
1079 return keplerianShifted;
1080 }
1081
1082 }
1083
1084
1085
1086
1087
1088
1089 @Override
1090 public FieldCircularOrbit<T> shiftedBy(final TimeOffset dt) {
1091
1092
1093 final Field<T> field = getField();
1094 final T dtValue = field.getOne().newInstance(dt.toDouble());
1095
1096
1097 final FieldCircularOrbit<T> keplerianShifted = keplerianShiftedBy(dt);
1098
1099
1100 if (!dt.isZero() && hasNonKeplerianRates()) {
1101 return new FieldCircularOrbit<>(shiftPVNonKeplerian(keplerianShifted.getPVCoordinates(), dtValue),
1102 getFrame(), getDate().shiftedBy(dt), getMu());
1103 }
1104
1105 else {
1106 return keplerianShifted;
1107 }
1108
1109 }
1110
1111
1112
1113
1114
1115
1116
1117 protected FieldCircularOrbit<T> keplerianShiftedBy(final T dt) {
1118 return new FieldCircularOrbit<>(a, ex, ey, i, raan,
1119 getAlphaM().add(getKeplerianMeanMotion().multiply(dt)),
1120 PositionAngleType.MEAN,
1121 cachedPositionAngleType,
1122 getFrame(),
1123 getDate().shiftedBy(dt),
1124 getMu());
1125 }
1126
1127
1128
1129
1130
1131
1132
1133
1134 private FieldCircularOrbit<T> keplerianShiftedBy(final TimeOffset dt) {
1135 return new FieldCircularOrbit<>(a, ex, ey, i, raan,
1136 getAlphaM().add(getKeplerianMeanMotion().multiply(dt.toDouble())),
1137 PositionAngleType.MEAN,
1138 cachedPositionAngleType,
1139 getFrame(),
1140 getDate().shiftedBy(dt),
1141 getMu());
1142 }
1143
1144
1145 @Override
1146 protected T[][] computeJacobianMeanWrtCartesian() {
1147
1148 final T[][] jacobian = MathArrays.buildArray(getOne().getField(), 6, 6);
1149
1150
1151 computePVWithoutA();
1152 final FieldVector3D<T> position = partialPV.getPosition();
1153 final FieldVector3D<T> velocity = partialPV.getVelocity();
1154
1155 final T x = position.getX();
1156 final T y = position.getY();
1157 final T z = position.getZ();
1158 final T vx = velocity.getX();
1159 final T vy = velocity.getY();
1160 final T vz = velocity.getZ();
1161 final T pv = FieldVector3D.dotProduct(position, velocity);
1162 final T r2 = position.getNorm2Sq();
1163 final T r = r2.sqrt();
1164 final T v2 = velocity.getNorm2Sq();
1165
1166 final T mu = getMu();
1167 final T oOsqrtMuA = getOne().divide(a.multiply(mu).sqrt());
1168 final T rOa = r.divide(a);
1169 final T aOr = a.divide(r);
1170 final T aOr2 = a.divide(r2);
1171 final T a2 = a.square();
1172
1173 final T ex2 = ex.square();
1174 final T ey2 = ey.square();
1175 final T e2 = ex2.add(ey2);
1176 final T epsilon = e2.negate().add(1.0).sqrt();
1177 final T beta = epsilon.add(1).reciprocal();
1178
1179 final T eCosE = rOa.negate().add(1);
1180 final T eSinE = pv.multiply(oOsqrtMuA);
1181
1182 final FieldSinCos<T> scI = FastMath.sinCos(i);
1183 final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
1184 final T cosI = scI.cos();
1185 final T sinI = scI.sin();
1186 final T cosRaan = scRaan.cos();
1187 final T sinRaan = scRaan.sin();
1188
1189
1190 fillHalfRow(aOr.multiply(2.0).multiply(aOr2), position, jacobian[0], 0);
1191 fillHalfRow(a2.multiply(mu.divide(2.).reciprocal()), velocity, jacobian[0], 3);
1192
1193
1194 final FieldVector3D<T> danP = new FieldVector3D<>(v2, position, pv.negate(), velocity);
1195 final FieldVector3D<T> danV = new FieldVector3D<>(r2, velocity, pv.negate(), position);
1196 final T recip = partialPV.getMomentum().getNorm().reciprocal();
1197 final T recip2 = recip.multiply(recip);
1198 final T recip2N = recip2.negate();
1199 final FieldVector3D<T> dwXP = new FieldVector3D<>(recip,
1200 new FieldVector3D<>(getZero(), vz, vy.negate()),
1201 recip2N.multiply(sinRaan).multiply(sinI),
1202 danP);
1203 final FieldVector3D<T> dwYP = new FieldVector3D<>(recip,
1204 new FieldVector3D<>(vz.negate(), getZero(), vx),
1205 recip2.multiply(cosRaan).multiply(sinI),
1206 danP);
1207 final FieldVector3D<T> dwZP = new FieldVector3D<>(recip,
1208 new FieldVector3D<>(vy, vx.negate(), getZero()),
1209 recip2N.multiply(cosI),
1210 danP);
1211 final FieldVector3D<T> dwXV = new FieldVector3D<>(recip,
1212 new FieldVector3D<>(getZero(), z.negate(), y),
1213 recip2N.multiply(sinRaan).multiply(sinI),
1214 danV);
1215 final FieldVector3D<T> dwYV = new FieldVector3D<>(recip,
1216 new FieldVector3D<>(z, getZero(), x.negate()),
1217 recip2.multiply(cosRaan).multiply(sinI),
1218 danV);
1219 final FieldVector3D<T> dwZV = new FieldVector3D<>(recip,
1220 new FieldVector3D<>(y.negate(), x, getZero()),
1221 recip2N.multiply(cosI),
1222 danV);
1223
1224
1225 fillHalfRow(sinRaan.multiply(cosI), dwXP, cosRaan.negate().multiply(cosI), dwYP, sinI.negate(), dwZP, jacobian[3], 0);
1226 fillHalfRow(sinRaan.multiply(cosI), dwXV, cosRaan.negate().multiply(cosI), dwYV, sinI.negate(), dwZV, jacobian[3], 3);
1227
1228
1229 fillHalfRow(sinRaan.divide(sinI), dwYP, cosRaan.divide(sinI), dwXP, jacobian[4], 0);
1230 fillHalfRow(sinRaan.divide(sinI), dwYV, cosRaan.divide(sinI), dwXV, jacobian[4], 3);
1231
1232
1233
1234 final T u = x.multiply(cosRaan).add(y.multiply(sinRaan));
1235 final T cv = x.negate().multiply(sinRaan).add(y.multiply(cosRaan));
1236 final T v = cv.multiply(cosI).add(z.multiply(sinI));
1237
1238
1239 final FieldVector3D<T> duP = new FieldVector3D<>(cv.multiply(cosRaan).divide(sinI), dwXP,
1240 cv.multiply(sinRaan).divide(sinI), dwYP,
1241 getOne(), new FieldVector3D<>(cosRaan, sinRaan, getZero()));
1242 final FieldVector3D<T> duV = new FieldVector3D<>(cv.multiply(cosRaan).divide(sinI), dwXV,
1243 cv.multiply(sinRaan).divide(sinI), dwYV);
1244
1245
1246 final FieldVector3D<T> dvP = new FieldVector3D<>(u.negate().multiply(cosRaan).multiply(cosI).divide(sinI).add(sinRaan.multiply(z)), dwXP,
1247 u.negate().multiply(sinRaan).multiply(cosI).divide(sinI).subtract(cosRaan.multiply(z)), dwYP,
1248 cv, dwZP,
1249 getOne(), new FieldVector3D<>(sinRaan.negate().multiply(cosI), cosRaan.multiply(cosI), sinI));
1250 final FieldVector3D<T> dvV = new FieldVector3D<>(u.negate().multiply(cosRaan).multiply(cosI).divide(sinI).add(sinRaan.multiply(z)), dwXV,
1251 u.negate().multiply(sinRaan).multiply(cosI).divide(sinI).subtract(cosRaan.multiply(z)), dwYV,
1252 cv, dwZV);
1253
1254 final FieldVector3D<T> dc1P = new FieldVector3D<>(aOr2.multiply(eSinE.multiply(eSinE).multiply(2).add(1).subtract(eCosE)).divide(r2), position,
1255 aOr2.multiply(-2).multiply(eSinE).multiply(oOsqrtMuA), velocity);
1256 final FieldVector3D<T> dc1V = new FieldVector3D<>(aOr2.multiply(-2).multiply(eSinE).multiply(oOsqrtMuA), position,
1257 getZero().newInstance(2).divide(mu), velocity);
1258 final FieldVector3D<T> dc2P = new FieldVector3D<>(aOr2.multiply(eSinE).multiply(eSinE.multiply(eSinE).subtract(e2.negate().add(1))).divide(r2.multiply(epsilon)), position,
1259 aOr2.multiply(e2.negate().add(1).subtract(eSinE.multiply(eSinE))).multiply(oOsqrtMuA).divide(epsilon), velocity);
1260 final FieldVector3D<T> dc2V = new FieldVector3D<>(aOr2.multiply(e2.negate().add(1).subtract(eSinE.multiply(eSinE))).multiply(oOsqrtMuA).divide(epsilon), position,
1261 eSinE.divide(epsilon.multiply(mu)), velocity);
1262
1263 final T cof1 = aOr2.multiply(eCosE.subtract(e2));
1264 final T cof2 = aOr2.multiply(epsilon).multiply(eSinE);
1265 final FieldVector3D<T> dexP = new FieldVector3D<>(u, dc1P, v, dc2P, cof1, duP, cof2, dvP);
1266 final FieldVector3D<T> dexV = new FieldVector3D<>(u, dc1V, v, dc2V, cof1, duV, cof2, dvV);
1267 final FieldVector3D<T> deyP = new FieldVector3D<>(v, dc1P, u.negate(), dc2P, cof1, dvP, cof2.negate(), duP);
1268 final FieldVector3D<T> deyV = new FieldVector3D<>(v, dc1V, u.negate(), dc2V, cof1, dvV, cof2.negate(), duV);
1269 fillHalfRow(getOne(), dexP, jacobian[1], 0);
1270 fillHalfRow(getOne(), dexV, jacobian[1], 3);
1271 fillHalfRow(getOne(), deyP, jacobian[2], 0);
1272 fillHalfRow(getOne(), deyV, jacobian[2], 3);
1273
1274 final T cle = u.divide(a).add(ex).subtract(eSinE.multiply(beta).multiply(ey));
1275 final T sle = v.divide(a).add(ey).add(eSinE.multiply(beta).multiply(ex));
1276 final T m1 = beta.multiply(eCosE);
1277 final T m2 = m1.multiply(eCosE).negate().add(1);
1278 final T m3 = (u.multiply(ey).subtract(v.multiply(ex))).add(eSinE.multiply(beta).multiply(u.multiply(ex).add(v.multiply(ey))));
1279 final T m4 = sle.negate().add(cle.multiply(eSinE).multiply(beta));
1280 final T m5 = cle.add(sle.multiply(eSinE).multiply(beta));
1281 final T kk = m3.multiply(2).divide(r).add(aOr.multiply(eSinE)).add(m1.multiply(eSinE).multiply(m1.add(1).subtract(aOr.add(1).multiply(m2))).divide(epsilon)).divide(r2);
1282 final T jj = (m1.multiply(m2).divide(epsilon).subtract(1)).multiply(oOsqrtMuA);
1283 fillHalfRow(kk, position,
1284 jj, velocity,
1285 m4, dexP,
1286 m5, deyP,
1287 sle.negate().divide(a), duP,
1288 cle.divide(a), dvP,
1289 jacobian[5], 0);
1290 final T ll = (m1.multiply(m2).divide(epsilon ).subtract(1)).multiply(oOsqrtMuA);
1291 final T mm = m3.multiply(2).add(eSinE.multiply(a)).add(m1.multiply(eSinE).multiply(r).multiply(eCosE.multiply(beta).multiply(2).subtract(aOr.multiply(m2))).divide(epsilon)).divide(mu);
1292
1293 fillHalfRow(ll, position,
1294 mm, velocity,
1295 m4, dexV,
1296 m5, deyV,
1297 sle.negate().divide(a), duV,
1298 cle.divide(a), dvV,
1299 jacobian[5], 3);
1300 return jacobian;
1301
1302 }
1303
1304
1305 @Override
1306 protected T[][] computeJacobianEccentricWrtCartesian() {
1307
1308
1309 final T[][] jacobian = computeJacobianMeanWrtCartesian();
1310
1311
1312
1313
1314
1315 final T alphaE = getAlphaE();
1316 final FieldSinCos<T> scAe = FastMath.sinCos(alphaE);
1317 final T cosAe = scAe.cos();
1318 final T sinAe = scAe.sin();
1319 final T aOr = getOne().divide(getOne().subtract(ex.multiply(cosAe)).subtract(ey.multiply(sinAe)));
1320
1321
1322 final T[] rowEx = jacobian[1];
1323 final T[] rowEy = jacobian[2];
1324 final T[] rowL = jacobian[5];
1325 for (int j = 0; j < 6; ++j) {
1326
1327 rowL[j] = aOr.multiply(rowL[j].add(sinAe.multiply(rowEx[j])).subtract( cosAe.multiply(rowEy[j])));
1328 }
1329 jacobian[5] = rowL;
1330 return jacobian;
1331
1332 }
1333
1334 @Override
1335 protected T[][] computeJacobianTrueWrtCartesian() {
1336
1337
1338 final T[][] jacobian = computeJacobianEccentricWrtCartesian();
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351 final T alphaE = getAlphaE();
1352 final FieldSinCos<T> scAe = FastMath.sinCos(alphaE);
1353 final T cosAe = scAe.cos();
1354 final T sinAe = scAe.sin();
1355 final T eSinE = ex.multiply(sinAe).subtract(ey.multiply(cosAe));
1356 final T ecosE = ex.multiply(cosAe).add(ey.multiply(sinAe));
1357 final T e2 = ex.multiply(ex).add(ey.multiply(ey));
1358 final T epsilon = (getOne().subtract(e2)).sqrt();
1359 final T onePeps = getOne().add(epsilon);
1360 final T d = onePeps.subtract(ecosE);
1361 final T cT = (d.multiply(d).add(eSinE.multiply(eSinE))).divide(2);
1362 final T cE = ecosE.multiply(onePeps).subtract(e2);
1363 final T cX = ex.multiply(eSinE).divide(epsilon).subtract(ey).add(sinAe.multiply(onePeps));
1364 final T cY = ey.multiply(eSinE).divide(epsilon).add(ex).subtract(cosAe.multiply(onePeps));
1365 final T factorLe = (cT.add(cE)).divide(cT);
1366 final T factorEx = cX.divide(cT);
1367 final T factorEy = cY.divide(cT);
1368
1369
1370 final T[] rowEx = jacobian[1];
1371 final T[] rowEy = jacobian[2];
1372 final T[] rowA = jacobian[5];
1373 for (int j = 0; j < 6; ++j) {
1374 rowA[j] = factorLe.multiply(rowA[j]).add(factorEx.multiply(rowEx[j])).add(factorEy.multiply(rowEy[j]));
1375 }
1376 return jacobian;
1377
1378 }
1379
1380
1381 @Override
1382 public void addKeplerContribution(final PositionAngleType type, final T gm, final T[] pDot) {
1383 pDot[5] = pDot[5].add(computeKeplerianAlphaDot(type, a, ex, ey, gm, cachedAlpha, cachedPositionAngleType));
1384 }
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399 private static <T extends CalculusFieldElement<T>> T computeKeplerianAlphaDot(final PositionAngleType type, final T a,
1400 final T ex, final T ey, final T mu,
1401 final T alpha, final PositionAngleType cachedType) {
1402 final T n = a.reciprocal().multiply(mu).sqrt().divide(a);
1403 if (type == PositionAngleType.MEAN) {
1404 return n;
1405 }
1406 final FieldSinCos<T> sc;
1407 final T ksi;
1408 if (type == PositionAngleType.ECCENTRIC) {
1409 sc = FastMath.sinCos(FieldCircularLatitudeArgumentUtility.convertAlpha(cachedType, alpha, ex, ey, type));
1410 ksi = ((ex.multiply(sc.cos())).add(ey.multiply(sc.sin()))).negate().add(1).reciprocal();
1411 return n.multiply(ksi);
1412 } else {
1413 sc = FastMath.sinCos(FieldCircularLatitudeArgumentUtility.convertAlpha(cachedType, alpha, ex, ey, type));
1414 final T one = n.getField().getOne();
1415 final T oMe2 = one.subtract(ex.square()).subtract(ey.square());
1416 ksi = one.add(ex.multiply(sc.cos())).add(ey.multiply(sc.sin()));
1417 return n.multiply(ksi.square()).divide(oMe2.multiply(oMe2.sqrt()));
1418 }
1419 }
1420
1421
1422
1423
1424 public String toString() {
1425 return "circular parameters: " + '{' +
1426 "a: " + a.getReal() +
1427 ", ex: " + ex.getReal() + ", ey: " + ey.getReal() +
1428 ", i: " + FastMath.toDegrees(i.getReal()) +
1429 ", raan: " + FastMath.toDegrees(raan.getReal()) +
1430 ", alphaV: " + FastMath.toDegrees(getAlphaV().getReal()) +
1431 ";}";
1432 }
1433
1434
1435 @Override
1436 public PositionAngleType getCachedPositionAngleType() {
1437 return cachedPositionAngleType;
1438 }
1439
1440
1441 @Override
1442 public boolean hasNonKeplerianRates() {
1443 return hasNonKeplerianAcceleration();
1444 }
1445
1446
1447 @Override
1448 public FieldCircularOrbit<T> withKeplerianRates() {
1449 return new FieldCircularOrbit<>(getCircularParameters(), getFrame(), getDate(), getMu());
1450 }
1451
1452
1453 @Override
1454 public CircularOrbit toOrbit() {
1455 final double cachedPositionAngle = cachedAlpha.getReal();
1456 return new CircularOrbit(a.getReal(), ex.getReal(), ey.getReal(),
1457 i.getReal(), raan.getReal(), cachedPositionAngle,
1458 aDot.getReal(), exDot.getReal(), eyDot.getReal(),
1459 iDot.getReal(), raanDot.getReal(), cachedAlphaDot.getReal(),
1460 cachedPositionAngleType, getFrame(),
1461 getDate().toAbsoluteDate(), getMu().getReal());
1462 }
1463
1464
1465 }