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