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