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