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