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