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