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
210
211 public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order) {
212
213 final DSFactory factory;
214 final DerivativeStructure x;
215 final DerivativeStructure y;
216 final DerivativeStructure z;
217 switch(order) {
218 case 0 :
219 factory = new DSFactory(1, order);
220 x = factory.build(position.getX());
221 y = factory.build(position.getY());
222 z = factory.build(position.getZ());
223 break;
224 case 1 :
225 factory = new DSFactory(1, order);
226 x = factory.build(position.getX(), velocity.getX());
227 y = factory.build(position.getY(), velocity.getY());
228 z = factory.build(position.getZ(), velocity.getZ());
229 break;
230 case 2 :
231 factory = new DSFactory(1, order);
232 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
233 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
234 z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
235 break;
236 default :
237 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
238 }
239
240 return new FieldVector3D<>(x, y, z);
241
242 }
243
244
245
246
247
248
249
250
251
252
253 public FieldVector3D<UnivariateDerivative1> toUnivariateDerivative1Vector() {
254
255 final UnivariateDerivative1 x = new UnivariateDerivative1(position.getX(), velocity.getX());
256 final UnivariateDerivative1 y = new UnivariateDerivative1(position.getY(), velocity.getY());
257 final UnivariateDerivative1 z = new UnivariateDerivative1(position.getZ(), velocity.getZ());
258
259 return new FieldVector3D<>(x, y, z);
260 }
261
262
263
264
265
266
267
268
269
270
271 public FieldVector3D<UnivariateDerivative2> toUnivariateDerivative2Vector() {
272
273 final UnivariateDerivative2 x = new UnivariateDerivative2(position.getX(), velocity.getX(), acceleration.getX());
274 final UnivariateDerivative2 y = new UnivariateDerivative2(position.getY(), velocity.getY(), acceleration.getY());
275 final UnivariateDerivative2 z = new UnivariateDerivative2(position.getZ(), velocity.getZ(), acceleration.getZ());
276
277 return new FieldVector3D<>(x, y, z);
278 }
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301 public FieldPVCoordinates<DerivativeStructure> toDerivativeStructurePV(final int order) {
302
303 final DSFactory factory;
304 final DerivativeStructure x0;
305 final DerivativeStructure y0;
306 final DerivativeStructure z0;
307 final DerivativeStructure x1;
308 final DerivativeStructure y1;
309 final DerivativeStructure z1;
310 final DerivativeStructure x2;
311 final DerivativeStructure y2;
312 final DerivativeStructure z2;
313 switch(order) {
314 case 0 :
315 factory = new DSFactory(1, order);
316 x0 = factory.build(position.getX());
317 y0 = factory.build(position.getY());
318 z0 = factory.build(position.getZ());
319 x1 = factory.build(velocity.getX());
320 y1 = factory.build(velocity.getY());
321 z1 = factory.build(velocity.getZ());
322 x2 = factory.build(acceleration.getX());
323 y2 = factory.build(acceleration.getY());
324 z2 = factory.build(acceleration.getZ());
325 break;
326 case 1 : {
327 factory = new DSFactory(1, order);
328 final double r2 = position.getNormSq();
329 final double r = FastMath.sqrt(r2);
330 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
331 final double a = acceleration.getNorm();
332 final double aOr = a / r;
333 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
334 x0 = factory.build(position.getX(), velocity.getX());
335 y0 = factory.build(position.getY(), velocity.getY());
336 z0 = factory.build(position.getZ(), velocity.getZ());
337 x1 = factory.build(velocity.getX(), acceleration.getX());
338 y1 = factory.build(velocity.getY(), acceleration.getY());
339 z1 = factory.build(velocity.getZ(), acceleration.getZ());
340 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
341 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
342 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
343 break;
344 }
345 case 2 : {
346 factory = new DSFactory(1, order);
347 final double r2 = position.getNormSq();
348 final double r = FastMath.sqrt(r2);
349 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
350 final double a = acceleration.getNorm();
351 final double aOr = a / r;
352 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
353 final double v2 = velocity.getNormSq();
354 final double pa = Vector3D.dotProduct(position, acceleration);
355 final double aj = Vector3D.dotProduct(acceleration, keplerianJerk);
356 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
357 4 * aOr * pvOr2 - aj / (a * r), velocity);
358 x0 = factory.build(position.getX(), velocity.getX(), acceleration.getX());
359 y0 = factory.build(position.getY(), velocity.getY(), acceleration.getY());
360 z0 = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
361 x1 = factory.build(velocity.getX(), acceleration.getX(), keplerianJerk.getX());
362 y1 = factory.build(velocity.getY(), acceleration.getY(), keplerianJerk.getY());
363 z1 = factory.build(velocity.getZ(), acceleration.getZ(), keplerianJerk.getZ());
364 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
365 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
366 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
367 break;
368 }
369 default :
370 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
371 }
372
373 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
374 new FieldVector3D<>(x1, y1, z1),
375 new FieldVector3D<>(x2, y2, z2));
376
377 }
378
379
380
381
382
383
384
385
386
387
388 public FieldPVCoordinates<UnivariateDerivative1> toUnivariateDerivative1PV() {
389
390 final double r2 = position.getNormSq();
391 final double r = FastMath.sqrt(r2);
392 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
393 final double a = acceleration.getNorm();
394 final double aOr = a / r;
395 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
396
397 final UnivariateDerivative1 x0 = new UnivariateDerivative1(position.getX(), velocity.getX());
398 final UnivariateDerivative1 y0 = new UnivariateDerivative1(position.getY(), velocity.getY());
399 final UnivariateDerivative1 z0 = new UnivariateDerivative1(position.getZ(), velocity.getZ());
400 final UnivariateDerivative1 x1 = new UnivariateDerivative1(velocity.getX(), acceleration.getX());
401 final UnivariateDerivative1 y1 = new UnivariateDerivative1(velocity.getY(), acceleration.getY());
402 final UnivariateDerivative1 z1 = new UnivariateDerivative1(velocity.getZ(), acceleration.getZ());
403 final UnivariateDerivative1 x2 = new UnivariateDerivative1(acceleration.getX(), keplerianJerk.getX());
404 final UnivariateDerivative1 y2 = new UnivariateDerivative1(acceleration.getY(), keplerianJerk.getY());
405 final UnivariateDerivative1 z2 = new UnivariateDerivative1(acceleration.getZ(), keplerianJerk.getZ());
406
407 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
408 new FieldVector3D<>(x1, y1, z1),
409 new FieldVector3D<>(x2, y2, z2));
410
411 }
412
413
414
415
416
417
418
419
420
421
422
423
424 public FieldPVCoordinates<UnivariateDerivative2> toUnivariateDerivative2PV() {
425
426 final double r2 = position.getNormSq();
427 final double r = FastMath.sqrt(r2);
428 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
429 final double a = acceleration.getNorm();
430 final double aOr = a / r;
431 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
432 final double v2 = velocity.getNormSq();
433 final double pa = Vector3D.dotProduct(position, acceleration);
434 final double aj = Vector3D.dotProduct(acceleration, keplerianJerk);
435 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
436 4 * aOr * pvOr2 - aj / (a * r), velocity);
437
438 final UnivariateDerivative2 x0 = new UnivariateDerivative2(position.getX(), velocity.getX(), acceleration.getX());
439 final UnivariateDerivative2 y0 = new UnivariateDerivative2(position.getY(), velocity.getY(), acceleration.getY());
440 final UnivariateDerivative2 z0 = new UnivariateDerivative2(position.getZ(), velocity.getZ(), acceleration.getZ());
441 final UnivariateDerivative2 x1 = new UnivariateDerivative2(velocity.getX(), acceleration.getX(), keplerianJerk.getX());
442 final UnivariateDerivative2 y1 = new UnivariateDerivative2(velocity.getY(), acceleration.getY(), keplerianJerk.getY());
443 final UnivariateDerivative2 z1 = new UnivariateDerivative2(velocity.getZ(), acceleration.getZ(), keplerianJerk.getZ());
444 final UnivariateDerivative2 x2 = new UnivariateDerivative2(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
445 final UnivariateDerivative2 y2 = new UnivariateDerivative2(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
446 final UnivariateDerivative2 z2 = new UnivariateDerivative2(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
447
448 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
449 new FieldVector3D<>(x1, y1, z1),
450 new FieldVector3D<>(x2, y2, z2));
451
452 }
453
454
455
456
457
458
459
460
461
462 public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
463 final double scale = 1.0 / dt;
464 return new Vector3D(scale, end, -scale, start);
465 }
466
467
468
469
470
471
472
473
474
475
476
477 public PVCoordinates shiftedBy(final double dt) {
478 return new PVCoordinates(new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
479 new Vector3D(1, velocity, dt, acceleration),
480 acceleration);
481 }
482
483
484
485
486 public Vector3D getPosition() {
487 return position;
488 }
489
490
491
492
493 public Vector3D getVelocity() {
494 return velocity;
495 }
496
497
498
499
500 public Vector3D getAcceleration() {
501 return acceleration;
502 }
503
504
505
506
507
508
509
510
511
512 public Vector3D getMomentum() {
513 return Vector3D.crossProduct(position, velocity);
514 }
515
516
517
518
519
520
521
522
523
524
525
526 public Vector3D getAngularVelocity() {
527 return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
528 }
529
530
531
532
533 public PVCoordinates negate() {
534 return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
535 }
536
537
538
539
540
541
542
543
544
545
546
547
548
549 public PVCoordinates normalize() {
550 final double inv = 1.0 / position.getNorm();
551 final Vector3D u = new Vector3D(inv, position);
552 final Vector3D v = new Vector3D(inv, velocity);
553 final Vector3D w = new Vector3D(inv, acceleration);
554 final double uv = Vector3D.dotProduct(u, v);
555 final double v2 = Vector3D.dotProduct(v, v);
556 final double uw = Vector3D.dotProduct(u, w);
557 final Vector3D uDot = new Vector3D(1, v, -uv, u);
558 final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
559 return new PVCoordinates(u, uDot, uDotDot);
560 }
561
562
563
564
565
566
567 public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) {
568 final Vector3D p1 = pv1.position;
569 final Vector3D v1 = pv1.velocity;
570 final Vector3D a1 = pv1.acceleration;
571 final Vector3D p2 = pv2.position;
572 final Vector3D v2 = pv2.velocity;
573 final Vector3D a2 = pv2.acceleration;
574 return new PVCoordinates(Vector3D.crossProduct(p1, p2),
575 new Vector3D(1, Vector3D.crossProduct(p1, v2),
576 1, Vector3D.crossProduct(v1, p2)),
577 new Vector3D(1, Vector3D.crossProduct(p1, a2),
578 2, Vector3D.crossProduct(v1, v2),
579 1, Vector3D.crossProduct(a1, p2)));
580 }
581
582
583
584
585 public String toString() {
586 final String comma = ", ";
587 return new StringBuffer().append('{').append("P(").
588 append(position.getX()).append(comma).
589 append(position.getY()).append(comma).
590 append(position.getZ()).append("), V(").
591 append(velocity.getX()).append(comma).
592 append(velocity.getY()).append(comma).
593 append(velocity.getZ()).append("), A(").
594 append(acceleration.getX()).append(comma).
595 append(acceleration.getY()).append(comma).
596 append(acceleration.getZ()).append(")}").toString();
597 }
598
599
600
601
602 private Object writeReplace() {
603 return new DTO(this);
604 }
605
606
607 private static class DTO implements Serializable {
608
609
610 private static final long serialVersionUID = 20140723L;
611
612
613 private double[] d;
614
615
616
617
618 private DTO(final PVCoordinates pv) {
619 this.d = new double[] {
620 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
621 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
622 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
623 };
624 }
625
626
627
628
629 private Object readResolve() {
630 return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
631 new Vector3D(d[3], d[4], d[5]),
632 new Vector3D(d[6], d[7], d[8]));
633 }
634
635 }
636
637 }