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