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