1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19
20 import java.util.Arrays;
21
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.Field;
24 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.hipparchus.util.MathArrays;
28 import org.orekit.frames.FieldKinematicTransform;
29 import org.orekit.frames.Frame;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.time.TimeOffset;
33 import org.orekit.utils.FieldPVCoordinates;
34 import org.orekit.utils.PVCoordinates;
35 import org.orekit.utils.TimeStampedFieldPVCoordinates;
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
74 public class FieldCartesianOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T> {
75
76
77 private final boolean hasNonKeplerianAcceleration;
78
79
80 private FieldEquinoctialOrbit<T> equinoctial;
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96 public FieldCartesianOrbit(final TimeStampedFieldPVCoordinates<T> pvaCoordinates,
97 final Frame frame, final T mu)
98 throws IllegalArgumentException {
99 super(pvaCoordinates, frame, mu);
100 hasNonKeplerianAcceleration = hasNonKeplerianAcceleration(pvaCoordinates, mu);
101 equinoctial = null;
102 }
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119 public FieldCartesianOrbit(final FieldPVCoordinates<T> pvaCoordinates, final Frame frame,
120 final FieldAbsoluteDate<T> date, final T mu)
121 throws IllegalArgumentException {
122 this(new TimeStampedFieldPVCoordinates<>(date, pvaCoordinates), frame, mu);
123 }
124
125
126
127
128 public FieldCartesianOrbit(final FieldOrbit<T> op) {
129 super(op.getPVCoordinates(), op.getFrame(), op.getMu());
130 hasNonKeplerianAcceleration = op.hasNonKeplerianAcceleration();
131 if (op instanceof FieldEquinoctialOrbit) {
132 equinoctial = (FieldEquinoctialOrbit<T>) op;
133 } else if (op instanceof FieldCartesianOrbit) {
134 equinoctial = ((FieldCartesianOrbit<T>) op).equinoctial;
135 } else {
136 equinoctial = null;
137 }
138 }
139
140
141
142
143
144
145
146 public FieldCartesianOrbit(final Field<T> field, final CartesianOrbit op) {
147 super(new TimeStampedFieldPVCoordinates<>(field, op.getPVCoordinates()), op.getFrame(),
148 field.getZero().newInstance(op.getMu()));
149 hasNonKeplerianAcceleration = op.hasNonKeplerianAcceleration();
150 if (op.isElliptical()) {
151 equinoctial = new FieldEquinoctialOrbit<>(field, new EquinoctialOrbit(op));
152 } else {
153 equinoctial = null;
154 }
155 }
156
157
158
159
160
161
162
163 public FieldCartesianOrbit(final Field<T> field, final Orbit op) {
164 this(field, new CartesianOrbit(op));
165 }
166
167
168 public OrbitParamsType getType() {
169 return OrbitParamsType.CARTESIAN;
170 }
171
172
173 private void initEquinoctial() {
174 if (equinoctial == null) {
175 if (hasNonKeplerianAcceleration()) {
176
177 equinoctial = new FieldEquinoctialOrbit<>(getPVCoordinates(), getFrame(), getDate(), getMu());
178 } else {
179
180
181 final FieldPVCoordinates<T> pva = getPVCoordinates();
182 equinoctial = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(pva.getPosition(), pva.getVelocity()),
183 getFrame(), getDate(), getMu());
184 }
185 }
186 }
187
188
189
190
191
192 private FieldPVCoordinates<FieldUnivariateDerivative2<T>> getPVDerivatives() {
193
194 final FieldPVCoordinates<T> pva = getPVCoordinates();
195 final FieldVector3D<T> p = pva.getPosition();
196 final FieldVector3D<T> v = pva.getVelocity();
197 final FieldVector3D<T> a = pva.getAcceleration();
198
199 final FieldVector3D<FieldUnivariateDerivative2<T>> pG = new FieldVector3D<>(new FieldUnivariateDerivative2<>(p.getX(), v.getX(), a.getX()),
200 new FieldUnivariateDerivative2<>(p.getY(), v.getY(), a.getY()),
201 new FieldUnivariateDerivative2<>(p.getZ(), v.getZ(), a.getZ()));
202 final FieldVector3D<FieldUnivariateDerivative2<T>> vG = new FieldVector3D<>(new FieldUnivariateDerivative2<>(v.getX(), a.getX(), getZero()),
203 new FieldUnivariateDerivative2<>(v.getY(), a.getY(), getZero()),
204 new FieldUnivariateDerivative2<>(v.getZ(), a.getZ(), getZero()));
205 return new FieldPVCoordinates<>(pG, vG);
206 }
207
208
209 public T getA() {
210
211 final FieldPVCoordinates<T> pva = getPVCoordinates();
212 final T r = pva.getPosition().getNorm();
213 final T V2 = pva.getVelocity().getNorm2Sq();
214 return r.divide(r.negate().multiply(V2).divide(getMu()).add(2));
215 }
216
217
218 public T getADot() {
219 if (hasNonKeplerianAcceleration()) {
220 final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
221 final FieldUnivariateDerivative2<T> r = pv.getPosition().getNorm();
222 final FieldUnivariateDerivative2<T> V2 = pv.getVelocity().getNorm2Sq();
223 final FieldUnivariateDerivative2<T> a = r.divide(r.multiply(V2).divide(getMu()).subtract(2).negate());
224 return a.getDerivative(1);
225 } else {
226 return getZero();
227 }
228 }
229
230
231 public T getE() {
232 final T muA = getA().multiply(getMu());
233 if (isElliptical()) {
234
235 final FieldPVCoordinates<T> pva = getPVCoordinates();
236 final FieldVector3D<T> pvP = pva.getPosition();
237 final FieldVector3D<T> pvV = pva.getVelocity();
238 final T rV2OnMu = pvP.getNorm().multiply(pvV.getNorm2Sq()).divide(getMu());
239 final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
240 final T eCE = rV2OnMu.subtract(1);
241 return (eCE.square().add(eSE.square())).sqrt();
242 } else {
243
244 final FieldVector3D<T> pvM = getPVCoordinates().getMomentum();
245 return pvM.getNorm2Sq().divide(muA).negate().add(1).sqrt();
246 }
247 }
248
249
250 public T getEDot() {
251 if (hasNonKeplerianAcceleration()) {
252 final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
253 final FieldUnivariateDerivative2<T> r = pv.getPosition().getNorm();
254 final FieldUnivariateDerivative2<T> V2 = pv.getVelocity().getNorm2Sq();
255 final FieldUnivariateDerivative2<T> rV2OnMu = r.multiply(V2).divide(getMu());
256 final FieldUnivariateDerivative2<T> a = r.divide(rV2OnMu.negate().add(2));
257 final FieldUnivariateDerivative2<T> eSE = FieldVector3D.dotProduct(pv.getPosition(), pv.getVelocity()).divide(a.multiply(getMu()).sqrt());
258 final FieldUnivariateDerivative2<T> eCE = rV2OnMu.subtract(1);
259 final FieldUnivariateDerivative2<T> e = eCE.square().add(eSE.square()).sqrt();
260 return e.getDerivative(1);
261 } else {
262 return getZero();
263 }
264 }
265
266
267 public T getI() {
268 return FieldVector3D.angle(new FieldVector3D<>(getZero(), getZero(), getOne()), getPVCoordinates().getMomentum());
269 }
270
271
272 public T getIDot() {
273 if (hasNonKeplerianAcceleration()) {
274 final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
275 final FieldVector3D<FieldUnivariateDerivative2<T>> momentum =
276 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity());
277 final FieldUnivariateDerivative2<T> i = FieldVector3D.angle(Vector3D.PLUS_K, momentum);
278 return i.getDerivative(1);
279 } else {
280 return getZero();
281 }
282 }
283
284
285 public T getEquinoctialEx() {
286 initEquinoctial();
287 return equinoctial.getEquinoctialEx();
288 }
289
290
291 public T getEquinoctialExDot() {
292 initEquinoctial();
293 return equinoctial.getEquinoctialExDot();
294 }
295
296
297 public T getEquinoctialEy() {
298 initEquinoctial();
299 return equinoctial.getEquinoctialEy();
300 }
301
302
303 public T getEquinoctialEyDot() {
304 initEquinoctial();
305 return equinoctial.getEquinoctialEyDot();
306 }
307
308
309 public T getHx() {
310 final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
311
312 final double x = w.getX().getReal();
313 final double y = w.getY().getReal();
314 final double z = w.getZ().getReal();
315 if ((x * x + y * y) == 0 && z < 0) {
316 return getZero().add(Double.NaN);
317 }
318 return w.getY().negate().divide(w.getZ().add(1));
319 }
320
321
322 public T getHxDot() {
323 if (hasNonKeplerianAcceleration()) {
324 final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
325 final FieldVector3D<FieldUnivariateDerivative2<T>> w =
326 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
327
328 final double x = w.getX().getValue().getReal();
329 final double y = w.getY().getValue().getReal();
330 final double z = w.getZ().getValue().getReal();
331 if ((x * x + y * y) == 0 && z < 0) {
332 return getZero().add(Double.NaN);
333 }
334 final FieldUnivariateDerivative2<T> hx = w.getY().negate().divide(w.getZ().add(1));
335 return hx.getDerivative(1);
336 } else {
337 return getZero();
338 }
339 }
340
341
342 public T getHy() {
343 final FieldVector3D<T> w = getPVCoordinates().getMomentum().normalize();
344
345 final double x = w.getX().getReal();
346 final double y = w.getY().getReal();
347 final double z = w.getZ().getReal();
348 if ((x * x + y * y) == 0 && z < 0) {
349 return getZero().add(Double.NaN);
350 }
351 return w.getX().divide(w.getZ().add(1));
352 }
353
354
355 public T getHyDot() {
356 if (hasNonKeplerianAcceleration()) {
357 final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv = getPVDerivatives();
358 final FieldVector3D<FieldUnivariateDerivative2<T>> w =
359 FieldVector3D.crossProduct(pv.getPosition(), pv.getVelocity()).normalize();
360
361 final double x = w.getX().getValue().getReal();
362 final double y = w.getY().getValue().getReal();
363 final double z = w.getZ().getValue().getReal();
364 if ((x * x + y * y) == 0 && z < 0) {
365 return getZero().add(Double.NaN);
366 }
367 final FieldUnivariateDerivative2<T> hy = w.getX().divide(w.getZ().add(1));
368 return hy.getDerivative(1);
369 } else {
370 return getZero();
371 }
372 }
373
374
375 public T getLv() {
376 initEquinoctial();
377 return equinoctial.getLv();
378 }
379
380
381 public T getLvDot() {
382 initEquinoctial();
383 return equinoctial.getLvDot();
384 }
385
386
387 public T getLE() {
388 initEquinoctial();
389 return equinoctial.getLE();
390 }
391
392
393 public T getLEDot() {
394 initEquinoctial();
395 return equinoctial.getLEDot();
396 }
397
398
399 public T getLM() {
400 initEquinoctial();
401 return equinoctial.getLM();
402 }
403
404
405 public T getLMDot() {
406 initEquinoctial();
407 return equinoctial.getLMDot();
408 }
409
410
411 @Override
412 public boolean hasNonKeplerianAcceleration() {
413 return hasNonKeplerianAcceleration;
414 }
415
416
417 @Override
418 protected FieldVector3D<T> nonKeplerianAcceleration() {
419 final T norm = getPosition().getNorm();
420 final T factor = getMu().divide(norm.square().multiply(norm));
421 return getPVCoordinates().getAcceleration().add(new FieldVector3D<>(factor, getPosition()));
422 }
423
424
425 protected FieldVector3D<T> initPosition() {
426
427 return getPVCoordinates().getPosition();
428 }
429
430
431 protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {
432
433 return getPVCoordinates();
434 }
435
436
437 @Override
438 public FieldCartesianOrbit<T> inFrame(final Frame inertialFrame) {
439 if (hasNonKeplerianAcceleration()) {
440 return new FieldCartesianOrbit<>(getPVCoordinates(inertialFrame), inertialFrame, getMu());
441 } else {
442 final FieldKinematicTransform<T> transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
443 return new FieldCartesianOrbit<>(transform.transformOnlyPV(getPVCoordinates()), inertialFrame, getDate(), getMu());
444 }
445 }
446
447
448 @Override
449 protected FieldCartesianOrbit<T> keplerianShiftedBy(final T dt) {
450 final FieldPVCoordinates<T> pvCoordinates = getPVCoordinates();
451 final FieldPVCoordinates<T> shiftedPV = KeplerianMotionCartesianUtility.predictPositionVelocity(dt,
452 pvCoordinates.getPosition(), pvCoordinates.getVelocity(), getMu());
453 return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
454 }
455
456
457 @Override
458 public FieldCartesianOrbit<T> shiftedBy(final TimeOffset dt) {
459 final FieldPVCoordinates<T> shiftedPV = shiftPV(getField().getOne().newInstance(dt.toDouble()));
460 return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
461 }
462
463
464 public FieldCartesianOrbit<T> shiftedBy(final double dt) {
465 return shiftedBy(getZero().newInstance(dt));
466 }
467
468
469 public FieldCartesianOrbit<T> shiftedBy(final T dt) {
470 final FieldPVCoordinates<T> shiftedPV = shiftPV(dt);
471 return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
472 }
473
474
475
476
477
478 private FieldPVCoordinates<T> shiftPV(final T dt) {
479 final FieldPVCoordinates<T> pvCoordinates = getPVCoordinates();
480 final FieldPVCoordinates<T> shiftedPV = KeplerianMotionCartesianUtility.predictPositionVelocity(dt,
481 pvCoordinates.getPosition(), pvCoordinates.getVelocity(), getMu());
482
483 if (!dt.isZero() && hasNonKeplerianAcceleration) {
484
485 return shiftPVNonKeplerian(shiftedPV, dt);
486
487 } else {
488
489
490 return shiftedPV;
491 }
492 }
493
494
495
496
497 private T[][] create6x6Identity() {
498
499 final T[][] identity = MathArrays.buildArray(getField(), 6, 6);
500 for (int i = 0; i < 6; i++) {
501 Arrays.fill(identity[i], getZero());
502 identity[i][i] = getOne();
503 }
504 return identity;
505 }
506
507 @Override
508 protected T[][] computeJacobianMeanWrtCartesian() {
509 return create6x6Identity();
510 }
511
512 @Override
513 protected T[][] computeJacobianEccentricWrtCartesian() {
514 return create6x6Identity();
515 }
516
517 @Override
518 protected T[][] computeJacobianTrueWrtCartesian() {
519 return create6x6Identity();
520 }
521
522
523 public void addKeplerContribution(final PositionAngleType type, final T gm,
524 final T[] pDot) {
525
526 final FieldPVCoordinates<T> pv = getPVCoordinates();
527
528
529 final FieldVector3D<T> velocity = pv.getVelocity();
530 pDot[0] = pDot[0].add(velocity.getX());
531 pDot[1] = pDot[1].add(velocity.getY());
532 pDot[2] = pDot[2].add(velocity.getZ());
533
534
535 final FieldVector3D<T> position = pv.getPosition();
536 final T r2 = position.getNorm2Sq();
537 final T coeff = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
538 pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
539 pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
540 pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));
541
542 }
543
544
545
546
547 public String toString() {
548
549 final String comma = ", ";
550 final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
551 final Vector3D p = pv.getPosition();
552 final Vector3D v = pv.getVelocity();
553 return "Cartesian parameters: {P(" +
554 p.getX() + comma +
555 p.getY() + comma +
556 p.getZ() + "), V(" +
557 v.getX() + comma +
558 v.getY() + comma +
559 v.getZ() + ")}";
560 }
561
562 @Override
563 public CartesianOrbit toOrbit() {
564 final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
565 final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
566 if (hasNonKeplerianAcceleration()) {
567
568 return new CartesianOrbit(pv, getFrame(), date, getMu().getReal());
569 } else {
570
571
572 return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
573 getFrame(), date, getMu().getReal());
574 }
575 }
576
577 }