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.frames;
18
19 import java.io.Serializable;
20 import java.util.ArrayList;
21 import java.util.Arrays;
22 import java.util.Collection;
23 import java.util.List;
24
25 import org.hipparchus.RealFieldElement;
26 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.Line;
29 import org.hipparchus.geometry.euclidean.threed.Rotation;
30 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
31 import org.hipparchus.geometry.euclidean.threed.Vector3D;
32 import org.orekit.errors.OrekitException;
33 import org.orekit.time.AbsoluteDate;
34 import org.orekit.time.TimeInterpolable;
35 import org.orekit.time.TimeShiftable;
36 import org.orekit.time.TimeStamped;
37 import org.orekit.utils.AngularCoordinates;
38 import org.orekit.utils.AngularDerivativesFilter;
39 import org.orekit.utils.CartesianDerivativesFilter;
40 import org.orekit.utils.FieldPVCoordinates;
41 import org.orekit.utils.PVCoordinates;
42 import org.orekit.utils.TimeStampedAngularCoordinates;
43 import org.orekit.utils.TimeStampedFieldPVCoordinates;
44 import org.orekit.utils.TimeStampedPVCoordinates;
45
46
47 /** Transformation class in three dimensional space.
48 *
49 * <p>This class represents the transformation engine between {@link Frame frames}.
50 * It is used both to define the relationship between each frame and its
51 * parent frame and to gather all individual transforms into one
52 * operation when converting between frames far away from each other.</p>
53 * <p> The convention used in OREKIT is vectorial transformation. It means
54 * that a transformation is defined as a transform to apply to the
55 * coordinates of a vector expressed in the old frame to obtain the
56 * same vector expressed in the new frame.
57 *
58 * <p>Instances of this class are guaranteed to be immutable.</p>
59 *
60 * <h1> Example </h1>
61 *
62 * <pre>
63 *
64 * 1 ) Example of translation from R<sub>A</sub> to R<sub>B</sub>:
65 * We want to transform the {@link PVCoordinates} PV<sub>A</sub> to PV<sub>B</sub>.
66 *
67 * With : PV<sub>A</sub> = ({1, 0, 0}, {2, 0, 0}, {3, 0, 0});
68 * and : PV<sub>B</sub> = ({0, 0, 0}, {0, 0, 0}, {0, 0, 0});
69 *
70 * The transform to apply then is defined as follows :
71 *
72 * Vector3D translation = new Vector3D(-1, 0, 0);
73 * Vector3D velocity = new Vector3D(-2, 0, 0);
74 * Vector3D acceleration = new Vector3D(-3, 0, 0);
75 *
76 * Transform R1toR2 = new Transform(date, translation, velocity, acceleration);
77 *
78 * PV<sub>B</sub> = R1toR2.transformPVCoordinates(PV<sub>A</sub>);
79 *
80 *
81 * 2 ) Example of rotation from R<sub>A</sub> to R<sub>B</sub>:
82 * We want to transform the {@link PVCoordinates} PV<sub>A</sub> to PV<sub>B</sub>.
83 *
84 * With : PV<sub>A</sub> = ({1, 0, 0}, {1, 0, 0});
85 * and : PV<sub>B</sub> = ({0, 1, 0}, {-2, 1, 0});
86 *
87 * The transform to apply then is defined as follows :
88 *
89 * Rotation rotation = new Rotation(Vector3D.PLUS_K, FastMath.PI / 2);
90 * Vector3D rotationRate = new Vector3D(0, 0, -2);
91 *
92 * Transform R1toR2 = new Transform(rotation, rotationRate);
93 *
94 * PV<sub>B</sub> = R1toR2.transformPVCoordinates(PV<sub>A</sub>);
95 *
96 * </pre>
97 *
98 * @author Luc Maisonobe
99 * @author Fabien Maussion
100 */
101 public class Transform
102 implements TimeStamped, TimeShiftable<Transform>, TimeInterpolable<Transform>, Serializable {
103
104 /** Identity transform. */
105 public static final Transform IDENTITY = new IdentityTransform();
106
107 /** Serializable UID. */
108 private static final long serialVersionUID = 210140410L;
109
110 /** Date of the transform. */
111 private final AbsoluteDate date;
112
113 /** Cartesian coordinates of the target frame with respect to the original frame. */
114 private final PVCoordinates cartesian;
115
116 /** Angular coordinates of the target frame with respect to the original frame. */
117 private final AngularCoordinates angular;
118
119 /** Build a transform from its primitive operations.
120 * @param date date of the transform
121 * @param cartesian Cartesian coordinates of the target frame with respect to the original frame
122 * @param angular angular coordinates of the target frame with respect to the original frame
123 */
124 private Transform(final AbsoluteDate date,
125 final PVCoordinates cartesian, final AngularCoordinates angular) {
126 this.date = date;
127 this.cartesian = cartesian;
128 this.angular = angular;
129 }
130
131 /** Build a translation transform.
132 * @param date date of the transform
133 * @param translation translation to apply (i.e. coordinates of
134 * the transformed origin, or coordinates of the origin of the
135 * old frame in the new frame)
136 */
137 public Transform(final AbsoluteDate date, final Vector3D translation) {
138 this(date,
139 new PVCoordinates(translation, Vector3D.ZERO, Vector3D.ZERO),
140 AngularCoordinates.IDENTITY);
141 }
142
143 /** Build a rotation transform.
144 * @param date date of the transform
145 * @param rotation rotation to apply ( i.e. rotation to apply to the
146 * coordinates of a vector expressed in the old frame to obtain the
147 * same vector expressed in the new frame )
148 */
149 public Transform(final AbsoluteDate date, final Rotation rotation) {
150 this(date,
151 PVCoordinates.ZERO,
152 new AngularCoordinates(rotation, Vector3D.ZERO));
153 }
154
155 /** Build a translation transform, with its first time derivative.
156 * @param date date of the transform
157 * @param translation translation to apply (i.e. coordinates of
158 * the transformed origin, or coordinates of the origin of the
159 * old frame in the new frame)
160 * @param velocity the velocity of the translation (i.e. origin
161 * of the old frame velocity in the new frame)
162 */
163 public Transform(final AbsoluteDate date, final Vector3D translation,
164 final Vector3D velocity) {
165 this(date,
166 new PVCoordinates(translation, velocity, Vector3D.ZERO),
167 AngularCoordinates.IDENTITY);
168 }
169
170 /** Build a translation transform, with its first and second time derivatives.
171 * @param date date of the transform
172 * @param translation translation to apply (i.e. coordinates of
173 * the transformed origin, or coordinates of the origin of the
174 * old frame in the new frame)
175 * @param velocity the velocity of the translation (i.e. origin
176 * of the old frame velocity in the new frame)
177 * @param acceleration the acceleration of the translation (i.e. origin
178 * of the old frame acceleration in the new frame)
179 */
180 public Transform(final AbsoluteDate date, final Vector3D translation,
181 final Vector3D velocity, final Vector3D acceleration) {
182 this(date,
183 new PVCoordinates(translation, velocity, acceleration),
184 AngularCoordinates.IDENTITY);
185 }
186
187 /** Build a translation transform, with its first time derivative.
188 * @param date date of the transform
189 * @param cartesian cartesian part of the transformation to apply (i.e. coordinates of
190 * the transformed origin, or coordinates of the origin of the
191 * old frame in the new frame, with their derivatives)
192 */
193 public Transform(final AbsoluteDate date, final PVCoordinates cartesian) {
194 this(date,
195 cartesian,
196 AngularCoordinates.IDENTITY);
197 }
198
199 /** Build a rotation transform.
200 * @param date date of the transform
201 * @param rotation rotation to apply ( i.e. rotation to apply to the
202 * coordinates of a vector expressed in the old frame to obtain the
203 * same vector expressed in the new frame )
204 * @param rotationRate the axis of the instant rotation
205 * expressed in the new frame. (norm representing angular rate)
206 */
207 public Transform(final AbsoluteDate date, final Rotation rotation, final Vector3D rotationRate) {
208 this(date,
209 PVCoordinates.ZERO,
210 new AngularCoordinates(rotation, rotationRate, Vector3D.ZERO));
211 }
212
213 /** Build a rotation transform.
214 * @param date date of the transform
215 * @param rotation rotation to apply ( i.e. rotation to apply to the
216 * coordinates of a vector expressed in the old frame to obtain the
217 * same vector expressed in the new frame )
218 * @param rotationRate the axis of the instant rotation
219 * @param rotationAcceleration the axis of the instant rotation
220 * expressed in the new frame. (norm representing angular rate)
221 */
222 public Transform(final AbsoluteDate date, final Rotation rotation, final Vector3D rotationRate,
223 final Vector3D rotationAcceleration) {
224 this(date,
225 PVCoordinates.ZERO,
226 new AngularCoordinates(rotation, rotationRate, rotationAcceleration));
227 }
228
229 /** Build a rotation transform.
230 * @param date date of the transform
231 * @param angular angular part of the transformation to apply (i.e. rotation to
232 * apply to the coordinates of a vector expressed in the old frame to obtain the
233 * same vector expressed in the new frame, with its rotation rate)
234 */
235 public Transform(final AbsoluteDate date, final AngularCoordinates angular) {
236 this(date, PVCoordinates.ZERO, angular);
237 }
238
239 /** Build a transform by combining two existing ones.
240 * <p>
241 * Note that the dates of the two existing transformed are <em>ignored</em>,
242 * and the combined transform date is set to the date supplied in this constructor
243 * without any attempt to shift the raw transforms. This is a design choice allowing
244 * user full control of the combination.
245 * </p>
246 * @param date date of the transform
247 * @param first first transform applied
248 * @param second second transform applied
249 */
250 public Transform(final AbsoluteDate date, final Transform first, final Transform second) {
251 this(date,
252 new PVCoordinates(compositeTranslation(first, second),
253 compositeVelocity(first, second),
254 compositeAcceleration(first, second)),
255 new AngularCoordinates(compositeRotation(first, second),
256 compositeRotationRate(first, second),
257 compositeRotationAcceleration(first, second)));
258 }
259
260 /** Compute a composite translation.
261 * @param first first applied transform
262 * @param second second applied transform
263 * @return translation part of the composite transform
264 */
265 private static Vector3D compositeTranslation(final Transform first, final Transform second) {
266
267 final Vector3D p1 = first.cartesian.getPosition();
268 final Rotation r1 = first.angular.getRotation();
269 final Vector3D p2 = second.cartesian.getPosition();
270
271 return p1.add(r1.applyInverseTo(p2));
272
273 }
274
275 /** Compute a composite velocity.
276 * @param first first applied transform
277 * @param second second applied transform
278 * @return velocity part of the composite transform
279 */
280 private static Vector3D compositeVelocity(final Transform first, final Transform second) {
281
282 final Vector3D v1 = first.cartesian.getVelocity();
283 final Rotation r1 = first.angular.getRotation();
284 final Vector3D o1 = first.angular.getRotationRate();
285 final Vector3D p2 = second.cartesian.getPosition();
286 final Vector3D v2 = second.cartesian.getVelocity();
287
288 final Vector3D crossP = Vector3D.crossProduct(o1, p2);
289
290 return v1.add(r1.applyInverseTo(v2.add(crossP)));
291
292 }
293
294 /** Compute a composite acceleration.
295 * @param first first applied transform
296 * @param second second applied transform
297 * @return acceleration part of the composite transform
298 */
299 private static Vector3D compositeAcceleration(final Transform first, final Transform second) {
300
301 final Vector3D a1 = first.cartesian.getAcceleration();
302 final Rotation r1 = first.angular.getRotation();
303 final Vector3D o1 = first.angular.getRotationRate();
304 final Vector3D oDot1 = first.angular.getRotationAcceleration();
305 final Vector3D p2 = second.cartesian.getPosition();
306 final Vector3D v2 = second.cartesian.getVelocity();
307 final Vector3D a2 = second.cartesian.getAcceleration();
308
309 final Vector3D crossCrossP = Vector3D.crossProduct(o1, Vector3D.crossProduct(o1, p2));
310 final Vector3D crossV = Vector3D.crossProduct(o1, v2);
311 final Vector3D crossDotP = Vector3D.crossProduct(oDot1, p2);
312
313 return a1.add(r1.applyInverseTo(new Vector3D(1, a2, 2, crossV, 1, crossCrossP, 1, crossDotP)));
314
315 }
316
317 /** Compute a composite rotation.
318 * @param first first applied transform
319 * @param second second applied transform
320 * @return rotation part of the composite transform
321 */
322 private static Rotation compositeRotation(final Transform first, final Transform second) {
323
324 final Rotation r1 = first.angular.getRotation();
325 final Rotation r2 = second.angular.getRotation();
326
327 return r1.compose(r2, RotationConvention.FRAME_TRANSFORM);
328
329 }
330
331 /** Compute a composite rotation rate.
332 * @param first first applied transform
333 * @param second second applied transform
334 * @return rotation rate part of the composite transform
335 */
336 private static Vector3D compositeRotationRate(final Transform first, final Transform second) {
337
338 final Vector3D o1 = first.angular.getRotationRate();
339 final Rotation r2 = second.angular.getRotation();
340 final Vector3D o2 = second.angular.getRotationRate();
341
342 return o2.add(r2.applyTo(o1));
343
344 }
345
346 /** Compute a composite rotation acceleration.
347 * @param first first applied transform
348 * @param second second applied transform
349 * @return rotation acceleration part of the composite transform
350 */
351 private static Vector3D compositeRotationAcceleration(final Transform first, final Transform second) {
352
353 final Vector3D o1 = first.angular.getRotationRate();
354 final Vector3D oDot1 = first.angular.getRotationAcceleration();
355 final Rotation r2 = second.angular.getRotation();
356 final Vector3D o2 = second.angular.getRotationRate();
357 final Vector3D oDot2 = second.angular.getRotationAcceleration();
358
359 return new Vector3D( 1, oDot2,
360 1, r2.applyTo(oDot1),
361 -1, Vector3D.crossProduct(o2, r2.applyTo(o1)));
362
363 }
364
365 /** {@inheritDoc} */
366 public AbsoluteDate getDate() {
367 return date;
368 }
369
370 /** {@inheritDoc} */
371 public Transform shiftedBy(final double dt) {
372 return new Transform(date.shiftedBy(dt), cartesian.shiftedBy(dt), angular.shiftedBy(dt));
373 };
374
375 /** {@inheritDoc}
376 * <p>
377 * Calling this method is equivalent to call {@link #interpolate(AbsoluteDate,
378 * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
379 * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
380 * {@link AngularDerivativesFilter#USE_RRA}
381 * set to true.
382 * </p>
383 * @exception OrekitException if the number of point is too small for interpolating
384 */
385 public Transform interpolate(final AbsoluteDate interpolationDate, final Collection<Transform> sample)
386 throws OrekitException {
387 return interpolate(interpolationDate,
388 CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
389 sample);
390 }
391
392 /** Interpolate a transform from a sample set of existing transforms.
393 * <p>
394 * Note that even if first time derivatives (velocities and rotation rates)
395 * from sample can be ignored, the interpolated instance always includes
396 * interpolated derivatives. This feature can be used explicitly to
397 * compute these derivatives when it would be too complex to compute them
398 * from an analytical formula: just compute a few sample points from the
399 * explicit formula and set the derivatives to zero in these sample points,
400 * then use interpolation to add derivatives consistent with the positions
401 * and rotations.
402 * </p>
403 * <p>
404 * As this implementation of interpolation is polynomial, it should be used only
405 * with small samples (about 10-20 points) in order to avoid <a
406 * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
407 * and numerical problems (including NaN appearing).
408 * </p>
409 * @param date interpolation date
410 * @param cFilter filter for derivatives from the sample to use in interpolation
411 * @param aFilter filter for derivatives from the sample to use in interpolation
412 * @param sample sample points on which interpolation should be done
413 * @return a new instance, interpolated at specified date
414 * @exception OrekitException if the number of point is too small for interpolating
415 * @since 7.0
416 */
417 public static Transform interpolate(final AbsoluteDate date,
418 final CartesianDerivativesFilter cFilter,
419 final AngularDerivativesFilter aFilter,
420 final Collection<Transform> sample)
421 throws OrekitException {
422 final List<TimeStampedPVCoordinates> datedPV = new ArrayList<TimeStampedPVCoordinates>(sample.size());
423 final List<TimeStampedAngularCoordinates> datedAC = new ArrayList<TimeStampedAngularCoordinates>(sample.size());
424 for (final Transform t : sample) {
425 datedPV.add(new TimeStampedPVCoordinates(t.getDate(), t.getTranslation(), t.getVelocity(), t.getAcceleration()));
426 datedAC.add(new TimeStampedAngularCoordinates(t.getDate(), t.getRotation(), t.getRotationRate(), t.getRotationAcceleration()));
427 }
428 final TimeStampedPVCoordinates interpolatedPV = TimeStampedPVCoordinates.interpolate(date, cFilter, datedPV);
429 final TimeStampedAngularCoordinates interpolatedAC = TimeStampedAngularCoordinates.interpolate(date, aFilter, datedAC);
430 return new Transform(date, interpolatedPV, interpolatedAC);
431 }
432
433 /** Get the inverse transform of the instance.
434 * @return inverse transform of the instance
435 */
436 public Transform getInverse() {
437
438 final Rotation r = angular.getRotation();
439 final Vector3D o = angular.getRotationRate();
440 final Vector3D oDot = angular.getRotationAcceleration();
441 final Vector3D rp = r.applyTo(cartesian.getPosition());
442 final Vector3D rv = r.applyTo(cartesian.getVelocity());
443 final Vector3D ra = r.applyTo(cartesian.getAcceleration());
444
445 final Vector3D pInv = rp.negate();
446 final Vector3D crossP = Vector3D.crossProduct(o, rp);
447 final Vector3D vInv = crossP.subtract(rv);
448 final Vector3D crossV = Vector3D.crossProduct(o, rv);
449 final Vector3D crossDotP = Vector3D.crossProduct(oDot, rp);
450 final Vector3D crossCrossP = Vector3D.crossProduct(o, crossP);
451 final Vector3D aInv = new Vector3D(-1, ra,
452 2, crossV,
453 1, crossDotP,
454 -1, crossCrossP);
455
456 return new Transform(date, new PVCoordinates(pInv, vInv, aInv), angular.revert());
457
458 }
459
460 /** Get a frozen transform.
461 * <p>
462 * This method creates a copy of the instance but frozen in time,
463 * i.e. with velocity, acceleration and rotation rate forced to zero.
464 * </p>
465 * @return a new transform, without any time-dependent parts
466 */
467 public Transform freeze() {
468 return new Transform(date,
469 new PVCoordinates(cartesian.getPosition(), Vector3D.ZERO, Vector3D.ZERO),
470 new AngularCoordinates(angular.getRotation(), Vector3D.ZERO, Vector3D.ZERO));
471 }
472
473 /** Transform a position vector (including translation effects).
474 * @param position vector to transform
475 * @return transformed position
476 */
477 public Vector3D transformPosition(final Vector3D position) {
478 return angular.getRotation().applyTo(cartesian.getPosition().add(position));
479 }
480
481 /** Transform a position vector (including translation effects).
482 * @param position vector to transform
483 * @param <T> the type of the field elements
484 * @return transformed position
485 */
486 public <T extends RealFieldElement<T>> FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
487 return FieldRotation.applyTo(angular.getRotation(), position.add(cartesian.getPosition()));
488 }
489
490 /** Transform a vector (ignoring translation effects).
491 * @param vector vector to transform
492 * @return transformed vector
493 */
494 public Vector3D transformVector(final Vector3D vector) {
495 return angular.getRotation().applyTo(vector);
496 }
497
498 /** Transform a vector (ignoring translation effects).
499 * @param vector vector to transform
500 * @param <T> the type of the field elements
501 * @return transformed vector
502 */
503 public <T extends RealFieldElement<T>> FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
504 return FieldRotation.applyTo(angular.getRotation(), vector);
505 }
506
507 /** Transform a line.
508 * @param line to transform
509 * @return transformed line
510 */
511 public Line transformLine(final Line line) {
512 final Vector3D transformedP0 = transformPosition(line.getOrigin());
513 final Vector3D transformedP1 = transformPosition(line.pointAt(1.0e6));
514 return new Line(transformedP0, transformedP1, 1.0e-10);
515 }
516
517 /** Transform {@link PVCoordinates} including kinematic effects.
518 * @param pva the position-velocity-acceleration triplet to transform.
519 * @return transformed position-velocity-acceleration
520 */
521 public PVCoordinates transformPVCoordinates(final PVCoordinates pva) {
522 return angular.applyTo(new PVCoordinates(1, pva, 1, cartesian));
523 }
524
525 /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
526 * <p>
527 * In order to allow the user more flexibility, this method does <em>not</em> check for
528 * consistency between the transform {@link #getDate() date} and the time-stamped
529 * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
530 * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
531 * the input argument, regardless of the instance {@link #getDate() date}.
532 * </p>
533 * @param pv time-stamped position-velocity to transform.
534 * @return transformed time-stamped position-velocity
535 * @since 7.0
536 */
537 public TimeStampedPVCoordinates transformPVCoordinates(final TimeStampedPVCoordinates pv) {
538 return angular.applyTo(new TimeStampedPVCoordinates(pv.getDate(), 1, pv, 1, cartesian));
539 }
540
541 /** Transform {@link FieldPVCoordinates} including kinematic effects.
542 * @param pv position-velocity to transform.
543 * @param <T> type of the field elements
544 * @return transformed position-velocity
545 */
546 public <T extends RealFieldElement<T>> FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
547
548 // apply translation
549 final FieldVector3D<T> intermediateP = pv.getPosition().add(cartesian.getPosition());
550 final FieldVector3D<T> intermediateV = pv.getVelocity().add(cartesian.getVelocity());
551 final FieldVector3D<T> intermediateA = pv.getAcceleration().add(cartesian.getAcceleration());
552
553 // apply rotation
554 final FieldVector3D<T> transformedP = FieldRotation.applyTo(angular.getRotation(), intermediateP);
555 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(angular.getRotationRate(), transformedP);
556 final FieldVector3D<T> transformedV = FieldRotation.applyTo(angular.getRotation(), intermediateV).subtract(crossP);
557 final FieldVector3D<T> crossV = FieldVector3D.crossProduct(angular.getRotationRate(), transformedV);
558 final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(angular.getRotationRate(), crossP);
559 final FieldVector3D<T> crossDotP = FieldVector3D.crossProduct(angular.getRotationAcceleration(), transformedP);
560 final FieldVector3D<T> transformedA =
561 new FieldVector3D<T>( 1, FieldRotation.applyTo(angular.getRotation(), intermediateA),
562 -2, crossV,
563 -1, crossCrossP,
564 -1, crossDotP);
565
566 // build transformed object
567 return new FieldPVCoordinates<T>(transformedP, transformedV, transformedA);
568
569 }
570
571 /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
572 * <p>
573 * In order to allow the user more flexibility, this method does <em>not</em> check for
574 * consistency between the transform {@link #getDate() date} and the time-stamped
575 * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
576 * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
577 * the input argument, regardless of the instance {@link #getDate() date}.
578 * </p>
579 * @param pv time-stamped position-velocity to transform.
580 * @param <T> type of the field elements
581 * @return transformed time-stamped position-velocity
582 * @since 7.0
583 */
584 public <T extends RealFieldElement<T>> TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedFieldPVCoordinates<T> pv) {
585
586 // apply translation
587 final FieldVector3D<T> intermediateP = pv.getPosition().add(cartesian.getPosition());
588 final FieldVector3D<T> intermediateV = pv.getVelocity().add(cartesian.getVelocity());
589 final FieldVector3D<T> intermediateA = pv.getAcceleration().add(cartesian.getAcceleration());
590
591 // apply rotation
592 final FieldVector3D<T> transformedP = FieldRotation.applyTo(angular.getRotation(), intermediateP);
593 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(angular.getRotationRate(), transformedP);
594 final FieldVector3D<T> transformedV = FieldRotation.applyTo(angular.getRotation(), intermediateV).subtract(crossP);
595 final FieldVector3D<T> crossV = FieldVector3D.crossProduct(angular.getRotationRate(), transformedV);
596 final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(angular.getRotationRate(), crossP);
597 final FieldVector3D<T> crossDotP = FieldVector3D.crossProduct(angular.getRotationAcceleration(), transformedP);
598 final FieldVector3D<T> transformedA =
599 new FieldVector3D<T>( 1, FieldRotation.applyTo(angular.getRotation(), intermediateA),
600 -2, crossV,
601 -1, crossCrossP,
602 -1, crossDotP);
603
604 // build transformed object
605 return new TimeStampedFieldPVCoordinates<T>(pv.getDate(), transformedP, transformedV, transformedA);
606
607 }
608
609 /** Compute the Jacobian of the {@link #transformPVCoordinates(PVCoordinates)}
610 * method of the transform.
611 * <p>
612 * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
613 * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j
614 * of the input {@link PVCoordinates} in method {@link #transformPVCoordinates(PVCoordinates)}.
615 * </p>
616 * <p>
617 * This definition implies that if we define position-velocity coordinates
618 * <pre>
619 * PV₁ = transform.transformPVCoordinates(PV₀), then
620 * </pre>
621 * <p> their differentials dPV₁ and dPV₀ will obey the following relation
622 * where J is the matrix computed by this method:
623 * <pre>
624 * dPV₁ = J × dPV₀
625 * </pre>
626 *
627 * @param selector selector specifying the size of the upper left corner that must be filled
628 * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
629 * velocities and accelerations)
630 * @param jacobian placeholder matrix whose upper-left corner is to be filled with
631 * the Jacobian, the rest of the matrix remaining untouched
632 */
633 public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {
634
635 // elementary matrix for rotation
636 final double[][] mData = angular.getRotation().getMatrix();
637
638 // dP1/dP0
639 System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
640 System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
641 System.arraycopy(mData[2], 0, jacobian[2], 0, 3);
642
643 if (selector.getMaxOrder() >= 1) {
644
645 // dP1/dV0
646 Arrays.fill(jacobian[0], 3, 6, 0.0);
647 Arrays.fill(jacobian[1], 3, 6, 0.0);
648 Arrays.fill(jacobian[2], 3, 6, 0.0);
649
650 // dV1/dP0
651 final Vector3D o = angular.getRotationRate();
652 final double ox = o.getX();
653 final double oy = o.getY();
654 final double oz = o.getZ();
655 for (int i = 0; i < 3; ++i) {
656 jacobian[3][i] = -(oy * mData[2][i] - oz * mData[1][i]);
657 jacobian[4][i] = -(oz * mData[0][i] - ox * mData[2][i]);
658 jacobian[5][i] = -(ox * mData[1][i] - oy * mData[0][i]);
659 }
660
661 // dV1/dV0
662 System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
663 System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
664 System.arraycopy(mData[2], 0, jacobian[5], 3, 3);
665
666 if (selector.getMaxOrder() >= 2) {
667
668 // dP1/dA0
669 Arrays.fill(jacobian[0], 6, 9, 0.0);
670 Arrays.fill(jacobian[1], 6, 9, 0.0);
671 Arrays.fill(jacobian[2], 6, 9, 0.0);
672
673 // dV1/dA0
674 Arrays.fill(jacobian[3], 6, 9, 0.0);
675 Arrays.fill(jacobian[4], 6, 9, 0.0);
676 Arrays.fill(jacobian[5], 6, 9, 0.0);
677
678 // dA1/dP0
679 final Vector3D oDot = angular.getRotationAcceleration();
680 final double oDotx = oDot.getX();
681 final double oDoty = oDot.getY();
682 final double oDotz = oDot.getZ();
683 for (int i = 0; i < 3; ++i) {
684 jacobian[6][i] = -(oDoty * mData[2][i] - oDotz * mData[1][i]) - (oy * jacobian[5][i] - oz * jacobian[4][i]);
685 jacobian[7][i] = -(oDotz * mData[0][i] - oDotx * mData[2][i]) - (oz * jacobian[3][i] - ox * jacobian[5][i]);
686 jacobian[8][i] = -(oDotx * mData[1][i] - oDoty * mData[0][i]) - (ox * jacobian[4][i] - oy * jacobian[3][i]);
687 }
688
689 // dA1/dV0
690 for (int i = 0; i < 3; ++i) {
691 jacobian[6][i + 3] = -2 * (oy * mData[2][i] - oz * mData[1][i]);
692 jacobian[7][i + 3] = -2 * (oz * mData[0][i] - ox * mData[2][i]);
693 jacobian[8][i + 3] = -2 * (ox * mData[1][i] - oy * mData[0][i]);
694 }
695
696 // dA1/dA0
697 System.arraycopy(mData[0], 0, jacobian[6], 6, 3);
698 System.arraycopy(mData[1], 0, jacobian[7], 6, 3);
699 System.arraycopy(mData[2], 0, jacobian[8], 6, 3);
700
701 }
702
703 }
704
705 }
706
707 /** Get the underlying elementary cartesian part.
708 * <p>A transform can be uniquely represented as an elementary
709 * translation followed by an elementary rotation. This method
710 * returns this unique elementary translation with its derivative.</p>
711 * @return underlying elementary cartesian part
712 * @see #getTranslation()
713 * @see #getVelocity()
714 */
715 public PVCoordinates getCartesian() {
716 return cartesian;
717 }
718
719 /** Get the underlying elementary translation.
720 * <p>A transform can be uniquely represented as an elementary
721 * translation followed by an elementary rotation. This method
722 * returns this unique elementary translation.</p>
723 * @return underlying elementary translation
724 * @see #getCartesian()
725 * @see #getVelocity()
726 * @see #getAcceleration()
727 */
728 public Vector3D getTranslation() {
729 return cartesian.getPosition();
730 }
731
732 /** Get the first time derivative of the translation.
733 * @return first time derivative of the translation
734 * @see #getCartesian()
735 * @see #getTranslation()
736 * @see #getAcceleration()
737 */
738 public Vector3D getVelocity() {
739 return cartesian.getVelocity();
740 }
741
742 /** Get the second time derivative of the translation.
743 * @return second time derivative of the translation
744 * @see #getCartesian()
745 * @see #getTranslation()
746 * @see #getVelocity()
747 */
748 public Vector3D getAcceleration() {
749 return cartesian.getAcceleration();
750 }
751
752 /** Get the underlying elementary angular part.
753 * <p>A transform can be uniquely represented as an elementary
754 * translation followed by an elementary rotation. This method
755 * returns this unique elementary rotation with its derivative.</p>
756 * @return underlying elementary angular part
757 * @see #getRotation()
758 * @see #getRotationRate()
759 * @see #getRotationAcceleration()
760 */
761 public AngularCoordinates getAngular() {
762 return angular;
763 }
764
765 /** Get the underlying elementary rotation.
766 * <p>A transform can be uniquely represented as an elementary
767 * translation followed by an elementary rotation. This method
768 * returns this unique elementary rotation.</p>
769 * @return underlying elementary rotation
770 * @see #getAngular()
771 * @see #getRotationRate()
772 * @see #getRotationAcceleration()
773 */
774 public Rotation getRotation() {
775 return angular.getRotation();
776 }
777
778 /** Get the first time derivative of the rotation.
779 * <p>The norm represents the angular rate.</p>
780 * @return First time derivative of the rotation
781 * @see #getAngular()
782 * @see #getRotation()
783 * @see #getRotationAcceleration()
784 */
785 public Vector3D getRotationRate() {
786 return angular.getRotationRate();
787 }
788
789 /** Get the second time derivative of the rotation.
790 * @return Second time derivative of the rotation
791 * @see #getAngular()
792 * @see #getRotation()
793 * @see #getRotationRate()
794 */
795 public Vector3D getRotationAcceleration() {
796 return angular.getRotationAcceleration();
797 }
798
799 /** Specialized class for identity transform. */
800 private static class IdentityTransform extends Transform {
801
802 /** Serializable UID. */
803 private static final long serialVersionUID = -9042082036141830517L;
804
805 /** Simple constructor. */
806 IdentityTransform() {
807 super(AbsoluteDate.J2000_EPOCH, PVCoordinates.ZERO, AngularCoordinates.IDENTITY);
808 }
809
810 /** {@inheritDoc} */
811 @Override
812 public Transform shiftedBy(final double dt) {
813 return this;
814 }
815
816 /** {@inheritDoc} */
817 @Override
818 public Transform getInverse() {
819 return this;
820 };
821
822 /** {@inheritDoc} */
823 @Override
824 public Vector3D transformPosition(final Vector3D position) {
825 return position;
826 }
827
828 /** {@inheritDoc} */
829 @Override
830 public Vector3D transformVector(final Vector3D vector) {
831 return vector;
832 }
833
834 /** {@inheritDoc} */
835 @Override
836 public Line transformLine(final Line line) {
837 return line;
838 }
839
840 /** {@inheritDoc} */
841 @Override
842 public PVCoordinates transformPVCoordinates(final PVCoordinates pv) {
843 return pv;
844 }
845
846 /** {@inheritDoc} */
847 @Override
848 public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) {
849 final int n = 3 * (selector.getMaxOrder() + 1);
850 for (int i = 0; i < n; ++i) {
851 Arrays.fill(jacobian[i], 0, n, 0.0);
852 jacobian[i][i] = 1.0;
853 }
854 }
855
856 }
857
858 }