1 /* Copyright 2022-2026 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.hipparchus.util.MathArrays;
24 import org.orekit.time.AbsoluteDate;
25 import org.orekit.time.FieldAbsoluteDate;
26 import org.orekit.utils.FieldPVCoordinates;
27 import org.orekit.utils.PVCoordinates;
28 import org.orekit.utils.TimeStampedFieldPVCoordinates;
29 import org.orekit.utils.TimeStampedPVCoordinates;
30
31 import java.util.Arrays;
32
33 /**
34 * A transform that only includes translation and rotation as well as their respective rates.
35 * It is kinematic in the sense that it cannot transform an acceleration vector.
36 *
37 * @param <T> type of the field elements
38 * @author Romain Serra
39 * @see FieldStaticTransform
40 * @see FieldTransform
41 * @see KinematicTransform
42 * @since 12.1
43 */
44 public interface FieldKinematicTransform<T extends CalculusFieldElement<T>> extends FieldStaticTransform<T> {
45
46 /**
47 * Get the identity kinematic transform.
48 *
49 * @param <T> type of the elements
50 * @param field field used by default
51 * @return identity transform.
52 */
53 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> getIdentity(final Field<T> field) {
54 return FieldTransform.getIdentity(field);
55 }
56
57 /** Compute a composite velocity.
58 * @param first first applied transform
59 * @param second second applied transform
60 * @param <T> the type of the field elements
61 * @return velocity part of the composite transform
62 */
63 static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeVelocity(final FieldKinematicTransform<T> first,
64 final FieldKinematicTransform<T> second) {
65
66 final FieldVector3D<T> v1 = first.getVelocity();
67 final FieldRotation<T> r1 = first.getRotation();
68 final FieldVector3D<T> o1 = first.getRotationRate();
69 final FieldVector3D<T> p2 = second.getTranslation();
70 final FieldVector3D<T> v2 = second.getVelocity();
71
72 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(o1, p2);
73
74 return v1.add(r1.applyInverseTo(v2.add(crossP)));
75 }
76
77 /** Compute a composite rotation rate.
78 * @param <T> type of the elements
79 * @param first first applied transform
80 * @param second second applied transform
81 * @return rotation rate part of the composite transform
82 */
83 static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationRate(final FieldKinematicTransform<T> first,
84 final FieldKinematicTransform<T> second) {
85
86 final FieldVector3D<T> o1 = first.getRotationRate();
87 final FieldRotation<T> r2 = second.getRotation();
88 final FieldVector3D<T> o2 = second.getRotationRate();
89
90 return o2.add(r2.applyTo(o1));
91 }
92
93 /** Transform {@link PVCoordinates}, without the acceleration vector.
94 * @param pv the position-velocity couple to transform.
95 * @return transformed position-velocity
96 */
97 default FieldPVCoordinates<T> transformOnlyPV(final FieldPVCoordinates<T> pv) {
98 final FieldVector3D<T> transformedP = transformPosition(pv.getPosition());
99 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(getRotationRate(), transformedP);
100 final FieldVector3D<T> transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
101 return new FieldPVCoordinates<>(transformedP, transformedV);
102 }
103
104 /** Transform {@link TimeStampedPVCoordinates}, without the acceleration vector.
105 * <p>
106 * In order to allow the user more flexibility, this method does <em>not</em> check for
107 * consistency between the transform {@link #getDate() date} and the time-stamped
108 * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
109 * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
110 * the input argument, regardless of the instance {@link #getDate() date}.
111 * </p>
112 * @param pv the position-velocity couple to transform.
113 * @return transformed position-velocity
114 */
115 default TimeStampedFieldPVCoordinates<T> transformOnlyPV(final TimeStampedFieldPVCoordinates<T> pv) {
116 final FieldVector3D<T> transformedP = transformPosition(pv.getPosition());
117 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(getRotationRate(), transformedP);
118 final FieldVector3D<T> transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
119 return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV,
120 FieldVector3D.getZero(pv.getDate().getField()));
121 }
122
123 /** Compute the Jacobian of the {@link #transformOnlyPV(FieldPVCoordinates)} (FieldPVCoordinates)}
124 * method of the transform.
125 * <p>
126 * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
127 * of the transformed {@link FieldPVCoordinates} with respect to Cartesian coordinate j
128 * of the input {@link FieldPVCoordinates} in method {@link #transformOnlyPV(FieldPVCoordinates)}.
129 * </p>
130 * <p>
131 * This definition implies that if we define position-velocity coordinates
132 * <pre>
133 * PV₁ = transform.transformPVCoordinates(PV₀), then
134 * </pre>
135 * <p> their differentials dPV₁ and dPV₀ will obey the following relation
136 * where J is the matrix computed by this method:
137 * <pre>
138 * dPV₁ = J × dPV₀
139 * </pre>
140 *
141 * @return Jacobian matrix
142 */
143 default T[][] getPVJacobian() {
144 final Field<T> field = getFieldDate().getField();
145 final T zero = field.getZero();
146 final T[][] jacobian = MathArrays.buildArray(field, 6, 6);
147
148 // elementary matrix for rotation
149 final T[][] mData = getRotation().getMatrix();
150
151 // dP1/dP0
152 System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
153 System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
154 System.arraycopy(mData[2], 0, jacobian[2], 0, 3);
155
156 // dP1/dV0
157 Arrays.fill(jacobian[0], 3, 6, zero);
158 Arrays.fill(jacobian[1], 3, 6, zero);
159 Arrays.fill(jacobian[2], 3, 6, zero);
160
161 // dV1/dP0
162 final FieldVector3D<T> o = getRotationRate();
163 final T ox = o.getX();
164 final T oy = o.getY();
165 final T oz = o.getZ();
166 for (int i = 0; i < 3; ++i) {
167 jacobian[3][i] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i]));
168 jacobian[4][i] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i]));
169 jacobian[5][i] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i]));
170 }
171
172 // dV1/dV0
173 System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
174 System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
175 System.arraycopy(mData[2], 0, jacobian[5], 3, 3);
176
177 return jacobian;
178 }
179
180 /** Get the first time derivative of the translation.
181 * @return first time derivative of the translation
182 * @see #getTranslation()
183 */
184 FieldVector3D<T> getVelocity();
185
186 /** Get the first time derivative of the rotation.
187 * <p>The norm represents the angular rate.</p>
188 * @return First time derivative of the rotation
189 * @see #getRotation()
190 */
191 FieldVector3D<T> getRotationRate();
192
193 /**
194 * Get the inverse transform of the instance.
195 *
196 * @return inverse transform of the instance
197 */
198 FieldKinematicTransform<T> getInverse();
199
200 /**
201 * Build a transform by combining two existing ones.
202 * <p>
203 * Note that the dates of the two existing transformed are <em>ignored</em>,
204 * and the combined transform date is set to the date supplied in this
205 * constructor without any attempt to shift the raw transforms. This is a
206 * design choice allowing user full control of the combination.
207 * </p>
208 *
209 * @param <T> type of the elements
210 * @param date date of the transform
211 * @param first first transform applied
212 * @param second second transform applied
213 * @return the newly created kinematic transform that has the same effect as
214 * applying {@code first}, then {@code second}.
215 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
216 */
217 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> compose(final FieldAbsoluteDate<T> date,
218 final FieldKinematicTransform<T> first,
219 final FieldKinematicTransform<T> second) {
220 final FieldVector3D<T> composedTranslation = FieldStaticTransform.compositeTranslation(first, second);
221 final FieldVector3D<T> composedTranslationRate = FieldKinematicTransform.compositeVelocity(first, second);
222 return of(date, new FieldPVCoordinates<>(composedTranslation, composedTranslationRate),
223 FieldStaticTransform.compositeRotation(first, second),
224 FieldKinematicTransform.compositeRotationRate(first, second));
225 }
226
227 /**
228 * Create a new kinematic transform from a rotation and zero, constant translation.
229 *
230 * @param <T> type of the elements
231 * @param date of translation.
232 * @param rotation to apply after the translation. That is after translating
233 * applying this rotation produces positions expressed in
234 * the new frame.
235 * @param rotationRate rate of rotation
236 * @return the newly created kinematic transform.
237 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
238 */
239 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
240 final FieldRotation<T> rotation,
241 final FieldVector3D<T> rotationRate) {
242 return of(date, FieldPVCoordinates.getZero(date.getField()), rotation, rotationRate);
243 }
244
245 /**
246 * Create a new kinematic transform from a translation and its rate.
247 *
248 * @param <T> type of the elements
249 * @param date of translation.
250 * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
251 * opposite of the coordinates of the new origin in the
252 * old frame.
253 * @return the newly created kinematic transform.
254 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
255 */
256 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
257 final FieldPVCoordinates<T> pvCoordinates) {
258 final Field<T> field = date.getField();
259 return of(date, pvCoordinates, FieldRotation.getIdentity(field), FieldVector3D.getZero(field));
260 }
261
262 /**
263 * Create a new kinematic transform from a non-Field version.
264 *
265 * @param <T> type of the elements
266 * @param field field.
267 * @param kinematicTransform non-Field kinematic transform
268 * @return the newly created kinematic transform.
269 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
270 */
271 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final Field<T> field,
272 final KinematicTransform kinematicTransform) {
273 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, kinematicTransform.getDate());
274 final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(field,
275 new PVCoordinates(kinematicTransform.getTranslation(), kinematicTransform.getVelocity()));
276 final FieldRotation<T> rotation = new FieldRotation<>(field, kinematicTransform.getRotation());
277 final FieldVector3D<T> rotationRate = new FieldVector3D<>(field, kinematicTransform.getRotationRate());
278 return of(date, pvCoordinates, rotation, rotationRate);
279 }
280
281 /**
282 * Create a new kinematic transform from a translation and rotation.
283 *
284 * @param <T> type of the elements
285 * @param date of translation.
286 * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
287 * opposite of the coordinates of the new origin in the
288 * old frame.
289 * @param rotation to apply after the translation. That is after
290 * translating applying this rotation produces positions
291 * expressed in the new frame.
292 * @param rotationRate rate of rotation
293 * @return the newly created kinematic transform.
294 * @see #compose(FieldAbsoluteDate, FieldKinematicTransform, FieldKinematicTransform)
295 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
296 * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
297 */
298 static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
299 final FieldPVCoordinates<T> pvCoordinates,
300 final FieldRotation<T> rotation,
301 final FieldVector3D<T> rotationRate) {
302 return new FieldKinematicTransform<T>() {
303
304 @Override
305 public FieldKinematicTransform<T> getInverse() {
306 final FieldRotation<T> r = getRotation();
307 final FieldVector3D<T> rp = r.applyTo(getTranslation());
308 final FieldVector3D<T> pInv = rp.negate();
309 final FieldVector3D<T> crossP = FieldVector3D.crossProduct(getRotationRate(), rp);
310 final FieldVector3D<T> vInv = crossP.subtract(getRotation().applyTo(getVelocity()));
311 final FieldRotation<T> rInv = r.revert();
312 return FieldKinematicTransform.of(date, new FieldPVCoordinates<>(pInv, vInv),
313 rInv, rInv.applyTo(getRotationRate()).negate());
314 }
315
316 @Override
317 public AbsoluteDate getDate() {
318 return date.toAbsoluteDate();
319 }
320
321 @Override
322 public FieldAbsoluteDate<T> getFieldDate() {
323 return date;
324 }
325
326 @Override
327 public FieldVector3D<T> getTranslation() {
328 return pvCoordinates.getPosition();
329 }
330
331 @Override
332 public FieldRotation<T> getRotation() {
333 return rotation;
334 }
335
336 @Override
337 public FieldVector3D<T> getVelocity() {
338 return pvCoordinates.getVelocity();
339 }
340
341 @Override
342 public FieldVector3D<T> getRotationRate() {
343 return rotationRate;
344 }
345 };
346 }
347
348 }