1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import java.io.Serializable;
20
21 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Rotation;
24 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.linear.MatrixUtils;
27 import org.hipparchus.util.FastMath;
28 import org.hipparchus.util.SinCos;
29 import org.orekit.annotation.DefaultDataContext;
30 import org.orekit.data.DataContext;
31 import org.orekit.frames.Frame;
32 import org.orekit.time.AbsoluteDate;
33 import org.orekit.utils.FieldPVCoordinates;
34 import org.orekit.utils.PVCoordinates;
35 import org.orekit.utils.TimeStampedPVCoordinates;
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 public class CartesianOrbit extends Orbit {
74
75
76 private static final long serialVersionUID = 20170414L;
77
78
79 private static final double[][] SIX_BY_SIX_IDENTITY = MatrixUtils.createRealIdentityMatrix(6).getData();
80
81
82 private final transient boolean hasNonKeplerianAcceleration;
83
84
85 private transient EquinoctialOrbit equinoctial;
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101 public CartesianOrbit(final TimeStampedPVCoordinates pvaCoordinates,
102 final Frame frame, final double mu)
103 throws IllegalArgumentException {
104 super(pvaCoordinates, frame, mu);
105 hasNonKeplerianAcceleration = hasNonKeplerianAcceleration(pvaCoordinates, mu);
106 equinoctial = null;
107 }
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124 public CartesianOrbit(final PVCoordinates pvaCoordinates, final Frame frame,
125 final AbsoluteDate date, final double mu)
126 throws IllegalArgumentException {
127 this(new TimeStampedPVCoordinates(date, pvaCoordinates), frame, mu);
128 }
129
130
131
132
133 public CartesianOrbit(final Orbit op) {
134 super(op.getPVCoordinates(), op.getFrame(), op.getMu());
135 hasNonKeplerianAcceleration = op.hasDerivatives();
136 if (op instanceof EquinoctialOrbit) {
137 equinoctial = (EquinoctialOrbit) op;
138 } else if (op instanceof CartesianOrbit) {
139 equinoctial = ((CartesianOrbit) op).equinoctial;
140 } else {
141 equinoctial = null;
142 }
143 }
144
145
146 public OrbitType getType() {
147 return OrbitType.CARTESIAN;
148 }
149
150
151 private void initEquinoctial() {
152 if (equinoctial == null) {
153 if (hasDerivatives()) {
154
155 equinoctial = new EquinoctialOrbit(getPVCoordinates(), getFrame(), getDate(), getMu());
156 } else {
157
158
159 equinoctial = new EquinoctialOrbit(new PVCoordinates(getPosition(),
160 getPVCoordinates().getVelocity()),
161 getFrame(), getDate(), getMu());
162 }
163 }
164 }
165
166
167
168
169
170 private FieldPVCoordinates<UnivariateDerivative2> getPVDerivatives() {
171
172 final PVCoordinates pva = getPVCoordinates();
173 final Vector3D p = pva.getPosition();
174 final Vector3D v = pva.getVelocity();
175 final Vector3D a = pva.getAcceleration();
176
177 final FieldVector3D<UnivariateDerivative2> pG = new FieldVector3D<>(new UnivariateDerivative2(p.getX(), v.getX(), a.getX()),
178 new UnivariateDerivative2(p.getY(), v.getY(), a.getY()),
179 new UnivariateDerivative2(p.getZ(), v.getZ(), a.getZ()));
180 final FieldVector3D<UnivariateDerivative2> vG = new FieldVector3D<>(new UnivariateDerivative2(v.getX(), a.getX(), 0.0),
181 new UnivariateDerivative2(v.getY(), a.getY(), 0.0),
182 new UnivariateDerivative2(v.getZ(), a.getZ(), 0.0));
183 return new FieldPVCoordinates<>(pG, vG);
184 }
185
186
187 public double getA() {
188 final double r = getPosition().getNorm();
189 final double V2 = getPVCoordinates().getVelocity().getNormSq();
190 return r / (2 - r * V2 / getMu());
191 }
192
193
194 public double getADot() {
195 if (hasDerivatives()) {
196 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
197 final UnivariateDerivative2 r = pv.getPosition().getNorm();
198 final UnivariateDerivative2 V2 = pv.getVelocity().getNormSq();
199 final UnivariateDerivative2 a = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
200 return a.getDerivative(1);
201 } else {
202 return Double.NaN;
203 }
204 }
205
206
207 public double getE() {
208 final double muA = getMu() * getA();
209 if (isElliptical()) {
210
211 final Vector3D pvP = getPosition();
212 final Vector3D pvV = getPVCoordinates().getVelocity();
213 final double rV2OnMu = pvP.getNorm() * pvV.getNormSq() / getMu();
214 final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
215 final double eCE = rV2OnMu - 1;
216 return FastMath.sqrt(eCE * eCE + eSE * eSE);
217 } else {
218
219 final Vector3D pvM = getPVCoordinates().getMomentum();
220 return FastMath.sqrt(1 - pvM.getNormSq() / muA);
221 }
222 }
223
224
225 public double getEDot() {
226 if (hasDerivatives()) {
227 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
228 final FieldVector3D<UnivariateDerivative2> pvP = pv.getPosition();
229 final FieldVector3D<UnivariateDerivative2> pvV = pv.getVelocity();
230 final UnivariateDerivative2 r = pvP.getNorm();
231 final UnivariateDerivative2 V2 = pvV.getNormSq();
232 final UnivariateDerivative2 rV2OnMu = r.multiply(V2).divide(getMu());
233 final UnivariateDerivative2 a = r.divide(rV2OnMu.negate().add(2));
234 final UnivariateDerivative2 eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
235 final UnivariateDerivative2 eCE = rV2OnMu.subtract(1);
236 final UnivariateDerivative2 e = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
237 return e.getDerivative(1);
238 } else {
239 return Double.NaN;
240 }
241 }
242
243
244 public double getI() {
245 return Vector3D.angle(Vector3D.PLUS_K, getPVCoordinates().getMomentum());
246 }
247
248
249 public double getIDot() {
250 if (hasDerivatives()) {
251 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
252 final FieldVector3D<UnivariateDerivative2> momentum =
253 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity());
254 final UnivariateDerivative2 i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
255 return i.getDerivative(1);
256 } else {
257 return Double.NaN;
258 }
259 }
260
261
262 public double getEquinoctialEx() {
263 initEquinoctial();
264 return equinoctial.getEquinoctialEx();
265 }
266
267
268 public double getEquinoctialExDot() {
269 initEquinoctial();
270 return equinoctial.getEquinoctialExDot();
271 }
272
273
274 public double getEquinoctialEy() {
275 initEquinoctial();
276 return equinoctial.getEquinoctialEy();
277 }
278
279
280 public double getEquinoctialEyDot() {
281 initEquinoctial();
282 return equinoctial.getEquinoctialEyDot();
283 }
284
285
286 public double getHx() {
287 final Vector3D w = getPVCoordinates().getMomentum().normalize();
288
289 if ((w.getX() * w.getX() + w.getY() * w.getY()) == 0 && w.getZ() < 0) {
290 return Double.NaN;
291 }
292 return -w.getY() / (1 + w.getZ());
293 }
294
295
296 public double getHxDot() {
297 if (hasDerivatives()) {
298 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
299 final FieldVector3D<UnivariateDerivative2> w =
300 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
301
302 final double x = w.getX().getValue();
303 final double y = w.getY().getValue();
304 final double z = w.getZ().getValue();
305 if ((x * x + y * y) == 0 && z < 0) {
306 return Double.NaN;
307 }
308 final UnivariateDerivative2 hx = w.getY().negate().divide(w.getZ().add(1));
309 return hx.getDerivative(1);
310 } else {
311 return Double.NaN;
312 }
313 }
314
315
316 public double getHy() {
317 final Vector3D w = getPVCoordinates().getMomentum().normalize();
318
319 if ((w.getX() * w.getX() + w.getY() * w.getY()) == 0 && w.getZ() < 0) {
320 return Double.NaN;
321 }
322 return w.getX() / (1 + w.getZ());
323 }
324
325
326 public double getHyDot() {
327 if (hasDerivatives()) {
328 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
329 final FieldVector3D<UnivariateDerivative2> w =
330 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
331
332 final double x = w.getX().getValue();
333 final double y = w.getY().getValue();
334 final double z = w.getZ().getValue();
335 if ((x * x + y * y) == 0 && z < 0) {
336 return Double.NaN;
337 }
338 final UnivariateDerivative2 hy = w.getX().divide(w.getZ().add(1));
339 return hy.getDerivative(1);
340 } else {
341 return Double.NaN;
342 }
343 }
344
345
346 public double getLv() {
347 initEquinoctial();
348 return equinoctial.getLv();
349 }
350
351
352 public double getLvDot() {
353 initEquinoctial();
354 return equinoctial.getLvDot();
355 }
356
357
358 public double getLE() {
359 initEquinoctial();
360 return equinoctial.getLE();
361 }
362
363
364 public double getLEDot() {
365 initEquinoctial();
366 return equinoctial.getLEDot();
367 }
368
369
370 public double getLM() {
371 initEquinoctial();
372 return equinoctial.getLM();
373 }
374
375
376 public double getLMDot() {
377 initEquinoctial();
378 return equinoctial.getLMDot();
379 }
380
381
382 public boolean hasDerivatives() {
383 return hasNonKeplerianAcceleration;
384 }
385
386
387 protected Vector3D initPosition() {
388
389 return getPVCoordinates().getPosition();
390 }
391
392
393 protected TimeStampedPVCoordinates initPVCoordinates() {
394
395 return getPVCoordinates();
396 }
397
398
399 public CartesianOrbit shiftedBy(final double dt) {
400 final PVCoordinates shiftedPV = isElliptical() ? shiftPVElliptic(dt) : shiftPVHyperbolic(dt);
401 return new CartesianOrbit(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
402 }
403
404
405
406
407
408 private PVCoordinates shiftPVElliptic(final double dt) {
409
410
411 final PVCoordinates pv = getPVCoordinates();
412 final Vector3D pvP = pv.getPosition();
413 final Vector3D pvV = pv.getVelocity();
414 final Vector3D pvM = pv.getMomentum();
415 final double r2 = pvP.getNormSq();
416 final double r = FastMath.sqrt(r2);
417 final double rV2OnMu = r * pvV.getNormSq() / getMu();
418 final double a = r / (2 - rV2OnMu);
419 final double muA = getMu() * a;
420
421
422 final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
423 final double eCE = rV2OnMu - 1;
424 final double E0 = FastMath.atan2(eSE, eCE);
425 final double M0 = E0 - eSE;
426
427 final double e = FastMath.sqrt(eCE * eCE + eSE * eSE);
428 final double sqrt = FastMath.sqrt((1 + e) / (1 - e));
429
430
431 final double v0 = 2 * FastMath.atan(sqrt * FastMath.tan(E0 / 2));
432 final Vector3D p = new Rotation(pvM, v0, RotationConvention.FRAME_TRANSFORM).applyTo(pvP).normalize();
433 final Vector3D q = Vector3D.crossProduct(pvM, p).normalize();
434
435
436 final double M1 = M0 + getKeplerianMeanMotion() * dt;
437 final double E1 = KeplerianAnomalyUtility.ellipticMeanToEccentric(e, M1);
438
439
440 final SinCos scE = FastMath.sinCos(E1);
441 final double cE = scE.cos();
442 final double sE = scE.sin();
443 final double sE2m1 = FastMath.sqrt((1 - e) * (1 + e));
444
445
446 final double x = a * (cE - e);
447 final double y = a * sE2m1 * sE;
448 final double factor = FastMath.sqrt(getMu() / a) / (1 - e * cE);
449 final double xDot = -factor * sE;
450 final double yDot = factor * sE2m1 * cE;
451
452 final Vector3D shiftedP = new Vector3D(x, p, y, q);
453 final Vector3D shiftedV = new Vector3D(xDot, p, yDot, q);
454 if (hasNonKeplerianAcceleration) {
455
456
457 final Vector3D nonKeplerianAcceleration = new Vector3D(1, getPVCoordinates().getAcceleration(),
458 getMu() / (r2 * r), pvP);
459
460
461 final Vector3D fixedP = new Vector3D(1, shiftedP,
462 0.5 * dt * dt, nonKeplerianAcceleration);
463 final double fixedR2 = fixedP.getNormSq();
464 final double fixedR = FastMath.sqrt(fixedR2);
465 final Vector3D fixedV = new Vector3D(1, shiftedV,
466 dt, nonKeplerianAcceleration);
467 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), shiftedP,
468 1, nonKeplerianAcceleration);
469
470 return new PVCoordinates(fixedP, fixedV, fixedA);
471
472 } else {
473
474
475 return new PVCoordinates(shiftedP, shiftedV);
476 }
477
478 }
479
480
481
482
483
484 private PVCoordinates shiftPVHyperbolic(final double dt) {
485
486 final PVCoordinates pv = getPVCoordinates();
487 final Vector3D pvP = pv.getPosition();
488 final Vector3D pvV = pv.getVelocity();
489 final Vector3D pvM = pv.getMomentum();
490 final double r2 = pvP.getNormSq();
491 final double r = FastMath.sqrt(r2);
492 final double rV2OnMu = r * pvV.getNormSq() / getMu();
493 final double a = getA();
494 final double muA = getMu() * a;
495 final double e = FastMath.sqrt(1 - Vector3D.dotProduct(pvM, pvM) / muA);
496 final double sqrt = FastMath.sqrt((e + 1) / (e - 1));
497
498
499 final double eSH = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA);
500 final double eCH = rV2OnMu - 1;
501 final double H0 = FastMath.log((eCH + eSH) / (eCH - eSH)) / 2;
502 final double M0 = e * FastMath.sinh(H0) - H0;
503
504
505 final double v0 = 2 * FastMath.atan(sqrt * FastMath.tanh(H0 / 2));
506 final Vector3D p = new Rotation(pvM, v0, RotationConvention.FRAME_TRANSFORM).applyTo(pvP).normalize();
507 final Vector3D q = Vector3D.crossProduct(pvM, p).normalize();
508
509
510 final double M1 = M0 + getKeplerianMeanMotion() * dt;
511 final double H1 = KeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M1);
512
513
514 final double cH = FastMath.cosh(H1);
515 final double sH = FastMath.sinh(H1);
516 final double sE2m1 = FastMath.sqrt((e - 1) * (e + 1));
517
518
519 final double x = a * (cH - e);
520 final double y = -a * sE2m1 * sH;
521 final double factor = FastMath.sqrt(getMu() / -a) / (e * cH - 1);
522 final double xDot = -factor * sH;
523 final double yDot = factor * sE2m1 * cH;
524
525 final Vector3D shiftedP = new Vector3D(x, p, y, q);
526 final Vector3D shiftedV = new Vector3D(xDot, p, yDot, q);
527 if (hasNonKeplerianAcceleration) {
528
529
530 final Vector3D nonKeplerianAcceleration = new Vector3D(1, getPVCoordinates().getAcceleration(),
531 getMu() / (r2 * r), pvP);
532
533
534 final Vector3D fixedP = new Vector3D(1, shiftedP,
535 0.5 * dt * dt, nonKeplerianAcceleration);
536 final double fixedR2 = fixedP.getNormSq();
537 final double fixedR = FastMath.sqrt(fixedR2);
538 final Vector3D fixedV = new Vector3D(1, shiftedV,
539 dt, nonKeplerianAcceleration);
540 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), shiftedP,
541 1, nonKeplerianAcceleration);
542
543 return new PVCoordinates(fixedP, fixedV, fixedA);
544
545 } else {
546
547
548 return new PVCoordinates(shiftedP, shiftedV);
549 }
550
551 }
552
553 @Override
554 protected double[][] computeJacobianMeanWrtCartesian() {
555 return SIX_BY_SIX_IDENTITY;
556 }
557
558 @Override
559 protected double[][] computeJacobianEccentricWrtCartesian() {
560 return SIX_BY_SIX_IDENTITY;
561 }
562
563 @Override
564 protected double[][] computeJacobianTrueWrtCartesian() {
565 return SIX_BY_SIX_IDENTITY;
566 }
567
568
569 public void addKeplerContribution(final PositionAngleType type, final double gm,
570 final double[] pDot) {
571
572 final PVCoordinates pv = getPVCoordinates();
573
574
575 final Vector3D velocity = pv.getVelocity();
576 pDot[0] += velocity.getX();
577 pDot[1] += velocity.getY();
578 pDot[2] += velocity.getZ();
579
580
581 final Vector3D position = pv.getPosition();
582 final double r2 = position.getNormSq();
583 final double coeff = -gm / (r2 * FastMath.sqrt(r2));
584 pDot[3] += coeff * position.getX();
585 pDot[4] += coeff * position.getY();
586 pDot[5] += coeff * position.getZ();
587
588 }
589
590
591
592
593 public String toString() {
594
595 final String comma = ", ";
596 final PVCoordinates pv = getPVCoordinates();
597 final Vector3D p = pv.getPosition();
598 final Vector3D v = pv.getVelocity();
599 return "Cartesian parameters: {P(" +
600 p.getX() + comma +
601 p.getY() + comma +
602 p.getZ() + "), V(" +
603 v.getX() + comma +
604 v.getY() + comma +
605 v.getZ() + ")}";
606 }
607
608
609
610
611
612
613
614
615
616 @DefaultDataContext
617 private Object writeReplace() {
618 return new DTO(this);
619 }
620
621
622 @DefaultDataContext
623 private static class DTO implements Serializable {
624
625
626 private static final long serialVersionUID = 20170414L;
627
628
629 private double[] d;
630
631
632 private final Frame frame;
633
634
635
636
637 private DTO(final CartesianOrbit orbit) {
638
639 final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();
640
641
642 final AbsoluteDate j2000Epoch =
643 DataContext.getDefault().getTimeScales().getJ2000Epoch();
644 final double epoch = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
645 final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));
646
647 if (orbit.hasDerivatives()) {
648 this.d = new double[] {
649 epoch, offset, orbit.getMu(),
650 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
651 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
652 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ()
653 };
654 } else {
655 this.d = new double[] {
656 epoch, offset, orbit.getMu(),
657 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
658 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ()
659 };
660 }
661
662 this.frame = orbit.getFrame();
663
664 }
665
666
667
668
669 private Object readResolve() {
670 final AbsoluteDate j2000Epoch =
671 DataContext.getDefault().getTimeScales().getJ2000Epoch();
672 if (d.length >= 12) {
673
674 return new CartesianOrbit(new TimeStampedPVCoordinates(j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
675 new Vector3D(d[3], d[ 4], d[ 5]),
676 new Vector3D(d[6], d[ 7], d[ 8]),
677 new Vector3D(d[9], d[10], d[11])),
678 frame, d[2]);
679 } else {
680
681 return new CartesianOrbit(new TimeStampedPVCoordinates(j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
682 new Vector3D(d[3], d[ 4], d[ 5]),
683 new Vector3D(d[6], d[ 7], d[ 8])),
684 frame, d[2]);
685 }
686 }
687
688 }
689
690 }