1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.attitudes;
18
19 import java.util.HashMap;
20 import java.util.Map;
21
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.Field;
24 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Rotation;
27 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
28 import org.hipparchus.geometry.euclidean.threed.RotationOrder;
29 import org.hipparchus.geometry.euclidean.threed.Vector3D;
30 import org.hipparchus.ode.DenseOutputModel;
31 import org.hipparchus.ode.FieldDenseOutputModel;
32 import org.hipparchus.ode.FieldExpandableODE;
33 import org.hipparchus.ode.FieldODEState;
34 import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
35 import org.hipparchus.ode.ODEState;
36 import org.hipparchus.ode.OrdinaryDifferentialEquation;
37 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
38 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
39 import org.hipparchus.special.elliptic.jacobi.CopolarN;
40 import org.hipparchus.special.elliptic.jacobi.FieldCopolarN;
41 import org.hipparchus.special.elliptic.jacobi.FieldJacobiElliptic;
42 import org.hipparchus.special.elliptic.jacobi.JacobiElliptic;
43 import org.hipparchus.special.elliptic.jacobi.JacobiEllipticBuilder;
44 import org.hipparchus.special.elliptic.legendre.LegendreEllipticIntegral;
45 import org.hipparchus.util.FastMath;
46 import org.hipparchus.util.MathArrays;
47 import org.orekit.frames.Frame;
48 import org.orekit.time.AbsoluteDate;
49 import org.orekit.time.FieldAbsoluteDate;
50 import org.orekit.utils.FieldPVCoordinatesProvider;
51 import org.orekit.utils.PVCoordinatesProvider;
52 import org.orekit.utils.TimeStampedAngularCoordinates;
53 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86 public class TorqueFree implements AttitudeProvider {
87
88
89 private final Attitude initialAttitude;
90
91
92 private final Inertia inertia;
93
94
95 private final DoubleModel doubleModel;
96
97
98 private final transient Map<Field<? extends CalculusFieldElement<?>>, FieldModel<? extends CalculusFieldElement<?>>> cachedModels;
99
100
101
102
103
104 public TorqueFree(final Attitude initialAttitude, final Inertia inertia) {
105
106 this.initialAttitude = initialAttitude;
107 this.inertia = inertia;
108
109
110 this.doubleModel = new DoubleModel();
111
112
113 this.cachedModels = new HashMap<>();
114
115 }
116
117
118
119
120 public Attitude getInitialAttitude() {
121 return initialAttitude;
122 }
123
124
125
126
127 public Inertia getInertia() {
128 return inertia;
129 }
130
131
132 public Attitude getAttitude(final PVCoordinatesProvider pvProv,
133 final AbsoluteDate date, final Frame frame) {
134
135
136 final Attitude attitude =
137 new Attitude(initialAttitude.getReferenceFrame(), doubleModel.evaluate(date));
138
139
140 return attitude.withReferenceFrame(frame);
141
142 }
143
144
145 public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
146 final FieldAbsoluteDate<T> date,
147 final Frame frame) {
148
149
150 @SuppressWarnings("unchecked")
151 FieldModel<T> fm = (FieldModel<T>) cachedModels.get(date.getField());
152 if (fm == null) {
153
154 fm = new FieldModel<>(date.getField());
155 cachedModels.put(date.getField(), fm);
156 }
157
158
159 final FieldAttitude<T> attitude =
160 new FieldAttitude<>(initialAttitude.getReferenceFrame(), fm.evaluate(date));
161
162
163 return attitude.withReferenceFrame(frame);
164
165 }
166
167
168 private class DoubleModel {
169
170
171 private final Inertia sortedInertia;
172
173
174 private final double o1Scale;
175
176
177 private final double o2Scale;
178
179
180 private final double o3Scale;
181
182
183 private final JacobiElliptic jacobi;
184
185
186 private final double tScale;
187
188
189 private final AbsoluteDate tRef;
190
191
192 private final Rotation inertToAligned;
193
194
195 private final Rotation sortedToBody;
196
197
198 private final double period;
199
200
201 private final double phiSlope;
202
203
204 private final DenseOutputModel phiQuadratureModel;
205
206
207 private final double integOnePeriod;
208
209
210
211 DoubleModel() {
212
213
214 final double i1 = inertia.getInertiaAxis1().getI();
215 final Vector3D a1 = inertia.getInertiaAxis1().getA();
216 final double i2 = inertia.getInertiaAxis2().getI();
217 final Vector3D a2 = inertia.getInertiaAxis2().getA();
218 final double i3 = inertia.getInertiaAxis3().getI();
219 final Vector3D a3 = inertia.getInertiaAxis3().getA();
220 final Vector3D n1 = a1.normalize();
221 final Vector3D n2 = a2.normalize();
222 final Vector3D n3 = Vector3D.dotProduct(Vector3D.crossProduct(a1, a2), a3) > 0 ?
223 a3.normalize() : a3.normalize().negate();
224
225 final Vector3D omega0 = initialAttitude.getSpin();
226 final Vector3D m0 = new Vector3D(i1 * Vector3D.dotProduct(omega0, n1), n1,
227 i2 * Vector3D.dotProduct(omega0, n2), n2,
228 i3 * Vector3D.dotProduct(omega0, n3), n3);
229
230
231 Inertia tmpInertia = new Inertia(new InertiaAxis(i1, n1), new InertiaAxis(i2, n2), new InertiaAxis(i3, n3));
232 if (tmpInertia.getInertiaAxis1().getI() > tmpInertia.getInertiaAxis2().getI()) {
233 tmpInertia = tmpInertia.swap12();
234 }
235 if (tmpInertia.getInertiaAxis2().getI() > tmpInertia.getInertiaAxis3().getI()) {
236 tmpInertia = tmpInertia.swap23();
237 }
238 if (tmpInertia.getInertiaAxis1().getI() > tmpInertia.getInertiaAxis2().getI()) {
239 tmpInertia = tmpInertia.swap12();
240 }
241
242
243
244 final double o1 = Vector3D.dotProduct(omega0, n1);
245 final double o2 = Vector3D.dotProduct(omega0, n2);
246 final double o3 = Vector3D.dotProduct(omega0, n3);
247 final double o12 = o1 * o1;
248 final double o22 = o2 * o2;
249 final double o32 = o3 * o3;
250 final double twoE = i1 * o12 + i2 * o22 + i3 * o32;
251 final double m2 = i1 * i1 * o12 + i2 * i2 * o22 + i3 * i3 * o32;
252 final double separatrixInertia = (twoE == 0) ? 0.0 : m2 / twoE;
253 final boolean clockwise;
254 if (separatrixInertia < tmpInertia.getInertiaAxis2().getI()) {
255
256
257
258 clockwise = true;
259 tmpInertia = tmpInertia.swap13();
260 } else {
261
262
263
264 clockwise = false;
265 }
266 sortedInertia = tmpInertia;
267
268 final double i1C = tmpInertia.getInertiaAxis1().getI();
269 final double i2C = tmpInertia.getInertiaAxis2().getI();
270 final double i3C = tmpInertia.getInertiaAxis3().getI();
271 final double i32 = i3C - i2C;
272 final double i31 = i3C - i1C;
273 final double i21 = i2C - i1C;
274
275
276 sortedToBody = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J,
277 tmpInertia.getInertiaAxis1().getA(), tmpInertia.getInertiaAxis2().getA());
278 final Vector3D omega0Sorted = sortedToBody.applyInverseTo(omega0);
279 final Vector3D m0Sorted = sortedToBody.applyInverseTo(m0);
280 final double phi0 = 0;
281 final double theta0 = FastMath.acos(m0Sorted.getZ() / m0Sorted.getNorm());
282 final double psi0 = FastMath.atan2(m0Sorted.getX(), m0Sorted.getY());
283
284
285 final Rotation alignedToSorted0 = new Rotation(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
286 phi0, theta0, psi0);
287 inertToAligned = alignedToSorted0.
288 applyInverseTo(sortedToBody.applyInverseTo(initialAttitude.getRotation()));
289
290
291 tScale = FastMath.copySign(FastMath.sqrt(i32 * (m2 - twoE * i1C) / (i1C * i2C * i3C)),
292 clockwise ? -omega0Sorted.getZ() : omega0Sorted.getZ());
293 o1Scale = FastMath.sqrt((twoE * i3C - m2) / (i1C * i31));
294 o2Scale = FastMath.sqrt((twoE * i3C - m2) / (i2C * i32));
295 o3Scale = FastMath.copySign(FastMath.sqrt((m2 - twoE * i1C) / (i3C * i31)), omega0Sorted.getZ());
296
297 final double k2 = (twoE == 0) ? 0.0 : i21 * (twoE * i3C - m2) / (i32 * (m2 - twoE * i1C));
298 jacobi = JacobiEllipticBuilder.build(k2);
299 period = 4 * LegendreEllipticIntegral.bigK(k2) / tScale;
300
301 final double dtRef;
302 if (o1Scale == 0) {
303
304
305 dtRef = 0;
306 } else {
307 if (FastMath.abs(omega0Sorted.getX()) >= FastMath.abs(omega0Sorted.getY())) {
308 if (omega0Sorted.getX() >= 0) {
309
310 dtRef = -jacobi.arcsn(omega0Sorted.getY() / o2Scale) / tScale;
311 } else {
312
313 dtRef = jacobi.arcsn(omega0Sorted.getY() / o2Scale) / tScale - 0.5 * period;
314 }
315 } else {
316 if (omega0Sorted.getY() >= 0) {
317
318 dtRef = -jacobi.arccn(omega0Sorted.getX() / o1Scale) / tScale;
319 } else {
320
321 dtRef = jacobi.arccn(omega0Sorted.getX() / o1Scale) / tScale;
322 }
323 }
324 }
325 tRef = initialAttitude.getDate().shiftedBy(dtRef);
326
327 phiSlope = FastMath.sqrt(m2) / i3C;
328 phiQuadratureModel = computePhiQuadratureModel(dtRef);
329 integOnePeriod = phiQuadratureModel.getInterpolatedState(phiQuadratureModel.getFinalTime()).getPrimaryState()[0];
330
331 }
332
333
334
335
336
337 private DenseOutputModel computePhiQuadratureModel(final double dtRef) {
338
339 final double i1C = sortedInertia.getInertiaAxis1().getI();
340 final double i2C = sortedInertia.getInertiaAxis2().getI();
341 final double i3C = sortedInertia.getInertiaAxis3().getI();
342
343 final double i32 = i3C - i2C;
344 final double i31 = i3C - i1C;
345 final double i21 = i2C - i1C;
346
347
348 final double b = phiSlope * i32 * i31;
349 final double c = i1C * i32;
350 final double d = i3C * i21;
351
352
353 final DormandPrince853Integrator integ = new DormandPrince853Integrator(1.0e-6 * period, 1.0e-2 * period,
354 phiSlope * period * 1.0e-13, 1.0e-13);
355 final DenseOutputModel model = new DenseOutputModel();
356 integ.addStepHandler(model);
357
358 integ.integrate(new OrdinaryDifferentialEquation() {
359
360
361 @Override
362 public int getDimension() {
363 return 1;
364 }
365
366
367 @Override
368 public double[] computeDerivatives(final double t, final double[] y) {
369 final double sn = jacobi.valuesN((t - dtRef) * tScale).sn();
370 return new double[] {
371 b / (c + d * sn * sn)
372 };
373 }
374
375 }, new ODEState(0, new double[1]), period);
376
377 return model;
378
379 }
380
381
382
383
384
385 public TimeStampedAngularCoordinates evaluate(final AbsoluteDate date) {
386
387
388 final CopolarN valuesN = jacobi.valuesN(date.durationFrom(tRef) * tScale);
389 final Vector3D omegaSorted = new Vector3D(o1Scale * valuesN.cn(), o2Scale * valuesN.sn(), o3Scale * valuesN.dn());
390 final Vector3D omegaBody = sortedToBody.applyTo(omegaSorted);
391
392
393 final Vector3D accelerationSorted = new Vector3D(o1Scale * tScale * valuesN.cn() * valuesN.dn(),
394 o2Scale * tScale * -valuesN.sn() * valuesN.dn(),
395 o3Scale * tScale * -jacobi.getM() * valuesN.sn() * valuesN.cn());
396 final Vector3D accelerationBody = sortedToBody.applyTo(accelerationSorted);
397
398
399 final double dt = date.durationFrom(initialAttitude.getDate());
400 final double psi = FastMath.atan2(sortedInertia.getInertiaAxis1().getI() * omegaSorted.getX(),
401 sortedInertia.getInertiaAxis2().getI() * omegaSorted.getY());
402 final double theta = FastMath.acos(omegaSorted.getZ() / phiSlope);
403 final double phiLinear = phiSlope * dt;
404
405
406 final int nbPeriods = (int) FastMath.floor(dt / period);
407 final double tStartInteg = nbPeriods * period;
408 final double integPartial = phiQuadratureModel.getInterpolatedState(dt - tStartInteg).getPrimaryState()[0];
409 final double phiQuadrature = nbPeriods * integOnePeriod + integPartial;
410 final double phi = phiLinear + phiQuadrature;
411
412
413
414 final Rotation alignedToSorted = new Rotation(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
415 phi, theta, psi);
416
417
418 final Rotation inertToBody = sortedToBody.applyTo(alignedToSorted.applyTo(inertToAligned));
419
420 return new TimeStampedAngularCoordinates(date, inertToBody, omegaBody, accelerationBody);
421 }
422
423 }
424
425
426
427
428 private class FieldModel <T extends CalculusFieldElement<T>> {
429
430
431 private final FieldInertia<T> sortedInertia;
432
433
434 private final T o1Scale;
435
436
437 private final T o2Scale;
438
439
440 private final T o3Scale;
441
442
443 private final FieldJacobiElliptic<T> jacobi;
444
445
446 private final T tScale;
447
448
449 private final FieldAbsoluteDate<T> tRef;
450
451
452 private final FieldRotation<T> inertToAligned;
453
454
455 private final FieldRotation<T> sortedToBody;
456
457
458 private final T period;
459
460
461 private final T phiSlope;
462
463
464 private final FieldDenseOutputModel<T> phiQuadratureModel;
465
466
467 private final T integOnePeriod;
468
469
470
471
472 FieldModel(final Field<T> field) {
473
474 final double i1 = inertia.getInertiaAxis1().getI();
475 final Vector3D a1 = inertia.getInertiaAxis1().getA();
476 final double i2 = inertia.getInertiaAxis2().getI();
477 final Vector3D a2 = inertia.getInertiaAxis2().getA();
478 final double i3 = inertia.getInertiaAxis3().getI();
479 final Vector3D a3 = inertia.getInertiaAxis3().getA();
480
481 final T zero = field.getZero();
482 final T fI1 = zero.newInstance(i1);
483 final FieldVector3D<T> fA1 = new FieldVector3D<>(field, a1);
484 final T fI2 = zero.newInstance(i2);
485 final FieldVector3D<T> fA2 = new FieldVector3D<>(field, a2);
486 final T fI3 = zero.newInstance(i3);
487 final FieldVector3D<T> fA3 = new FieldVector3D<>(field, a3);
488
489
490 final FieldVector3D<T> n1 = fA1.normalize();
491 final FieldVector3D<T> n2 = fA2.normalize();
492 final FieldVector3D<T> n3 = Vector3D.dotProduct(Vector3D.crossProduct(a1, a2), a3) > 0 ?
493 fA3.normalize() : fA3.normalize().negate();
494
495 final FieldVector3D<T> omega0 = new FieldVector3D<>(field, initialAttitude.getSpin());
496 final FieldVector3D<T> m0 = new FieldVector3D<>(fI1.multiply(FieldVector3D.dotProduct(omega0, n1)), n1,
497 fI2.multiply(FieldVector3D.dotProduct(omega0, n2)), n2,
498 fI3.multiply(FieldVector3D.dotProduct(omega0, n3)), n3);
499
500
501 FieldInertia<T> tmpInertia = new FieldInertia<>(new FieldInertiaAxis<>(fI1, n1),
502 new FieldInertiaAxis<>(fI2, n2),
503 new FieldInertiaAxis<>(fI3, n3));
504 if (tmpInertia.getInertiaAxis1().getI().subtract(tmpInertia.getInertiaAxis2().getI()).getReal() > 0) {
505 tmpInertia = tmpInertia.swap12();
506 }
507 if (tmpInertia.getInertiaAxis2().getI().subtract(tmpInertia.getInertiaAxis3().getI()).getReal() > 0) {
508 tmpInertia = tmpInertia.swap23();
509 }
510 if (tmpInertia.getInertiaAxis1().getI().subtract(tmpInertia.getInertiaAxis2().getI()).getReal() > 0) {
511 tmpInertia = tmpInertia.swap12();
512 }
513
514
515
516 final T o1 = FieldVector3D.dotProduct(omega0, n1);
517 final T o2 = FieldVector3D.dotProduct(omega0, n2);
518 final T o3 = FieldVector3D.dotProduct(omega0, n3);
519 final T o12 = o1.square();
520 final T o22 = o2.square();
521 final T o32 = o3.square();
522 final T twoE = fI1.multiply(o12).add(fI2.multiply(o22)).add(fI3.multiply(o32));
523 final T m2 = fI1.square().multiply(o12).add(fI2.square().multiply(o22)).add(fI3.square().multiply(o32));
524 final T separatrixInertia = (twoE.isZero()) ? zero : m2.divide(twoE);
525 final boolean clockwise;
526 if (separatrixInertia.subtract(tmpInertia.getInertiaAxis2().getI()).getReal() < 0) {
527
528
529
530 clockwise = true;
531 tmpInertia = tmpInertia.swap13();
532 } else {
533
534
535
536 clockwise = false;
537 }
538 sortedInertia = tmpInertia;
539
540 final T i1C = tmpInertia.getInertiaAxis1().getI();
541 final T i2C = tmpInertia.getInertiaAxis2().getI();
542 final T i3C = tmpInertia.getInertiaAxis3().getI();
543 final T i32 = i3C.subtract(i2C);
544 final T i31 = i3C.subtract(i1C);
545 final T i21 = i2C.subtract(i1C);
546
547
548 sortedToBody = new FieldRotation<>(FieldVector3D.getPlusI(field),
549 FieldVector3D.getPlusJ(field),
550 tmpInertia.getInertiaAxis1().getA(),
551 tmpInertia.getInertiaAxis2().getA());
552 final FieldVector3D<T> omega0Sorted = sortedToBody.applyInverseTo(omega0);
553 final FieldVector3D<T> m0Sorted = sortedToBody.applyInverseTo(m0);
554 final T phi0 = zero;
555 final T theta0 = FastMath.acos(m0Sorted.getZ().divide(m0Sorted.getNorm()));
556 final T psi0 = FastMath.atan2(m0Sorted.getX(), m0Sorted.getY());
557
558
559 final FieldRotation<T> alignedToSorted0 = new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
560 phi0, theta0, psi0);
561 inertToAligned = alignedToSorted0.
562 applyInverseTo(sortedToBody.applyInverseTo(new FieldRotation<>(field, initialAttitude.getRotation())));
563
564
565 tScale = FastMath.copySign(FastMath.sqrt(i32.multiply(m2.subtract(twoE.multiply(i1C))).divide(i1C.multiply(i2C).multiply(i3C))),
566 clockwise ? omega0Sorted.getZ().negate() : omega0Sorted.getZ());
567 o1Scale = FastMath.sqrt(twoE.multiply(i3C).subtract(m2).divide(i1C.multiply(i31)));
568 o2Scale = FastMath.sqrt(twoE.multiply(i3C).subtract(m2).divide(i2C.multiply(i32)));
569 o3Scale = FastMath.copySign(FastMath.sqrt(m2.subtract(twoE.multiply(i1C)).divide(i3C.multiply(i31))),
570 omega0Sorted.getZ());
571
572 final T k2 = (twoE.isZero()) ?
573 zero :
574 i21.multiply(twoE.multiply(i3C).subtract(m2)).
575 divide(i32.multiply(m2.subtract(twoE.multiply(i1C))));
576 jacobi = JacobiEllipticBuilder.build(k2);
577 period = LegendreEllipticIntegral.bigK(k2).multiply(4).divide(tScale);
578
579 final T dtRef;
580 if (o1Scale.isZero()) {
581
582
583 dtRef = zero;
584 } else {
585 if (FastMath.abs(omega0Sorted.getX()).subtract(FastMath.abs(omega0Sorted.getY())).getReal() >= 0) {
586 if (omega0Sorted.getX().getReal() >= 0) {
587
588 dtRef = jacobi.arcsn(omega0Sorted.getY().divide(o2Scale)).divide(tScale).negate();
589 } else {
590
591 dtRef = jacobi.arcsn(omega0Sorted.getY().divide(o2Scale)).divide(tScale).subtract(period.multiply(0.5));
592 }
593 } else {
594 if (omega0Sorted.getY().getReal() >= 0) {
595
596 dtRef = jacobi.arccn(omega0Sorted.getX().divide(o1Scale)).divide(tScale).negate();
597 } else {
598
599 dtRef = jacobi.arccn(omega0Sorted.getX().divide(o1Scale)).divide(tScale);
600 }
601 }
602 }
603 tRef = new FieldAbsoluteDate<>(field, initialAttitude.getDate()).shiftedBy(dtRef);
604
605 phiSlope = FastMath.sqrt(m2).divide(i3C);
606 phiQuadratureModel = computePhiQuadratureModel(dtRef);
607 integOnePeriod = phiQuadratureModel.getInterpolatedState(phiQuadratureModel.getFinalTime()).getPrimaryState()[0];
608
609 }
610
611
612
613
614
615 private FieldDenseOutputModel<T> computePhiQuadratureModel(final T dtRef) {
616
617 final T zero = dtRef.getField().getZero();
618
619 final T i1C = sortedInertia.getInertiaAxis1().getI();
620 final T i2C = sortedInertia.getInertiaAxis2().getI();
621 final T i3C = sortedInertia.getInertiaAxis3().getI();
622
623 final T i32 = i3C.subtract(i2C);
624 final T i31 = i3C.subtract(i1C);
625 final T i21 = i2C.subtract(i1C);
626
627
628 final T b = phiSlope.multiply(i32).multiply(i31);
629 final T c = i1C.multiply(i32);
630 final T d = i3C.multiply(i21);
631
632
633 final DormandPrince853FieldIntegrator<T> integ = new DormandPrince853FieldIntegrator<>(dtRef.getField(),
634 1.0e-6 * period.getReal(),
635 1.0e-2 * period.getReal(),
636 phiSlope.getReal() * period.getReal() * 1.0e-13,
637 1.0e-13);
638 final FieldDenseOutputModel<T> model = new FieldDenseOutputModel<>();
639 integ.addStepHandler(model);
640
641 integ.integrate(new FieldExpandableODE<>(new FieldOrdinaryDifferentialEquation<T>() {
642
643
644
645
646 @Override
647 public int getDimension() {
648 return 1;
649 }
650
651
652
653
654 @Override
655 public T[] computeDerivatives(final T t, final T[] y) {
656 final T sn = jacobi.valuesN(t.subtract(dtRef).multiply(tScale)).sn();
657 final T[] yDot = MathArrays.buildArray(dtRef.getField(), 1);
658 yDot[0] = b.divide(c.add(d.multiply(sn.square())));
659 return yDot;
660 }
661
662 }), new FieldODEState<>(zero, MathArrays.buildArray(dtRef.getField(), 1)), period);
663
664 return model;
665
666 }
667
668
669
670
671
672 public TimeStampedFieldAngularCoordinates<T> evaluate(final FieldAbsoluteDate<T> date) {
673
674
675 final FieldCopolarN<T> valuesN = jacobi.valuesN(date.durationFrom(tRef).multiply(tScale));
676 final FieldVector3D<T> omegaSorted = new FieldVector3D<>(valuesN.cn().multiply(o1Scale),
677 valuesN.sn().multiply(o2Scale),
678 valuesN.dn().multiply(o3Scale));
679 final FieldVector3D<T> omegaBody = sortedToBody.applyTo(omegaSorted);
680
681
682 final FieldVector3D<T> accelerationSorted =
683 new FieldVector3D<>(o1Scale.multiply(tScale).multiply(valuesN.cn()).multiply(valuesN.dn()),
684 o2Scale.multiply(tScale).multiply(valuesN.sn().negate()).multiply(valuesN.dn()),
685 o3Scale.multiply(tScale).multiply(jacobi.getM().negate()).multiply(valuesN.sn()).multiply(valuesN.cn()));
686 final FieldVector3D<T> accelerationBody = sortedToBody.applyTo(accelerationSorted);
687
688
689 final T dt = date.durationFrom(initialAttitude.getDate());
690 final T psi = FastMath.atan2(sortedInertia.getInertiaAxis1().getI().multiply(omegaSorted.getX()),
691 sortedInertia.getInertiaAxis2().getI().multiply(omegaSorted.getY()));
692 final T theta = FastMath.acos(omegaSorted.getZ().divide(phiSlope));
693 final T phiLinear = dt.multiply(phiSlope);
694
695
696 final int nbPeriods = (int) FastMath.floor(dt.divide(period)).getReal();
697 final T tStartInteg = period.multiply(nbPeriods);
698 final T integPartial = phiQuadratureModel.getInterpolatedState(dt.subtract(tStartInteg)).getPrimaryState()[0];
699 final T phiQuadrature = integOnePeriod.multiply(nbPeriods).add(integPartial);
700 final T phi = phiLinear.add(phiQuadrature);
701
702
703
704 final FieldRotation<T> alignedToSorted = new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
705 phi, theta, psi);
706
707
708 final FieldRotation<T> inertToBody = sortedToBody.applyTo(alignedToSorted.applyTo(inertToAligned));
709
710 return new TimeStampedFieldAngularCoordinates<>(date, inertToBody, omegaBody, accelerationBody);
711
712 }
713
714 }
715
716 }