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 import java.util.stream.Stream;
21
22 import org.hipparchus.analysis.differentiation.DerivativeStructure;
23 import org.hipparchus.analysis.interpolation.HermiteInterpolator;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.FastMath;
27 import org.orekit.annotation.DefaultDataContext;
28 import org.orekit.data.DataContext;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.errors.OrekitIllegalArgumentException;
31 import org.orekit.errors.OrekitInternalError;
32 import org.orekit.errors.OrekitMessages;
33 import org.orekit.frames.Frame;
34 import org.orekit.frames.Transform;
35 import org.orekit.time.AbsoluteDate;
36 import org.orekit.time.TimeInterpolable;
37 import org.orekit.time.TimeStamped;
38
39
40
41 public class AbsolutePVCoordinates extends TimeStampedPVCoordinates
42 implements TimeStamped, TimeInterpolable<AbsolutePVCoordinates>,
43 Serializable, PVCoordinatesProvider {
44
45
46 private static final long serialVersionUID = 20150824L;
47
48
49 private final Frame frame;
50
51
52
53
54
55
56
57
58 public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date,
59 final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
60 super(date, position, velocity, acceleration);
61 this.frame = frame;
62 }
63
64
65
66
67
68
69
70 public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date,
71 final Vector3D position,
72 final Vector3D velocity) {
73 this(frame, date, position, velocity, Vector3D.ZERO);
74 }
75
76
77
78
79
80
81 public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date, final PVCoordinates pva) {
82 super(date, pva);
83 this.frame = frame;
84 }
85
86
87
88
89
90 public AbsolutePVCoordinates(final Frame frame, final TimeStampedPVCoordinates pva) {
91 super(pva.getDate(), pva);
92 this.frame = frame;
93 }
94
95
96
97
98
99
100
101
102 public AbsolutePVCoordinates(final AbsoluteDate date,
103 final double a, final AbsolutePVCoordinates AbsPva) {
104 super(date, a, AbsPva);
105 this.frame = AbsPva.frame;
106 }
107
108
109
110
111
112
113
114
115
116
117 public AbsolutePVCoordinates(final AbsoluteDate date,
118 final AbsolutePVCoordinatessolutePVCoordinates">AbsolutePVCoordinates start, final AbsolutePVCoordinates end) {
119 super(date, start, end);
120 ensureIdenticalFrames(start, end);
121 this.frame = start.frame;
122 }
123
124
125
126
127
128
129
130
131
132
133
134
135 public AbsolutePVCoordinates(final AbsoluteDate date,
136 final double a1, final AbsolutePVCoordinates absPv1,
137 final double a2, final AbsolutePVCoordinates absPv2) {
138 super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates());
139 ensureIdenticalFrames(absPv1, absPv2);
140 this.frame = absPv1.getFrame();
141 }
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156 public AbsolutePVCoordinates(final AbsoluteDate date,
157 final double a1, final AbsolutePVCoordinates absPv1,
158 final double a2, final AbsolutePVCoordinates absPv2,
159 final double a3, final AbsolutePVCoordinates absPv3) {
160 super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates(),
161 a3, absPv3.getPVCoordinates());
162 ensureIdenticalFrames(absPv1, absPv2);
163 ensureIdenticalFrames(absPv1, absPv3);
164 this.frame = absPv1.getFrame();
165 }
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182 public AbsolutePVCoordinates(final AbsoluteDate date,
183 final double a1, final AbsolutePVCoordinates absPv1,
184 final double a2, final AbsolutePVCoordinates absPv2,
185 final double a3, final AbsolutePVCoordinates absPv3,
186 final double a4, final AbsolutePVCoordinates absPv4) {
187 super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates(),
188 a3, absPv3.getPVCoordinates(), a4, absPv4.getPVCoordinates());
189 ensureIdenticalFrames(absPv1, absPv2);
190 ensureIdenticalFrames(absPv1, absPv3);
191 ensureIdenticalFrames(absPv1, absPv4);
192 this.frame = absPv1.getFrame();
193 }
194
195
196
197
198
199
200
201
202
203
204 public AbsolutePVCoordinates(final Frame frame, final AbsoluteDate date,
205 final FieldVector3D<DerivativeStructure> p) {
206 super(date, p);
207 this.frame = frame;
208 }
209
210
211
212
213
214
215 private static void ensureIdenticalFrames(final AbsolutePVCoordinatesolutePVCoordinates">AbsolutePVCoordinates absPv1, final AbsolutePVCoordinates absPv2)
216 throws OrekitIllegalArgumentException {
217 if (!absPv1.frame.equals(absPv2.frame)) {
218 throw new OrekitIllegalArgumentException(OrekitMessages.INCOMPATIBLE_FRAMES,
219 absPv1.frame.getName(), absPv2.frame.getName());
220 }
221 }
222
223
224
225
226
227
228
229
230
231
232
233 public AbsolutePVCoordinates shiftedBy(final double dt) {
234 final TimeStampedPVCoordinates spv = super.shiftedBy(dt);
235 return new AbsolutePVCoordinates(frame, spv);
236 }
237
238
239
240
241
242
243
244
245
246 public PVCoordinatesProvider toTaylorProvider() {
247 return new PVCoordinatesProvider() {
248
249 public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate d, final Frame f) {
250 final TimeStampedPVCoordinates shifted = shiftedBy(d.durationFrom(getDate()));
251 final Transform transform = frame.getTransformTo(f, d);
252 return transform.transformPVCoordinates(shifted);
253 }
254 };
255 }
256
257
258
259
260 public Frame getFrame() {
261 return frame;
262 }
263
264
265
266
267 public TimeStampedPVCoordinates getPVCoordinates() {
268 return this;
269 }
270
271
272
273
274
275
276
277 public TimeStampedPVCoordinates getPVCoordinates(final Frame outputFrame) {
278
279
280 if (outputFrame == frame) {
281 return getPVCoordinates();
282 }
283
284
285 final Transform t = frame.getTransformTo(outputFrame, getDate());
286 return t.transformPVCoordinates(getPVCoordinates());
287 }
288
289 @Override
290 public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate otherDate, final Frame outputFrame) {
291 return shiftedBy(otherDate.durationFrom(getDate())).getPVCoordinates(outputFrame);
292 }
293
294 @Override
295 public AbsolutePVCoordinates interpolate(final AbsoluteDate date, final Stream<AbsolutePVCoordinates> sample) {
296 return interpolate(getFrame(), date, CartesianDerivativesFilter.USE_PVA, sample);
297 }
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321 public static AbsolutePVCoordinates interpolate(final Frame frame, final AbsoluteDate date,
322 final CartesianDerivativesFilter filter,
323 final Stream<AbsolutePVCoordinates> sample) {
324
325
326
327 final HermiteInterpolator interpolator = new HermiteInterpolator();
328
329
330 switch (filter) {
331 case USE_P :
332
333 sample.forEach(pv -> {
334 final Vector3D position = pv.getPosition();
335 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
336 position.toArray());
337 });
338 break;
339 case USE_PV :
340
341 sample.forEach(pv -> {
342 final Vector3D position = pv.getPosition();
343 final Vector3D velocity = pv.getVelocity();
344 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
345 position.toArray(), velocity.toArray());
346 });
347 break;
348 case USE_PVA :
349
350 sample.forEach(pv -> {
351 final Vector3D position = pv.getPosition();
352 final Vector3D velocity = pv.getVelocity();
353 final Vector3D acceleration = pv.getAcceleration();
354 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
355 position.toArray(), velocity.toArray(), acceleration.toArray());
356 });
357 break;
358 default :
359
360 throw new OrekitInternalError(null);
361 }
362
363
364 final double[][] p = interpolator.derivatives(0.0, 2);
365
366
367 return new AbsolutePVCoordinates(frame, date, new Vector3D(p[0]), new Vector3D(p[1]), new Vector3D(p[2]));
368
369 }
370
371
372
373
374 @DefaultDataContext
375 private Object writeReplace() {
376 return new DTO(this);
377 }
378
379
380 @DefaultDataContext
381 private static class DTO implements Serializable {
382
383
384 private static final long serialVersionUID = 20150916L;
385
386
387 private double[] d;
388
389
390 private final Frame frame;
391
392
393
394
395 private DTO(final AbsolutePVCoordinates absPva) {
396
397
398 final AbsoluteDate j2000Epoch =
399 DataContext.getDefault().getTimeScales().getJ2000Epoch();
400 final double epoch = FastMath.floor(absPva.getDate().durationFrom(j2000Epoch));
401 final double offset = absPva.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));
402
403 this.d = new double[] {
404 epoch, offset,
405 absPva.getPosition().getX(), absPva.getPosition().getY(), absPva.getPosition().getZ(),
406 absPva.getVelocity().getX(), absPva.getVelocity().getY(), absPva.getVelocity().getZ(),
407 absPva.getAcceleration().getX(), absPva.getAcceleration().getY(), absPva.getAcceleration().getZ()
408 };
409 this.frame = absPva.frame;
410
411 }
412
413
414
415
416 private Object readResolve() {
417 final AbsoluteDate j2000Epoch =
418 DataContext.getDefault().getTimeScales().getJ2000Epoch();
419 return new AbsolutePVCoordinates(frame,
420 j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
421 new Vector3D(d[2], d[3], d[ 4]),
422 new Vector3D(d[5], d[6], d[ 7]),
423 new Vector3D(d[8], d[9], d[10]));
424 }
425
426 }
427
428 }
429
430
431