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