1   /* Copyright 2002-2019 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 PVCoordinatesPVCoordinates">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(finalPVCoordinateslass="jxr_keyword">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 PVCoordinatesCoordinates">PVCoordinateshtml#PVCoordinates">PVCoordinates(final PVCoordinatesCoordinates">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(finalPVCoordinatesass="jxr_keyword">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(finalPVCoordinatesass="jxr_keyword">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(finalPVCoordinatesass="jxr_keyword">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}&lt;{@link DerivativeStructure}&gt;.
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}&lt;{@link DerivativeStructure}&gt;.
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      */
207     public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order) {
208 
209         final DSFactory factory;
210         final DerivativeStructure x;
211         final DerivativeStructure y;
212         final DerivativeStructure z;
213         switch(order) {
214             case 0 :
215                 factory = new DSFactory(1, order);
216                 x = factory.build(position.getX());
217                 y = factory.build(position.getY());
218                 z = factory.build(position.getZ());
219                 break;
220             case 1 :
221                 factory = new DSFactory(1, order);
222                 x = factory.build(position.getX(), velocity.getX());
223                 y = factory.build(position.getY(), velocity.getY());
224                 z = factory.build(position.getZ(), velocity.getZ());
225                 break;
226             case 2 :
227                 factory = new DSFactory(1, order);
228                 x = factory.build(position.getX(), velocity.getX(), acceleration.getX());
229                 y = factory.build(position.getY(), velocity.getY(), acceleration.getY());
230                 z = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
231                 break;
232             default :
233                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
234         }
235 
236         return new FieldVector3D<>(x, y, z);
237 
238     }
239 
240     /** Transform the instance to a {@link FieldPVCoordinates}&lt;{@link DerivativeStructure}&gt;.
241      * <p>
242      * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
243      * to the user-specified order. As both the instance components {@link #getPosition() position},
244      * {@link #getVelocity() velocity} and {@link #getAcceleration() acceleration} and the
245      * {@link DerivativeStructure#getPartialDerivative(int...) derivatives} of the components
246      * holds time-derivatives, there are several ways to retrieve these derivatives. If for example
247      * the {@code order} is set to 2, then both {@code pv.getPosition().getX().getPartialDerivative(2)},
248      * {@code pv.getVelocity().getX().getPartialDerivative(1)} and
249      * {@code pv.getAcceleration().getX().getValue()} return the exact same value.
250      * </p>
251      * <p>
252      * If derivation order is 1, the first derivative of acceleration will be computed as a
253      * Keplerian-only jerk. If derivation order is 2, the second derivative of velocity (which
254      * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
255      * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
256      * </p>
257      * @param order derivation order for the vector components (must be either 0, 1 or 2)
258      * @return pv coordinates with time-derivatives embedded within the coordinates
259           * @since 9.2
260      */
261     public FieldPVCoordinates<DerivativeStructure> toDerivativeStructurePV(final int order) {
262 
263         final DSFactory factory;
264         final DerivativeStructure x0;
265         final DerivativeStructure y0;
266         final DerivativeStructure z0;
267         final DerivativeStructure x1;
268         final DerivativeStructure y1;
269         final DerivativeStructure z1;
270         final DerivativeStructure x2;
271         final DerivativeStructure y2;
272         final DerivativeStructure z2;
273         switch(order) {
274             case 0 :
275                 factory = new DSFactory(1, order);
276                 x0 = factory.build(position.getX());
277                 y0 = factory.build(position.getY());
278                 z0 = factory.build(position.getZ());
279                 x1 = factory.build(velocity.getX());
280                 y1 = factory.build(velocity.getY());
281                 z1 = factory.build(velocity.getZ());
282                 x2 = factory.build(acceleration.getX());
283                 y2 = factory.build(acceleration.getY());
284                 z2 = factory.build(acceleration.getZ());
285                 break;
286             case 1 : {
287                 factory = new DSFactory(1, order);
288                 final double   r2            = position.getNormSq();
289                 final double   r             = FastMath.sqrt(r2);
290                 final double   pvOr2         = Vector3D.dotProduct(position, velocity) / r2;
291                 final double   a             = acceleration.getNorm();
292                 final double   aOr           = a / r;
293                 final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
294                 x0 = factory.build(position.getX(),     velocity.getX());
295                 y0 = factory.build(position.getY(),     velocity.getY());
296                 z0 = factory.build(position.getZ(),     velocity.getZ());
297                 x1 = factory.build(velocity.getX(),     acceleration.getX());
298                 y1 = factory.build(velocity.getY(),     acceleration.getY());
299                 z1 = factory.build(velocity.getZ(),     acceleration.getZ());
300                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
301                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
302                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
303                 break;
304             }
305             case 2 : {
306                 factory = new DSFactory(1, order);
307                 final double   r2              = position.getNormSq();
308                 final double   r               = FastMath.sqrt(r2);
309                 final double   pvOr2           = Vector3D.dotProduct(position, velocity) / r2;
310                 final double   a               = acceleration.getNorm();
311                 final double   aOr             = a / r;
312                 final Vector3D keplerianJerk   = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
313                 final double   v2              = velocity.getNormSq();
314                 final double   pa              = Vector3D.dotProduct(position, acceleration);
315                 final double   aj              = Vector3D.dotProduct(acceleration, keplerianJerk);
316                 final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration,
317                                                               4 * aOr * pvOr2 - aj / (a * r), velocity);
318                 x0 = factory.build(position.getX(),     velocity.getX(),      acceleration.getX());
319                 y0 = factory.build(position.getY(),     velocity.getY(),      acceleration.getY());
320                 z0 = factory.build(position.getZ(),     velocity.getZ(),      acceleration.getZ());
321                 x1 = factory.build(velocity.getX(),     acceleration.getX(),  keplerianJerk.getX());
322                 y1 = factory.build(velocity.getY(),     acceleration.getY(),  keplerianJerk.getY());
323                 z1 = factory.build(velocity.getZ(),     acceleration.getZ(),  keplerianJerk.getZ());
324                 x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
325                 y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
326                 z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
327                 break;
328             }
329             default :
330                 throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
331         }
332 
333         return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0),
334                                         new FieldVector3D<>(x1, y1, z1),
335                                         new FieldVector3D<>(x2, y2, z2));
336 
337     }
338 
339     /** Estimate velocity between two positions.
340      * <p>Estimation is based on a simple fixed velocity translation
341      * during the time interval between the two positions.</p>
342      * @param start start position
343      * @param end end position
344      * @param dt time elapsed between the dates of the two positions
345      * @return velocity allowing to go from start to end positions
346      */
347     public static Vector3D estimateVelocity(final Vector3D start, final Vector3D end, final double dt) {
348         final double scale = 1.0 / dt;
349         return new Vector3D(scale, end, -scale, start);
350     }
351 
352     /** Get a time-shifted state.
353      * <p>
354      * The state can be slightly shifted to close dates. This shift is based on
355      * a simple Taylor expansion. It is <em>not</em> intended as a replacement for
356      * proper orbit propagation (it is not even Keplerian!) but should be sufficient
357      * for either small time shifts or coarse accuracy.
358      * </p>
359      * @param dt time shift in seconds
360      * @return a new state, shifted with respect to the instance (which is immutable)
361      */
362     public PVCoordinates shiftedBy(final double dt) {
363         return new PVCoordinates(new Vector3D(1, position, dt, velocity, 0.5 * dt * dt, acceleration),
364                                  new Vector3D(1, velocity, dt, acceleration),
365                                  acceleration);
366     }
367 
368     /** Gets the position.
369      * @return the position vector (m).
370      */
371     public Vector3D getPosition() {
372         return position;
373     }
374 
375     /** Gets the velocity.
376      * @return the velocity vector (m/s).
377      */
378     public Vector3D getVelocity() {
379         return velocity;
380     }
381 
382     /** Gets the acceleration.
383      * @return the acceleration vector (m/s²).
384      */
385     public Vector3D getAcceleration() {
386         return acceleration;
387     }
388 
389     /** Gets the momentum.
390      * <p>This vector is the p &otimes; v where p is position, v is velocity
391      * and &otimes; is cross product. To get the real physical angular momentum
392      * you need to multiply this vector by the mass.</p>
393      * <p>The returned vector is recomputed each time this method is called, it
394      * is not cached.</p>
395      * @return a new instance of the momentum vector (m²/s).
396      */
397     public Vector3D getMomentum() {
398         return Vector3D.crossProduct(position, velocity);
399     }
400 
401     /**
402      * Get the angular velocity (spin) of this point as seen from the origin.
403      *
404      * <p> The angular velocity vector is parallel to the {@link #getMomentum()
405      * angular momentum} and is computed by ω = p &times; v / ||p||²
406      *
407      * @return the angular velocity vector
408      * @see <a href="http://en.wikipedia.org/wiki/Angular_velocity">Angular Velocity on
409      *      Wikipedia</a>
410      */
411     public Vector3D getAngularVelocity() {
412         return this.getMomentum().scalarMultiply(1.0 / this.getPosition().getNormSq());
413     }
414 
415     /** Get the opposite of the instance.
416      * @return a new position-velocity which is opposite to the instance
417      */
418     public PVCoordinates negate() {
419         return new PVCoordinates(position.negate(), velocity.negate(), acceleration.negate());
420     }
421 
422     /** Normalize the position part of the instance.
423      * <p>
424      * The computed coordinates first component (position) will be a
425      * normalized vector, the second component (velocity) will be the
426      * derivative of the first component (hence it will generally not
427      * be normalized), and the third component (acceleration) will be the
428      * derivative of the second component (hence it will generally not
429      * be normalized).
430      * </p>
431      * @return a new instance, with first component normalized and
432      * remaining component computed to have consistent derivatives
433      */
434     public PVCoordinates normalize() {
435         final double   inv     = 1.0 / position.getNorm();
436         final Vector3D u       = new Vector3D(inv, position);
437         final Vector3D v       = new Vector3D(inv, velocity);
438         final Vector3D w       = new Vector3D(inv, acceleration);
439         final double   uv      = Vector3D.dotProduct(u, v);
440         final double   v2      = Vector3D.dotProduct(v, v);
441         final double   uw      = Vector3D.dotProduct(u, w);
442         final Vector3D uDot    = new Vector3D(1, v, -uv, u);
443         final Vector3D uDotDot = new Vector3D(1, w, -2 * uv, v, 3 * uv * uv - v2 - uw, u);
444         return new PVCoordinates(u, uDot, uDotDot);
445     }
446 
447     /** Compute the cross-product of two instances.
448      * @param pv1 first instances
449      * @param pv2 second instances
450      * @return the cross product v1 ^ v2 as a new instance
451      */
452     public static PVCoordinatesPVCoordinates">PVCoordinatesnates">PVCoordinates crossProduct(final PVCoordinatesPVCoordinates">PVCoordinates pv1, final PVCoordinates pv2) {
453         final Vector3D p1 = pv1.position;
454         final Vector3D v1 = pv1.velocity;
455         final Vector3D a1 = pv1.acceleration;
456         final Vector3D p2 = pv2.position;
457         final Vector3D v2 = pv2.velocity;
458         final Vector3D a2 = pv2.acceleration;
459         return new PVCoordinates(Vector3D.crossProduct(p1, p2),
460                                  new Vector3D(1, Vector3D.crossProduct(p1, v2),
461                                               1, Vector3D.crossProduct(v1, p2)),
462                                  new Vector3D(1, Vector3D.crossProduct(p1, a2),
463                                               2, Vector3D.crossProduct(v1, v2),
464                                               1, Vector3D.crossProduct(a1, p2)));
465     }
466 
467     /** Return a string representation of this position/velocity pair.
468      * @return string representation of this position/velocity pair
469      */
470     public String toString() {
471         final String comma = ", ";
472         return new StringBuffer().append('{').append("P(").
473                 append(position.getX()).append(comma).
474                 append(position.getY()).append(comma).
475                 append(position.getZ()).append("), V(").
476                 append(velocity.getX()).append(comma).
477                 append(velocity.getY()).append(comma).
478                 append(velocity.getZ()).append("), A(").
479                 append(acceleration.getX()).append(comma).
480                 append(acceleration.getY()).append(comma).
481                 append(acceleration.getZ()).append(")}").toString();
482     }
483 
484     /** Replace the instance with a data transfer object for serialization.
485      * @return data transfer object that will be serialized
486      */
487     private Object writeReplace() {
488         return new DTO(this);
489     }
490 
491     /** Internal class used only for serialization. */
492     private static class DTO implements Serializable {
493 
494         /** Serializable UID. */
495         private static final long serialVersionUID = 20140723L;
496 
497         /** Double values. */
498         private double[] d;
499 
500         /** Simple constructor.
501          * @param pv instance to serialize
502          */
503         private DTO(final PVCoordinates pv) {
504             this.d = new double[] {
505                 pv.getPosition().getX(),     pv.getPosition().getY(),     pv.getPosition().getZ(),
506                 pv.getVelocity().getX(),     pv.getVelocity().getY(),     pv.getVelocity().getZ(),
507                 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
508             };
509         }
510 
511         /** Replace the deserialized data transfer object with a {@link PVCoordinates}.
512          * @return replacement {@link PVCoordinates}
513          */
514         private Object readResolve() {
515             return new PVCoordinates(new Vector3D(d[0], d[1], d[2]),
516                                      new Vector3D(d[3], d[4], d[5]),
517                                      new Vector3D(d[6], d[7], d[8]));
518         }
519 
520     }
521 
522 }