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