1 /* Copyright 2022-2024 Romain Serra
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 org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.orekit.time.AbsoluteDate;
24 import org.orekit.time.FieldAbsoluteDate;
25 import org.orekit.utils.FieldPVCoordinates;
26 import org.orekit.utils.PVCoordinates;
27 import org.orekit.utils.TimeStampedFieldPVCoordinates;
28 import org.orekit.utils.TimeStampedPVCoordinates;
29
30 /**
31 * A transform that only includes translation and rotation as well as their respective rates.
32 * It is kinematic in the sense that it cannot transform an acceleration vector.
33 *
34 * @author Romain Serra
35 * @see FieldStaticTransform
36 * @see FieldTransform
37 * @see KinematicTransform
38 * @since 12.1
39 */
40 public interface FieldKinematicTransform<T extends CalculusFieldElement<T>> extends FieldStaticTransform<T> {
41
42 /**
43 * Get the identity kinematic transform.
44 *
45 * @param <T> type of the elements
46 * @param field field used by default
47 * @return identity transform.
48 */
49 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> getIdentity(final Field<T> field) {
50 return FieldTransform.getIdentity(field);
51 }
52
53 /** Compute a composite velocity.
54 * @param first first applied transform
55 * @param second second applied transform
56 * @param <T> the type of the field elements
57 * @return velocity part of the composite transform
58 */
59 static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeVelocity(final FieldKinematicTransform<T> first,
60 final FieldKinematicTransform<T> second) {
61
62 final FieldVector3D<T> v1 = first.getVelocity();
63 final FieldRotation<T> r1 = first.getRotation();
64 final FieldVector3D<T> o1 = first.getRotationRate();
65 final FieldVector3D<T> p2 = second.getTranslation();
66 final FieldVector3D<T> v2 = second.getVelocity();
67
68 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(o1, p2);
69
70 return v1.add(r1.applyInverseTo(v2.add(crossP)));
71 }
72
73 /** Compute a composite rotation rate.
74 * @param <T> type of the elements
75 * @param first first applied transform
76 * @param second second applied transform
77 * @return rotation rate part of the composite transform
78 */
79 static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationRate(final FieldKinematicTransform<T> first,
80 final FieldKinematicTransform<T> second) {
81
82 final FieldVector3D<T> o1 = first.getRotationRate();
83 final FieldRotation<T> r2 = second.getRotation();
84 final FieldVector3D<T> o2 = second.getRotationRate();
85
86 return o2.add(r2.applyTo(o1));
87 }
88
89 /** Transform {@link PVCoordinates}, without the acceleration vector.
90 * @param pv the position-velocity couple to transform.
91 * @return transformed position-velocity
92 */
93 default FieldPVCoordinates<T> transformOnlyPV(final FieldPVCoordinates<T> pv) {
94 final FieldVector3D<T> transformedP = transformPosition(pv.getPosition());
95 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(getRotationRate(), transformedP);
96 final FieldVector3D<T> transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
97 return new FieldPVCoordinates<>(transformedP, transformedV);
98 }
99
100 /** Transform {@link TimeStampedPVCoordinates}, without the acceleration vector.
101 * <p>
102 * In order to allow the user more flexibility, this method does <em>not</em> check for
103 * consistency between the transform {@link #getDate() date} and the time-stamped
104 * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
105 * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
106 * the input argument, regardless of the instance {@link #getDate() date}.
107 * </p>
108 * @param pv the position-velocity couple to transform.
109 * @return transformed position-velocity
110 */
111 default TimeStampedFieldPVCoordinates<T> transformOnlyPV(final TimeStampedFieldPVCoordinates<T> pv) {
112 final FieldVector3D<T> transformedP = transformPosition(pv.getPosition());
113 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(getRotationRate(), transformedP);
114 final FieldVector3D<T> transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
115 return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV,
116 FieldVector3D.getZero(pv.getDate().getField()));
117 }
118
119 /** Get the first time derivative of the translation.
120 * @return first time derivative of the translation
121 * @see #getTranslation()
122 */
123 FieldVector3D<T> getVelocity();
124
125 /** Get the first time derivative of the rotation.
126 * <p>The norm represents the angular rate.</p>
127 * @return First time derivative of the rotation
128 * @see #getRotation()
129 */
130 FieldVector3D<T> getRotationRate();
131
132 /**
133 * Get the inverse transform of the instance.
134 *
135 * @return inverse transform of the instance
136 */
137 FieldKinematicTransform<T> getInverse();
138
139 /**
140 * Build a transform by combining two existing ones.
141 * <p>
142 * Note that the dates of the two existing transformed are <em>ignored</em>,
143 * and the combined transform date is set to the date supplied in this
144 * constructor without any attempt to shift the raw transforms. This is a
145 * design choice allowing user full control of the combination.
146 * </p>
147 *
148 * @param <T> type of the elements
149 * @param date date of the transform
150 * @param first first transform applied
151 * @param second second transform applied
152 * @return the newly created kinematic transform that has the same effect as
153 * applying {@code first}, then {@code second}.
154 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
155 */
156 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> compose(final FieldAbsoluteDate<T> date,
157 final FieldKinematicTransform<T> first,
158 final FieldKinematicTransform<T> second) {
159 final FieldVector3D<T> composedTranslation = FieldStaticTransform.compositeTranslation(first, second);
160 final FieldVector3D<T> composedTranslationRate = FieldKinematicTransform.compositeVelocity(first, second);
161 return of(date, new FieldPVCoordinates<>(composedTranslation, composedTranslationRate),
162 FieldStaticTransform.compositeRotation(first, second),
163 FieldKinematicTransform.compositeRotationRate(first, second));
164 }
165
166 /**
167 * Create a new kinematic transform from a rotation and zero, constant translation.
168 *
169 * @param <T> type of the elements
170 * @param date of translation.
171 * @param rotation to apply after the translation. That is after translating
172 * applying this rotation produces positions expressed in
173 * the new frame.
174 * @param rotationRate rate of rotation
175 * @return the newly created kinematic transform.
176 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
177 */
178 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
179 final FieldRotation<T> rotation,
180 final FieldVector3D<T> rotationRate) {
181 return of(date, FieldPVCoordinates.getZero(date.getField()), rotation, rotationRate);
182 }
183
184 /**
185 * Create a new kinematic transform from a translation and its rate.
186 *
187 * @param <T> type of the elements
188 * @param date of translation.
189 * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
190 * opposite of the coordinates of the new origin in the
191 * old frame.
192 * @return the newly created kinematic transform.
193 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
194 */
195 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
196 final FieldPVCoordinates<T> pvCoordinates) {
197 final Field<T> field = date.getField();
198 return of(date, pvCoordinates, FieldRotation.getIdentity(field), FieldVector3D.getZero(field));
199 }
200
201 /**
202 * Create a new kinematic transform from a non-Field version.
203 *
204 * @param <T> type of the elements
205 * @param field field.
206 * @param kinematicTransform non-Field kinematic transform
207 * @return the newly created kinematic transform.
208 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
209 */
210 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final Field<T> field,
211 final KinematicTransform kinematicTransform) {
212 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, kinematicTransform.getDate());
213 final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(field,
214 new PVCoordinates(kinematicTransform.getTranslation(), kinematicTransform.getVelocity()));
215 final FieldRotation<T> rotation = new FieldRotation<>(field, kinematicTransform.getRotation());
216 final FieldVector3D<T> rotationRate = new FieldVector3D<>(field, kinematicTransform.getRotationRate());
217 return of(date, pvCoordinates, rotation, rotationRate);
218 }
219
220 /**
221 * Create a new kinematic transform from a translation and rotation.
222 *
223 * @param <T> type of the elements
224 * @param date of translation.
225 * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
226 * opposite of the coordinates of the new origin in the
227 * old frame.
228 * @param rotation to apply after the translation. That is after
229 * translating applying this rotation produces positions
230 * expressed in the new frame.
231 * @param rotationRate rate of rotation
232 * @return the newly created kinematic transform.
233 * @see #compose(FieldAbsoluteDate, FieldKinematicTransform, FieldKinematicTransform)
234 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
235 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
236 */
237 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
238 final FieldPVCoordinates<T> pvCoordinates,
239 final FieldRotation<T> rotation,
240 final FieldVector3D<T> rotationRate) {
241 return new FieldKinematicTransform<T>() {
242
243 @Override
244 public FieldKinematicTransform<T> getInverse() {
245 final FieldRotation<T> r = getRotation();
246 final FieldVector3D<T> rp = r.applyTo(getTranslation());
247 final FieldVector3D<T> pInv = rp.negate();
248 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(getRotationRate(), rp);
249 final FieldVector3D<T> vInv = crossP.subtract(getRotation().applyTo(getVelocity()));
250 final FieldRotation<T> rInv = r.revert();
251 return FieldKinematicTransform.of(date, new FieldPVCoordinates<>(pInv, vInv),
252 rInv, rInv.applyTo(getRotationRate()).negate());
253 }
254
255 @Override
256 public AbsoluteDate getDate() {
257 return date.toAbsoluteDate();
258 }
259
260 @Override
261 public FieldAbsoluteDate<T> getFieldDate() {
262 return date;
263 }
264
265 @Override
266 public FieldVector3D<T> getTranslation() {
267 return pvCoordinates.getPosition();
268 }
269
270 @Override
271 public FieldRotation<T> getRotation() {
272 return rotation;
273 }
274
275 @Override
276 public FieldVector3D<T> getVelocity() {
277 return pvCoordinates.getVelocity();
278 }
279
280 @Override
281 public FieldVector3D<T> getRotationRate() {
282 return rotationRate;
283 }
284 };
285 }
286
287 }