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.utils;
18
19
20 import org.hipparchus.Field;
21 import org.hipparchus.CalculusFieldElement;
22 import org.hipparchus.analysis.differentiation.FieldDerivative;
23 import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
24 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
27 import org.orekit.time.AbsoluteDate;
28 import org.orekit.time.FieldAbsoluteDate;
29 import org.orekit.time.FieldTimeStamped;
30 import org.orekit.time.TimeStamped;
31
32 /** {@link TimeStamped time-stamped} version of {@link FieldAngularCoordinates}.
33 * <p>Instances of this class are guaranteed to be immutable.</p>
34 * @param <T> the type of the field elements
35 * @author Luc Maisonobe
36 * @since 7.0
37 */
38 public class TimeStampedFieldAngularCoordinates<T extends CalculusFieldElement<T>>
39 extends FieldAngularCoordinates<T> implements FieldTimeStamped<T> {
40
41 /** The date. */
42 private final FieldAbsoluteDate<T> date;
43
44 /** Build the rotation that transforms a pair of pv coordinates into another pair.
45
46 * <p><em>WARNING</em>! This method requires much more stringent assumptions on
47 * its parameters than the similar {@link org.hipparchus.geometry.euclidean.threed.Rotation#Rotation(
48 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D,
49 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D)
50 * constructor} from the {@link org.hipparchus.geometry.euclidean.threed.Rotation Rotation} class.
51 * As far as the Rotation constructor is concerned, the {@code v₂} vector from
52 * the second pair can be slightly misaligned. The Rotation constructor will
53 * compensate for this misalignment and create a rotation that ensure {@code
54 * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
55 * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
56 * preserved, this constructor works <em>only</em> if the two pairs are fully
57 * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
58 * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
59 * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>
60
61 * @param date coordinates date
62 * @param u1 first vector of the origin pair
63 * @param u2 second vector of the origin pair
64 * @param v1 desired image of u1 by the rotation
65 * @param v2 desired image of u2 by the rotation
66 * @param tolerance relative tolerance factor used to check singularities
67 */
68 public TimeStampedFieldAngularCoordinates (final AbsoluteDate date,
69 final FieldPVCoordinates<T> u1, final FieldPVCoordinates<T> u2,
70 final FieldPVCoordinates<T> v1, final FieldPVCoordinates<T> v2,
71 final double tolerance) {
72 this(new FieldAbsoluteDate<>(u1.getPosition().getX().getField(), date),
73 u1, u2, v1, v2, tolerance);
74 }
75
76 /** Build the rotation that transforms a pair of pv coordinates into another pair.
77
78 * <p><em>WARNING</em>! This method requires much more stringent assumptions on
79 * its parameters than the similar {@link org.hipparchus.geometry.euclidean.threed.Rotation#Rotation(
80 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D,
81 * org.hipparchus.geometry.euclidean.threed.Vector3D, org.hipparchus.geometry.euclidean.threed.Vector3D)
82 * constructor} from the {@link org.hipparchus.geometry.euclidean.threed.Rotation Rotation} class.
83 * As far as the Rotation constructor is concerned, the {@code v₂} vector from
84 * the second pair can be slightly misaligned. The Rotation constructor will
85 * compensate for this misalignment and create a rotation that ensure {@code
86 * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT
87 * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be
88 * preserved, this constructor works <em>only</em> if the two pairs are fully
89 * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code
90 * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt
91 * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p>
92
93 * @param date coordinates date
94 * @param u1 first vector of the origin pair
95 * @param u2 second vector of the origin pair
96 * @param v1 desired image of u1 by the rotation
97 * @param v2 desired image of u2 by the rotation
98 * @param tolerance relative tolerance factor used to check singularities
99 */
100 public TimeStampedFieldAngularCoordinates (final FieldAbsoluteDate<T> date,
101 final FieldPVCoordinates<T> u1, final FieldPVCoordinates<T> u2,
102 final FieldPVCoordinates<T> v1, final FieldPVCoordinates<T> v2,
103 final double tolerance) {
104 super(u1, u2, v1, v2, tolerance);
105 this.date = date;
106 }
107
108 /** Builds a rotation/rotation rate pair.
109 * @param date coordinates date
110 * @param rotation rotation
111 * @param rotationRate rotation rate Ω (rad/s)
112 * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²)
113 */
114 public TimeStampedFieldAngularCoordinates(final AbsoluteDate date,
115 final FieldRotation<T> rotation,
116 final FieldVector3D<T> rotationRate,
117 final FieldVector3D<T> rotationAcceleration) {
118 this(new FieldAbsoluteDate<>(rotation.getQ0().getField(), date),
119 rotation, rotationRate, rotationAcceleration);
120 }
121
122 /** Builds a rotation/rotation rate pair.
123 * @param date coordinates date
124 * @param rotation rotation
125 * @param rotationRate rotation rate Ω (rad/s)
126 * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²)
127 */
128 public TimeStampedFieldAngularCoordinates(final FieldAbsoluteDate<T> date,
129 final FieldRotation<T> rotation,
130 final FieldVector3D<T> rotationRate,
131 final FieldVector3D<T> rotationAcceleration) {
132 super(rotation, rotationRate, rotationAcceleration);
133 this.date = date;
134 }
135
136 /** Builds an instance for a regular {@link TimeStampedAngularCoordinates}.
137 * @param field fields to which the elements belong
138 * @param ac coordinates to convert
139 * @since 9.0
140 */
141 public TimeStampedFieldAngularCoordinates(final Field<T> field,
142 final TimeStampedAngularCoordinates ac) {
143 this(new FieldAbsoluteDate<>(field, ac.getDate()),
144 new FieldRotation<>(field, ac.getRotation()),
145 new FieldVector3D<>(field, ac.getRotationRate()),
146 new FieldVector3D<>(field, ac.getRotationAcceleration()));
147 }
148
149 /** Builds a TimeStampedFieldAngularCoordinates from a {@link FieldRotation}<{@link FieldDerivativeStructure}>.
150 * <p>
151 * The rotation components must have time as their only derivation parameter and
152 * have consistent derivation orders.
153 * </p>
154 * @param date coordinates date
155 * @param r rotation with time-derivatives embedded within the coordinates
156 * @param <U> type of the derivative
157 * @since 9.2
158 */
159 public <U extends FieldDerivative<T, U>> TimeStampedFieldAngularCoordinates(final FieldAbsoluteDate<T> date,
160 final FieldRotation<U> r) {
161 super(r);
162 this.date = date;
163 }
164
165 /** Revert a rotation/rotation rate pair.
166 * Build a pair which reverse the effect of another pair.
167 * @return a new pair whose effect is the reverse of the effect
168 * of the instance
169 */
170 public TimeStampedFieldAngularCoordinates<T> revert() {
171 return new TimeStampedFieldAngularCoordinates<>(date,
172 getRotation().revert(),
173 getRotation().applyInverseTo(getRotationRate().negate()),
174 getRotation().applyInverseTo(getRotationAcceleration().negate()));
175 }
176
177 /** {@inheritDoc} */
178 @Override
179 public FieldAbsoluteDate<T> getDate() {
180 return date;
181 }
182
183 /** Get a time-shifted state.
184 * <p>
185 * The state can be slightly shifted to close dates. This shift is based on
186 * a simple linear model. It is <em>not</em> intended as a replacement for
187 * proper attitude propagation but should be sufficient for either small
188 * time shifts or coarse accuracy.
189 * </p>
190 * @param dt time shift in seconds
191 * @return a new state, shifted with respect to the instance (which is immutable)
192 */
193 public TimeStampedFieldAngularCoordinates<T> shiftedBy(final double dt) {
194 return shiftedBy(getDate().getField().getZero().newInstance(dt));
195 }
196
197 /** Get a time-shifted state.
198 * <p>
199 * The state can be slightly shifted to close dates. This shift is based on
200 * a simple linear model. It is <em>not</em> intended as a replacement for
201 * proper attitude propagation but should be sufficient for either small
202 * time shifts or coarse accuracy.
203 * </p>
204 * @param dt time shift in seconds
205 * @return a new state, shifted with respect to the instance (which is immutable)
206 */
207 public TimeStampedFieldAngularCoordinates<T> shiftedBy(final T dt) {
208 final FieldAngularCoordinates<T> sac = super.shiftedBy(dt);
209 return new TimeStampedFieldAngularCoordinates<>(date.shiftedBy(dt),
210 sac.getRotation(), sac.getRotationRate(), sac.getRotationAcceleration());
211
212 }
213
214 /** Add an offset from the instance.
215 * <p>
216 * We consider here that the offset rotation is applied first and the
217 * instance is applied afterward. Note that angular coordinates do <em>not</em>
218 * commute under this operation, i.e. {@code a.addOffset(b)} and {@code
219 * b.addOffset(a)} lead to <em>different</em> results in most cases.
220 * </p>
221 * <p>
222 * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
223 * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
224 * so that round trip applications are possible. This means that both {@code
225 * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
226 * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
227 * </p>
228 * @param offset offset to subtract
229 * @return new instance, with offset subtracted
230 * @see #subtractOffset(FieldAngularCoordinates)
231 */
232 public TimeStampedFieldAngularCoordinates<T> addOffset(final FieldAngularCoordinates<T> offset) {
233 final FieldVector3D<T> rOmega = getRotation().applyTo(offset.getRotationRate());
234 final FieldVector3D<T> rOmegaDot = getRotation().applyTo(offset.getRotationAcceleration());
235 return new TimeStampedFieldAngularCoordinates<>(date,
236 getRotation().compose(offset.getRotation(), RotationConvention.VECTOR_OPERATOR),
237 getRotationRate().add(rOmega),
238 new FieldVector3D<>( 1.0, getRotationAcceleration(),
239 1.0, rOmegaDot,
240 -1.0, FieldVector3D.crossProduct(getRotationRate(), rOmega)));
241 }
242
243 /** Subtract an offset from the instance.
244 * <p>
245 * We consider here that the offset Rotation is applied first and the
246 * instance is applied afterward. Note that angular coordinates do <em>not</em>
247 * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code
248 * b.subtractOffset(a)} lead to <em>different</em> results in most cases.
249 * </p>
250 * <p>
251 * The two methods {@link #addOffset(FieldAngularCoordinates) addOffset} and
252 * {@link #subtractOffset(FieldAngularCoordinates) subtractOffset} are designed
253 * so that round trip applications are possible. This means that both {@code
254 * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code
255 * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1.
256 * </p>
257 * @param offset offset to subtract
258 * @return new instance, with offset subtracted
259 * @see #addOffset(FieldAngularCoordinates)
260 */
261 public TimeStampedFieldAngularCoordinates<T> subtractOffset(final FieldAngularCoordinates<T> offset) {
262 return addOffset(offset.revert());
263 }
264
265 }