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 OrbitType getType() {
169 return OrbitType.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 public FieldCartesianOrbit<T> shiftedBy(final TimeOffset dt) {
450 final FieldPVCoordinates<T> shiftedPV = shiftPV(getField().getOne().newInstance(dt.toDouble()));
451 return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
452 }
453
454
455 public FieldCartesianOrbit<T> shiftedBy(final double dt) {
456 return shiftedBy(getZero().newInstance(dt));
457 }
458
459
460 public FieldCartesianOrbit<T> shiftedBy(final T dt) {
461 final FieldPVCoordinates<T> shiftedPV = shiftPV(dt);
462 return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
463 }
464
465
466
467
468
469 private FieldPVCoordinates<T> shiftPV(final T dt) {
470 final FieldPVCoordinates<T> pvCoordinates = getPVCoordinates();
471 final FieldPVCoordinates<T> shiftedPV = KeplerianMotionCartesianUtility.predictPositionVelocity(dt,
472 pvCoordinates.getPosition(), pvCoordinates.getVelocity(), getMu());
473
474 if (!dt.isZero() && hasNonKeplerianAcceleration) {
475
476 return shiftNonKeplerian(shiftedPV, dt);
477
478 } else {
479
480
481 return shiftedPV;
482 }
483 }
484
485
486
487
488 private T[][] create6x6Identity() {
489
490 final T[][] identity = MathArrays.buildArray(getField(), 6, 6);
491 for (int i = 0; i < 6; i++) {
492 Arrays.fill(identity[i], getZero());
493 identity[i][i] = getOne();
494 }
495 return identity;
496 }
497
498 @Override
499 protected T[][] computeJacobianMeanWrtCartesian() {
500 return create6x6Identity();
501 }
502
503 @Override
504 protected T[][] computeJacobianEccentricWrtCartesian() {
505 return create6x6Identity();
506 }
507
508 @Override
509 protected T[][] computeJacobianTrueWrtCartesian() {
510 return create6x6Identity();
511 }
512
513
514 public void addKeplerContribution(final PositionAngleType type, final T gm,
515 final T[] pDot) {
516
517 final FieldPVCoordinates<T> pv = getPVCoordinates();
518
519
520 final FieldVector3D<T> velocity = pv.getVelocity();
521 pDot[0] = pDot[0].add(velocity.getX());
522 pDot[1] = pDot[1].add(velocity.getY());
523 pDot[2] = pDot[2].add(velocity.getZ());
524
525
526 final FieldVector3D<T> position = pv.getPosition();
527 final T r2 = position.getNorm2Sq();
528 final T coeff = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
529 pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
530 pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
531 pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));
532
533 }
534
535
536
537
538 public String toString() {
539
540 final String comma = ", ";
541 final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
542 final Vector3D p = pv.getPosition();
543 final Vector3D v = pv.getVelocity();
544 return "Cartesian parameters: {P(" +
545 p.getX() + comma +
546 p.getY() + comma +
547 p.getZ() + "), V(" +
548 v.getX() + comma +
549 v.getY() + comma +
550 v.getZ() + ")}";
551 }
552
553 @Override
554 public CartesianOrbit toOrbit() {
555 final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
556 final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
557 if (hasNonKeplerianAcceleration()) {
558
559 return new CartesianOrbit(pv, getFrame(), date, getMu().getReal());
560 } else {
561
562
563 return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
564 getFrame(), date, getMu().getReal());
565 }
566 }
567
568 }