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