1 /* Copyright 2002-2020 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.utils;
18
19 import java.io.Serializable;
20 import java.util.Collection;
21 import java.util.stream.Stream;
22
23 import org.hipparchus.analysis.differentiation.DerivativeStructure;
24 import org.hipparchus.analysis.interpolation.HermiteInterpolator;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.hipparchus.util.FastMath;
28 import org.orekit.annotation.DefaultDataContext;
29 import org.orekit.data.DataContext;
30 import org.orekit.errors.OrekitInternalError;
31 import org.orekit.frames.Frame;
32 import org.orekit.frames.Transform;
33 import org.orekit.time.AbsoluteDate;
34 import org.orekit.time.TimeScale;
35 import org.orekit.time.TimeStamped;
36
37 /** {@link TimeStamped time-stamped} version of {@link PVCoordinates}.
38 * <p>Instances of this class are guaranteed to be immutable.</p>
39 * @author Luc Maisonobe
40 * @since 7.0
41 */
42 public class TimeStampedPVCoordinates extends PVCoordinates implements TimeStamped {
43
44 /** Serializable UID. */
45 private static final long serialVersionUID = 20140723L;
46
47 /** The date. */
48 private final AbsoluteDate date;
49
50 /** Builds a TimeStampedPVCoordinates pair.
51 * @param date coordinates date
52 * @param position the position vector (m)
53 * @param velocity the velocity vector (m/s)
54 * @param acceleration the acceleration vector (m/s²)
55 */
56 public TimeStampedPVCoordinates(final AbsoluteDate date,
57 final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
58 super(position, velocity, acceleration);
59 this.date = date;
60 }
61
62 /**
63 * Build from position and velocity. Acceleration is set to zero.
64 *
65 * @param date coordinates date
66 * @param position the position vector (m)
67 * @param velocity the velocity vector (m/s)
68 */
69 public TimeStampedPVCoordinates(final AbsoluteDate date,
70 final Vector3D position,
71 final Vector3D velocity) {
72 this(date, position, velocity, Vector3D.ZERO);
73 }
74
75 /**
76 * Build from position velocity acceleration coordinates.
77 *
78 * @param date coordinates date
79 * @param pv position velocity, and acceleration coordinates, in meters and seconds.
80 */
81 public TimeStampedPVCoordinates(final AbsoluteDate date, final PVCoordinates pv) {
82 this(date, pv.getPosition(), pv.getVelocity(), pv.getAcceleration());
83 }
84
85 /** Multiplicative constructor
86 * <p>Build a TimeStampedPVCoordinates from another one and a scale factor.</p>
87 * <p>The TimeStampedPVCoordinates built will be a * pv</p>
88 * @param date date of the built coordinates
89 * @param a scale factor
90 * @param pv base (unscaled) PVCoordinates
91 */
92 public TimeStampedPVCoordinates(final AbsoluteDate date,
93 final double a, final PVCoordinates pv) {
94 super(new Vector3D(a, pv.getPosition()),
95 new Vector3D(a, pv.getVelocity()),
96 new Vector3D(a, pv.getAcceleration()));
97 this.date = date;
98 }
99
100 /** Subtractive constructor
101 * <p>Build a relative TimeStampedPVCoordinates from a start and an end position.</p>
102 * <p>The TimeStampedPVCoordinates built will be end - start.</p>
103 * @param date date of the built coordinates
104 * @param start Starting PVCoordinates
105 * @param end ending PVCoordinates
106 */
107 public TimeStampedPVCoordinates(final AbsoluteDate date,
108 final PVCoordinatesCoordinates">PVCoordinates start, final PVCoordinates end) {
109 super(end.getPosition().subtract(start.getPosition()),
110 end.getVelocity().subtract(start.getVelocity()),
111 end.getAcceleration().subtract(start.getAcceleration()));
112 this.date = date;
113 }
114
115 /** Linear constructor
116 * <p>Build a TimeStampedPVCoordinates from two other ones and corresponding scale factors.</p>
117 * <p>The TimeStampedPVCoordinates built will be a1 * u1 + a2 * u2</p>
118 * @param date date of the built coordinates
119 * @param a1 first scale factor
120 * @param pv1 first base (unscaled) PVCoordinates
121 * @param a2 second scale factor
122 * @param pv2 second base (unscaled) PVCoordinates
123 */
124 public TimeStampedPVCoordinates(final AbsoluteDate date,
125 final double a1, final PVCoordinates pv1,
126 final double a2, final PVCoordinates pv2) {
127 super(new Vector3D(a1, pv1.getPosition(), a2, pv2.getPosition()),
128 new Vector3D(a1, pv1.getVelocity(), a2, pv2.getVelocity()),
129 new Vector3D(a1, pv1.getAcceleration(), a2, pv2.getAcceleration()));
130 this.date = date;
131 }
132
133 /** Linear constructor
134 * <p>Build a TimeStampedPVCoordinates from three other ones and corresponding scale factors.</p>
135 * <p>The TimeStampedPVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
136 * @param date date of the built coordinates
137 * @param a1 first scale factor
138 * @param pv1 first base (unscaled) PVCoordinates
139 * @param a2 second scale factor
140 * @param pv2 second base (unscaled) PVCoordinates
141 * @param a3 third scale factor
142 * @param pv3 third base (unscaled) PVCoordinates
143 */
144 public TimeStampedPVCoordinates(final AbsoluteDate date,
145 final double a1, final PVCoordinates pv1,
146 final double a2, final PVCoordinates pv2,
147 final double a3, final PVCoordinates pv3) {
148 super(new Vector3D(a1, pv1.getPosition(), a2, pv2.getPosition(), a3, pv3.getPosition()),
149 new Vector3D(a1, pv1.getVelocity(), a2, pv2.getVelocity(), a3, pv3.getVelocity()),
150 new Vector3D(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(), a3, pv3.getAcceleration()));
151 this.date = date;
152 }
153
154 /** Linear constructor
155 * <p>Build a TimeStampedPVCoordinates from four other ones and corresponding scale factors.</p>
156 * <p>The TimeStampedPVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
157 * @param date date of the built coordinates
158 * @param a1 first scale factor
159 * @param pv1 first base (unscaled) PVCoordinates
160 * @param a2 second scale factor
161 * @param pv2 second base (unscaled) PVCoordinates
162 * @param a3 third scale factor
163 * @param pv3 third base (unscaled) PVCoordinates
164 * @param a4 fourth scale factor
165 * @param pv4 fourth base (unscaled) PVCoordinates
166 */
167 public TimeStampedPVCoordinates(final AbsoluteDate date,
168 final double a1, final PVCoordinates pv1,
169 final double a2, final PVCoordinates pv2,
170 final double a3, final PVCoordinates pv3,
171 final double a4, final PVCoordinates pv4) {
172 super(new Vector3D(a1, pv1.getPosition(), a2, pv2.getPosition(), a3, pv3.getPosition(), a4, pv4.getPosition()),
173 new Vector3D(a1, pv1.getVelocity(), a2, pv2.getVelocity(), a3, pv3.getVelocity(), a4, pv4.getVelocity()),
174 new Vector3D(a1, pv1.getAcceleration(), a2, pv2.getAcceleration(), a3, pv3.getAcceleration(), a4, pv4.getAcceleration()));
175 this.date = date;
176 }
177
178 /** Builds a TimeStampedPVCoordinates triplet from a {@link FieldVector3D}<{@link DerivativeStructure}>.
179 * <p>
180 * The vector components must have time as their only derivation parameter and
181 * have consistent derivation orders.
182 * </p>
183 * @param date date of the built coordinates
184 * @param p vector with time-derivatives embedded within the coordinates
185 */
186 public TimeStampedPVCoordinates(final AbsoluteDate date,
187 final FieldVector3D<DerivativeStructure> p) {
188 super(p);
189 this.date = date;
190 }
191
192 /** {@inheritDoc} */
193 public AbsoluteDate getDate() {
194 return date;
195 }
196
197 /** Get a time-shifted state.
198 * <p>
199 * The state can be slightly shifted to close dates. This shift is based on
200 * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
201 * proper orbit propagation (it is not even Keplerian!) but should be sufficient
202 * for either small time shifts or coarse accuracy.
203 * </p>
204 * @param dt time shift in seconds
205 * @return a new state, shifted with respect to the instance (which is immutable)
206 */
207 public TimeStampedPVCoordinates shiftedBy(final double dt) {
208 final PVCoordinates spv = super.shiftedBy(dt);
209 return new TimeStampedPVCoordinates(date.shiftedBy(dt),
210 spv.getPosition(), spv.getVelocity(), spv.getAcceleration());
211 }
212
213 /** Create a local provider using simply Taylor expansion through {@link #shiftedBy(double)}.
214 * <p>
215 * The time evolution is based on a simple Taylor expansion. It is <em>not</em> intended as a
216 * replacement for proper orbit propagation (it is not even Keplerian!) but should be sufficient
217 * for either small time shifts or coarse accuracy.
218 * </p>
219 * @param instanceFrame frame in which the instance is defined
220 * @return provider based on Taylor expansion, for small time shifts around instance date
221 */
222 public PVCoordinatesProvider toTaylorProvider(final Frame instanceFrame) {
223 return new PVCoordinatesProvider() {
224 /** {@inheritDoc} */
225 public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate d, final Frame f) {
226 final TimeStampedPVCoordinates shifted = shiftedBy(d.durationFrom(date));
227 final Transform transform = instanceFrame.getTransformTo(f, d);
228 return transform.transformPVCoordinates(shifted);
229 }
230 };
231 }
232
233 /** Interpolate position-velocity.
234 * <p>
235 * The interpolated instance is created by polynomial Hermite interpolation
236 * ensuring velocity remains the exact derivative of position.
237 * </p>
238 * <p>
239 * Note that even if first time derivatives (velocities)
240 * from sample can be ignored, the interpolated instance always includes
241 * interpolated derivatives. This feature can be used explicitly to
242 * compute these derivatives when it would be too complex to compute them
243 * from an analytical formula: just compute a few sample points from the
244 * explicit formula and set the derivatives to zero in these sample points,
245 * then use interpolation to add derivatives consistent with the positions.
246 * </p>
247 * @param date interpolation date
248 * @param filter filter for derivatives from the sample to use in interpolation
249 * @param sample sample points on which interpolation should be done
250 * @return a new position-velocity, interpolated at specified date
251 */
252 public static TimeStampedPVCoordinates interpolate(final AbsoluteDate date,
253 final CartesianDerivativesFilter filter,
254 final Collection<TimeStampedPVCoordinates> sample) {
255 return interpolate(date, filter, sample.stream());
256 }
257
258 /** Interpolate position-velocity.
259 * <p>
260 * The interpolated instance is created by polynomial Hermite interpolation
261 * ensuring velocity remains the exact derivative of position.
262 * </p>
263 * <p>
264 * Note that even if first time derivatives (velocities)
265 * from sample can be ignored, the interpolated instance always includes
266 * interpolated derivatives. This feature can be used explicitly to
267 * compute these derivatives when it would be too complex to compute them
268 * from an analytical formula: just compute a few sample points from the
269 * explicit formula and set the derivatives to zero in these sample points,
270 * then use interpolation to add derivatives consistent with the positions.
271 * </p>
272 * @param date interpolation date
273 * @param filter filter for derivatives from the sample to use in interpolation
274 * @param sample sample points on which interpolation should be done
275 * @return a new position-velocity, interpolated at specified date
276 * @since 9.0
277 */
278 public static TimeStampedPVCoordinates interpolate(final AbsoluteDate date,
279 final CartesianDerivativesFilter filter,
280 final Stream<TimeStampedPVCoordinates> sample) {
281
282 // set up an interpolator taking derivatives into account
283 final HermiteInterpolator interpolator = new HermiteInterpolator();
284
285 // add sample points
286 switch (filter) {
287 case USE_P :
288 // populate sample with position data, ignoring velocity
289 sample.forEach(pv -> {
290 final Vector3D position = pv.getPosition();
291 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
292 position.toArray());
293 });
294 break;
295 case USE_PV :
296 // populate sample with position and velocity data
297 sample.forEach(pv -> {
298 final Vector3D position = pv.getPosition();
299 final Vector3D velocity = pv.getVelocity();
300 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
301 position.toArray(), velocity.toArray());
302 });
303 break;
304 case USE_PVA :
305 // populate sample with position, velocity and acceleration data
306 sample.forEach(pv -> {
307 final Vector3D position = pv.getPosition();
308 final Vector3D velocity = pv.getVelocity();
309 final Vector3D acceleration = pv.getAcceleration();
310 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
311 position.toArray(), velocity.toArray(), acceleration.toArray());
312 });
313 break;
314 default :
315 // this should never happen
316 throw new OrekitInternalError(null);
317 }
318
319 // interpolate
320 final double[][] p = interpolator.derivatives(0.0, 2);
321
322 // build a new interpolated instance
323 return new TimeStampedPVCoordinates(date, new Vector3D(p[0]), new Vector3D(p[1]), new Vector3D(p[2]));
324
325 }
326
327 /** Return a string representation of this date, position, velocity, and acceleration.
328 *
329 * <p>This method uses the {@link DataContext#getDefault() default data context}.
330 *
331 * @return string representation of this.
332 */
333 @Override
334 @DefaultDataContext
335 public String toString() {
336 return toString(DataContext.getDefault().getTimeScales().getUTC());
337 }
338
339 /**
340 * Return a string representation of this date, position, velocity, and acceleration.
341 *
342 * @param utc time scale used to print the date.
343 * @return string representation of this.
344 */
345 public String toString(final TimeScale utc) {
346 final String comma = ", ";
347 return new StringBuffer().append('{').
348 append(date.toString(utc)).append(", P(").
349 append(getPosition().getX()).append(comma).
350 append(getPosition().getY()).append(comma).
351 append(getPosition().getZ()).append("), V(").
352 append(getVelocity().getX()).append(comma).
353 append(getVelocity().getY()).append(comma).
354 append(getVelocity().getZ()).append("), A(").
355 append(getAcceleration().getX()).append(comma).
356 append(getAcceleration().getY()).append(comma).
357 append(getAcceleration().getZ()).append(")}").toString();
358 }
359
360 /** Replace the instance with a data transfer object for serialization.
361 * @return data transfer object that will be serialized
362 */
363 @DefaultDataContext
364 private Object writeReplace() {
365 return new DTO(this);
366 }
367
368 /** Internal class used only for serialization. */
369 @DefaultDataContext
370 private static class DTO implements Serializable {
371
372 /** Serializable UID. */
373 private static final long serialVersionUID = 20140723L;
374
375 /** Double values. */
376 private double[] d;
377
378 /** Simple constructor.
379 * @param pv instance to serialize
380 */
381 private DTO(final TimeStampedPVCoordinates pv) {
382
383 // decompose date
384 final AbsoluteDate j2000Epoch =
385 DataContext.getDefault().getTimeScales().getJ2000Epoch();
386 final double epoch = FastMath.floor(pv.getDate().durationFrom(j2000Epoch));
387 final double offset = pv.getDate().durationFrom(j2000Epoch.shiftedBy(epoch));
388
389 this.d = new double[] {
390 epoch, offset,
391 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
392 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
393 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ()
394 };
395
396 }
397
398 /** Replace the deserialized data transfer object with a {@link TimeStampedPVCoordinates}.
399 * @return replacement {@link TimeStampedPVCoordinates}
400 */
401 private Object readResolve() {
402 final AbsoluteDate j2000Epoch =
403 DataContext.getDefault().getTimeScales().getJ2000Epoch();
404 return new TimeStampedPVCoordinates(j2000Epoch.shiftedBy(d[0]).shiftedBy(d[1]),
405 new Vector3D(d[2], d[3], d[ 4]),
406 new Vector3D(d[5], d[6], d[ 7]),
407 new Vector3D(d[8], d[9], d[10]));
408 }
409
410 }
411
412 }