1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.files.ccsds.ndm.adm;
18
19 import java.util.Arrays;
20 import java.util.Collections;
21 import java.util.HashMap;
22 import java.util.Map;
23
24 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
25 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
26 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.Rotation;
29 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
30 import org.hipparchus.geometry.euclidean.threed.RotationOrder;
31 import org.hipparchus.geometry.euclidean.threed.Vector3D;
32 import org.hipparchus.util.FastMath;
33 import org.hipparchus.util.MathUtils;
34 import org.hipparchus.util.SinCos;
35 import org.orekit.attitudes.Attitude;
36 import org.orekit.errors.OrekitException;
37 import org.orekit.errors.OrekitMessages;
38 import org.orekit.files.ccsds.definitions.Units;
39 import org.orekit.files.ccsds.utils.ContextBinding;
40 import org.orekit.time.AbsoluteDate;
41 import org.orekit.utils.AccurateFormatter;
42 import org.orekit.utils.AngularDerivativesFilter;
43 import org.orekit.utils.TimeStampedAngularCoordinates;
44 import org.orekit.utils.units.Unit;
45
46
47
48
49
50 public enum AttitudeType {
51
52
53 QUATERNION(Collections.singleton(new VersionedName(1.0, "QUATERNION")),
54 AngularDerivativesFilter.USE_R,
55 Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE) {
56
57
58 @Override
59 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
60 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
61 final TimeStampedAngularCoordinates coordinates) {
62
63
64 final int[] quaternionIndex = isFirst ? new int[] {0, 1, 2, 3} : new int[] {3, 0, 1, 2};
65
66 Rotation rotation = coordinates.getRotation();
67 if (!isExternal2SpacecraftBody) {
68 rotation = rotation.revert();
69 }
70
71
72 final double[] data = new double[4];
73 data[quaternionIndex[0]] = rotation.getQ0();
74 data[quaternionIndex[1]] = rotation.getQ1();
75 data[quaternionIndex[2]] = rotation.getQ2();
76 data[quaternionIndex[3]] = rotation.getQ3();
77
78 return data;
79
80 }
81
82
83 @Override
84 public TimeStampedAngularCoordinates build(final boolean isFirst,
85 final boolean isExternal2SpacecraftBody,
86 final RotationOrder eulerRotSequence,
87 final boolean isSpacecraftBodyRate,
88 final AbsoluteDate date,
89 final double... components) {
90
91 Rotation rotation = isFirst ?
92 new Rotation(components[0], components[1], components[2], components[3], true) :
93 new Rotation(components[3], components[0], components[1], components[2], true);
94 if (!isExternal2SpacecraftBody) {
95 rotation = rotation.revert();
96 }
97
98
99 return new TimeStampedAngularCoordinates(date, rotation, Vector3D.ZERO, Vector3D.ZERO);
100
101 }
102
103 },
104
105
106 QUATERNION_DERIVATIVE(Collections.singleton(new VersionedName(1.0, "QUATERNION/DERIVATIVE")),
107 AngularDerivativesFilter.USE_RR,
108 Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE,
109 Units.ONE_PER_S, Units.ONE_PER_S, Units.ONE_PER_S, Units.ONE_PER_S) {
110
111
112 @Override
113 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
114 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
115 final TimeStampedAngularCoordinates coordinates) {
116
117 FieldRotation<UnivariateDerivative1> rotation = coordinates.toUnivariateDerivative1Rotation();
118 if (!isExternal2SpacecraftBody) {
119 rotation = rotation.revert();
120 }
121
122
123 final int[] quaternionIndex = isFirst ?
124 new int[] {0, 1, 2, 3, 4, 5, 6, 7} :
125 new int[] {3, 0, 1, 2, 7, 4, 5, 6};
126
127
128 final double[] data = new double[8];
129 data[quaternionIndex[0]] = rotation.getQ0().getValue();
130 data[quaternionIndex[1]] = rotation.getQ1().getValue();
131 data[quaternionIndex[2]] = rotation.getQ2().getValue();
132 data[quaternionIndex[3]] = rotation.getQ3().getValue();
133 data[quaternionIndex[4]] = rotation.getQ0().getFirstDerivative();
134 data[quaternionIndex[5]] = rotation.getQ1().getFirstDerivative();
135 data[quaternionIndex[6]] = rotation.getQ2().getFirstDerivative();
136 data[quaternionIndex[7]] = rotation.getQ3().getFirstDerivative();
137
138 return data;
139
140 }
141
142
143 @Override
144 public TimeStampedAngularCoordinates build(final boolean isFirst,
145 final boolean isExternal2SpacecraftBody,
146 final RotationOrder eulerRotSequence,
147 final boolean isSpacecraftBodyRate,
148 final AbsoluteDate date,
149 final double... components) {
150 FieldRotation<UnivariateDerivative1> rotation =
151 isFirst ?
152 new FieldRotation<>(new UnivariateDerivative1(components[0], components[4]),
153 new UnivariateDerivative1(components[1], components[5]),
154 new UnivariateDerivative1(components[2], components[6]),
155 new UnivariateDerivative1(components[3], components[7]),
156 true) :
157 new FieldRotation<>(new UnivariateDerivative1(components[3], components[7]),
158 new UnivariateDerivative1(components[0], components[4]),
159 new UnivariateDerivative1(components[1], components[5]),
160 new UnivariateDerivative1(components[2], components[6]),
161 true);
162 if (!isExternal2SpacecraftBody) {
163 rotation = rotation.revert();
164 }
165
166 return new TimeStampedAngularCoordinates(date, rotation);
167
168 }
169
170 },
171
172
173 QUATERNION_EULER_RATES(Collections.singleton(new VersionedName(1.0, "QUATERNION/RATE")),
174 AngularDerivativesFilter.USE_RR,
175 Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE,
176 Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {
177
178
179 @Override
180 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
181 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
182 final TimeStampedAngularCoordinates coordinates) {
183
184
185 final int[] quaternionIndex = isFirst ? new int[] {0, 1, 2, 3} : new int[] {3, 0, 1, 2};
186
187
188 FieldRotation<UnivariateDerivative1> rotation = coordinates.toUnivariateDerivative1Rotation();
189 if (!isExternal2SpacecraftBody) {
190 rotation = rotation.revert();
191 }
192 final UnivariateDerivative1[] euler = rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);
193
194
195 final double[] data = new double[7];
196 data[quaternionIndex[0]] = rotation.getQ0().getValue();
197 data[quaternionIndex[1]] = rotation.getQ1().getValue();
198 data[quaternionIndex[2]] = rotation.getQ2().getValue();
199 data[quaternionIndex[3]] = rotation.getQ3().getValue();
200 data[4] = euler[0].getFirstDerivative();
201 data[5] = euler[1].getFirstDerivative();
202 data[6] = euler[2].getFirstDerivative();
203
204 return data;
205
206 }
207
208
209 @Override
210 public TimeStampedAngularCoordinates build(final boolean isFirst,
211 final boolean isExternal2SpacecraftBody,
212 final RotationOrder eulerRotSequence,
213 final boolean isSpacecraftBodyRate,
214 final AbsoluteDate date,
215 final double... components) {
216
217 final Rotation rotation = isFirst ?
218 new Rotation(components[0], components[1], components[2], components[3], true) :
219 new Rotation(components[3], components[0], components[1], components[2], true);
220 final double[] euler = rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);
221 final FieldRotation<UnivariateDerivative1> rUD1 =
222 new FieldRotation<>(eulerRotSequence, RotationConvention.FRAME_TRANSFORM,
223 new UnivariateDerivative1(euler[0], components[4]),
224 new UnivariateDerivative1(euler[1], components[5]),
225 new UnivariateDerivative1(euler[2], components[6]));
226
227
228 final TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(date, rUD1);
229 return isExternal2SpacecraftBody ? ac : ac.revert();
230
231 }
232
233 },
234
235
236 QUATERNION_ANGVEL(Collections.singleton(new VersionedName(2.0, "QUATERNION/ANGVEL")),
237 AngularDerivativesFilter.USE_RR,
238 Unit.ONE, Unit.ONE, Unit.ONE, Unit.ONE,
239 Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {
240
241
242 @Override
243 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
244 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
245 final TimeStampedAngularCoordinates coordinates) {
246
247
248 final int[] quaternionIndex = isFirst ? new int[] {0, 1, 2, 3} : new int[] {3, 0, 1, 2};
249
250
251 final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
252 final Vector3D rotationRate = QUATERNION_ANGVEL.metadataRate(isSpacecraftBodyRate, c.getRotationRate(), c.getRotation());
253
254
255 final double[] data = new double[7];
256 data[quaternionIndex[0]] = c.getRotation().getQ0();
257 data[quaternionIndex[1]] = c.getRotation().getQ1();
258 data[quaternionIndex[2]] = c.getRotation().getQ2();
259 data[quaternionIndex[3]] = c.getRotation().getQ3();
260 data[4] = rotationRate.getX();
261 data[5] = rotationRate.getY();
262 data[6] = rotationRate.getZ();
263
264 return data;
265
266 }
267
268
269 @Override
270 public TimeStampedAngularCoordinates build(final boolean isFirst,
271 final boolean isExternal2SpacecraftBody,
272 final RotationOrder eulerRotSequence,
273 final boolean isSpacecraftBodyRate,
274 final AbsoluteDate date,
275 final double... components) {
276
277 final Rotation rotation = isFirst ?
278 new Rotation(components[0], components[1], components[2], components[3], true) :
279 new Rotation(components[3], components[0], components[1], components[2], true);
280 final Vector3D rotationRate = QUATERNION_ANGVEL.orekitRate(isSpacecraftBodyRate,
281 new Vector3D(components[4], components[5], components[6]),
282 rotation);
283
284
285 final TimeStampedAngularCoordinates ac =
286 new TimeStampedAngularCoordinates(date, rotation, rotationRate, Vector3D.ZERO);
287 return isExternal2SpacecraftBody ? ac : ac.revert();
288
289 }
290
291 },
292
293
294 EULER_ANGLE(Collections.singleton(new VersionedName(1.0, "EULER_ANGLE")),
295 AngularDerivativesFilter.USE_R,
296 Unit.DEGREE, Unit.DEGREE, Unit.DEGREE) {
297
298
299 @Override
300 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
301 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
302 final TimeStampedAngularCoordinates coordinates) {
303
304
305 Rotation rotation = coordinates.getRotation();
306 if (!isExternal2SpacecraftBody) {
307 rotation = rotation.revert();
308 }
309
310 return rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);
311
312 }
313
314
315 @Override
316 public TimeStampedAngularCoordinates build(final boolean isFirst,
317 final boolean isExternal2SpacecraftBody,
318 final RotationOrder eulerRotSequence,
319 final boolean isSpacecraftBodyRate,
320 final AbsoluteDate date,
321 final double... components) {
322
323
324 Rotation rotation = new Rotation(eulerRotSequence, RotationConvention.FRAME_TRANSFORM,
325 components[0], components[1], components[2]);
326 if (!isExternal2SpacecraftBody) {
327 rotation = rotation.revert();
328 }
329
330
331 return new TimeStampedAngularCoordinates(date, rotation, Vector3D.ZERO, Vector3D.ZERO);
332 }
333
334 },
335
336
337 EULER_ANGLE_DERIVATIVE(Arrays.asList(new VersionedName(1.0, "EULER_ANGLE/RATE"),
338 new VersionedName(2.0, "EULER_ANGLE/DERIVATIVE")),
339 AngularDerivativesFilter.USE_RR,
340 Unit.DEGREE, Unit.DEGREE, Unit.DEGREE,
341 Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {
342
343
344 @Override
345 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
346 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
347 final TimeStampedAngularCoordinates coordinates) {
348
349
350 FieldRotation<UnivariateDerivative1> rotation = coordinates.toUnivariateDerivative1Rotation();
351 if (!isExternal2SpacecraftBody) {
352 rotation = rotation.revert();
353 }
354
355 final UnivariateDerivative1[] angles = rotation.getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);
356
357 return new double[] {
358 angles[0].getValue(),
359 angles[1].getValue(),
360 angles[2].getValue(),
361 angles[0].getFirstDerivative(),
362 angles[1].getFirstDerivative(),
363 angles[2].getFirstDerivative()
364 };
365
366 }
367
368
369 @Override
370 public TimeStampedAngularCoordinates build(final boolean isFirst,
371 final boolean isExternal2SpacecraftBody,
372 final RotationOrder eulerRotSequence,
373 final boolean isSpacecraftBodyRate,
374 final AbsoluteDate date,
375 final double... components) {
376
377
378 FieldRotation<UnivariateDerivative1> rotation =
379 new FieldRotation<>(eulerRotSequence, RotationConvention.FRAME_TRANSFORM,
380 new UnivariateDerivative1(components[0], components[3]),
381 new UnivariateDerivative1(components[1], components[4]),
382 new UnivariateDerivative1(components[2], components[5]));
383 if (!isExternal2SpacecraftBody) {
384 rotation = rotation.revert();
385 }
386
387 return new TimeStampedAngularCoordinates(date, rotation);
388
389 }
390
391 },
392
393
394
395
396 EULER_ANGLE_ANGVEL(Collections.singleton(new VersionedName(2.0, "EULER_ANGLE/ANGVEL")),
397 AngularDerivativesFilter.USE_RR,
398 Unit.DEGREE, Unit.DEGREE, Unit.DEGREE,
399 Units.DEG_PER_S, Units.DEG_PER_S, Units.DEG_PER_S) {
400
401
402 @Override
403 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
404 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
405 final TimeStampedAngularCoordinates coordinates) {
406
407
408 final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
409 final Vector3D rotationRate = EULER_ANGLE_ANGVEL.metadataRate(isSpacecraftBodyRate, c.getRotationRate(), c.getRotation());
410 final double[] angles = c.getRotation().getAngles(eulerRotSequence, RotationConvention.FRAME_TRANSFORM);
411
412 return new double[] {
413 angles[0],
414 angles[1],
415 angles[2],
416 rotationRate.getX(),
417 rotationRate.getY(),
418 rotationRate.getZ()
419 };
420
421 }
422
423
424 @Override
425 public TimeStampedAngularCoordinates build(final boolean isFirst,
426 final boolean isExternal2SpacecraftBody,
427 final RotationOrder eulerRotSequence,
428 final boolean isSpacecraftBodyRate,
429 final AbsoluteDate date,
430 final double... components) {
431
432
433 final Rotation rotation = new Rotation(eulerRotSequence,
434 RotationConvention.FRAME_TRANSFORM,
435 components[0],
436 components[1],
437 components[2]);
438 final Vector3D rotationRate = EULER_ANGLE_ANGVEL.orekitRate(isSpacecraftBodyRate,
439 new Vector3D(components[3], components[4], components[5]),
440 rotation);
441
442 final TimeStampedAngularCoordinates ac =
443 new TimeStampedAngularCoordinates(date, rotation, rotationRate, Vector3D.ZERO);
444 return isExternal2SpacecraftBody ? ac : ac.revert();
445
446 }
447
448 },
449
450
451 SPIN(Collections.singleton(new VersionedName(1.0, "SPIN")),
452 AngularDerivativesFilter.USE_R,
453 Unit.DEGREE, Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S) {
454
455
456 @Override
457 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
458 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
459 final TimeStampedAngularCoordinates coordinates) {
460
461
462 final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
463 final SpinFinder sf = new SpinFinder(c);
464 final double spinAngleVel = coordinates.getRotationRate().getZ();
465
466 return new double[] {
467 sf.getSpinAlpha(), sf.getSpinDelta(), sf.getSpinAngle(), spinAngleVel
468 };
469
470 }
471
472
473 @Override
474 public TimeStampedAngularCoordinates build(final boolean isFirst,
475 final boolean isExternal2SpacecraftBody,
476 final RotationOrder eulerRotSequence,
477 final boolean isSpacecraftBodyRate,
478 final AbsoluteDate date,
479 final double... components) {
480
481
482 final Rotation rotation = new Rotation(RotationOrder.ZXZ,
483 RotationConvention.FRAME_TRANSFORM,
484 MathUtils.SEMI_PI + components[0],
485 MathUtils.SEMI_PI - components[1],
486 components[2]);
487 final Vector3D rotationRate = new Vector3D(0, 0, components[3]);
488
489
490 final TimeStampedAngularCoordinates ac =
491 new TimeStampedAngularCoordinates(date, rotation, rotationRate, Vector3D.ZERO);
492 return isExternal2SpacecraftBody ? ac : ac.revert();
493
494 }
495
496 },
497
498
499 SPIN_NUTATION(Collections.singleton(new VersionedName(1.0, "SPIN/NUTATION")),
500 AngularDerivativesFilter.USE_RR,
501 Unit.DEGREE, Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S,
502 Unit.DEGREE, Unit.SECOND, Unit.DEGREE) {
503
504
505 @Override
506 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
507 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
508 final TimeStampedAngularCoordinates coordinates) {
509
510
511 final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
512 final SpinFinder sf = new SpinFinder(c);
513
514
515 final FieldRotation<UnivariateDerivative2> c2 = c.toUnivariateDerivative2Rotation();
516 final FieldVector3D<UnivariateDerivative2> spinAxis = c2.applyInverseTo(Vector3D.PLUS_K);
517 final PrecessionFinder pf = new PrecessionFinder(spinAxis);
518
519
520 final Rotation intermediate2Inert = new Rotation(Vector3D.PLUS_K, pf.getAxis());
521
522
523 final FieldRotation<UnivariateDerivative2> intermediate2Body = c2.applyTo(intermediate2Inert);
524 final UnivariateDerivative2[] euler = intermediate2Body.
525 getAngles(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM);
526
527 return new double[] {
528 sf.getSpinAlpha(),
529 sf.getSpinDelta(),
530 sf.getSpinAngle(),
531 euler[2].getFirstDerivative(),
532 pf.getPrecessionAngle(),
533 MathUtils.TWO_PI / pf.getAngularVelocity(),
534 euler[2].getValue() - MathUtils.SEMI_PI
535 };
536
537 }
538
539
540 @Override
541 public TimeStampedAngularCoordinates build(final boolean isFirst,
542 final boolean isExternal2SpacecraftBody,
543 final RotationOrder eulerRotSequence,
544 final boolean isSpacecraftBodyRate,
545 final AbsoluteDate date,
546 final double... components) {
547
548
549 final Rotation inert2Body0 = new Rotation(RotationOrder.ZXZ,
550 RotationConvention.FRAME_TRANSFORM,
551 MathUtils.SEMI_PI + components[0],
552 MathUtils.SEMI_PI - components[1],
553 components[2]);
554
555
556 final SinCos scNutation = FastMath.sinCos(components[4]);
557 final SinCos scPhase = FastMath.sinCos(components[6]);
558 final Vector3D momentumBody = new Vector3D( scNutation.sin() * scPhase.cos(),
559 -scNutation.sin() * scPhase.sin(),
560 scNutation.cos());
561 final Vector3D momentumInert = inert2Body0.applyInverseTo(momentumBody);
562 final Rotation inert2Intermediate = new Rotation(momentumInert, Vector3D.PLUS_K);
563
564
565 final Rotation intermediate2Body0 = inert2Body0.applyTo(inert2Intermediate.revert());
566 final double[] euler0 = intermediate2Body0.getAngles(RotationOrder.ZXZ,
567 RotationConvention.FRAME_TRANSFORM);
568
569
570 final FieldRotation<UnivariateDerivative2> intermediate2Body =
571 new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
572 new UnivariateDerivative2(euler0[0], MathUtils.TWO_PI / components[5], 0.0),
573 new UnivariateDerivative2(euler0[1], 0.0, 0.0),
574 new UnivariateDerivative2(euler0[2], components[3], 0.0));
575
576
577 final FieldRotation<UnivariateDerivative2> inert2Body = intermediate2Body.applyTo(inert2Intermediate);
578
579 final TimeStampedAngularCoordinates ac =
580 new TimeStampedAngularCoordinates(date, inert2Body);
581 return isExternal2SpacecraftBody ? ac : ac.revert();
582
583 }
584
585 },
586
587
588
589
590 SPIN_NUTATION_MOMENTUM(Collections.singleton(new VersionedName(2.0, "SPIN/NUTATION_MOM")),
591 AngularDerivativesFilter.USE_RR,
592 Unit.DEGREE, Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S,
593 Unit.DEGREE, Unit.DEGREE, Units.DEG_PER_S) {
594
595
596 @Override
597 public double[] generateData(final boolean isFirst, final boolean isExternal2SpacecraftBody,
598 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
599 final TimeStampedAngularCoordinates coordinates) {
600
601
602 final TimeStampedAngularCoordinates c = isExternal2SpacecraftBody ? coordinates : coordinates.revert();
603 final SpinFinder sf = new SpinFinder(c);
604
605
606 final FieldRotation<UnivariateDerivative2> c2 = c.toUnivariateDerivative2Rotation();
607 final FieldVector3D<UnivariateDerivative2> spinAxis = c2.applyInverseTo(Vector3D.PLUS_K);
608 final PrecessionFinder pf = new PrecessionFinder(spinAxis);
609
610
611 final Rotation intermediate2Inert = new Rotation(Vector3D.PLUS_K, pf.getAxis());
612
613
614 final FieldRotation<UnivariateDerivative2> intermediate2Body = c2.applyTo(intermediate2Inert);
615 final double spinAngleVel = intermediate2Body.
616 getAngles(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM)[2].
617 getFirstDerivative();
618
619 return new double[] {
620 sf.getSpinAlpha(),
621 sf.getSpinDelta(),
622 sf.getSpinAngle(),
623 spinAngleVel,
624 pf.getAxis().getAlpha(),
625 pf.getAxis().getDelta(),
626 pf.getAngularVelocity()
627 };
628
629 }
630
631
632 @Override
633 public TimeStampedAngularCoordinates build(final boolean isFirst,
634 final boolean isExternal2SpacecraftBody,
635 final RotationOrder eulerRotSequence,
636 final boolean isSpacecraftBodyRate,
637 final AbsoluteDate date,
638 final double... components) {
639
640
641 final SinCos scAlpha = FastMath.sinCos(components[4]);
642 final SinCos scDelta = FastMath.sinCos(components[5]);
643 final Vector3D momentumInert = new Vector3D(scAlpha.cos() * scDelta.cos(),
644 scAlpha.sin() * scDelta.cos(),
645 scDelta.sin());
646 final Rotation inert2Intermediate = new Rotation(momentumInert, Vector3D.PLUS_K);
647
648
649 final Rotation inert2Body0 = new Rotation(RotationOrder.ZXZ,
650 RotationConvention.FRAME_TRANSFORM,
651 MathUtils.SEMI_PI + components[0],
652 MathUtils.SEMI_PI - components[1],
653 components[2]);
654 final Rotation intermediate2Body0 = inert2Body0.applyTo(inert2Intermediate.revert());
655 final double[] euler0 = intermediate2Body0.getAngles(RotationOrder.ZXZ,
656 RotationConvention.FRAME_TRANSFORM);
657
658
659 final FieldRotation<UnivariateDerivative2> intermediate2Body =
660 new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
661 new UnivariateDerivative2(euler0[0], components[6], 0.0),
662 new UnivariateDerivative2(euler0[1], 0.0, 0.0),
663 new UnivariateDerivative2(euler0[2], components[3], 0.0));
664
665
666 final FieldRotation<UnivariateDerivative2> inert2Body = intermediate2Body.applyTo(inert2Intermediate);
667
668
669 final TimeStampedAngularCoordinates ac =
670 new TimeStampedAngularCoordinates(date, inert2Body);
671 return isExternal2SpacecraftBody ? ac : ac.revert();
672
673 }
674
675 };
676
677
678
679
680 private static final Map<String, AttitudeType> MAP = new HashMap<>();
681 static {
682 for (final AttitudeType type : values()) {
683 for (final VersionedName vn : type.ccsdsNames) {
684 MAP.put(vn.name, type);
685 }
686 }
687 }
688
689
690 private final Iterable<VersionedName> ccsdsNames;
691
692
693 private final AngularDerivativesFilter filter;
694
695
696 private final Unit[] units;
697
698
699
700
701
702
703 AttitudeType(final Iterable<VersionedName> ccsdsNames, final AngularDerivativesFilter filter, final Unit... units) {
704 this.ccsdsNames = ccsdsNames;
705 this.filter = filter;
706 this.units = units.clone();
707 }
708
709
710
711
712
713
714 public String getName(final double formatVersion) {
715 String name = null;
716 for (final VersionedName vn : ccsdsNames) {
717 if (name == null || formatVersion >= vn.since) {
718 name = vn.name;
719 }
720 }
721 return name;
722 }
723
724
725 @Override
726 public String toString() {
727
728 return getName(Double.POSITIVE_INFINITY);
729 }
730
731
732
733
734
735 public static AttitudeType parseType(final String typeSpecification) {
736 final AttitudeType type = MAP.get(typeSpecification);
737 if (type == null) {
738 throw new OrekitException(OrekitMessages.CCSDS_UNKNOWN_ATTITUDE_TYPE, typeSpecification);
739 }
740 return type;
741 }
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756 public String[] createDataFields(final boolean isFirst, final boolean isExternal2SpacecraftBody,
757 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
758 final TimeStampedAngularCoordinates attitude) {
759
760
761 final double[] data = generateData(isFirst, isExternal2SpacecraftBody,
762 eulerRotSequence, isSpacecraftBodyRate, attitude);
763
764
765 final String[] fields = new String[data.length];
766 for (int i = 0; i < data.length; ++i) {
767 fields[i] = AccurateFormatter.format(units[i].fromSI(data[i]));
768 }
769
770 return fields;
771
772 }
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788 public abstract double[] generateData(boolean isFirst, boolean isExternal2SpacecraftBody,
789 RotationOrder eulerRotSequence, boolean isSpacecraftBodyRate,
790 TimeStampedAngularCoordinates attitude);
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806 public TimeStampedAngularCoordinates parse(final boolean isFirst, final boolean isExternal2SpacecraftBody,
807 final RotationOrder eulerRotSequence, final boolean isSpacecraftBodyRate,
808 final ContextBinding context, final String[] fields) {
809
810
811 final AbsoluteDate date = context.getTimeSystem().getConverter(context).parse(fields[0]);
812 final double[] components = new double[fields.length - 1];
813 for (int i = 0; i < components.length; ++i) {
814 components[i] = units[i].toSI(Double.parseDouble(fields[i + 1]));
815 }
816
817
818 return build(isFirst, isExternal2SpacecraftBody, eulerRotSequence, isSpacecraftBodyRate,
819 date, components);
820
821 }
822
823
824
825
826
827
828
829
830
831
832
833 public abstract TimeStampedAngularCoordinates build(boolean isFirst, boolean isExternal2SpacecraftBody,
834 RotationOrder eulerRotSequence, boolean isSpacecraftBodyRate,
835 AbsoluteDate date, double... components);
836
837
838
839
840
841 public AngularDerivativesFilter getAngularDerivativesFilter() {
842 return filter;
843 }
844
845
846
847
848
849
850
851 private Vector3D metadataRate(final boolean isSpacecraftBodyRate, final Vector3D rate, final Rotation rotation) {
852 return isSpacecraftBodyRate ? rate : rotation.applyInverseTo(rate);
853 }
854
855
856
857
858
859
860
861 private Vector3D orekitRate(final boolean isSpacecraftBodyRate, final Vector3D rate, final Rotation rotation) {
862 return isSpacecraftBodyRate ? rate : rotation.applyTo(rate);
863 }
864
865
866
867
868 private static class VersionedName {
869
870
871 private final double since;
872
873
874 private final String name;
875
876
877
878
879
880 VersionedName(final double since, final String name) {
881 this.since = since;
882 this.name = name;
883 }
884
885 }
886
887 }