1 /* Copyright 2002-2017 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
21 import org.hipparchus.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.DerivativeStructure;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.orekit.errors.OrekitException;
26 import org.orekit.errors.OrekitMessages;
27 import org.orekit.time.TimeShiftable;
28
29 /** Simple container for Position/Velocity/Acceleration triplets.
30 * <p>
31 * The state can be slightly shifted to close dates. This shift is based on
32 * a simple quadratic model. It is <em>not</em> intended as a replacement for
33 * proper orbit propagation (it is not even Keplerian!) but should be sufficient
34 * for either small time shifts or coarse accuracy.
35 * </p>
36 * <p>
37 * This class is the angular counterpart to {@link AngularCoordinates}.
38 * </p>
39 * <p>Instances of this class are guaranteed to be immutable.</p>
40 * @author Fabien Maussion
41 * @author Luc Maisonobe
42 */
43 public class PVCoordinates implements TimeShiftable<PVCoordinates>, Serializable {
44
45 /** Fixed position/velocity at origin (both p, v and a are zero vectors). */
46 public static final PVCoordinates ZERO = new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);
47
48 /** Serializable UID. */
49 private static final long serialVersionUID = 20140407L;
50
51 /** The position. */
52 private final Vector3D position;
53
54 /** The velocity. */
55 private final Vector3D velocity;
56
57 /** The acceleration. */
58 private final Vector3D acceleration;
59
60 /** Simple constructor.
61 * <p> Set the Coordinates to default : (0 0 0), (0 0 0), (0 0 0).</p>
62 */
63 public PVCoordinates() {
64 position = Vector3D.ZERO;
65 velocity = Vector3D.ZERO;
66 acceleration = Vector3D.ZERO;
67 }
68
69 /** Builds a PVCoordinates triplet with zero acceleration.
70 * <p>Acceleration is set to zero</p>
71 * @param position the position vector (m)
72 * @param velocity the velocity vector (m/s)
73 */
74 public PVCoordinates(final Vector3D position, final Vector3D velocity) {
75 this.position = position;
76 this.velocity = velocity;
77 this.acceleration = Vector3D.ZERO;
78 }
79
80 /** Builds a PVCoordinates triplet.
81 * @param position the position vector (m)
82 * @param velocity the velocity vector (m/s)
83 * @param acceleration the acceleration vector (m/s²)
84 */
85 public PVCoordinates(final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
86 this.position = position;
87 this.velocity = velocity;
88 this.acceleration = acceleration;
89 }
90
91 /** Multiplicative constructor.
92 * <p>Build a PVCoordinates from another one and a scale factor.</p>
93 * <p>The PVCoordinates built will be a * pv</p>
94 * @param a scale factor
95 * @param pv base (unscaled) PVCoordinates
96 */
97 public PVCoordinates(final double a, final PVCoordinates pv) {
98 position = new Vector3D(a, pv.position);
99 velocity = new Vector3D(a, pv.velocity);
100 acceleration = new Vector3D(a, pv.acceleration);
101 }
102
103 /** Subtractive constructor.
104 * <p>Build a relative PVCoordinates from a start and an end position.</p>
105 * <p>The PVCoordinates built will be end - start.</p>
106 * @param start Starting PVCoordinates
107 * @param end ending PVCoordinates
108 */
109 public PVCoordinates(final PVCoordinates start, final PVCoordinates end) {
110 this.position = end.position.subtract(start.position);
111 this.velocity = end.velocity.subtract(start.velocity);
112 this.acceleration = end.acceleration.subtract(start.acceleration);
113 }
114
115 /** Linear constructor.
116 * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
117 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
118 * @param a1 first scale factor
119 * @param pv1 first base (unscaled) PVCoordinates
120 * @param a2 second scale factor
121 * @param pv2 second base (unscaled) PVCoordinates
122 */
123 public PVCoordinates(final double a1, final PVCoordinates pv1,
124 final double a2, final PVCoordinates pv2) {
125 position = new Vector3D(a1, pv1.position, a2, pv2.position);
126 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity);
127 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration);
128 }
129
130 /** Linear constructor.
131 * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
132 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
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 PVCoordinates(final double a1, final PVCoordinates pv1,
141 final double a2, final PVCoordinates pv2,
142 final double a3, final PVCoordinates pv3) {
143 position = new Vector3D(a1, pv1.position, a2, pv2.position, a3, pv3.position);
144 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity, a3, pv3.velocity);
145 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration);
146 }
147
148 /** Linear constructor.
149 * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
150 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
151 * @param a1 first scale factor
152 * @param pv1 first base (unscaled) PVCoordinates
153 * @param a2 second scale factor
154 * @param pv2 second base (unscaled) PVCoordinates
155 * @param a3 third scale factor
156 * @param pv3 third base (unscaled) PVCoordinates
157 * @param a4 fourth scale factor
158 * @param pv4 fourth base (unscaled) PVCoordinates
159 */
160 public PVCoordinates(final double a1, final PVCoordinates pv1,
161 final double a2, final PVCoordinates pv2,
162 final double a3, final PVCoordinates pv3,
163 final double a4, final PVCoordinates pv4) {
164 position = new Vector3D(a1, pv1.position, a2, pv2.position,
165 a3, pv3.position, a4, pv4.position);
166 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity,
167 a3, pv3.velocity, a4, pv4.velocity);
168 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration,
169 a3, pv3.acceleration, a4, pv4.acceleration);
170 }
171
172 /** Builds a PVCoordinates triplet from a {@link FieldVector3D}<{@link DerivativeStructure}>.
173 * <p>
174 * The vector components must have time as their only derivation parameter and
175 * have consistent derivation orders.
176 * </p>
177 * @param p vector with time-derivatives embedded within the coordinates
178 */
179 public PVCoordinates(final FieldVector3D<DerivativeStructure> p) {
180 position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal());
181 if (p.getX().getOrder() >= 1) {
182 velocity = new Vector3D(p.getX().getPartialDerivative(1),
183 p.getY().getPartialDerivative(1),
184 p.getZ().getPartialDerivative(1));
185 if (p.getX().getOrder() >= 2) {
186 acceleration = new Vector3D(p.getX().getPartialDerivative(2),
187 p.getY().getPartialDerivative(2),
188 p.getZ().getPartialDerivative(2));
189 } else {
190 acceleration = Vector3D.ZERO;
191 }
192 } else {
193 velocity = Vector3D.ZERO;
194 acceleration = Vector3D.ZERO;
195 }
196 }
197
198 /** Transform the instance to a {@link FieldVector3D}<{@link DerivativeStructure}>.
199 * <p>
200 * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
201 * to the user-specified order.
202 * </p>
203 * @param order derivation order for the vector components (must be either 0, 1 or 2)
204 * @return vector with time-derivatives embedded within the coordinates
205 * @exception OrekitException if the user specified order is too large
206 */
207 public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order)
208 throws OrekitException {
209
210 final DSFactory factory;
211 final DerivativeStructure x;
212 final DerivativeStructure y;
213 final DerivativeStructure z;
214 switch(order) {
215 case 0 :
216 factory = new DSFactory(1, order);
217 x = factory.build(position.getX());
218 y = factory.build(position.getY());
219 z = factory.build(position.getZ());
220 break;
221 case 1 :
222 factory = new DSFactory(1, order);
223 x = factory.build(position.getX(), velocity.getX());
224 y = factory.build(position.getY(), velocity.getY());
225 z = factory.build(position.getZ(), velocity.getZ());
226 break;
227 case 2 :
228 factory = new DSFactory(1, order);
229 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
230 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
231 z = factory.build(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<>(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 /** Gets the position.
271 * @return the position vector (m).
272 */
273 public Vector3D getPosition() {
274 return position;
275 }
276
277 /** Gets the velocity.
278 * @return the velocity vector (m/s).
279 */
280 public Vector3D getVelocity() {
281 return velocity;
282 }
283
284 /** Gets the acceleration.
285 * @return the acceleration vector (m/s²).
286 */
287 public Vector3D getAcceleration() {
288 return acceleration;
289 }
290
291 /** Gets the momentum.
292 * <p>This vector is the p ⊗ v where p is position, v is velocity
293 * and ⊗ is cross product. To get the real physical angular momentum
294 * you need to multiply this vector by the mass.</p>
295 * <p>The returned vector is recomputed each time this method is called, it
296 * is not cached.</p>
297 * @return a new instance of the momentum vector (m²/s).
298 */
299 public Vector3D getMomentum() {
300 return Vector3D.crossProduct(position, velocity);
301 }
302
303 /**
304 * Get the angular velocity (spin) of this point as seen from the origin.
305 *
306 * <p> The angular velocity vector is parallel to the {@link #getMomentum()
307 * angular momentum} and is computed by ω = p × v / ||p||²
308 *
309 * @return the angular velocity vector
310 * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
311 * Wikipedia</a>
312 */
313 public Vector3D getAngularVelocity() {
314 return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
315 }
316
317 /** Get the opposite of the instance.
318 * @return a new position-velocity which is opposite to the instance
319 */
320 public PVCoordinates negate() {
321 return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
322 }
323
324 /** Normalize the position part of the instance.
325 * <p>
326 * The computed coordinates first component (position) will be a
327 * normalized vector, the second component (velocity) will be the
328 * derivative of the first component (hence it will generally not
329 * be normalized), and the third component (acceleration) will be the
330 * derivative of the second component (hence it will generally not
331 * be normalized).
332 * </p>
333 * @return a new instance, with first component normalized and
334 * remaining component computed to have consistent derivatives
335 */
336 public PVCoordinates normalize() {
337 final double inv = 1.0 / position.getNorm();
338 final Vector3D u = new Vector3D(inv, position);
339 final Vector3D v = new Vector3D(inv, velocity);
340 final Vector3D w = new Vector3D(inv, acceleration);
341 final double uv = Vector3D.dotProduct(u, v);
342 final double v2 = Vector3D.dotProduct(v, v);
343 final double uw = Vector3D.dotProduct(u, w);
344 final Vector3D uDot = new Vector3D(1, v, -uv, u);
345 final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
346 return new PVCoordinates(u, uDot, uDotDot);
347 }
348
349 /** Compute the cross-product of two instances.
350 * @param pv1 first instances
351 * @param pv2 second instances
352 * @return the cross product v1 ^ v2 as a new instance
353 */
354 public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) {
355 final Vector3D p1 = pv1.position;
356 final Vector3D v1 = pv1.velocity;
357 final Vector3D a1 = pv1.acceleration;
358 final Vector3D p2 = pv2.position;
359 final Vector3D v2 = pv2.velocity;
360 final Vector3D a2 = pv2.acceleration;
361 return new PVCoordinates(Vector3D.crossProduct(p1, p2),
362 new Vector3D(1, Vector3D.crossProduct(p1, v2),
363 1, Vector3D.crossProduct(v1, p2)),
364 new Vector3D(1, Vector3D.crossProduct(p1, a2),
365 2, Vector3D.crossProduct(v1, v2),
366 1, Vector3D.crossProduct(a1, p2)));
367 }
368
369 /** Return a string representation of this position/velocity pair.
370 * @return string representation of this position/velocity pair
371 */
372 public String toString() {
373 final String comma = ", ";
374 return new StringBuffer().append('{').append("P(").
375 append(position.getX()).append(comma).
376 append(position.getY()).append(comma).
377 append(position.getZ()).append("), V(").
378 append(velocity.getX()).append(comma).
379 append(velocity.getY()).append(comma).
380 append(velocity.getZ()).append("), A(").
381 append(acceleration.getX()).append(comma).
382 append(acceleration.getY()).append(comma).
383 append(acceleration.getZ()).append(")}").toString();
384 }
385
386 /** Replace the instance with a data transfer object for serialization.
387 * @return data transfer object that will be serialized
388 */
389 private Object writeReplace() {
390 return new DTO(this);
391 }
392
393 /** Internal class used only for serialization. */
394 private static class DTO implements Serializable {
395
396 /** Serializable UID. */
397 private static final long serialVersionUID = 20140723L;
398
399 /** Double values. */
400 private double[] d;
401
402 /** Simple constructor.
403 * @param pv instance to serialize
404 */
405 private DTO(final PVCoordinates pv) {
406 this.d = new double[] {
407 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
408 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
409 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
410 };
411 }
412
413 /** Replace the deserialized data transfer object with a {@link PVCoordinates}.
414 * @return replacement {@link PVCoordinates}
415 */
416 private Object readResolve() {
417 return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
418 new Vector3D(d[3], d[4], d[5]),
419 new Vector3D(d[6], d[7], d[8]));
420 }
421
422 }
423
424 }