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