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