1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
18
19 import java.io.Serializable;
20
21 import org.hipparchus.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.Derivative;
23 import org.hipparchus.analysis.differentiation.DerivativeStructure;
24 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
25 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
26 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.hipparchus.util.FastMath;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.errors.OrekitMessages;
31 import org.orekit.time.TimeShiftable;
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47 public class PVCoordinates implements TimeShiftable<PVCoordinates>, Serializable {
48
49
50 public static final PVCoordinates ZERO = new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);
51
52
53 private static final long serialVersionUID = 20140407L;
54
55
56 private final Vector3D position;
57
58
59 private final Vector3D velocity;
60
61
62 private final Vector3D acceleration;
63
64
65
66
67 public PVCoordinates() {
68 position = Vector3D.ZERO;
69 velocity = Vector3D.ZERO;
70 acceleration = Vector3D.ZERO;
71 }
72
73
74
75
76
77
78 public PVCoordinates(final Vector3D position, final Vector3D velocity) {
79 this.position = position;
80 this.velocity = velocity;
81 this.acceleration = Vector3D.ZERO;
82 }
83
84
85
86
87
88
89 public PVCoordinates(final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
90 this.position = position;
91 this.velocity = velocity;
92 this.acceleration = acceleration;
93 }
94
95
96
97
98
99
100
101 public PVCoordinates(final double a, final PVCoordinates pv) {
102 position = new Vector3D(a, pv.position);
103 velocity = new Vector3D(a, pv.velocity);
104 acceleration = new Vector3D(a, pv.acceleration);
105 }
106
107
108
109
110
111
112
113 public PVCoordinates(final PVCoordinates start, final PVCoordinates end) {
114 this.position = end.position.subtract(start.position);
115 this.velocity = end.velocity.subtract(start.velocity);
116 this.acceleration = end.acceleration.subtract(start.acceleration);
117 }
118
119
120
121
122
123
124
125
126
127 public PVCoordinates(final double a1, final PVCoordinates pv1,
128 final double a2, final PVCoordinates pv2) {
129 position = new Vector3D(a1, pv1.position, a2, pv2.position);
130 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity);
131 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration);
132 }
133
134
135
136
137
138
139
140
141
142
143
144 public PVCoordinates(final double a1, final PVCoordinates pv1,
145 final double a2, final PVCoordinates pv2,
146 final double a3, final PVCoordinates pv3) {
147 position = new Vector3D(a1, pv1.position, a2, pv2.position, a3, pv3.position);
148 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity, a3, pv3.velocity);
149 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration);
150 }
151
152
153
154
155
156
157
158
159
160
161
162
163
164 public PVCoordinates(final double a1, final PVCoordinates pv1,
165 final double a2, final PVCoordinates pv2,
166 final double a3, final PVCoordinates pv3,
167 final double a4, final PVCoordinates pv4) {
168 position = new Vector3D(a1, pv1.position, a2, pv2.position,
169 a3, pv3.position, a4, pv4.position);
170 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity,
171 a3, pv3.velocity, a4, pv4.velocity);
172 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration,
173 a3, pv3.acceleration, a4, pv4.acceleration);
174 }
175
176
177
178
179
180
181
182
183
184 public <U extends Derivative<U>> PVCoordinates(final FieldVector3D<U> p) {
185 position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal());
186 if (p.getX().getOrder() >= 1) {
187 velocity = new Vector3D(p.getX().getPartialDerivative(1),
188 p.getY().getPartialDerivative(1),
189 p.getZ().getPartialDerivative(1));
190 if (p.getX().getOrder() >= 2) {
191 acceleration = new Vector3D(p.getX().getPartialDerivative(2),
192 p.getY().getPartialDerivative(2),
193 p.getZ().getPartialDerivative(2));
194 } else {
195 acceleration = Vector3D.ZERO;
196 }
197 } else {
198 velocity = Vector3D.ZERO;
199 acceleration = Vector3D.ZERO;
200 }
201 }
202
203
204
205
206
207
208
209 public PVCoordinates(final Vector3D position) {
210 this(position, Vector3D.ZERO);
211 }
212
213
214
215
216
217
218
219
220
221 public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order) {
222
223 final DSFactory factory;
224 final DerivativeStructure x;
225 final DerivativeStructure y;
226 final DerivativeStructure z;
227 switch (order) {
228 case 0 :
229 factory = new DSFactory(1, order);
230 x = factory.build(position.getX());
231 y = factory.build(position.getY());
232 z = factory.build(position.getZ());
233 break;
234 case 1 :
235 factory = new DSFactory(1, order);
236 x = factory.build(position.getX(), velocity.getX());
237 y = factory.build(position.getY(), velocity.getY());
238 z = factory.build(position.getZ(), velocity.getZ());
239 break;
240 case 2 :
241 factory = new DSFactory(1, order);
242 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
243 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
244 z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
245 break;
246 default :
247 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
248 }
249
250 return new FieldVector3D<>(x, y, z);
251
252 }
253
254
255
256
257
258
259
260
261
262
263 public FieldVector3D<UnivariateDerivative1> toUnivariateDerivative1Vector() {
264
265 final UnivariateDerivative1 x = new UnivariateDerivative1(position.getX(), velocity.getX());
266 final UnivariateDerivative1 y = new UnivariateDerivative1(position.getY(), velocity.getY());
267 final UnivariateDerivative1 z = new UnivariateDerivative1(position.getZ(), velocity.getZ());
268
269 return new FieldVector3D<>(x, y, z);
270 }
271
272
273
274
275
276
277
278
279
280
281 public FieldVector3D<UnivariateDerivative2> toUnivariateDerivative2Vector() {
282
283 final UnivariateDerivative2 x = new UnivariateDerivative2(position.getX(), velocity.getX(), acceleration.getX());
284 final UnivariateDerivative2 y = new UnivariateDerivative2(position.getY(), velocity.getY(), acceleration.getY());
285 final UnivariateDerivative2 z = new UnivariateDerivative2(position.getZ(), velocity.getZ(), acceleration.getZ());
286
287 return new FieldVector3D<>(x, y, z);
288 }
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311 public FieldPVCoordinates<DerivativeStructure> toDerivativeStructurePV(final int order) {
312
313 final DSFactory factory;
314 final DerivativeStructure x0;
315 final DerivativeStructure y0;
316 final DerivativeStructure z0;
317 final DerivativeStructure x1;
318 final DerivativeStructure y1;
319 final DerivativeStructure z1;
320 final DerivativeStructure x2;
321 final DerivativeStructure y2;
322 final DerivativeStructure z2;
323 switch (order) {
324 case 0 :
325 factory = new DSFactory(1, order);
326 x0 = factory.build(position.getX());
327 y0 = factory.build(position.getY());
328 z0 = factory.build(position.getZ());
329 x1 = factory.build(velocity.getX());
330 y1 = factory.build(velocity.getY());
331 z1 = factory.build(velocity.getZ());
332 x2 = factory.build(acceleration.getX());
333 y2 = factory.build(acceleration.getY());
334 z2 = factory.build(acceleration.getZ());
335 break;
336 case 1 : {
337 factory = new DSFactory(1, order);
338 final double r2 = position.getNormSq();
339 final double r = FastMath.sqrt(r2);
340 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
341 final double a = acceleration.getNorm();
342 final double aOr = a / r;
343 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
344 x0 = factory.build(position.getX(), velocity.getX());
345 y0 = factory.build(position.getY(), velocity.getY());
346 z0 = factory.build(position.getZ(), velocity.getZ());
347 x1 = factory.build(velocity.getX(), acceleration.getX());
348 y1 = factory.build(velocity.getY(), acceleration.getY());
349 z1 = factory.build(velocity.getZ(), acceleration.getZ());
350 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
351 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
352 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
353 break;
354 }
355 case 2 : {
356 factory = new DSFactory(1, order);
357 final double r2 = position.getNormSq();
358 final double r = FastMath.sqrt(r2);
359 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
360 final double a = acceleration.getNorm();
361 final double aOr = a / r;
362 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
363 final double v2 = velocity.getNormSq();
364 final double pa = Vector3D.dotProduct(position, acceleration);
365 final double aj = Vector3D.dotProduct(acceleration, keplerianJerk);
366 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
367 4 * aOr * pvOr2 - aj / (a * r), velocity);
368 x0 = factory.build(position.getX(), velocity.getX(), acceleration.getX());
369 y0 = factory.build(position.getY(), velocity.getY(), acceleration.getY());
370 z0 = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
371 x1 = factory.build(velocity.getX(), acceleration.getX(), keplerianJerk.getX());
372 y1 = factory.build(velocity.getY(), acceleration.getY(), keplerianJerk.getY());
373 z1 = factory.build(velocity.getZ(), acceleration.getZ(), keplerianJerk.getZ());
374 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
375 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
376 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
377 break;
378 }
379 default :
380 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
381 }
382
383 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
384 new FieldVector3D<>(x1, y1, z1),
385 new FieldVector3D<>(x2, y2, z2));
386
387 }
388
389
390
391
392
393
394
395
396
397
398 public FieldPVCoordinates<UnivariateDerivative1> toUnivariateDerivative1PV() {
399
400 final double r2 = position.getNormSq();
401 final double r = FastMath.sqrt(r2);
402 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
403 final double a = acceleration.getNorm();
404 final double aOr = a / r;
405 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
406
407 final UnivariateDerivative1 x0 = new UnivariateDerivative1(position.getX(), velocity.getX());
408 final UnivariateDerivative1 y0 = new UnivariateDerivative1(position.getY(), velocity.getY());
409 final UnivariateDerivative1 z0 = new UnivariateDerivative1(position.getZ(), velocity.getZ());
410 final UnivariateDerivative1 x1 = new UnivariateDerivative1(velocity.getX(), acceleration.getX());
411 final UnivariateDerivative1 y1 = new UnivariateDerivative1(velocity.getY(), acceleration.getY());
412 final UnivariateDerivative1 z1 = new UnivariateDerivative1(velocity.getZ(), acceleration.getZ());
413 final UnivariateDerivative1 x2 = new UnivariateDerivative1(acceleration.getX(), keplerianJerk.getX());
414 final UnivariateDerivative1 y2 = new UnivariateDerivative1(acceleration.getY(), keplerianJerk.getY());
415 final UnivariateDerivative1 z2 = new UnivariateDerivative1(acceleration.getZ(), keplerianJerk.getZ());
416
417 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
418 new FieldVector3D<>(x1, y1, z1),
419 new FieldVector3D<>(x2, y2, z2));
420
421 }
422
423
424
425
426
427
428
429
430
431
432
433
434 public FieldPVCoordinates<UnivariateDerivative2> toUnivariateDerivative2PV() {
435
436 final double r2 = position.getNormSq();
437 final double r = FastMath.sqrt(r2);
438 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
439 final double a = acceleration.getNorm();
440 final double aOr = a / r;
441 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
442 final double v2 = velocity.getNormSq();
443 final double pa = Vector3D.dotProduct(position, acceleration);
444 final double aj = Vector3D.dotProduct(acceleration, keplerianJerk);
445 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
446 4 * aOr * pvOr2 - aj / (a * r), velocity);
447
448 final UnivariateDerivative2 x0 = new UnivariateDerivative2(position.getX(), velocity.getX(), acceleration.getX());
449 final UnivariateDerivative2 y0 = new UnivariateDerivative2(position.getY(), velocity.getY(), acceleration.getY());
450 final UnivariateDerivative2 z0 = new UnivariateDerivative2(position.getZ(), velocity.getZ(), acceleration.getZ());
451 final UnivariateDerivative2 x1 = new UnivariateDerivative2(velocity.getX(), acceleration.getX(), keplerianJerk.getX());
452 final UnivariateDerivative2 y1 = new UnivariateDerivative2(velocity.getY(), acceleration.getY(), keplerianJerk.getY());
453 final UnivariateDerivative2 z1 = new UnivariateDerivative2(velocity.getZ(), acceleration.getZ(), keplerianJerk.getZ());
454 final UnivariateDerivative2 x2 = new UnivariateDerivative2(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
455 final UnivariateDerivative2 y2 = new UnivariateDerivative2(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
456 final UnivariateDerivative2 z2 = new UnivariateDerivative2(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
457
458 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
459 new FieldVector3D<>(x1, y1, z1),
460 new FieldVector3D<>(x2, y2, z2));
461
462 }
463
464
465
466
467
468
469
470
471
472 public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
473 final double scale = 1.0 / dt;
474 return new Vector3D(scale, end, -scale, start);
475 }
476
477
478
479
480
481
482
483
484
485
486
487 public PVCoordinates shiftedBy(final double dt) {
488 return new PVCoordinates(positionShiftedBy(dt),
489 new Vector3D(1, velocity, dt, acceleration),
490 acceleration);
491 }
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507 public Vector3D positionShiftedBy(final double dt) {
508 return new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration);
509 }
510
511
512
513
514 public Vector3D getPosition() {
515 return position;
516 }
517
518
519
520
521 public Vector3D getVelocity() {
522 return velocity;
523 }
524
525
526
527
528 public Vector3D getAcceleration() {
529 return acceleration;
530 }
531
532
533
534
535
536
537
538
539
540 public Vector3D getMomentum() {
541 return Vector3D.crossProduct(position, velocity);
542 }
543
544
545
546
547
548
549
550
551
552
553
554 public Vector3D getAngularVelocity() {
555 return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
556 }
557
558
559
560
561 public PVCoordinates negate() {
562 return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
563 }
564
565
566
567
568
569
570
571
572
573
574
575
576
577 public PVCoordinates normalize() {
578 final double inv = 1.0 / position.getNorm();
579 final Vector3D u = new Vector3D(inv, position);
580 final Vector3D v = new Vector3D(inv, velocity);
581 final Vector3D w = new Vector3D(inv, acceleration);
582 final double uv = Vector3D.dotProduct(u, v);
583 final double v2 = Vector3D.dotProduct(v, v);
584 final double uw = Vector3D.dotProduct(u, w);
585 final Vector3D uDot = new Vector3D(1, v, -uv, u);
586 final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
587 return new PVCoordinates(u, uDot, uDotDot);
588 }
589
590
591
592
593
594
595 public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) {
596 final Vector3D p1 = pv1.position;
597 final Vector3D v1 = pv1.velocity;
598 final Vector3D a1 = pv1.acceleration;
599 final Vector3D p2 = pv2.position;
600 final Vector3D v2 = pv2.velocity;
601 final Vector3D a2 = pv2.acceleration;
602 return new PVCoordinates(Vector3D.crossProduct(p1, p2),
603 new Vector3D(1, Vector3D.crossProduct(p1, v2),
604 1, Vector3D.crossProduct(v1, p2)),
605 new Vector3D(1, Vector3D.crossProduct(p1, a2),
606 2, Vector3D.crossProduct(v1, v2),
607 1, Vector3D.crossProduct(a1, p2)));
608 }
609
610
611
612
613 public String toString() {
614 final String comma = ", ";
615 return new StringBuilder().append('{').append("P(").
616 append(position.getX()).append(comma).
617 append(position.getY()).append(comma).
618 append(position.getZ()).append("), V(").
619 append(velocity.getX()).append(comma).
620 append(velocity.getY()).append(comma).
621 append(velocity.getZ()).append("), A(").
622 append(acceleration.getX()).append(comma).
623 append(acceleration.getY()).append(comma).
624 append(acceleration.getZ()).append(")}").toString();
625 }
626
627
628
629
630 private Object writeReplace() {
631 return new DTO(this);
632 }
633
634
635 private static class DTO implements Serializable {
636
637
638 private static final long serialVersionUID = 20140723L;
639
640
641 private double[] d;
642
643
644
645
646 private DTO(final PVCoordinates pv) {
647 this.d = new double[] {
648 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
649 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
650 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
651 };
652 }
653
654
655
656
657 private Object readResolve() {
658 return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
659 new Vector3D(d[3], d[4], d[5]),
660 new Vector3D(d[6], d[7], d[8]));
661 }
662
663 }
664
665 }