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