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