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.util.ArrayList;
20 import java.util.Arrays;
21 import java.util.Collection;
22 import java.util.List;
23 import java.util.stream.Stream;
24
25 import org.hipparchus.CalculusFieldElement;
26 import org.hipparchus.Field;
27 import org.hipparchus.geometry.euclidean.threed.FieldLine;
28 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
29 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.time.FieldTimeInterpolator;
33 import org.orekit.time.FieldTimeShiftable;
34 import org.orekit.utils.AngularDerivativesFilter;
35 import org.orekit.utils.CartesianDerivativesFilter;
36 import org.orekit.utils.FieldAngularCoordinates;
37 import org.orekit.utils.FieldPVCoordinates;
38 import org.orekit.utils.PVCoordinates;
39 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
40 import org.orekit.utils.TimeStampedFieldAngularCoordinatesHermiteInterpolator;
41 import org.orekit.utils.TimeStampedFieldPVCoordinates;
42 import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
43 import org.orekit.utils.TimeStampedPVCoordinates;
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 FieldPVCoordinates} 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>
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.transformPVCoordinate(PVA);
78 * </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 FieldPVCoordinates} 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>
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 * </pre>
97 *
98 * @author Luc Maisonobe
99 * @author Fabien Maussion
100 * @param <T> the type of the field elements
101 * @since 9.0
102 */
103 public class FieldTransform<T extends CalculusFieldElement<T>>
104 implements FieldTimeShiftable<FieldTransform<T>, T>, FieldKinematicTransform<T> {
105
106 /** Date of the transform. */
107 private final FieldAbsoluteDate<T> date;
108
109 /** Date of the transform. */
110 private final AbsoluteDate aDate;
111
112 /** Cartesian coordinates of the target frame with respect to the original frame. */
113 private final FieldPVCoordinates<T> cartesian;
114
115 /** Angular coordinates of the target frame with respect to the original frame. */
116 private final FieldAngularCoordinates<T> angular;
117
118 /** Build a transform from its primitive operations.
119 * @param date date of the transform
120 * @param aDate date of the transform
121 * @param cartesian Cartesian coordinates of the target frame with respect to the original frame
122 * @param angular angular coordinates of the target frame with respect to the original frame
123 */
124 private FieldTransform(final FieldAbsoluteDate<T> date, final AbsoluteDate aDate,
125 final FieldPVCoordinates<T> cartesian,
126 final FieldAngularCoordinates<T> angular) {
127 this.date = date;
128 this.aDate = aDate;
129 this.cartesian = cartesian;
130 this.angular = angular;
131 }
132
133 /** Build a transform from a regular transform.
134 * @param field field of the elements
135 * @param transform regular transform to convert
136 */
137 public FieldTransform(final Field<T> field, final Transform transform) {
138 this(new FieldAbsoluteDate<>(field, transform.getDate()), transform.getDate(),
139 new FieldPVCoordinates<>(field, transform.getCartesian()),
140 new FieldAngularCoordinates<>(field, transform.getAngular()));
141 }
142
143 /** Build a translation transform.
144 * @param date date of the transform
145 * @param translation translation to apply (i.e. coordinates of
146 * the transformed origin, or coordinates of the origin of the
147 * old frame in the new frame)
148 */
149 public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation) {
150 this(date, date.toAbsoluteDate(),
151 new FieldPVCoordinates<>(translation,
152 FieldVector3D.getZero(date.getField()),
153 FieldVector3D.getZero(date.getField())),
154 FieldAngularCoordinates.getIdentity(date.getField()));
155 }
156
157 /** Build a rotation transform.
158 * @param date date of the transform
159 * @param rotation rotation to apply ( i.e. rotation to apply to the
160 * coordinates of a vector expressed in the old frame to obtain the
161 * same vector expressed in the new frame )
162 */
163 public FieldTransform(final FieldAbsoluteDate<T> date, final FieldRotation<T> rotation) {
164 this(date, date.toAbsoluteDate(),
165 FieldPVCoordinates.getZero(date.getField()),
166 new FieldAngularCoordinates<>(rotation, FieldVector3D.getZero(date.getField())));
167 }
168
169 /** Build a translation transform, with its first time derivative.
170 * @param date date of the transform
171 * @param translation translation to apply (i.e. coordinates of
172 * the transformed origin, or coordinates of the origin of the
173 * old frame in the new frame)
174 * @param velocity the velocity of the translation (i.e. origin
175 * of the old frame velocity in the new frame)
176 */
177 public FieldTransform(final FieldAbsoluteDate<T> date,
178 final FieldVector3D<T> translation,
179 final FieldVector3D<T> velocity) {
180 this(date,
181 new FieldPVCoordinates<>(translation,
182 velocity,
183 FieldVector3D.getZero(date.getField())));
184 }
185
186 /** Build a translation transform, with its first and second time derivatives.
187 * @param date date of the transform
188 * @param translation translation to apply (i.e. coordinates of
189 * the transformed origin, or coordinates of the origin of the
190 * old frame in the new frame)
191 * @param velocity the velocity of the translation (i.e. origin
192 * of the old frame velocity in the new frame)
193 * @param acceleration the acceleration of the translation (i.e. origin
194 * of the old frame acceleration in the new frame)
195 */
196 public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation,
197 final FieldVector3D<T> velocity, final FieldVector3D<T> acceleration) {
198 this(date,
199 new FieldPVCoordinates<>(translation, velocity, acceleration));
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 FieldTransform(final FieldAbsoluteDate<T> date, final FieldPVCoordinates<T> cartesian) {
209 this(date, date.toAbsoluteDate(),
210 cartesian,
211 FieldAngularCoordinates.getIdentity(date.getField()));
212 }
213
214 /** Build a combined translation and rotation transform.
215 * @param date date of the transform
216 * @param translation translation to apply (i.e. coordinates of
217 * the transformed origin, or coordinates of the origin of the
218 * old frame in the new frame)
219 * @param rotation rotation to apply ( i.e. rotation to apply to the
220 * coordinates of a vector expressed in the old frame to obtain the
221 * same vector expressed in the new frame )
222 * @since 12.1
223 */
224 public FieldTransform(final FieldAbsoluteDate<T> date, final FieldVector3D<T> translation,
225 final FieldRotation<T> rotation) {
226 this(date, date.toAbsoluteDate(), new FieldPVCoordinates<>(translation, FieldVector3D.getZero(date.getField())),
227 new FieldAngularCoordinates<>(rotation, FieldVector3D.getZero(date.getField())));
228 }
229
230 /** Build a rotation transform.
231 * @param date date of the transform
232 * @param rotation rotation to apply ( i.e. rotation to apply to the
233 * coordinates of a vector expressed in the old frame to obtain the
234 * same vector expressed in the new frame )
235 * @param rotationRate the axis of the instant rotation
236 * expressed in the new frame. (norm representing angular rate)
237 */
238 public FieldTransform(final FieldAbsoluteDate<T> date,
239 final FieldRotation<T> rotation,
240 final FieldVector3D<T> rotationRate) {
241 this(date,
242 new FieldAngularCoordinates<>(rotation,
243 rotationRate,
244 FieldVector3D.getZero(date.getField())));
245 }
246
247 /** Build a rotation transform.
248 * @param date date of the transform
249 * @param rotation rotation to apply ( i.e. rotation to apply to the
250 * coordinates of a vector expressed in the old frame to obtain the
251 * same vector expressed in the new frame )
252 * @param rotationRate the axis of the instant rotation
253 * @param rotationAcceleration the axis of the instant rotation
254 * expressed in the new frame. (norm representing angular rate)
255 */
256 public FieldTransform(final FieldAbsoluteDate<T> date,
257 final FieldRotation<T> rotation,
258 final FieldVector3D<T> rotationRate,
259 final FieldVector3D<T> rotationAcceleration) {
260 this(date, new FieldAngularCoordinates<>(rotation, rotationRate, rotationAcceleration));
261 }
262
263 /** Build a rotation transform.
264 * @param date date of the transform
265 * @param angular angular part of the transformation to apply (i.e. rotation to
266 * apply to the coordinates of a vector expressed in the old frame to obtain the
267 * same vector expressed in the new frame, with its rotation rate)
268 */
269 public FieldTransform(final FieldAbsoluteDate<T> date, final FieldAngularCoordinates<T> angular) {
270 this(date, date.toAbsoluteDate(),
271 FieldPVCoordinates.getZero(date.getField()),
272 angular);
273 }
274
275 /** Build a transform by combining two existing ones.
276 * <p>
277 * Note that the dates of the two existing transformed are <em>ignored</em>,
278 * and the combined transform date is set to the date supplied in this constructor
279 * without any attempt to shift the raw transforms. This is a design choice allowing
280 * user full control of the combination.
281 * </p>
282 * @param date date of the transform
283 * @param first first transform applied
284 * @param second second transform applied
285 */
286 public FieldTransform(final FieldAbsoluteDate<T> date,
287 final FieldTransform<T> first,
288 final FieldTransform<T> second) {
289 this(date, date.toAbsoluteDate(),
290 new FieldPVCoordinates<>(FieldStaticTransform.compositeTranslation(first, second),
291 compositeVelocity(first, second),
292 compositeAcceleration(first, second)),
293 new FieldAngularCoordinates<>(FieldStaticTransform.compositeRotation(first, second),
294 compositeRotationRate(first, second),
295 compositeRotationAcceleration(first, second)));
296 }
297
298 /** Get the identity transform.
299 * @param field field for the components
300 * @param <T> the type of the field elements
301 * @return identity transform
302 */
303 public static <T extends CalculusFieldElement<T>> FieldTransform<T> getIdentity(final Field<T> field) {
304 return new FieldIdentityTransform<>(field);
305 }
306
307 /** Compute a composite velocity.
308 * @param first first applied transform
309 * @param second second applied transform
310 * @param <T> the type of the field elements
311 * @return velocity part of the composite transform
312 */
313 private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeVelocity(final FieldTransform<T> first, final FieldTransform<T> second) {
314
315 final FieldVector3D<T> v1 = first.cartesian.getVelocity();
316 final FieldRotation<T> r1 = first.angular.getRotation();
317 final FieldVector3D<T> o1 = first.angular.getRotationRate();
318 final FieldVector3D<T> p2 = second.cartesian.getPosition();
319 final FieldVector3D<T> v2 = second.cartesian.getVelocity();
320
321 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(o1, p2);
322
323 return v1.add(r1.applyInverseTo(v2.add(crossP)));
324
325 }
326
327 /** Compute a composite acceleration.
328 * @param first first applied transform
329 * @param second second applied transform
330 * @param <T> the type of the field elements
331 * @return acceleration part of the composite transform
332 */
333 private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {
334
335 final FieldVector3D<T> a1 = first.cartesian.getAcceleration();
336 final FieldRotation<T> r1 = first.angular.getRotation();
337 final FieldVector3D<T> o1 = first.angular.getRotationRate();
338 final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
339 final FieldVector3D<T> p2 = second.cartesian.getPosition();
340 final FieldVector3D<T> v2 = second.cartesian.getVelocity();
341 final FieldVector3D<T> a2 = second.cartesian.getAcceleration();
342
343 final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o1, FieldVector3D.crossProduct(o1, p2));
344 final FieldVector3D<T> crossV = FieldVector3D.crossProduct(o1, v2);
345 final FieldVector3D<T> crossDotP = FieldVector3D.crossProduct(oDot1, p2);
346
347 return a1.add(r1.applyInverseTo(new FieldVector3D<>(1, a2, 2, crossV, 1, crossCrossP, 1, crossDotP)));
348
349 }
350
351 /** Compute a composite rotation rate.
352 * @param first first applied transform
353 * @param second second applied transform
354 * @param <T> the type of the field elements
355 * @return rotation rate part of the composite transform
356 */
357 private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationRate(final FieldTransform<T> first, final FieldTransform<T> second) {
358
359 final FieldVector3D<T> o1 = first.angular.getRotationRate();
360 final FieldRotation<T> r2 = second.angular.getRotation();
361 final FieldVector3D<T> o2 = second.angular.getRotationRate();
362
363 return o2.add(r2.applyTo(o1));
364
365 }
366
367 /** Compute a composite rotation acceleration.
368 * @param first first applied transform
369 * @param second second applied transform
370 * @param <T> the type of the field elements
371 * @return rotation acceleration part of the composite transform
372 */
373 private static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationAcceleration(final FieldTransform<T> first, final FieldTransform<T> second) {
374
375 final FieldVector3D<T> o1 = first.angular.getRotationRate();
376 final FieldVector3D<T> oDot1 = first.angular.getRotationAcceleration();
377 final FieldRotation<T> r2 = second.angular.getRotation();
378 final FieldVector3D<T> o2 = second.angular.getRotationRate();
379 final FieldVector3D<T> oDot2 = second.angular.getRotationAcceleration();
380
381 return new FieldVector3D<>( 1, oDot2,
382 1, r2.applyTo(oDot1),
383 -1, FieldVector3D.crossProduct(o2, r2.applyTo(o1)));
384
385 }
386
387 /** {@inheritDoc} */
388 @Override
389 public AbsoluteDate getDate() {
390 return aDate;
391 }
392
393 /** Get the date.
394 * @return date attached to the object
395 */
396 @Override
397 public FieldAbsoluteDate<T> getFieldDate() {
398 return date;
399 }
400
401 /** {@inheritDoc} */
402 @Override
403 public FieldTransform<T> shiftedBy(final double dt) {
404 return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt),
405 cartesian.shiftedBy(dt), angular.shiftedBy(dt));
406 }
407
408 /** Get a time-shifted instance.
409 * @param dt time shift in seconds
410 * @return a new instance, shifted with respect to instance (which is not changed)
411 */
412 public FieldTransform<T> shiftedBy(final T dt) {
413 return new FieldTransform<>(date.shiftedBy(dt), aDate.shiftedBy(dt.getReal()),
414 cartesian.shiftedBy(dt), angular.shiftedBy(dt));
415 }
416
417 /**
418 * Shift the transform in time considering all rates, then return only the
419 * translation and rotation portion of the transform.
420 *
421 * @param dt time shift in seconds.
422 * @return shifted transform as a static transform. It is static in the
423 * sense that it can only be used to transform directions and positions, but
424 * not velocities or accelerations.
425 * @see #shiftedBy(double)
426 */
427 public FieldStaticTransform<T> staticShiftedBy(final T dt) {
428 return FieldStaticTransform.of(date.shiftedBy(dt),
429 cartesian.positionShiftedBy(dt),
430 angular.rotationShiftedBy(dt));
431 }
432
433 /**
434 * Create a so-called static transform from the instance.
435 *
436 * @return static part of the transform. It is static in the
437 * sense that it can only be used to transform directions and positions, but
438 * not velocities or accelerations.
439 * @see FieldStaticTransform
440 */
441 public FieldStaticTransform<T> toStaticTransform() {
442 return FieldStaticTransform.of(date, cartesian.getPosition(), angular.getRotation());
443 }
444
445 /** Interpolate a transform from a sample set of existing transforms.
446 * <p>
447 * Calling this method is equivalent to call {@link #interpolate(FieldAbsoluteDate,
448 * CartesianDerivativesFilter, AngularDerivativesFilter, Collection)} with {@code cFilter}
449 * set to {@link CartesianDerivativesFilter#USE_PVA} and {@code aFilter} set to
450 * {@link AngularDerivativesFilter#USE_RRA}
451 * set to true.
452 * </p>
453 * @param interpolationDate interpolation date
454 * @param sample sample points on which interpolation should be done
455 * @param <T> the type of the field elements
456 * @return a new instance, interpolated at specified date
457 */
458 public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> interpolationDate,
459 final Collection<FieldTransform<T>> sample) {
460 return interpolate(interpolationDate,
461 CartesianDerivativesFilter.USE_PVA, AngularDerivativesFilter.USE_RRA,
462 sample);
463 }
464
465 /** Interpolate a transform from a sample set of existing transforms.
466 * <p>
467 * Note that even if first time derivatives (velocities and rotation rates)
468 * from sample can be ignored, the interpolated instance always includes
469 * interpolated derivatives. This feature can be used explicitly to
470 * compute these derivatives when it would be too complex to compute them
471 * from an analytical formula: just compute a few sample points from the
472 * explicit formula and set the derivatives to zero in these sample points,
473 * then use interpolation to add derivatives consistent with the positions
474 * and rotations.
475 * </p>
476 * <p>
477 * As this implementation of interpolation is polynomial, it should be used only
478 * with small samples (about 10-20 points) in order to avoid <a
479 * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
480 * and numerical problems (including NaN appearing).
481 * </p>
482 * @param date interpolation date
483 * @param cFilter filter for derivatives from the sample to use in interpolation
484 * @param aFilter filter for derivatives from the sample to use in interpolation
485 * @param sample sample points on which interpolation should be done
486 * @return a new instance, interpolated at specified date
487 * @param <T> the type of the field elements
488 */
489 public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> date,
490 final CartesianDerivativesFilter cFilter,
491 final AngularDerivativesFilter aFilter,
492 final Collection<FieldTransform<T>> sample) {
493 return interpolate(date, cFilter, aFilter, sample.stream());
494 }
495
496 /** Interpolate a transform from a sample set of existing transforms.
497 * <p>
498 * Note that even if first time derivatives (velocities and rotation rates)
499 * from sample can be ignored, the interpolated instance always includes
500 * interpolated derivatives. This feature can be used explicitly to
501 * compute these derivatives when it would be too complex to compute them
502 * from an analytical formula: just compute a few sample points from the
503 * explicit formula and set the derivatives to zero in these sample points,
504 * then use interpolation to add derivatives consistent with the positions
505 * and rotations.
506 * </p>
507 * <p>
508 * As this implementation of interpolation is polynomial, it should be used only
509 * with small samples (about 10-20 points) in order to avoid <a
510 * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
511 * and numerical problems (including NaN appearing).
512 * </p>
513 * @param date interpolation date
514 * @param cFilter filter for derivatives from the sample to use in interpolation
515 * @param aFilter filter for derivatives from the sample to use in interpolation
516 * @param sample sample points on which interpolation should be done
517 * @return a new instance, interpolated at specified date
518 * @param <T> the type of the field elements
519 */
520 public static <T extends CalculusFieldElement<T>> FieldTransform<T> interpolate(final FieldAbsoluteDate<T> date,
521 final CartesianDerivativesFilter cFilter,
522 final AngularDerivativesFilter aFilter,
523 final Stream<FieldTransform<T>> sample) {
524
525 // Create samples
526 final List<TimeStampedFieldPVCoordinates<T>> datedPV = new ArrayList<>();
527 final List<TimeStampedFieldAngularCoordinates<T>> datedAC = new ArrayList<>();
528 sample.forEach(t -> {
529 datedPV.add(new TimeStampedFieldPVCoordinates<>(t.getDate(), t.getTranslation(), t.getVelocity(), t.getAcceleration()));
530 datedAC.add(new TimeStampedFieldAngularCoordinates<>(t.getDate(), t.getRotation(), t.getRotationRate(), t.getRotationAcceleration()));
531 });
532
533 // Create interpolators
534 final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> pvInterpolator =
535 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(datedPV.size(), cFilter);
536
537 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<T>, T> angularInterpolator =
538 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(datedPV.size(), aFilter);
539
540 // Interpolate
541 final TimeStampedFieldPVCoordinates<T> interpolatedPV = pvInterpolator.interpolate(date, datedPV);
542 final TimeStampedFieldAngularCoordinates<T> interpolatedAC = angularInterpolator.interpolate(date, datedAC);
543
544 return new FieldTransform<>(date, date.toAbsoluteDate(), interpolatedPV, interpolatedAC);
545 }
546
547 /** Get the inverse transform of the instance.
548 * @return inverse transform of the instance
549 */
550 @Override
551 public FieldTransform<T> getInverse() {
552
553 final FieldRotation<T> r = angular.getRotation();
554 final FieldVector3D<T> o = angular.getRotationRate();
555 final FieldVector3D<T> oDot = angular.getRotationAcceleration();
556 final FieldVector3D<T> rp = r.applyTo(cartesian.getPosition());
557 final FieldVector3D<T> rv = r.applyTo(cartesian.getVelocity());
558 final FieldVector3D<T> ra = r.applyTo(cartesian.getAcceleration());
559
560 final FieldVector3D<T> pInv = rp.negate();
561 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(o, rp);
562 final FieldVector3D<T> vInv = crossP.subtract(rv);
563 final FieldVector3D<T> crossV = FieldVector3D.crossProduct(o, rv);
564 final FieldVector3D<T> crossDotP = FieldVector3D.crossProduct(oDot, rp);
565 final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o, crossP);
566 final FieldVector3D<T> aInv = new FieldVector3D<>(-1, ra,
567 2, crossV,
568 1, crossDotP,
569 -1, crossCrossP);
570
571 return new FieldTransform<>(date, aDate, new FieldPVCoordinates<>(pInv, vInv, aInv), angular.revert());
572
573 }
574
575 /** Get a frozen transform.
576 * <p>
577 * This method creates a copy of the instance but frozen in time,
578 * i.e. with velocity, acceleration and rotation rate forced to zero.
579 * </p>
580 * @return a new transform, without any time-dependent parts
581 */
582 public FieldTransform<T> freeze() {
583 return new FieldTransform<>(date, aDate,
584 new FieldPVCoordinates<>(cartesian.getPosition(),
585 FieldVector3D.getZero(date.getField()),
586 FieldVector3D.getZero(date.getField())),
587 new FieldAngularCoordinates<>(angular.getRotation(),
588 FieldVector3D.getZero(date.getField()),
589 FieldVector3D.getZero(date.getField())));
590 }
591
592 /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
593 * <p>
594 * In order to allow the user more flexibility, this method does <em>not</em> check for
595 * consistency between the transform {@link #getDate() date} and the time-stamped
596 * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
597 * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
598 * the input argument, regardless of the instance {@link #getDate() date}.
599 * </p>
600 * @param pv time-stamped position-velocity to transform.
601 * @return transformed time-stamped position-velocity
602 */
603 public FieldPVCoordinates<T> transformPVCoordinates(final PVCoordinates pv) {
604 return angular.applyTo(new FieldPVCoordinates<>(cartesian.getPosition().add(pv.getPosition()),
605 cartesian.getVelocity().add(pv.getVelocity()),
606 cartesian.getAcceleration().add(pv.getAcceleration())));
607 }
608
609 /** Transform {@link TimeStampedPVCoordinates} including kinematic effects.
610 * <p>
611 * In order to allow the user more flexibility, this method does <em>not</em> check for
612 * consistency between the transform {@link #getDate() date} and the time-stamped
613 * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
614 * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
615 * the input argument, regardless of the instance {@link #getDate() date}.
616 * </p>
617 * @param pv time-stamped position-velocity to transform.
618 * @return transformed time-stamped position-velocity
619 */
620 public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedPVCoordinates pv) {
621 return angular.applyTo(new TimeStampedFieldPVCoordinates<>(pv.getDate(),
622 cartesian.getPosition().add(pv.getPosition()),
623 cartesian.getVelocity().add(pv.getVelocity()),
624 cartesian.getAcceleration().add(pv.getAcceleration())));
625 }
626
627 /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
628 * <p>
629 * BEWARE! This method does explicit computation of velocity and acceleration by combining
630 * the transform velocity, acceleration, rotation rate and rotation acceleration with the
631 * velocity and acceleration from the argument. This implies that this method should
632 * <em>not</em> be used when derivatives are contained in the {@link CalculusFieldElement field
633 * elements} (typically when using {@link org.hipparchus.analysis.differentiation.DerivativeStructure
634 * DerivativeStructure} elements where time is one of the differentiation parameter), otherwise
635 * the time derivatives would be computed twice, once explicitly in this method and once implicitly
636 * in the field operations. If time derivatives are contained in the field elements themselves,
637 * then rather than this method the {@link #transformPosition(FieldVector3D) transformPosition}
638 * method should be used, so the derivatives are computed once, as part of the field. This
639 * method is rather expected to be used when the field elements are {@link
640 * org.hipparchus.analysis.differentiation.DerivativeStructure DerivativeStructure} instances
641 * where the differentiation parameters are not time (they can typically be initial state
642 * for computing state transition matrices or force models parameters, or ground stations
643 * positions, ...).
644 * </p>
645 * <p>
646 * In order to allow the user more flexibility, this method does <em>not</em> check for
647 * consistency between the transform {@link #getDate() date} and the time-stamped
648 * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
649 * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
650 * the input argument, regardless of the instance {@link #getDate() date}.
651 * </p>
652 * @param pv time-stamped position-velocity to transform.
653 * @return transformed time-stamped position-velocity
654 */
655 public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
656 return angular.applyTo(new FieldPVCoordinates<>(pv.getPosition().add(cartesian.getPosition()),
657 pv.getVelocity().add(cartesian.getVelocity()),
658 pv.getAcceleration().add(cartesian.getAcceleration())));
659 }
660
661 /** Transform {@link TimeStampedFieldPVCoordinates} including kinematic effects.
662 * <p>
663 * BEWARE! This method does explicit computation of velocity and acceleration by combining
664 * the transform velocity, acceleration, rotation rate and rotation acceleration with the
665 * velocity and acceleration from the argument. This implies that this method should
666 * <em>not</em> be used when derivatives are contained in the {@link CalculusFieldElement field
667 * elements} (typically when using {@link org.hipparchus.analysis.differentiation.DerivativeStructure
668 * DerivativeStructure} elements where time is one of the differentiation parameter), otherwise
669 * the time derivatives would be computed twice, once explicitly in this method and once implicitly
670 * in the field operations. If time derivatives are contained in the field elements themselves,
671 * then rather than this method the {@link #transformPosition(FieldVector3D) transformPosition}
672 * method should be used, so the derivatives are computed once, as part of the field. This
673 * method is rather expected to be used when the field elements are {@link
674 * org.hipparchus.analysis.differentiation.DerivativeStructure DerivativeStructure} instances
675 * where the differentiation parameters are not time (they can typically be initial state
676 * for computing state transition matrices or force models parameters, or ground stations
677 * positions, ...).
678 * </p>
679 * <p>
680 * In order to allow the user more flexibility, this method does <em>not</em> check for
681 * consistency between the transform {@link #getDate() date} and the time-stamped
682 * position-velocity {@link TimeStampedFieldPVCoordinates#getDate() date}. The returned
683 * value will always have the same {@link TimeStampedFieldPVCoordinates#getDate() date} as
684 * the input argument, regardless of the instance {@link #getDate() date}.
685 * </p>
686 * @param pv time-stamped position-velocity to transform.
687 * @return transformed time-stamped position-velocity
688 */
689 public TimeStampedFieldPVCoordinates<T> transformPVCoordinates(final TimeStampedFieldPVCoordinates<T> pv) {
690 return angular.applyTo(new TimeStampedFieldPVCoordinates<>(pv.getDate(),
691 pv.getPosition().add(cartesian.getPosition()),
692 pv.getVelocity().add(cartesian.getVelocity()),
693 pv.getAcceleration().add(cartesian.getAcceleration())));
694 }
695
696 /** Compute the Jacobian of the {@link #transformPVCoordinates(FieldPVCoordinates)}
697 * method of the transform.
698 * <p>
699 * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
700 * of the transformed {@link FieldPVCoordinates} with respect to Cartesian coordinate j
701 * of the input {@link FieldPVCoordinates} in method {@link #transformPVCoordinates(FieldPVCoordinates)}.
702 * </p>
703 * <p>
704 * This definition implies that if we define position-velocity coordinates
705 * <pre>PV₁ = transform.transformPVCoordinates(PV₀)</pre>
706 * then their differentials dPV₁ and dPV₀ will obey the following relation
707 * where J is the matrix computed by this method:
708 * <pre>dPV₁ = J × dPV₀</pre>
709 *
710 * @param selector selector specifying the size of the upper left corner that must be filled
711 * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions,
712 * velocities and accelerations)
713 * @param jacobian placeholder matrix whose upper-left corner is to be filled with
714 * the Jacobian, the rest of the matrix remaining untouched
715 */
716 public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {
717
718 final T zero = date.getField().getZero();
719
720 // elementary matrix for rotation
721 final T[][] mData = angular.getRotation().getMatrix();
722
723 // dP1/dP0
724 System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
725 System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
726 System.arraycopy(mData[2], 0, jacobian[2], 0, 3);
727
728 if (selector.getMaxOrder() >= 1) {
729
730 // dP1/dV0
731 Arrays.fill(jacobian[0], 3, 6, zero);
732 Arrays.fill(jacobian[1], 3, 6, zero);
733 Arrays.fill(jacobian[2], 3, 6, zero);
734
735 // dV1/dP0
736 final FieldVector3D<T> o = angular.getRotationRate();
737 final T ox = o.getX();
738 final T oy = o.getY();
739 final T oz = o.getZ();
740 for (int i = 0; i < 3; ++i) {
741 jacobian[3][i] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i]));
742 jacobian[4][i] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i]));
743 jacobian[5][i] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i]));
744 }
745
746 // dV1/dV0
747 System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
748 System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
749 System.arraycopy(mData[2], 0, jacobian[5], 3, 3);
750
751 if (selector.getMaxOrder() >= 2) {
752
753 // dP1/dA0
754 Arrays.fill(jacobian[0], 6, 9, zero);
755 Arrays.fill(jacobian[1], 6, 9, zero);
756 Arrays.fill(jacobian[2], 6, 9, zero);
757
758 // dV1/dA0
759 Arrays.fill(jacobian[3], 6, 9, zero);
760 Arrays.fill(jacobian[4], 6, 9, zero);
761 Arrays.fill(jacobian[5], 6, 9, zero);
762
763 // dA1/dP0
764 final FieldVector3D<T> oDot = angular.getRotationAcceleration();
765 final T oDotx = oDot.getX();
766 final T oDoty = oDot.getY();
767 final T oDotz = oDot.getZ();
768 for (int i = 0; i < 3; ++i) {
769 jacobian[6][i] = oDotz.multiply(mData[1][i]).subtract(oDoty.multiply(mData[2][i])).add(oz.multiply(jacobian[4][i]).subtract(oy.multiply(jacobian[5][i])));
770 jacobian[7][i] = oDotx.multiply(mData[2][i]).subtract(oDotz.multiply(mData[0][i])).add(ox.multiply(jacobian[5][i]).subtract(oz.multiply(jacobian[3][i])));
771 jacobian[8][i] = oDoty.multiply(mData[0][i]).subtract(oDotx.multiply(mData[1][i])).add(oy.multiply(jacobian[3][i]).subtract(ox.multiply(jacobian[4][i])));
772 }
773
774 // dA1/dV0
775 for (int i = 0; i < 3; ++i) {
776 jacobian[6][i + 3] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i])).multiply(2);
777 jacobian[7][i + 3] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i])).multiply(2);
778 jacobian[8][i + 3] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i])).multiply(2);
779 }
780
781 // dA1/dA0
782 System.arraycopy(mData[0], 0, jacobian[6], 6, 3);
783 System.arraycopy(mData[1], 0, jacobian[7], 6, 3);
784 System.arraycopy(mData[2], 0, jacobian[8], 6, 3);
785
786 }
787
788 }
789
790 }
791
792 /** Get the underlying elementary Cartesian part.
793 * <p>A transform can be uniquely represented as an elementary
794 * translation followed by an elementary rotation. This method
795 * returns this unique elementary translation with its derivative.</p>
796 * @return underlying elementary Cartesian part
797 * @see #getTranslation()
798 * @see #getVelocity()
799 */
800 public FieldPVCoordinates<T> getCartesian() {
801 return cartesian;
802 }
803
804 /** Get the underlying elementary translation.
805 * <p>A transform can be uniquely represented as an elementary
806 * translation followed by an elementary rotation. This method
807 * returns this unique elementary translation.</p>
808 * @return underlying elementary translation
809 * @see #getCartesian()
810 * @see #getVelocity()
811 * @see #getAcceleration()
812 */
813 public FieldVector3D<T> getTranslation() {
814 return cartesian.getPosition();
815 }
816
817 /** Get the first time derivative of the translation.
818 * @return first time derivative of the translation
819 * @see #getCartesian()
820 * @see #getTranslation()
821 * @see #getAcceleration()
822 */
823 public FieldVector3D<T> getVelocity() {
824 return cartesian.getVelocity();
825 }
826
827 /** Get the second time derivative of the translation.
828 * @return second time derivative of the translation
829 * @see #getCartesian()
830 * @see #getTranslation()
831 * @see #getVelocity()
832 */
833 public FieldVector3D<T> getAcceleration() {
834 return cartesian.getAcceleration();
835 }
836
837 /** Get the underlying elementary angular part.
838 * <p>A transform can be uniquely represented as an elementary
839 * translation followed by an elementary rotation. This method
840 * returns this unique elementary rotation with its derivative.</p>
841 * @return underlying elementary angular part
842 * @see #getRotation()
843 * @see #getRotationRate()
844 * @see #getRotationAcceleration()
845 */
846 public FieldAngularCoordinates<T> getAngular() {
847 return angular;
848 }
849
850 /** Get the underlying elementary rotation.
851 * <p>A transform can be uniquely represented as an elementary
852 * translation followed by an elementary rotation. This method
853 * returns this unique elementary rotation.</p>
854 * @return underlying elementary rotation
855 * @see #getAngular()
856 * @see #getRotationRate()
857 * @see #getRotationAcceleration()
858 */
859 public FieldRotation<T> getRotation() {
860 return angular.getRotation();
861 }
862
863 /** Get the first time derivative of the rotation.
864 * <p>The norm represents the angular rate.</p>
865 * @return First time derivative of the rotation
866 * @see #getAngular()
867 * @see #getRotation()
868 * @see #getRotationAcceleration()
869 */
870 public FieldVector3D<T> getRotationRate() {
871 return angular.getRotationRate();
872 }
873
874 /** Get the second time derivative of the rotation.
875 * @return Second time derivative of the rotation
876 * @see #getAngular()
877 * @see #getRotation()
878 * @see #getRotationRate()
879 */
880 public FieldVector3D<T> getRotationAcceleration() {
881 return angular.getRotationAcceleration();
882 }
883
884 /** Specialized class for identity transform. */
885 private static class FieldIdentityTransform<T extends CalculusFieldElement<T>> extends FieldTransform<T> {
886
887 /** Simple constructor.
888 * @param field field for the components
889 */
890 FieldIdentityTransform(final Field<T> field) {
891 super(FieldAbsoluteDate.getArbitraryEpoch(field),
892 FieldAbsoluteDate.getArbitraryEpoch(field).toAbsoluteDate(),
893 FieldPVCoordinates.getZero(field),
894 FieldAngularCoordinates.getIdentity(field));
895 }
896
897 /** {@inheritDoc} */
898 @Override
899 public FieldTransform<T> shiftedBy(final double dt) {
900 return this;
901 }
902
903 /** {@inheritDoc} */
904 @Override
905 public FieldTransform<T> getInverse() {
906 return this;
907 }
908
909 /** {@inheritDoc} */
910 @Override
911 public FieldVector3D<T> transformPosition(final FieldVector3D<T> position) {
912 return position;
913 }
914
915 /** {@inheritDoc} */
916 @Override
917 public FieldVector3D<T> transformVector(final FieldVector3D<T> vector) {
918 return vector;
919 }
920
921 /** {@inheritDoc} */
922 @Override
923 public FieldLine<T> transformLine(final FieldLine<T> line) {
924 return line;
925 }
926
927 /** {@inheritDoc} */
928 @Override
929 public FieldPVCoordinates<T> transformPVCoordinates(final FieldPVCoordinates<T> pv) {
930 return pv;
931 }
932
933 /** {@inheritDoc} */
934 @Override
935 public FieldTransform<T> freeze() {
936 return this;
937 }
938
939 /** {@inheritDoc} */
940 @Override
941 public void getJacobian(final CartesianDerivativesFilter selector, final T[][] jacobian) {
942 final int n = 3 * (selector.getMaxOrder() + 1);
943 for (int i = 0; i < n; ++i) {
944 Arrays.fill(jacobian[i], 0, n, getFieldDate().getField().getZero());
945 jacobian[i][i] = getFieldDate().getField().getOne();
946 }
947 }
948
949 }
950
951 }