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