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.analysis.differentiation.UnivariateDerivative2;
20 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.linear.MatrixUtils;
23 import org.hipparchus.util.FastMath;
24 import org.orekit.frames.Frame;
25 import org.orekit.frames.KinematicTransform;
26 import org.orekit.time.AbsoluteDate;
27 import org.orekit.time.TimeOffset;
28 import org.orekit.utils.FieldPVCoordinates;
29 import org.orekit.utils.PVCoordinates;
30 import org.orekit.utils.TimeStampedPVCoordinates;
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68 public class CartesianOrbit extends Orbit {
69
70
71 private static final double[][] SIX_BY_SIX_IDENTITY = MatrixUtils.createRealIdentityMatrix(6).getData();
72
73
74 private final boolean hasNonKeplerianAcceleration;
75
76
77 private EquinoctialOrbit equinoctial;
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93 public CartesianOrbit(final TimeStampedPVCoordinates pvaCoordinates,
94 final Frame frame, final double mu)
95 throws IllegalArgumentException {
96 super(pvaCoordinates, frame, mu);
97 hasNonKeplerianAcceleration = hasNonKeplerianAcceleration(pvaCoordinates, mu);
98 equinoctial = null;
99 }
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116 public CartesianOrbit(final PVCoordinates pvaCoordinates, final Frame frame,
117 final AbsoluteDate date, final double mu)
118 throws IllegalArgumentException {
119 this(new TimeStampedPVCoordinates(date, pvaCoordinates), frame, mu);
120 }
121
122
123
124
125 public CartesianOrbit(final Orbit op) {
126 super(op.getPVCoordinates(), op.getFrame(), op.getMu());
127 hasNonKeplerianAcceleration = op.hasNonKeplerianAcceleration();
128 if (op instanceof EquinoctialOrbit) {
129 equinoctial = (EquinoctialOrbit) op;
130 } else if (op instanceof CartesianOrbit) {
131 equinoctial = ((CartesianOrbit) op).equinoctial;
132 } else {
133 equinoctial = null;
134 }
135 }
136
137
138 public OrbitType getType() {
139 return OrbitType.CARTESIAN;
140 }
141
142
143 @Override
144 protected Vector3D nonKeplerianAcceleration() {
145 final double norm = getPosition().getNorm();
146 return getPVCoordinates().getAcceleration().add(new Vector3D(getMu() / (norm * norm * norm), getPosition()));
147 }
148
149
150 private void initEquinoctial() {
151 if (equinoctial == null) {
152 if (hasNonKeplerianAcceleration()) {
153
154 equinoctial = new EquinoctialOrbit(getPVCoordinates(), getFrame(), getDate(), getMu());
155 } else {
156
157
158 equinoctial = new EquinoctialOrbit(new PVCoordinates(getPosition(),
159 getPVCoordinates().getVelocity()),
160 getFrame(), getDate(), getMu());
161 }
162 }
163 }
164
165
166
167
168
169 private FieldPVCoordinates<UnivariateDerivative2> getPVDerivatives() {
170
171 final PVCoordinates pva = getPVCoordinates();
172 final Vector3D p = pva.getPosition();
173 final Vector3D v = pva.getVelocity();
174 final Vector3D a = pva.getAcceleration();
175
176 final FieldVector3D<UnivariateDerivative2> pG = new FieldVector3D<>(new UnivariateDerivative2(p.getX(), v.getX(), a.getX()),
177 new UnivariateDerivative2(p.getY(), v.getY(), a.getY()),
178 new UnivariateDerivative2(p.getZ(), v.getZ(), a.getZ()));
179 final FieldVector3D<UnivariateDerivative2> vG = new FieldVector3D<>(new UnivariateDerivative2(v.getX(), a.getX(), 0.0),
180 new UnivariateDerivative2(v.getY(), a.getY(), 0.0),
181 new UnivariateDerivative2(v.getZ(), a.getZ(), 0.0));
182 return new FieldPVCoordinates<>(pG, vG);
183 }
184
185
186 public double getA() {
187 final double r = getPosition().getNorm();
188 final double V2 = getPVCoordinates().getVelocity().getNorm2Sq();
189 return r / (2 - r * V2 / getMu());
190 }
191
192
193 public double getADot() {
194 if (hasNonKeplerianAcceleration) {
195 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
196 final UnivariateDerivative2 r = pv.getPosition().getNorm();
197 final UnivariateDerivative2 V2 = pv.getVelocity().getNorm2Sq();
198 final UnivariateDerivative2 a = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
199 return a.getDerivative(1);
200 } else {
201 return 0.;
202 }
203 }
204
205
206 public double getE() {
207 final double muA = getMu() * getA();
208 if (isElliptical()) {
209
210 final Vector3D pvP = getPosition();
211 final Vector3D pvV = getPVCoordinates().getVelocity();
212 final double rV2OnMu = pvP.getNorm() * pvV.getNorm2Sq() / getMu();
213 final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
214 final double eCE = rV2OnMu - 1;
215 return FastMath.sqrt(eCE * eCE + eSE * eSE);
216 } else {
217
218 final Vector3D pvM = getPVCoordinates().getMomentum();
219 return FastMath.sqrt(1 - pvM.getNorm2Sq() / muA);
220 }
221 }
222
223
224 public double getEDot() {
225 if (hasNonKeplerianAcceleration) {
226 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
227 final FieldVector3D<UnivariateDerivative2> pvP = pv.getPosition();
228 final FieldVector3D<UnivariateDerivative2> pvV = pv.getVelocity();
229 final UnivariateDerivative2 r = pvP.getNorm();
230 final UnivariateDerivative2 V2 = pvV.getNorm2Sq();
231 final UnivariateDerivative2 rV2OnMu = r.multiply(V2).divide(getMu());
232 final UnivariateDerivative2 a = r.divide(rV2OnMu.negate().add(2));
233 final UnivariateDerivative2 eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
234 final UnivariateDerivative2 eCE = rV2OnMu.subtract(1);
235 final UnivariateDerivative2 e = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
236 return e.getDerivative(1);
237 } else {
238 return 0.;
239 }
240 }
241
242
243 public double getI() {
244 return Vector3D.angle(Vector3D.PLUS_K, getPVCoordinates().getMomentum());
245 }
246
247
248 public double getIDot() {
249 if (hasNonKeplerianAcceleration) {
250 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
251 final FieldVector3D<UnivariateDerivative2> momentum =
252 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity());
253 final UnivariateDerivative2 i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
254 return i.getDerivative(1);
255 } else {
256 return 0.;
257 }
258 }
259
260
261 public double getEquinoctialEx() {
262 initEquinoctial();
263 return equinoctial.getEquinoctialEx();
264 }
265
266
267 public double getEquinoctialExDot() {
268 initEquinoctial();
269 return equinoctial.getEquinoctialExDot();
270 }
271
272
273 public double getEquinoctialEy() {
274 initEquinoctial();
275 return equinoctial.getEquinoctialEy();
276 }
277
278
279 public double getEquinoctialEyDot() {
280 initEquinoctial();
281 return equinoctial.getEquinoctialEyDot();
282 }
283
284
285 public double getHx() {
286 final Vector3D w = getPVCoordinates().getMomentum().normalize();
287
288 if ((w.getX() * w.getX() + w.getY() * w.getY()) == 0 && w.getZ() < 0) {
289 return Double.NaN;
290 }
291 return -w.getY() / (1 + w.getZ());
292 }
293
294
295 public double getHxDot() {
296 if (hasNonKeplerianAcceleration) {
297 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
298 final FieldVector3D<UnivariateDerivative2> w =
299 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
300
301 final double x = w.getX().getValue();
302 final double y = w.getY().getValue();
303 final double z = w.getZ().getValue();
304 if ((x * x + y * y) == 0 && z < 0) {
305 return Double.NaN;
306 }
307 final UnivariateDerivative2 hx = w.getY().negate().divide(w.getZ().add(1));
308 return hx.getDerivative(1);
309 } else {
310 return 0.;
311 }
312 }
313
314
315 public double getHy() {
316 final Vector3D w = getPVCoordinates().getMomentum().normalize();
317
318 if ((w.getX() * w.getX() + w.getY() * w.getY()) == 0 && w.getZ() < 0) {
319 return Double.NaN;
320 }
321 return w.getX() / (1 + w.getZ());
322 }
323
324
325 public double getHyDot() {
326 if (hasNonKeplerianAcceleration) {
327 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
328 final FieldVector3D<UnivariateDerivative2> w =
329 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
330
331 final double x = w.getX().getValue();
332 final double y = w.getY().getValue();
333 final double z = w.getZ().getValue();
334 if ((x * x + y * y) == 0 && z < 0) {
335 return Double.NaN;
336 }
337 final UnivariateDerivative2 hy = w.getX().divide(w.getZ().add(1));
338 return hy.getDerivative(1);
339 } else {
340 return 0.;
341 }
342 }
343
344
345 public double getLv() {
346 initEquinoctial();
347 return equinoctial.getLv();
348 }
349
350
351 public double getLvDot() {
352 initEquinoctial();
353 return equinoctial.getLvDot();
354 }
355
356
357 public double getLE() {
358 initEquinoctial();
359 return equinoctial.getLE();
360 }
361
362
363 public double getLEDot() {
364 initEquinoctial();
365 return equinoctial.getLEDot();
366 }
367
368
369 public double getLM() {
370 initEquinoctial();
371 return equinoctial.getLM();
372 }
373
374
375 public double getLMDot() {
376 initEquinoctial();
377 return equinoctial.getLMDot();
378 }
379
380
381 @Override
382 public boolean hasNonKeplerianAcceleration() {
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 @Override
400 public CartesianOrbit inFrame(final Frame inertialFrame) {
401 if (hasNonKeplerianAcceleration()) {
402 return new CartesianOrbit(getPVCoordinates(inertialFrame), inertialFrame, getMu());
403 } else {
404 final KinematicTransform transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
405 return new CartesianOrbit(transform.transformOnlyPV(getPVCoordinates()), inertialFrame, getDate(), getMu());
406 }
407 }
408
409
410 public CartesianOrbit shiftedBy(final double dt) {
411 final PVCoordinates shiftedPV = shiftPV(dt);
412 return new CartesianOrbit(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
413 }
414
415
416 public CartesianOrbit shiftedBy(final TimeOffset dt) {
417 final PVCoordinates shiftedPV = shiftPV(dt.toDouble());
418 return new CartesianOrbit(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
419 }
420
421
422
423
424
425 private PVCoordinates shiftPV(final double dt) {
426
427 final Vector3D pvP = getPosition();
428 final PVCoordinates shiftedPV = KeplerianMotionCartesianUtility.predictPositionVelocity(dt, pvP,
429 getPVCoordinates().getVelocity(), getMu());
430
431 if (dt != 0. && hasNonKeplerianAcceleration) {
432
433 return shiftNonKeplerian(shiftedPV, dt);
434
435 } else {
436
437
438 return shiftedPV;
439 }
440
441 }
442
443 @Override
444 protected double[][] computeJacobianMeanWrtCartesian() {
445 return SIX_BY_SIX_IDENTITY;
446 }
447
448 @Override
449 protected double[][] computeJacobianEccentricWrtCartesian() {
450 return SIX_BY_SIX_IDENTITY;
451 }
452
453 @Override
454 protected double[][] computeJacobianTrueWrtCartesian() {
455 return SIX_BY_SIX_IDENTITY;
456 }
457
458
459 public void addKeplerContribution(final PositionAngleType type, final double gm,
460 final double[] pDot) {
461
462 final PVCoordinates pv = getPVCoordinates();
463
464
465 final Vector3D velocity = pv.getVelocity();
466 pDot[0] += velocity.getX();
467 pDot[1] += velocity.getY();
468 pDot[2] += velocity.getZ();
469
470
471 final Vector3D position = pv.getPosition();
472 final double r2 = position.getNorm2Sq();
473 final double coeff = -gm / (r2 * FastMath.sqrt(r2));
474 pDot[3] += coeff * position.getX();
475 pDot[4] += coeff * position.getY();
476 pDot[5] += coeff * position.getZ();
477
478 }
479
480
481
482
483 public String toString() {
484
485 final String comma = ", ";
486 final PVCoordinates pv = getPVCoordinates();
487 final Vector3D p = pv.getPosition();
488 final Vector3D v = pv.getVelocity();
489 return "Cartesian parameters: {P(" +
490 p.getX() + comma +
491 p.getY() + comma +
492 p.getZ() + "), V(" +
493 v.getX() + comma +
494 v.getY() + comma +
495 v.getZ() + ")}";
496 }
497
498 }