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