1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.files.ccsds.ndm.adm.acm;
18
19 import java.util.List;
20 import java.util.stream.Collectors;
21 import java.util.stream.Stream;
22
23 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
24 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
25 import org.hipparchus.geometry.euclidean.threed.Rotation;
26 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
27 import org.hipparchus.geometry.euclidean.threed.RotationOrder;
28 import org.hipparchus.geometry.euclidean.threed.Vector3D;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.errors.OrekitMessages;
31 import org.orekit.time.AbsoluteDate;
32 import org.orekit.utils.AngularCoordinates;
33 import org.orekit.utils.TimeStampedAngularCoordinates;
34 import org.orekit.utils.units.Unit;
35
36
37
38
39
40 public enum RateElementsType {
41
42
43
44
45 ANGVEL("Angular velocity",
46 "°/s", "°/s", "°/s") {
47
48
49 @Override
50 public TimeStampedAngularCoordinates toAngular(final AbsoluteDate date,
51 final RotationOrder order,
52 final Rotation rotation,
53 final int first,
54 final double[] elements) {
55 return new TimeStampedAngularCoordinates(date,
56 rotation,
57 new Vector3D(elements[first], elements[first + 1], elements[first + 2]),
58 Vector3D.ZERO);
59 }
60
61
62 @Override
63 public double[] toRawElements(final TimeStampedAngularCoordinates angularCoordinates,
64 final RotationOrder order) {
65 return new double[] {
66 angularCoordinates.getRotationRate().getX(),
67 angularCoordinates.getRotationRate().getY(),
68 angularCoordinates.getRotationRate().getZ()
69 };
70 }
71
72 },
73
74
75 Q_DOT("Quaternion derivatives",
76 "s⁻¹", "s⁻¹", "s⁻¹", "s⁻¹") {
77
78
79 @Override
80 public TimeStampedAngularCoordinates toAngular(final AbsoluteDate date,
81 final RotationOrder order,
82 final Rotation rotation,
83 final int first,
84 final double[] elements) {
85 final UnivariateDerivative1 q0 = new UnivariateDerivative1(rotation.getQ0(), elements[first + 3]);
86 final UnivariateDerivative1 q1 = new UnivariateDerivative1(rotation.getQ1(), elements[first]);
87 final UnivariateDerivative1 q2 = new UnivariateDerivative1(rotation.getQ2(), elements[first + 1]);
88 final UnivariateDerivative1 q3 = new UnivariateDerivative1(rotation.getQ3(), elements[first + 2]);
89 return new TimeStampedAngularCoordinates(date, new FieldRotation<>(q0, q1, q2, q3, false));
90 }
91
92
93 @Override
94 public double[] toRawElements(final TimeStampedAngularCoordinates angularCoordinates,
95 final RotationOrder order) {
96 final FieldRotation<UnivariateDerivative1> ac1 = angularCoordinates.toUnivariateDerivative1Rotation();
97 return new double[] {
98 ac1.getQ1().getFirstDerivative(),
99 ac1.getQ2().getFirstDerivative(),
100 ac1.getQ3().getFirstDerivative(),
101 ac1.getQ0().getFirstDerivative()
102 };
103 }
104
105 },
106
107
108 EULER_RATE("Euler rates",
109 "°/s", "°/s", "°/s") {
110
111
112 @Override
113 public TimeStampedAngularCoordinates toAngular(final AbsoluteDate date,
114 final RotationOrder order,
115 final Rotation rotation,
116 final int first,
117 final double[] elements) {
118 final double[] euler0 = rotation.getAngles(order, RotationConvention.FRAME_TRANSFORM);
119 final UnivariateDerivative1 alpha0 = new UnivariateDerivative1(euler0[0], elements[first]);
120 final UnivariateDerivative1 alpha1 = new UnivariateDerivative1(euler0[1], elements[first + 1]);
121 final UnivariateDerivative1 alpha2 = new UnivariateDerivative1(euler0[2], elements[first + 2]);
122 return new TimeStampedAngularCoordinates(date, new FieldRotation<>(order, RotationConvention.FRAME_TRANSFORM,
123 alpha0, alpha1, alpha2));
124 }
125
126
127 @Override
128 public double[] toRawElements(final TimeStampedAngularCoordinates angularCoordinates,
129 final RotationOrder order) {
130 final FieldRotation<UnivariateDerivative1> ac1 = angularCoordinates.toUnivariateDerivative1Rotation();
131 final UnivariateDerivative1[] euler = ac1.getAngles(order, RotationConvention.FRAME_TRANSFORM);
132 return new double[] {
133 euler[0].getFirstDerivative(), euler[1].getFirstDerivative(), euler[2].getFirstDerivative(),
134 };
135 }
136
137 },
138
139
140 GYRO_BIAS("Gyro rate corrections",
141 "°/s", "°/s", "°/s") {
142
143
144 @Override
145 public TimeStampedAngularCoordinates toAngular(final AbsoluteDate date,
146 final RotationOrder order,
147 final Rotation rotation,
148 final int first,
149 final double[] elements) {
150 throw new OrekitException(OrekitMessages.CCSDS_UNSUPPORTED_ELEMENT_SET_TYPE, name(), toString());
151 }
152
153
154
155 @Override
156 public double[] toRawElements(final TimeStampedAngularCoordinates angularCoordinates,
157 final RotationOrder order) {
158 throw new OrekitException(OrekitMessages.CCSDS_UNSUPPORTED_ELEMENT_SET_TYPE, name(), toString());
159 }
160
161 },
162
163
164 NONE("no rates") {
165
166 @Override
167 public TimeStampedAngularCoordinates toAngular(final AbsoluteDate date,
168 final RotationOrder order,
169 final Rotation rotation,
170 final int first,
171 final double[] elements) {
172 return new TimeStampedAngularCoordinates(date, new AngularCoordinates(rotation));
173 }
174
175
176 @Override
177 public double[] toRawElements(final TimeStampedAngularCoordinates angularCoordinates,
178 final RotationOrder order) {
179 return new double[0];
180 }
181
182 };
183
184
185
186
187 private final String description;
188
189
190 private final List<Unit> units;
191
192
193
194
195
196 RateElementsType(final String description, final String... unitsSpecifications) {
197 this.description = description;
198 this.units = Stream.of(unitsSpecifications).
199 map(Unit::parse).
200 collect(Collectors.toList());
201 }
202
203
204
205
206 public List<Unit> getUnits() {
207 return units;
208 }
209
210
211
212
213
214
215
216
217
218 public abstract TimeStampedAngularCoordinates toAngular(AbsoluteDate date,
219 RotationOrder order,
220 Rotation rotation,
221 int first,
222 double[] elements);
223
224
225
226
227
228
229
230 public abstract double[] toRawElements(TimeStampedAngularCoordinates angularCoordinates, RotationOrder order);
231
232
233 @Override
234 public String toString() {
235 return description;
236 }
237
238 }