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