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 transient boolean hasNonKeplerianAcceleration;
77
78
79 private transient 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 protected FieldVector3D<T> initPosition() {
417
418 return getPVCoordinates().getPosition();
419 }
420
421
422 protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {
423
424 return getPVCoordinates();
425 }
426
427
428 @Override
429 public FieldCartesianOrbit<T> inFrame(final Frame inertialFrame) {
430 if (hasNonKeplerianAcceleration()) {
431 return new FieldCartesianOrbit<>(getPVCoordinates(inertialFrame), inertialFrame, getMu());
432 } else {
433 final FieldKinematicTransform<T> transform = getFrame().getKinematicTransformTo(inertialFrame, getDate());
434 return new FieldCartesianOrbit<>(transform.transformOnlyPV(getPVCoordinates()), inertialFrame, getDate(), getMu());
435 }
436 }
437
438
439 public FieldCartesianOrbit<T> shiftedBy(final double dt) {
440 return shiftedBy(getZero().newInstance(dt));
441 }
442
443
444 public FieldCartesianOrbit<T> shiftedBy(final T dt) {
445 final FieldPVCoordinates<T> shiftedPV = shiftPV(dt);
446 return new FieldCartesianOrbit<>(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
447 }
448
449
450
451
452
453 private FieldPVCoordinates<T> shiftPV(final T dt) {
454 final FieldPVCoordinates<T> pvCoordinates = getPVCoordinates();
455 final FieldPVCoordinates<T> shiftedPV = KeplerianMotionCartesianUtility.predictPositionVelocity(dt,
456 pvCoordinates.getPosition(), pvCoordinates.getVelocity(), getMu());
457
458 if (hasNonKeplerianAcceleration) {
459
460 final FieldVector3D<T> pvP = getPosition();
461 final T r2 = pvP.getNormSq();
462 final T r = r2.sqrt();
463
464 final FieldVector3D<T> nonKeplerianAcceleration = new FieldVector3D<>(getOne(), getPVCoordinates().getAcceleration(),
465 r.multiply(r2).reciprocal().multiply(getMu()), pvP);
466
467
468 final FieldVector3D<T> shiftedP = shiftedPV.getPosition();
469 final FieldVector3D<T> shiftedV = shiftedPV.getVelocity();
470 final FieldVector3D<T> fixedP = new FieldVector3D<>(getOne(), shiftedP,
471 dt.square().multiply(0.5), nonKeplerianAcceleration);
472 final T fixedR2 = fixedP.getNormSq();
473 final T fixedR = fixedR2.sqrt();
474 final FieldVector3D<T> fixedV = new FieldVector3D<>(getOne(), shiftedV,
475 dt, nonKeplerianAcceleration);
476 final FieldVector3D<T> fixedA = new FieldVector3D<>(fixedR.multiply(fixedR2).reciprocal().multiply(getMu().negate()), shiftedP,
477 getOne(), nonKeplerianAcceleration);
478
479 return new FieldPVCoordinates<>(fixedP, fixedV, fixedA);
480
481 } else {
482
483
484 return shiftedPV;
485 }
486 }
487
488
489
490
491 private T[][] create6x6Identity() {
492
493 final T[][] identity = MathArrays.buildArray(getField(), 6, 6);
494 for (int i = 0; i < 6; i++) {
495 Arrays.fill(identity[i], getZero());
496 identity[i][i] = getOne();
497 }
498 return identity;
499 }
500
501 @Override
502 protected T[][] computeJacobianMeanWrtCartesian() {
503 return create6x6Identity();
504 }
505
506 @Override
507 protected T[][] computeJacobianEccentricWrtCartesian() {
508 return create6x6Identity();
509 }
510
511 @Override
512 protected T[][] computeJacobianTrueWrtCartesian() {
513 return create6x6Identity();
514 }
515
516
517 public void addKeplerContribution(final PositionAngleType type, final T gm,
518 final T[] pDot) {
519
520 final FieldPVCoordinates<T> pv = getPVCoordinates();
521
522
523 final FieldVector3D<T> velocity = pv.getVelocity();
524 pDot[0] = pDot[0].add(velocity.getX());
525 pDot[1] = pDot[1].add(velocity.getY());
526 pDot[2] = pDot[2].add(velocity.getZ());
527
528
529 final FieldVector3D<T> position = pv.getPosition();
530 final T r2 = position.getNormSq();
531 final T coeff = r2.multiply(r2.sqrt()).reciprocal().negate().multiply(gm);
532 pDot[3] = pDot[3].add(coeff.multiply(position.getX()));
533 pDot[4] = pDot[4].add(coeff.multiply(position.getY()));
534 pDot[5] = pDot[5].add(coeff.multiply(position.getZ()));
535
536 }
537
538
539
540
541 public String toString() {
542
543 final String comma = ", ";
544 final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
545 final Vector3D p = pv.getPosition();
546 final Vector3D v = pv.getVelocity();
547 return "Cartesian parameters: {P(" +
548 p.getX() + comma +
549 p.getY() + comma +
550 p.getZ() + "), V(" +
551 v.getX() + comma +
552 v.getY() + comma +
553 v.getZ() + ")}";
554 }
555
556 @Override
557 public CartesianOrbit toOrbit() {
558 final PVCoordinates pv = getPVCoordinates().toPVCoordinates();
559 final AbsoluteDate date = getPVCoordinates().getDate().toAbsoluteDate();
560 if (hasNonKeplerianAcceleration()) {
561
562 return new CartesianOrbit(pv, getFrame(), date, getMu().getReal());
563 } else {
564
565
566 return new CartesianOrbit(new PVCoordinates(pv.getPosition(), pv.getVelocity()),
567 getFrame(), date, getMu().getReal());
568 }
569 }
570
571 }