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