1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
18
19 import java.io.Serializable;
20
21 import org.hipparchus.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.DerivativeStructure;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.util.FastMath;
26 import org.orekit.errors.OrekitException;
27 import org.orekit.errors.OrekitMessages;
28 import org.orekit.time.TimeShiftable;
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44 public class PVCoordinates implements TimeShiftable<PVCoordinates>, Serializable {
45
46
47 public static final PVCoordinatesPVCoordinates">PVCoordinates ZERO = new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);
48
49
50 private static final long serialVersionUID = 20140407L;
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(finalPVCoordinateslass="jxr_keyword">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 PVCoordinatesCoordinates">PVCoordinateshtml#PVCoordinates">PVCoordinates(final PVCoordinatesCoordinates">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(finalPVCoordinatesass="jxr_keyword">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(finalPVCoordinatesass="jxr_keyword">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(finalPVCoordinatesass="jxr_keyword">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 public PVCoordinates(final FieldVector3D<DerivativeStructure> p) {
181 position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal());
182 if (p.getX().getOrder() >= 1) {
183 velocity = new Vector3D(p.getX().getPartialDerivative(1),
184 p.getY().getPartialDerivative(1),
185 p.getZ().getPartialDerivative(1));
186 if (p.getX().getOrder() >= 2) {
187 acceleration = new Vector3D(p.getX().getPartialDerivative(2),
188 p.getY().getPartialDerivative(2),
189 p.getZ().getPartialDerivative(2));
190 } else {
191 acceleration = Vector3D.ZERO;
192 }
193 } else {
194 velocity = Vector3D.ZERO;
195 acceleration = Vector3D.ZERO;
196 }
197 }
198
199
200
201
202
203
204
205
206
207 public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order) {
208
209 final DSFactory factory;
210 final DerivativeStructure x;
211 final DerivativeStructure y;
212 final DerivativeStructure z;
213 switch(order) {
214 case 0 :
215 factory = new DSFactory(1, order);
216 x = factory.build(position.getX());
217 y = factory.build(position.getY());
218 z = factory.build(position.getZ());
219 break;
220 case 1 :
221 factory = new DSFactory(1, order);
222 x = factory.build(position.getX(), velocity.getX());
223 y = factory.build(position.getY(), velocity.getY());
224 z = factory.build(position.getZ(), velocity.getZ());
225 break;
226 case 2 :
227 factory = new DSFactory(1, order);
228 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
229 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
230 z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
231 break;
232 default :
233 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
234 }
235
236 return new FieldVector3D<>(x, y, z);
237
238 }
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261 public FieldPVCoordinates<DerivativeStructure> toDerivativeStructurePV(final int order) {
262
263 final DSFactory factory;
264 final DerivativeStructure x0;
265 final DerivativeStructure y0;
266 final DerivativeStructure z0;
267 final DerivativeStructure x1;
268 final DerivativeStructure y1;
269 final DerivativeStructure z1;
270 final DerivativeStructure x2;
271 final DerivativeStructure y2;
272 final DerivativeStructure z2;
273 switch(order) {
274 case 0 :
275 factory = new DSFactory(1, order);
276 x0 = factory.build(position.getX());
277 y0 = factory.build(position.getY());
278 z0 = factory.build(position.getZ());
279 x1 = factory.build(velocity.getX());
280 y1 = factory.build(velocity.getY());
281 z1 = factory.build(velocity.getZ());
282 x2 = factory.build(acceleration.getX());
283 y2 = factory.build(acceleration.getY());
284 z2 = factory.build(acceleration.getZ());
285 break;
286 case 1 : {
287 factory = new DSFactory(1, order);
288 final double r2 = position.getNormSq();
289 final double r = FastMath.sqrt(r2);
290 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
291 final double a = acceleration.getNorm();
292 final double aOr = a / r;
293 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
294 x0 = factory.build(position.getX(), velocity.getX());
295 y0 = factory.build(position.getY(), velocity.getY());
296 z0 = factory.build(position.getZ(), velocity.getZ());
297 x1 = factory.build(velocity.getX(), acceleration.getX());
298 y1 = factory.build(velocity.getY(), acceleration.getY());
299 z1 = factory.build(velocity.getZ(), acceleration.getZ());
300 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
301 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
302 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
303 break;
304 }
305 case 2 : {
306 factory = new DSFactory(1, order);
307 final double r2 = position.getNormSq();
308 final double r = FastMath.sqrt(r2);
309 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
310 final double a = acceleration.getNorm();
311 final double aOr = a / r;
312 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
313 final double v2 = velocity.getNormSq();
314 final double pa = Vector3D.dotProduct(position, acceleration);
315 final double aj = Vector3D.dotProduct(acceleration, keplerianJerk);
316 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
317 4 * aOr * pvOr2 - aj / (a * r), velocity);
318 x0 = factory.build(position.getX(), velocity.getX(), acceleration.getX());
319 y0 = factory.build(position.getY(), velocity.getY(), acceleration.getY());
320 z0 = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
321 x1 = factory.build(velocity.getX(), acceleration.getX(), keplerianJerk.getX());
322 y1 = factory.build(velocity.getY(), acceleration.getY(), keplerianJerk.getY());
323 z1 = factory.build(velocity.getZ(), acceleration.getZ(), keplerianJerk.getZ());
324 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
325 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
326 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
327 break;
328 }
329 default :
330 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
331 }
332
333 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
334 new FieldVector3D<>(x1, y1, z1),
335 new FieldVector3D<>(x2, y2, z2));
336
337 }
338
339
340
341
342
343
344
345
346
347 public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
348 final double scale = 1.0 / dt;
349 return new Vector3D(scale, end, -scale, start);
350 }
351
352
353
354
355
356
357
358
359
360
361
362 public PVCoordinates shiftedBy(final double dt) {
363 return new PVCoordinates(new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
364 new Vector3D(1, velocity, dt, acceleration),
365 acceleration);
366 }
367
368
369
370
371 public Vector3D getPosition() {
372 return position;
373 }
374
375
376
377
378 public Vector3D getVelocity() {
379 return velocity;
380 }
381
382
383
384
385 public Vector3D getAcceleration() {
386 return acceleration;
387 }
388
389
390
391
392
393
394
395
396
397 public Vector3D getMomentum() {
398 return Vector3D.crossProduct(position, velocity);
399 }
400
401
402
403
404
405
406
407
408
409
410
411 public Vector3D getAngularVelocity() {
412 return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
413 }
414
415
416
417
418 public PVCoordinates negate() {
419 return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
420 }
421
422
423
424
425
426
427
428
429
430
431
432
433
434 public PVCoordinates normalize() {
435 final double inv = 1.0 / position.getNorm();
436 final Vector3D u = new Vector3D(inv, position);
437 final Vector3D v = new Vector3D(inv, velocity);
438 final Vector3D w = new Vector3D(inv, acceleration);
439 final double uv = Vector3D.dotProduct(u, v);
440 final double v2 = Vector3D.dotProduct(v, v);
441 final double uw = Vector3D.dotProduct(u, w);
442 final Vector3D uDot = new Vector3D(1, v, -uv, u);
443 final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
444 return new PVCoordinates(u, uDot, uDotDot);
445 }
446
447
448
449
450
451
452 public static PVCoordinatesPVCoordinates">PVCoordinatesnates">PVCoordinates crossProduct(final PVCoordinatesPVCoordinates">PVCoordinates pv1, final PVCoordinates pv2) {
453 final Vector3D p1 = pv1.position;
454 final Vector3D v1 = pv1.velocity;
455 final Vector3D a1 = pv1.acceleration;
456 final Vector3D p2 = pv2.position;
457 final Vector3D v2 = pv2.velocity;
458 final Vector3D a2 = pv2.acceleration;
459 return new PVCoordinates(Vector3D.crossProduct(p1, p2),
460 new Vector3D(1, Vector3D.crossProduct(p1, v2),
461 1, Vector3D.crossProduct(v1, p2)),
462 new Vector3D(1, Vector3D.crossProduct(p1, a2),
463 2, Vector3D.crossProduct(v1, v2),
464 1, Vector3D.crossProduct(a1, p2)));
465 }
466
467
468
469
470 public String toString() {
471 final String comma = ", ";
472 return new StringBuffer().append('{').append("P(").
473 append(position.getX()).append(comma).
474 append(position.getY()).append(comma).
475 append(position.getZ()).append("), V(").
476 append(velocity.getX()).append(comma).
477 append(velocity.getY()).append(comma).
478 append(velocity.getZ()).append("), A(").
479 append(acceleration.getX()).append(comma).
480 append(acceleration.getY()).append(comma).
481 append(acceleration.getZ()).append(")}").toString();
482 }
483
484
485
486
487 private Object writeReplace() {
488 return new DTO(this);
489 }
490
491
492 private static class DTO implements Serializable {
493
494
495 private static final long serialVersionUID = 20140723L;
496
497
498 private double[] d;
499
500
501
502
503 private DTO(final PVCoordinates pv) {
504 this.d = new double[] {
505 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
506 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
507 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
508 };
509 }
510
511
512
513
514 private Object readResolve() {
515 return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
516 new Vector3D(d[3], d[4], d[5]),
517 new Vector3D(d[6], d[7], d[8]));
518 }
519
520 }
521
522 }