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.ArrayList;
21 import java.util.Collection;
22 import java.util.List;
23
24 import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
25 import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
26 import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
27 import org.apache.commons.math3.util.Pair;
28 import org.orekit.errors.OrekitException;
29 import org.orekit.errors.OrekitMessages;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.time.TimeShiftable;
32
33 /** Simple container for Position/Velocity/Acceleration triplets.
34 * <p>
35 * The state can be slightly shifted to close dates. This shift is based on
36 * a simple quadratic model. It is <em>not</em> intended as a replacement for
37 * proper orbit propagation (it is not even Keplerian!) but should be sufficient
38 * for either small time shifts or coarse accuracy.
39 * </p>
40 * <p>
41 * This class is the angular counterpart to {@link AngularCoordinates}.
42 * </p>
43 * <p>Instances of this class are guaranteed to be immutable.</p>
44 * @author Fabien Maussion
45 * @author Luc Maisonobe
46 */
47 public class PVCoordinates implements TimeShiftable<PVCoordinates>, Serializable {
48
49 /** Fixed position/velocity at origin (both p, v and a are zero vectors). */
50 public static final PVCoordinates ZERO = new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);
51
52 /** Serializable UID. */
53 private static final long serialVersionUID = 20140407L;
54
55 /** The position. */
56 private final Vector3D position;
57
58 /** The velocity. */
59 private final Vector3D velocity;
60
61 /** The acceleration. */
62 private final Vector3D acceleration;
63
64 /** Simple constructor.
65 * <p> Set the Coordinates to default : (0 0 0), (0 0 0), (0 0 0).</p>
66 */
67 public PVCoordinates() {
68 position = Vector3D.ZERO;
69 velocity = Vector3D.ZERO;
70 acceleration = Vector3D.ZERO;
71 }
72
73 /** Builds a PVCoordinates triplet with zero acceleration.
74 * <p>Acceleration is set to zero</p>
75 * @param position the position vector (m)
76 * @param velocity the velocity vector (m/s)
77 */
78 public PVCoordinates(final Vector3D position, final Vector3D velocity) {
79 this.position = position;
80 this.velocity = velocity;
81 this.acceleration = Vector3D.ZERO;
82 }
83
84 /** Builds a PVCoordinates triplet.
85 * @param position the position vector (m)
86 * @param velocity the velocity vector (m/s)
87 * @param acceleration the acceleration vector (m/s²)
88 */
89 public PVCoordinates(final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
90 this.position = position;
91 this.velocity = velocity;
92 this.acceleration = acceleration;
93 }
94
95 /** Multiplicative constructor.
96 * <p>Build a PVCoordinates from another one and a scale factor.</p>
97 * <p>The PVCoordinates built will be a * pv</p>
98 * @param a scale factor
99 * @param pv base (unscaled) PVCoordinates
100 */
101 public PVCoordinates(final double a, final PVCoordinates pv) {
102 position = new Vector3D(a, pv.position);
103 velocity = new Vector3D(a, pv.velocity);
104 acceleration = new Vector3D(a, pv.acceleration);
105 }
106
107 /** Subtractive constructor.
108 * <p>Build a relative PVCoordinates from a start and an end position.</p>
109 * <p>The PVCoordinates built will be end - start.</p>
110 * @param start Starting PVCoordinates
111 * @param end ending PVCoordinates
112 */
113 public PVCoordinates(final PVCoordinates start, final PVCoordinates end) {
114 this.position = end.position.subtract(start.position);
115 this.velocity = end.velocity.subtract(start.velocity);
116 this.acceleration = end.acceleration.subtract(start.acceleration);
117 }
118
119 /** Linear constructor.
120 * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
121 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
122 * @param a1 first scale factor
123 * @param pv1 first base (unscaled) PVCoordinates
124 * @param a2 second scale factor
125 * @param pv2 second base (unscaled) PVCoordinates
126 */
127 public PVCoordinates(final double a1, final PVCoordinates pv1,
128 final double a2, final PVCoordinates pv2) {
129 position = new Vector3D(a1, pv1.position, a2, pv2.position);
130 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity);
131 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration);
132 }
133
134 /** Linear constructor.
135 * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
136 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
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 PVCoordinates(final double a1, final PVCoordinates pv1,
145 final double a2, final PVCoordinates pv2,
146 final double a3, final PVCoordinates pv3) {
147 position = new Vector3D(a1, pv1.position, a2, pv2.position, a3, pv3.position);
148 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity, a3, pv3.velocity);
149 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration);
150 }
151
152 /** Linear constructor.
153 * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
154 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
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 PVCoordinates(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 position = new Vector3D(a1, pv1.position, a2, pv2.position,
169 a3, pv3.position, a4, pv4.position);
170 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity,
171 a3, pv3.velocity, a4, pv4.velocity);
172 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration,
173 a3, pv3.acceleration, a4, pv4.acceleration);
174 }
175
176 /** Builds a PVCoordinates 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 p vector with time-derivatives embedded within the coordinates
182 */
183 public PVCoordinates(final FieldVector3D<DerivativeStructure> p) {
184 position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal());
185 if (p.getX().getOrder() >= 1) {
186 velocity = new Vector3D(p.getX().getPartialDerivative(1),
187 p.getY().getPartialDerivative(1),
188 p.getZ().getPartialDerivative(1));
189 if (p.getX().getOrder() >= 2) {
190 acceleration = new Vector3D(p.getX().getPartialDerivative(2),
191 p.getY().getPartialDerivative(2),
192 p.getZ().getPartialDerivative(2));
193 } else {
194 acceleration = Vector3D.ZERO;
195 }
196 } else {
197 velocity = Vector3D.ZERO;
198 acceleration = Vector3D.ZERO;
199 }
200 }
201
202 /** Transform the instance to a {@link FieldVector3D}<{@link DerivativeStructure}>.
203 * <p>
204 * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
205 * to the user-specified order.
206 * </p>
207 * @param order derivation order for the vector components (must be either 0, 1 or 2)
208 * @return vector with time-derivatives embedded within the coordinates
209 * @exception OrekitException if the user specified order is too large
210 */
211 public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order)
212 throws OrekitException {
213
214 final DerivativeStructure x;
215 final DerivativeStructure y;
216 final DerivativeStructure z;
217 switch(order) {
218 case 0 :
219 x = new DerivativeStructure(1, 0, position.getX());
220 y = new DerivativeStructure(1, 0, position.getY());
221 z = new DerivativeStructure(1, 0, position.getZ());
222 break;
223 case 1 :
224 x = new DerivativeStructure(1, 1, position.getX(), velocity.getX());
225 y = new DerivativeStructure(1, 1, position.getY(), velocity.getY());
226 z = new DerivativeStructure(1, 1, position.getZ(), velocity.getZ());
227 break;
228 case 2 :
229 x = new DerivativeStructure(1, 2, position.getX(), velocity.getX(), acceleration.getX());
230 y = new DerivativeStructure(1, 2, position.getY(), velocity.getY(), acceleration.getY());
231 z = new DerivativeStructure(1, 2, position.getZ(), velocity.getZ(), acceleration.getZ());
232 break;
233 default :
234 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
235 }
236
237 return new FieldVector3D<DerivativeStructure>(x, y, z);
238
239 }
240
241 /** Estimate velocity between two positions.
242 * <p>Estimation is based on a simple fixed velocity translation
243 * during the time interval between the two positions.</p>
244 * @param start start position
245 * @param end end position
246 * @param dt time elapsed between the dates of the two positions
247 * @return velocity allowing to go from start to end positions
248 */
249 public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
250 final double scale = 1.0 / dt;
251 return new Vector3D(scale, end, -scale, start);
252 }
253
254 /** Get a time-shifted state.
255 * <p>
256 * The state can be slightly shifted to close dates. This shift is based on
257 * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
258 * proper orbit propagation (it is not even Keplerian!) but should be sufficient
259 * for either small time shifts or coarse accuracy.
260 * </p>
261 * @param dt time shift in seconds
262 * @return a new state, shifted with respect to the instance (which is immutable)
263 */
264 public PVCoordinates shiftedBy(final double dt) {
265 return new PVCoordinates(new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
266 new Vector3D(1, velocity, dt, acceleration),
267 acceleration);
268 }
269
270 /** Interpolate position-velocity.
271 * <p>
272 * The interpolated instance is created by polynomial Hermite interpolation
273 * ensuring velocity remains the exact derivative of position.
274 * </p>
275 * <p>
276 * Note that even if first time derivatives (velocities)
277 * from sample can be ignored, the interpolated instance always includes
278 * interpolated derivatives. This feature can be used explicitly to
279 * compute these derivatives when it would be too complex to compute them
280 * from an analytical formula: just compute a few sample points from the
281 * explicit formula and set the derivatives to zero in these sample points,
282 * then use interpolation to add derivatives consistent with the positions.
283 * </p>
284 * @param date interpolation date
285 * @param useVelocities if true, use sample points velocities,
286 * otherwise ignore them and use only positions
287 * @param sample sample points on which interpolation should be done
288 * @return a new position-velocity, interpolated at specified date
289 * @deprecated since 7.0 replaced with {@link TimeStampedPVCoordinates#interpolate(AbsoluteDate, CartesianDerivativesFilter, Collection)}
290 */
291 @Deprecated
292 public static PVCoordinates interpolate(final AbsoluteDate date, final boolean useVelocities,
293 final Collection<Pair<AbsoluteDate, PVCoordinates>> sample) {
294 final List<TimeStampedPVCoordinates> list = new ArrayList<TimeStampedPVCoordinates>(sample.size());
295 for (final Pair<AbsoluteDate, PVCoordinates> pair : sample) {
296 list.add(new TimeStampedPVCoordinates(pair.getFirst(),
297 pair.getSecond().getPosition(),
298 pair.getSecond().getVelocity(),
299 pair.getSecond().getAcceleration()));
300 }
301 return TimeStampedPVCoordinates.interpolate(date,
302 useVelocities ? CartesianDerivativesFilter.USE_PV : CartesianDerivativesFilter.USE_P,
303 list);
304 }
305
306 /** Gets the position.
307 * @return the position vector (m).
308 */
309 public Vector3D getPosition() {
310 return position;
311 }
312
313 /** Gets the velocity.
314 * @return the velocity vector (m/s).
315 */
316 public Vector3D getVelocity() {
317 return velocity;
318 }
319
320 /** Gets the acceleration.
321 * @return the acceleration vector (m/s²).
322 */
323 public Vector3D getAcceleration() {
324 return acceleration;
325 }
326
327 /** Gets the momentum.
328 * <p>This vector is the p ⊗ v where p is position, v is velocity
329 * and ⊗ is cross product. To get the real physical angular momentum
330 * you need to multiply this vector by the mass.</p>
331 * <p>The returned vector is recomputed each time this method is called, it
332 * is not cached.</p>
333 * @return a new instance of the momentum vector (m²/s).
334 */
335 public Vector3D getMomentum() {
336 return Vector3D.crossProduct(position, velocity);
337 }
338
339 /**
340 * Get the angular velocity (spin) of this point as seen from the origin.
341 * <p/>
342 * The angular velocity vector is parallel to the {@link #getMomentum() angular
343 * momentum} and is computed by ω = p × v / ||p||²
344 *
345 * @return the angular velocity vector
346 * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
347 * Wikipedia</a>
348 */
349 public Vector3D getAngularVelocity() {
350 return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
351 }
352
353 /** Get the opposite of the instance.
354 * @return a new position-velocity which is opposite to the instance
355 */
356 public PVCoordinates negate() {
357 return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
358 }
359
360 /** Normalize the position part of the instance.
361 * <p>
362 * The computed coordinates first component (position) will be a
363 * normalized vector, the second component (velocity) will be the
364 * derivative of the first component (hence it will generally not
365 * be normalized), and the third component (acceleration) will be the
366 * derivative of the second component (hence it will generally not
367 * be normalized).
368 * </p>
369 * @return a new instance, with first component normalized and
370 * remaining component computed to have consistent derivatives
371 */
372 public PVCoordinates normalize() {
373 final double inv = 1.0 / position.getNorm();
374 final Vector3D u = new Vector3D(inv, position);
375 final Vector3D v = new Vector3D(inv, velocity);
376 final Vector3D w = new Vector3D(inv, acceleration);
377 final double uv = Vector3D.dotProduct(u, v);
378 final double v2 = Vector3D.dotProduct(v, v);
379 final double uw = Vector3D.dotProduct(u, w);
380 final Vector3D uDot = new Vector3D(1, v, -uv, u);
381 final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
382 return new PVCoordinates(u, uDot, uDotDot);
383 }
384
385 /** Compute the cross-product of two instances.
386 * @param pv1 first instances
387 * @param pv2 second instances
388 * @return the cross product v1 ^ v2 as a new instance
389 */
390 public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) {
391 final Vector3D p1 = pv1.position;
392 final Vector3D v1 = pv1.velocity;
393 final Vector3D a1 = pv1.acceleration;
394 final Vector3D p2 = pv2.position;
395 final Vector3D v2 = pv2.velocity;
396 final Vector3D a2 = pv2.acceleration;
397 return new PVCoordinates(Vector3D.crossProduct(p1, p2),
398 new Vector3D(1, Vector3D.crossProduct(p1, v2),
399 1, Vector3D.crossProduct(v1, p2)),
400 new Vector3D(1, Vector3D.crossProduct(p1, a2),
401 2, Vector3D.crossProduct(v1, v2),
402 1, Vector3D.crossProduct(a1, p2)));
403 }
404
405 /** Return a string representation of this position/velocity pair.
406 * @return string representation of this position/velocity pair
407 */
408 public String toString() {
409 final String comma = ", ";
410 return new StringBuffer().append('{').append("P(").
411 append(position.getX()).append(comma).
412 append(position.getY()).append(comma).
413 append(position.getZ()).append("), V(").
414 append(velocity.getX()).append(comma).
415 append(velocity.getY()).append(comma).
416 append(velocity.getZ()).append("), A(").
417 append(acceleration.getX()).append(comma).
418 append(acceleration.getY()).append(comma).
419 append(acceleration.getZ()).append(")}").toString();
420 }
421
422 /** Replace the instance with a data transfer object for serialization.
423 * @return data transfer object that will be serialized
424 */
425 private Object writeReplace() {
426 return new DTO(this);
427 }
428
429 /** Internal class used only for serialization. */
430 private static class DTO implements Serializable {
431
432 /** Serializable UID. */
433 private static final long serialVersionUID = 20140723L;
434
435 /** Double values. */
436 private double[] d;
437
438 /** Simple constructor.
439 * @param pv instance to serialize
440 */
441 private DTO(final PVCoordinates pv) {
442 this.d = new double[] {
443 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
444 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
445 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
446 };
447 }
448
449 /** Replace the deserialized data transfer object with a {@link PVCoordinates}.
450 * @return replacement {@link PVCoordinates}
451 */
452 private Object readResolve() {
453 return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
454 new Vector3D(d[3], d[4], d[5]),
455 new Vector3D(d[6], d[7], d[8]));
456 }
457
458 }
459
460 }