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
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.hipparchus.util.FastMath;
26 import org.orekit.errors.OrekitException;
27 import org.orekit.errors.OrekitMessages;
28 import org.orekit.time.TimeShiftable;
29
30 /** Simple container for Position/Velocity/Acceleration triplets.
31 * <p>
32 * The state can be slightly shifted to close dates. This shift is based on
33 * a simple quadratic model. It is <em>not</em> intended as a replacement for
34 * proper orbit propagation (it is not even Keplerian!) but should be sufficient
35 * for either small time shifts or coarse accuracy.
36 * </p>
37 * <p>
38 * This class is the angular counterpart to {@link AngularCoordinates}.
39 * </p>
40 * <p>Instances of this class are guaranteed to be immutable.</p>
41 * @author Fabien Maussion
42 * @author Luc Maisonobe
43 */
44 public class PVCoordinates implements TimeShiftable<PVCoordinates>, Serializable {
45
46 /** Fixed position/velocity at origin (both p, v and a are zero vectors). */
47 public static final PVCoordinates ZERO = new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);
48
49 /** Serializable UID. */
50 private static final long serialVersionUID = 20140407L;
51
52 /** The position. */
53 private final Vector3D position;
54
55 /** The velocity. */
56 private final Vector3D velocity;
57
58 /** The acceleration. */
59 private final Vector3D acceleration;
60
61 /** Simple constructor.
62 * <p> Set the Coordinates to default : (0 0 0), (0 0 0), (0 0 0).</p>
63 */
64 public PVCoordinates() {
65 position = Vector3D.ZERO;
66 velocity = Vector3D.ZERO;
67 acceleration = Vector3D.ZERO;
68 }
69
70 /** Builds a PVCoordinates triplet with zero acceleration.
71 * <p>Acceleration is set to zero</p>
72 * @param position the position vector (m)
73 * @param velocity the velocity vector (m/s)
74 */
75 public PVCoordinates(final Vector3D position, final Vector3D velocity) {
76 this.position = position;
77 this.velocity = velocity;
78 this.acceleration = Vector3D.ZERO;
79 }
80
81 /** Builds a PVCoordinates triplet.
82 * @param position the position vector (m)
83 * @param velocity the velocity vector (m/s)
84 * @param acceleration the acceleration vector (m/s²)
85 */
86 public PVCoordinates(final Vector3D position, final Vector3D velocity, final Vector3D acceleration) {
87 this.position = position;
88 this.velocity = velocity;
89 this.acceleration = acceleration;
90 }
91
92 /** Multiplicative constructor.
93 * <p>Build a PVCoordinates from another one and a scale factor.</p>
94 * <p>The PVCoordinates built will be a * pv</p>
95 * @param a scale factor
96 * @param pv base (unscaled) PVCoordinates
97 */
98 public PVCoordinates(final double a, final PVCoordinates pv) {
99 position = new Vector3D(a, pv.position);
100 velocity = new Vector3D(a, pv.velocity);
101 acceleration = new Vector3D(a, pv.acceleration);
102 }
103
104 /** Subtractive constructor.
105 * <p>Build a relative PVCoordinates from a start and an end position.</p>
106 * <p>The PVCoordinates built will be end - start.</p>
107 * @param start Starting PVCoordinates
108 * @param end ending PVCoordinates
109 */
110 public PVCoordinates(final PVCoordinates start, final PVCoordinates end) {
111 this.position = end.position.subtract(start.position);
112 this.velocity = end.velocity.subtract(start.velocity);
113 this.acceleration = end.acceleration.subtract(start.acceleration);
114 }
115
116 /** Linear constructor.
117 * <p>Build a PVCoordinates from two other ones and corresponding scale factors.</p>
118 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2</p>
119 * @param a1 first scale factor
120 * @param pv1 first base (unscaled) PVCoordinates
121 * @param a2 second scale factor
122 * @param pv2 second base (unscaled) PVCoordinates
123 */
124 public PVCoordinates(final double a1, final PVCoordinates pv1,
125 final double a2, final PVCoordinates pv2) {
126 position = new Vector3D(a1, pv1.position, a2, pv2.position);
127 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity);
128 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration);
129 }
130
131 /** Linear constructor.
132 * <p>Build a PVCoordinates from three other ones and corresponding scale factors.</p>
133 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3</p>
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 PVCoordinates(final double a1, final PVCoordinates pv1,
142 final double a2, final PVCoordinates pv2,
143 final double a3, final PVCoordinates pv3) {
144 position = new Vector3D(a1, pv1.position, a2, pv2.position, a3, pv3.position);
145 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity, a3, pv3.velocity);
146 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration, a3, pv3.acceleration);
147 }
148
149 /** Linear constructor.
150 * <p>Build a PVCoordinates from four other ones and corresponding scale factors.</p>
151 * <p>The PVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4</p>
152 * @param a1 first scale factor
153 * @param pv1 first base (unscaled) PVCoordinates
154 * @param a2 second scale factor
155 * @param pv2 second base (unscaled) PVCoordinates
156 * @param a3 third scale factor
157 * @param pv3 third base (unscaled) PVCoordinates
158 * @param a4 fourth scale factor
159 * @param pv4 fourth base (unscaled) PVCoordinates
160 */
161 public PVCoordinates(final double a1, final PVCoordinates pv1,
162 final double a2, final PVCoordinates pv2,
163 final double a3, final PVCoordinates pv3,
164 final double a4, final PVCoordinates pv4) {
165 position = new Vector3D(a1, pv1.position, a2, pv2.position,
166 a3, pv3.position, a4, pv4.position);
167 velocity = new Vector3D(a1, pv1.velocity, a2, pv2.velocity,
168 a3, pv3.velocity, a4, pv4.velocity);
169 acceleration = new Vector3D(a1, pv1.acceleration, a2, pv2.acceleration,
170 a3, pv3.acceleration, a4, pv4.acceleration);
171 }
172
173 /** Builds a PVCoordinates triplet from a {@link FieldVector3D}<{@link DerivativeStructure}>.
174 * <p>
175 * The vector components must have time as their only derivation parameter and
176 * have consistent derivation orders.
177 * </p>
178 * @param p vector with time-derivatives embedded within the coordinates
179 */
180 public PVCoordinates(final FieldVector3D<DerivativeStructure> p) {
181 position = new Vector3D(p.getX().getReal(), p.getY().getReal(), p.getZ().getReal());
182 if (p.getX().getOrder() >= 1) {
183 velocity = new Vector3D(p.getX().getPartialDerivative(1),
184 p.getY().getPartialDerivative(1),
185 p.getZ().getPartialDerivative(1));
186 if (p.getX().getOrder() >= 2) {
187 acceleration = new Vector3D(p.getX().getPartialDerivative(2),
188 p.getY().getPartialDerivative(2),
189 p.getZ().getPartialDerivative(2));
190 } else {
191 acceleration = Vector3D.ZERO;
192 }
193 } else {
194 velocity = Vector3D.ZERO;
195 acceleration = Vector3D.ZERO;
196 }
197 }
198
199 /** Transform the instance to a {@link FieldVector3D}<{@link DerivativeStructure}>.
200 * <p>
201 * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
202 * to the user-specified order.
203 * </p>
204 * @param order derivation order for the vector components (must be either 0, 1 or 2)
205 * @return vector with time-derivatives embedded within the coordinates
206 * @exception OrekitException if the user specified order is too large
207 */
208 public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order)
209 throws OrekitException {
210
211 final DSFactory factory;
212 final DerivativeStructure x;
213 final DerivativeStructure y;
214 final DerivativeStructure z;
215 switch(order) {
216 case 0 :
217 factory = new DSFactory(1, order);
218 x = factory.build(position.getX());
219 y = factory.build(position.getY());
220 z = factory.build(position.getZ());
221 break;
222 case 1 :
223 factory = new DSFactory(1, order);
224 x = factory.build(position.getX(), velocity.getX());
225 y = factory.build(position.getY(), velocity.getY());
226 z = factory.build(position.getZ(), velocity.getZ());
227 break;
228 case 2 :
229 factory = new DSFactory(1, order);
230 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
231 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
232 z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
233 break;
234 default :
235 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
236 }
237
238 return new FieldVector3D<>(x, y, z);
239
240 }
241
242 /** Transform the instance to a {@link FieldPVCoordinates}<{@link DerivativeStructure}>.
243 * <p>
244 * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
245 * to the user-specified order. As both the instance components {@link #getPosition() position},
246 * {@link #getVelocity() velocity} and {@link #getAcceleration() acceleration} and the
247 * {@link DerivativeStructure#getPartialDerivative(int...) derivatives} of the components
248 * holds time-derivatives, there are several ways to retrieve these derivatives. If for example
249 * the {@code order} is set to 2, then both {@code pv.getPosition().getX().getPartialDerivative(2)},
250 * {@code pv.getVelocity().getX().getPartialDerivative(1)} and
251 * {@code pv.getAcceleration().getX().getValue()} return the exact same value.
252 * </p>
253 * <p>
254 * If derivation order is 1, the first derivative of acceleration will be computed as a
255 * Keplerian-only jerk. If derivation order is 2, the second derivative of velocity (which
256 * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
257 * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
258 * </p>
259 * @param order derivation order for the vector components (must be either 0, 1 or 2)
260 * @return pv coordinates with time-derivatives embedded within the coordinates
261 * @exception OrekitException if the user specified order is too large
262 * @since 9.2
263 */
264 public FieldPVCoordinates<DerivativeStructure> toDerivativeStructurePV(final int order)
265 throws OrekitException {
266
267 final DSFactory factory;
268 final DerivativeStructure x0;
269 final DerivativeStructure y0;
270 final DerivativeStructure z0;
271 final DerivativeStructure x1;
272 final DerivativeStructure y1;
273 final DerivativeStructure z1;
274 final DerivativeStructure x2;
275 final DerivativeStructure y2;
276 final DerivativeStructure z2;
277 switch(order) {
278 case 0 :
279 factory = new DSFactory(1, order);
280 x0 = factory.build(position.getX());
281 y0 = factory.build(position.getY());
282 z0 = factory.build(position.getZ());
283 x1 = factory.build(velocity.getX());
284 y1 = factory.build(velocity.getY());
285 z1 = factory.build(velocity.getZ());
286 x2 = factory.build(acceleration.getX());
287 y2 = factory.build(acceleration.getY());
288 z2 = factory.build(acceleration.getZ());
289 break;
290 case 1 : {
291 factory = new DSFactory(1, order);
292 final double r2 = position.getNormSq();
293 final double r = FastMath.sqrt(r2);
294 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
295 final double a = acceleration.getNorm();
296 final double aOr = a / r;
297 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
298 x0 = factory.build(position.getX(), velocity.getX());
299 y0 = factory.build(position.getY(), velocity.getY());
300 z0 = factory.build(position.getZ(), velocity.getZ());
301 x1 = factory.build(velocity.getX(), acceleration.getX());
302 y1 = factory.build(velocity.getY(), acceleration.getY());
303 z1 = factory.build(velocity.getZ(), acceleration.getZ());
304 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
305 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
306 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
307 break;
308 }
309 case 2 : {
310 factory = new DSFactory(1, order);
311 final double r2 = position.getNormSq();
312 final double r = FastMath.sqrt(r2);
313 final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
314 final double a = acceleration.getNorm();
315 final double aOr = a / r;
316 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
317 final double v2 = velocity.getNormSq();
318 final double pa = Vector3D.dotProduct(position, acceleration);
319 final double aj = Vector3D.dotProduct(acceleration, keplerianJerk);
320 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
321 4 * aOr * pvOr2 - aj / (a * r), velocity);
322 x0 = factory.build(position.getX(), velocity.getX(), acceleration.getX());
323 y0 = factory.build(position.getY(), velocity.getY(), acceleration.getY());
324 z0 = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
325 x1 = factory.build(velocity.getX(), acceleration.getX(), keplerianJerk.getX());
326 y1 = factory.build(velocity.getY(), acceleration.getY(), keplerianJerk.getY());
327 z1 = factory.build(velocity.getZ(), acceleration.getZ(), keplerianJerk.getZ());
328 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
329 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
330 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
331 break;
332 }
333 default :
334 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
335 }
336
337 return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
338 new FieldVector3D<>(x1, y1, z1),
339 new FieldVector3D<>(x2, y2, z2));
340
341 }
342
343 /** Estimate velocity between two positions.
344 * <p>Estimation is based on a simple fixed velocity translation
345 * during the time interval between the two positions.</p>
346 * @param start start position
347 * @param end end position
348 * @param dt time elapsed between the dates of the two positions
349 * @return velocity allowing to go from start to end positions
350 */
351 public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
352 final double scale = 1.0 / dt;
353 return new Vector3D(scale, end, -scale, start);
354 }
355
356 /** Get a time-shifted state.
357 * <p>
358 * The state can be slightly shifted to close dates. This shift is based on
359 * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
360 * proper orbit propagation (it is not even Keplerian!) but should be sufficient
361 * for either small time shifts or coarse accuracy.
362 * </p>
363 * @param dt time shift in seconds
364 * @return a new state, shifted with respect to the instance (which is immutable)
365 */
366 public PVCoordinates shiftedBy(final double dt) {
367 return new PVCoordinates(new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
368 new Vector3D(1, velocity, dt, acceleration),
369 acceleration);
370 }
371
372 /** Gets the position.
373 * @return the position vector (m).
374 */
375 public Vector3D getPosition() {
376 return position;
377 }
378
379 /** Gets the velocity.
380 * @return the velocity vector (m/s).
381 */
382 public Vector3D getVelocity() {
383 return velocity;
384 }
385
386 /** Gets the acceleration.
387 * @return the acceleration vector (m/s²).
388 */
389 public Vector3D getAcceleration() {
390 return acceleration;
391 }
392
393 /** Gets the momentum.
394 * <p>This vector is the p ⊗ v where p is position, v is velocity
395 * and ⊗ is cross product. To get the real physical angular momentum
396 * you need to multiply this vector by the mass.</p>
397 * <p>The returned vector is recomputed each time this method is called, it
398 * is not cached.</p>
399 * @return a new instance of the momentum vector (m²/s).
400 */
401 public Vector3D getMomentum() {
402 return Vector3D.crossProduct(position, velocity);
403 }
404
405 /**
406 * Get the angular velocity (spin) of this point as seen from the origin.
407 *
408 * <p> The angular velocity vector is parallel to the {@link #getMomentum()
409 * angular momentum} and is computed by ω = p × v / ||p||²
410 *
411 * @return the angular velocity vector
412 * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
413 * Wikipedia</a>
414 */
415 public Vector3D getAngularVelocity() {
416 return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
417 }
418
419 /** Get the opposite of the instance.
420 * @return a new position-velocity which is opposite to the instance
421 */
422 public PVCoordinates negate() {
423 return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
424 }
425
426 /** Normalize the position part of the instance.
427 * <p>
428 * The computed coordinates first component (position) will be a
429 * normalized vector, the second component (velocity) will be the
430 * derivative of the first component (hence it will generally not
431 * be normalized), and the third component (acceleration) will be the
432 * derivative of the second component (hence it will generally not
433 * be normalized).
434 * </p>
435 * @return a new instance, with first component normalized and
436 * remaining component computed to have consistent derivatives
437 */
438 public PVCoordinates normalize() {
439 final double inv = 1.0 / position.getNorm();
440 final Vector3D u = new Vector3D(inv, position);
441 final Vector3D v = new Vector3D(inv, velocity);
442 final Vector3D w = new Vector3D(inv, acceleration);
443 final double uv = Vector3D.dotProduct(u, v);
444 final double v2 = Vector3D.dotProduct(v, v);
445 final double uw = Vector3D.dotProduct(u, w);
446 final Vector3D uDot = new Vector3D(1, v, -uv, u);
447 final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
448 return new PVCoordinates(u, uDot, uDotDot);
449 }
450
451 /** Compute the cross-product of two instances.
452 * @param pv1 first instances
453 * @param pv2 second instances
454 * @return the cross product v1 ^ v2 as a new instance
455 */
456 public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) {
457 final Vector3D p1 = pv1.position;
458 final Vector3D v1 = pv1.velocity;
459 final Vector3D a1 = pv1.acceleration;
460 final Vector3D p2 = pv2.position;
461 final Vector3D v2 = pv2.velocity;
462 final Vector3D a2 = pv2.acceleration;
463 return new PVCoordinates(Vector3D.crossProduct(p1, p2),
464 new Vector3D(1, Vector3D.crossProduct(p1, v2),
465 1, Vector3D.crossProduct(v1, p2)),
466 new Vector3D(1, Vector3D.crossProduct(p1, a2),
467 2, Vector3D.crossProduct(v1, v2),
468 1, Vector3D.crossProduct(a1, p2)));
469 }
470
471 /** Return a string representation of this position/velocity pair.
472 * @return string representation of this position/velocity pair
473 */
474 public String toString() {
475 final String comma = ", ";
476 return new StringBuffer().append('{').append("P(").
477 append(position.getX()).append(comma).
478 append(position.getY()).append(comma).
479 append(position.getZ()).append("), V(").
480 append(velocity.getX()).append(comma).
481 append(velocity.getY()).append(comma).
482 append(velocity.getZ()).append("), A(").
483 append(acceleration.getX()).append(comma).
484 append(acceleration.getY()).append(comma).
485 append(acceleration.getZ()).append(")}").toString();
486 }
487
488 /** Replace the instance with a data transfer object for serialization.
489 * @return data transfer object that will be serialized
490 */
491 private Object writeReplace() {
492 return new DTO(this);
493 }
494
495 /** Internal class used only for serialization. */
496 private static class DTO implements Serializable {
497
498 /** Serializable UID. */
499 private static final long serialVersionUID = 20140723L;
500
501 /** Double values. */
502 private double[] d;
503
504 /** Simple constructor.
505 * @param pv instance to serialize
506 */
507 private DTO(final PVCoordinates pv) {
508 this.d = new double[] {
509 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
510 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
511 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
512 };
513 }
514
515 /** Replace the deserialized data transfer object with a {@link PVCoordinates}.
516 * @return replacement {@link PVCoordinates}
517 */
518 private Object readResolve() {
519 return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
520 new Vector3D(d[3], d[4], d[5]),
521 new Vector3D(d[6], d[7], d[8]));
522 }
523
524 }
525
526 }