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