1 /* Copyright 2002-2016 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
22 import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
23 import org.apache.commons.math3.analysis.interpolation.HermiteInterpolator;
24 import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
25 import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
26 import org.apache.commons.math3.util.FastMath;
27 import org.orekit.errors.OrekitException;
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 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 throws OrekitException {
224 final TimeStampedPVCoordinates shifted = shiftedBy(d.durationFrom(date));
225 final Transform transform = instanceFrame.getTransformTo(f, d);
226 return transform.transformPVCoordinates(shifted);
227 }
228 };
229 }
230
231 /** Interpolate position-velocity.
232 * <p>
233 * The interpolated instance is created by polynomial Hermite interpolation
234 * ensuring velocity remains the exact derivative of position.
235 * </p>
236 * <p>
237 * Note that even if first time derivatives (velocities)
238 * from sample can be ignored, the interpolated instance always includes
239 * interpolated derivatives. This feature can be used explicitly to
240 * compute these derivatives when it would be too complex to compute them
241 * from an analytical formula: just compute a few sample points from the
242 * explicit formula and set the derivatives to zero in these sample points,
243 * then use interpolation to add derivatives consistent with the positions.
244 * </p>
245 * @param date interpolation date
246 * @param filter filter for derivatives from the sample to use in interpolation
247 * @param sample sample points on which interpolation should be done
248 * @return a new position-velocity, interpolated at specified date
249 */
250 public static TimeStampedPVCoordinates interpolate(final AbsoluteDate date,
251 final CartesianDerivativesFilter filter,
252 final Collection<TimeStampedPVCoordinates> sample) {
253
254 // set up an interpolator taking derivatives into account
255 final HermiteInterpolator interpolator = new HermiteInterpolator();
256
257 // add sample points
258 switch (filter) {
259 case USE_P :
260 // populate sample with position data, ignoring velocity
261 for (final TimeStampedPVCoordinates pv : sample) {
262 final Vector3D position = pv.getPosition();
263 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
264 new double[] {
265 position.getX(), position.getY(), position.getZ()
266 });
267 }
268 break;
269 case USE_PV :
270 // populate sample with position and velocity data
271 for (final TimeStampedPVCoordinates pv : sample) {
272 final Vector3D position = pv.getPosition();
273 final Vector3D velocity = pv.getVelocity();
274 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
275 new double[] {
276 position.getX(), position.getY(), position.getZ()
277 }, new double[] {
278 velocity.getX(), velocity.getY(), velocity.getZ()
279 });
280 }
281 break;
282 case USE_PVA :
283 // populate sample with position, velocity and acceleration data
284 for (final TimeStampedPVCoordinates pv : sample) {
285 final Vector3D position = pv.getPosition();
286 final Vector3D velocity = pv.getVelocity();
287 final Vector3D acceleration = pv.getAcceleration();
288 interpolator.addSamplePoint(pv.getDate().durationFrom(date),
289 new double[] {
290 position.getX(), position.getY(), position.getZ()
291 }, new double[] {
292 velocity.getX(), velocity.getY(), velocity.getZ()
293 }, new double[] {
294 acceleration.getX(), acceleration.getY(), acceleration.getZ()
295 });
296 }
297 break;
298 default :
299 // this should never happen
300 throw new OrekitInternalError(null);
301 }
302
303 // interpolate
304 final DerivativeStructure zero = new DerivativeStructure(1, 2, 0, 0.0);
305 final DerivativeStructure[] p = interpolator.value(zero);
306
307 // build a new interpolated instance
308 return new TimeStampedPVCoordinates(date,
309 new Vector3D(p[0].getValue(),
310 p[1].getValue(),
311 p[2].getValue()),
312 new Vector3D(p[0].getPartialDerivative(1),
313 p[1].getPartialDerivative(1),
314 p[2].getPartialDerivative(1)),
315 new Vector3D(p[0].getPartialDerivative(2),
316 p[1].getPartialDerivative(2),
317 p[2].getPartialDerivative(2)));
318
319 }
320
321 /** Return a string representation of this position/velocity pair.
322 * @return string representation of this position/velocity pair
323 */
324 public String toString() {
325 final String comma = ", ";
326 return new StringBuffer().append('{').append(date).append(", P(").
327 append(getPosition().getX()).append(comma).
328 append(getPosition().getY()).append(comma).
329 append(getPosition().getZ()).append("), V(").
330 append(getVelocity().getX()).append(comma).
331 append(getVelocity().getY()).append(comma).
332 append(getVelocity().getZ()).append("), A(").
333 append(getAcceleration().getX()).append(comma).
334 append(getAcceleration().getY()).append(comma).
335 append(getAcceleration().getZ()).append(")}").toString();
336 }
337
338 /** Replace the instance with a data transfer object for serialization.
339 * @return data transfer object that will be serialized
340 */
341 private Object writeReplace() {
342 return new DTO(this);
343 }
344
345 /** Internal class used only for serialization. */
346 private static class DTO implements Serializable {
347
348 /** Serializable UID. */
349 private static final long serialVersionUID = 20140723L;
350
351 /** Double values. */
352 private double[] d;
353
354 /** Simple constructor.
355 * @param pv instance to serialize
356 */
357 private DTO(final TimeStampedPVCoordinates pv) {
358
359 // decompose date
360 final double epoch = FastMath.floor(pv.getDate().durationFrom(AbsoluteDate.J2000_EPOCH));
361 final double offset = pv.getDate().durationFrom(AbsoluteDate.J2000_EPOCH.shiftedBy(epoch));
362
363 this.d = new double[] {
364 epoch, offset,
365 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
366 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
367 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ()
368 };
369
370 }
371
372 /** Replace the deserialized data transfer object with a {@link TimeStampedPVCoordinates}.
373 * @return replacement {@link TimeStampedPVCoordinates}
374 */
375 private Object readResolve() {
376 return new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(d[0]).shiftedBy(d[1]),
377 new Vector3D(d[2], d[3], d[ 4]),
378 new Vector3D(d[5], d[6], d[ 7]),
379 new Vector3D(d[8], d[9], d[10]));
380 }
381
382 }
383
384 }