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