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