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