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 switch (op) {
129 case EquinoctialOrbit orbit1 -> equinoctial = orbit1;
130 case CartesianOrbit orbit -> equinoctial = orbit.equinoctial;
131 case null, default -> equinoctial = null;
132 }
133 }
134
135
136 public OrbitType getType() {
137 return OrbitType.CARTESIAN;
138 }
139
140
141 @Override
142 protected Vector3D nonKeplerianAcceleration() {
143 final double norm = getPosition().getNorm();
144 return getPVCoordinates().getAcceleration().add(new Vector3D(getMu() / (norm * norm * norm), getPosition()));
145 }
146
147
148 private void initEquinoctial() {
149 if (equinoctial == null) {
150 if (hasNonKeplerianAcceleration()) {
151
152 equinoctial = new EquinoctialOrbit(getPVCoordinates(), getFrame(), getDate(), getMu());
153 } else {
154
155
156 equinoctial = new EquinoctialOrbit(new PVCoordinates(getPosition(),
157 getPVCoordinates().getVelocity()),
158 getFrame(), getDate(), getMu());
159 }
160 }
161 }
162
163
164
165
166
167 private FieldPVCoordinates<UnivariateDerivative2> getPVDerivatives() {
168
169 final PVCoordinates pva = getPVCoordinates();
170 final Vector3D p = pva.getPosition();
171 final Vector3D v = pva.getVelocity();
172 final Vector3D a = pva.getAcceleration();
173
174 final FieldVector3D<UnivariateDerivative2> pG = new FieldVector3D<>(new UnivariateDerivative2(p.getX(), v.getX(), a.getX()),
175 new UnivariateDerivative2(p.getY(), v.getY(), a.getY()),
176 new UnivariateDerivative2(p.getZ(), v.getZ(), a.getZ()));
177 final FieldVector3D<UnivariateDerivative2> vG = new FieldVector3D<>(new UnivariateDerivative2(v.getX(), a.getX(), 0.0),
178 new UnivariateDerivative2(v.getY(), a.getY(), 0.0),
179 new UnivariateDerivative2(v.getZ(), a.getZ(), 0.0));
180 return new FieldPVCoordinates<>(pG, vG);
181 }
182
183
184 public double getA() {
185 final double r = getPosition().getNorm();
186 final double V2 = getPVCoordinates().getVelocity().getNorm2Sq();
187 return r / (2 - r * V2 / getMu());
188 }
189
190
191 public double getADot() {
192 if (hasNonKeplerianAcceleration) {
193 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
194 final UnivariateDerivative2 r = pv.getPosition().getNorm();
195 final UnivariateDerivative2 V2 = pv.getVelocity().getNorm2Sq();
196 final UnivariateDerivative2 a = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
197 return a.getDerivative(1);
198 } else {
199 return 0.;
200 }
201 }
202
203
204 public double getE() {
205 final double muA = getMu() * getA();
206 if (isElliptical()) {
207
208 final Vector3D pvP = getPosition();
209 final Vector3D pvV = getPVCoordinates().getVelocity();
210 final double rV2OnMu = pvP.getNorm() * pvV.getNorm2Sq() / getMu();
211 final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
212 final double eCE = rV2OnMu - 1;
213 return FastMath.sqrt(eCE * eCE + eSE * eSE);
214 } else {
215
216 final Vector3D pvM = getPVCoordinates().getMomentum();
217 return FastMath.sqrt(1 - pvM.getNorm2Sq() / muA);
218 }
219 }
220
221
222 public double getEDot() {
223 if (hasNonKeplerianAcceleration) {
224 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
225 final FieldVector3D<UnivariateDerivative2> pvP = pv.getPosition();
226 final FieldVector3D<UnivariateDerivative2> pvV = pv.getVelocity();
227 final UnivariateDerivative2 r = pvP.getNorm();
228 final UnivariateDerivative2 V2 = pvV.getNorm2Sq();
229 final UnivariateDerivative2 rV2OnMu = r.multiply(V2).divide(getMu());
230 final UnivariateDerivative2 a = r.divide(rV2OnMu.negate().add(2));
231 final UnivariateDerivative2 eSE = FieldVector3D.dotProduct(pvP, pvV).divide(a.multiply(getMu()).sqrt());
232 final UnivariateDerivative2 eCE = rV2OnMu.subtract(1);
233 final UnivariateDerivative2 e = eCE.multiply(eCE).add(eSE.multiply(eSE)).sqrt();
234 return e.getDerivative(1);
235 } else {
236 return 0.;
237 }
238 }
239
240
241 public double getI() {
242 return Vector3D.angle(Vector3D.PLUS_K, getPVCoordinates().getMomentum());
243 }
244
245
246 public double getIDot() {
247 if (hasNonKeplerianAcceleration) {
248 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
249 final FieldVector3D<UnivariateDerivative2> momentum =
250 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity());
251 final UnivariateDerivative2 i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
252 return i.getDerivative(1);
253 } else {
254 return 0.;
255 }
256 }
257
258
259 public double getEquinoctialEx() {
260 initEquinoctial();
261 return equinoctial.getEquinoctialEx();
262 }
263
264
265 public double getEquinoctialExDot() {
266 initEquinoctial();
267 return equinoctial.getEquinoctialExDot();
268 }
269
270
271 public double getEquinoctialEy() {
272 initEquinoctial();
273 return equinoctial.getEquinoctialEy();
274 }
275
276
277 public double getEquinoctialEyDot() {
278 initEquinoctial();
279 return equinoctial.getEquinoctialEyDot();
280 }
281
282
283 public double getHx() {
284 final Vector3D w = getPVCoordinates().getMomentum().normalize();
285
286 if ((w.getX() * w.getX() + w.getY() * w.getY()) == 0 && w.getZ() < 0) {
287 return Double.NaN;
288 }
289 return -w.getY() / (1 + w.getZ());
290 }
291
292
293 public double getHxDot() {
294 if (hasNonKeplerianAcceleration) {
295 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
296 final FieldVector3D<UnivariateDerivative2> w =
297 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
298
299 final double x = w.getX().getValue();
300 final double y = w.getY().getValue();
301 final double z = w.getZ().getValue();
302 if ((x * x + y * y) == 0 && z < 0) {
303 return Double.NaN;
304 }
305 final UnivariateDerivative2 hx = w.getY().negate().divide(w.getZ().add(1));
306 return hx.getDerivative(1);
307 } else {
308 return 0.;
309 }
310 }
311
312
313 public double getHy() {
314 final Vector3D w = getPVCoordinates().getMomentum().normalize();
315
316 if ((w.getX() * w.getX() + w.getY() * w.getY()) == 0 && w.getZ() < 0) {
317 return Double.NaN;
318 }
319 return w.getX() / (1 + w.getZ());
320 }
321
322
323 public double getHyDot() {
324 if (hasNonKeplerianAcceleration) {
325 final FieldPVCoordinates<UnivariateDerivative2> pv = getPVDerivatives();
326 final FieldVector3D<UnivariateDerivative2> w =
327 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
328
329 final double x = w.getX().getValue();
330 final double y = w.getY().getValue();
331 final double z = w.getZ().getValue();
332 if ((x * x + y * y) == 0 && z < 0) {
333 return Double.NaN;
334 }
335 final UnivariateDerivative2 hy = w.getX().divide(w.getZ().add(1));
336 return hy.getDerivative(1);
337 } else {
338 return 0.;
339 }
340 }
341
342
343 public double getLv() {
344 initEquinoctial();
345 return equinoctial.getLv();
346 }
347
348
349 public double getLvDot() {
350 initEquinoctial();
351 return equinoctial.getLvDot();
352 }
353
354
355 public double getLE() {
356 initEquinoctial();
357 return equinoctial.getLE();
358 }
359
360
361 public double getLEDot() {
362 initEquinoctial();
363 return equinoctial.getLEDot();
364 }
365
366
367 public double getLM() {
368 initEquinoctial();
369 return equinoctial.getLM();
370 }
371
372
373 public double getLMDot() {
374 initEquinoctial();
375 return equinoctial.getLMDot();
376 }
377
378
379 @Override
380 public boolean hasNonKeplerianAcceleration() {
381 return hasNonKeplerianAcceleration;
382 }
383
384
385 protected Vector3D initPosition() {
386
387 return getPVCoordinates().getPosition();
388 }
389
390
391 protected TimeStampedPVCoordinates initPVCoordinates() {
392
393 return getPVCoordinates();
394 }
395
396
397 @Override
398 public CartesianOrbit inFrame(final Frame inertialFrame) {
399 if (hasNonKeplerianAcceleration()) {
400 return new CartesianOrbit(getPVCoordinates(inertialFrame), inertialFrame, getMu());
401 } else {
402 final KinematicTransform transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
403 return new CartesianOrbit(transform.transformOnlyPV(getPVCoordinates()), inertialFrame, getDate(), getMu());
404 }
405 }
406
407
408 public CartesianOrbit shiftedBy(final double dt) {
409 final PVCoordinates shiftedPV = shiftPV(dt);
410 return new CartesianOrbit(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
411 }
412
413
414 public CartesianOrbit shiftedBy(final TimeOffset dt) {
415 final PVCoordinates shiftedPV = shiftPV(dt.toDouble());
416 return new CartesianOrbit(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
417 }
418
419
420
421
422
423 private PVCoordinates shiftPV(final double dt) {
424
425 final Vector3D pvP = getPosition();
426 final PVCoordinates shiftedPV = KeplerianMotionCartesianUtility.predictPositionVelocity(dt, pvP,
427 getPVCoordinates().getVelocity(), getMu());
428
429 if (dt != 0. && hasNonKeplerianAcceleration) {
430
431 return shiftNonKeplerian(shiftedPV, dt);
432
433 } else {
434
435
436 return shiftedPV;
437 }
438
439 }
440
441 @Override
442 protected double[][] computeJacobianMeanWrtCartesian() {
443 return SIX_BY_SIX_IDENTITY;
444 }
445
446 @Override
447 protected double[][] computeJacobianEccentricWrtCartesian() {
448 return SIX_BY_SIX_IDENTITY;
449 }
450
451 @Override
452 protected double[][] computeJacobianTrueWrtCartesian() {
453 return SIX_BY_SIX_IDENTITY;
454 }
455
456
457 public void addKeplerContribution(final PositionAngleType type, final double gm,
458 final double[] pDot) {
459
460 final PVCoordinates pv = getPVCoordinates();
461
462
463 final Vector3D velocity = pv.getVelocity();
464 pDot[0] += velocity.getX();
465 pDot[1] += velocity.getY();
466 pDot[2] += velocity.getZ();
467
468
469 final Vector3D position = pv.getPosition();
470 final double r2 = position.getNorm2Sq();
471 final double coeff = -gm / (r2 * FastMath.sqrt(r2));
472 pDot[3] += coeff * position.getX();
473 pDot[4] += coeff * position.getY();
474 pDot[5] += coeff * position.getZ();
475
476 }
477
478
479
480
481 public String toString() {
482
483 final String comma = ", ";
484 final PVCoordinates pv = getPVCoordinates();
485 final Vector3D p = pv.getPosition();
486 final Vector3D v = pv.getVelocity();
487 return "Cartesian parameters: {P(" +
488 p.getX() + comma +
489 p.getY() + comma +
490 p.getZ() + "), V(" +
491 v.getX() + comma +
492 v.getY() + comma +
493 v.getZ() + ")}";
494 }
495
496 }