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