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