1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.frames;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
22 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
23 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
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.bodies.CelestialBody;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.utils.FieldPVCoordinates;
33 import org.orekit.utils.TimeStampedFieldPVCoordinates;
34 import org.orekit.utils.TimeStampedPVCoordinates;
35
36
37
38
39
40 class CR3BPRotatingTransformProvider implements TransformProvider {
41
42
43 private final Frame frame;
44
45
46 private final CelestialBody secondaryBody;
47
48
49 private final double mu;
50
51
52
53
54
55
56
57 CR3BPRotatingTransformProvider(final double mu, final CelestialBody primaryBody, final CelestialBody secondaryBody) {
58 this.secondaryBody = secondaryBody;
59 this.frame = primaryBody.getInertiallyOrientedFrame();
60 this.mu = mu;
61 }
62
63
64 @Override
65 public Transform getTransform(final AbsoluteDate date) {
66 final FieldPVCoordinates<UnivariateDerivative2> pv21 = secondaryBody.getPVCoordinates(date, frame).toUnivariateDerivative2PV();
67 final Field<UnivariateDerivative2> field = pv21.getPosition().getX().getField();
68 final FieldVector3D<UnivariateDerivative2> translation = FieldVector3D.getPlusI(field).scalarMultiply(pv21.getPosition().getNorm().multiply(mu)).negate();
69
70 final FieldRotation<UnivariateDerivative2> rotation = new FieldRotation<>(pv21.getPosition(), pv21.getMomentum(),
71 FieldVector3D.getPlusI(field),
72 FieldVector3D.getPlusK(field));
73
74 final UnivariateDerivative2[] rotationRates = rotation.getAngles(RotationOrder.XYZ, RotationConvention.FRAME_TRANSFORM);
75 final Vector3D rotationRate = new Vector3D(rotationRates[0].getPartialDerivative(1), rotationRates[1].getPartialDerivative(1), rotationRates[2].getPartialDerivative(1));
76 final Vector3D rotationAcc = new Vector3D(rotationRates[0].getPartialDerivative(2), rotationRates[1].getPartialDerivative(2), rotationRates[2].getPartialDerivative(2));
77 final Vector3D velocity = new Vector3D(translation.getX().getPartialDerivative(1), translation.getY().getPartialDerivative(1), translation.getZ().getPartialDerivative(1));
78 final Vector3D acceleration = new Vector3D(translation.getX().getPartialDerivative(2), translation.getY().getPartialDerivative(2), translation.getZ().getPartialDerivative(2));
79
80 final Transform transform1 = new Transform(date, translation.toVector3D(), velocity, acceleration);
81 final Transform transform2 = new Transform(date, rotation.toRotation(), rotationRate, rotationAcc);
82 return new Transform(date, transform2, transform1);
83 }
84
85
86 @Override
87 public StaticTransform getStaticTransform(final AbsoluteDate date) {
88 final TimeStampedPVCoordinates pv = secondaryBody.getPVCoordinates(date, frame);
89 final Vector3D translation = Vector3D.PLUS_I
90 .scalarMultiply(pv.getPosition().getNorm() * mu).negate();
91
92 final Rotation rotation = new Rotation(
93 pv.getPosition(), pv.getMomentum(),
94 Vector3D.PLUS_I, Vector3D.PLUS_K);
95
96 final StaticTransform transform1 = StaticTransform.of(date, translation);
97 final StaticTransform transform2 = StaticTransform.of(date, rotation);
98 return StaticTransform.compose(date, transform2, transform1);
99 }
100
101
102 @Override
103 public <T extends CalculusFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date) {
104 final FieldPVCoordinates<T> pv21 = secondaryBody.getPVCoordinates(date, frame);
105 final Field<T> field = pv21.getPosition().getX().getField();
106
107 final FieldVector3D<T> translationField = FieldVector3D.getPlusI(field).scalarMultiply(pv21.getPosition().getNorm().multiply(mu)).negate();
108 final FieldRotation<T> rotationField = new FieldRotation<>(pv21.getPosition(), pv21.getMomentum(),
109 FieldVector3D.getPlusI(field),
110 FieldVector3D.getPlusK(field));
111
112 final FieldPVCoordinates<FieldUnivariateDerivative2<T>> pv21FDS = secondaryBody.getPVCoordinates(date, frame).toUnivariateDerivative2PV();
113 final Field<FieldUnivariateDerivative2<T>> fieldUD = pv21FDS.getPosition().getX().getField();
114 final FieldVector3D<FieldUnivariateDerivative2<T>> translationFDS = FieldVector3D.getPlusI(fieldUD).scalarMultiply(pv21FDS.getPosition().getNorm().multiply(mu)).negate();
115
116 final FieldRotation<FieldUnivariateDerivative2<T>> rotationFDS = new FieldRotation<>(pv21FDS.getPosition(), pv21FDS.getMomentum(),
117 FieldVector3D.getPlusI(fieldUD),
118 FieldVector3D.getPlusK(fieldUD));
119 final FieldUnivariateDerivative2<T>[] rotationRates = rotationFDS.getAngles(RotationOrder.XYZ, RotationConvention.FRAME_TRANSFORM);
120 final FieldVector3D<T> rotationRate = new FieldVector3D<>(rotationRates[0].getPartialDerivative(1), rotationRates[1].getPartialDerivative(1), rotationRates[2].getPartialDerivative(1));
121 final FieldVector3D<T> rotationAcc = new FieldVector3D<>(rotationRates[0].getPartialDerivative(2), rotationRates[1].getPartialDerivative(2), rotationRates[2].getPartialDerivative(2));
122 final FieldVector3D<T> velocity = new FieldVector3D<>(translationFDS.getX().getPartialDerivative(1), translationFDS.getY().getPartialDerivative(1), translationFDS.getZ().getPartialDerivative(1));
123 final FieldVector3D<T> acceleration = new FieldVector3D<>(translationFDS.getX().getPartialDerivative(2), translationFDS.getY().getPartialDerivative(2), translationFDS.getZ().getPartialDerivative(2));
124
125 final FieldTransform<T> transform1 = new FieldTransform<>(date, translationField, velocity, acceleration);
126 final FieldTransform<T> transform2 = new FieldTransform<>(date, rotationField, rotationRate, rotationAcc);
127 return new FieldTransform<>(date, transform2, transform1);
128 }
129
130
131 @Override
132 public <T extends CalculusFieldElement<T>> FieldStaticTransform<T> getStaticTransform(final FieldAbsoluteDate<T> date) {
133 final Field<T> field = date.getField();
134 final TimeStampedFieldPVCoordinates<T> pv = secondaryBody.getPVCoordinates(date, frame);
135 final FieldVector3D<T> translation = FieldVector3D.getPlusI(field).
136 scalarMultiply(pv.getPosition().getNorm().multiply(mu)).negate();
137
138 final FieldRotation<T> rotation = new FieldRotation<>(
139 pv.getPosition(), pv.getMomentum(),
140 FieldVector3D.getPlusI(field), FieldVector3D.getMinusK(field));
141
142 final FieldStaticTransform<T> transform1 = FieldStaticTransform.of(date, translation);
143 final FieldStaticTransform<T> transform2 = FieldStaticTransform.of(date, rotation);
144 return FieldStaticTransform.compose(date, transform2, transform1);
145 }
146
147 }