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