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