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.Field;
20 import org.hipparchus.CalculusFieldElement;
21 import org.hipparchus.analysis.CalculusFieldUnivariateFunction;
22 import org.hipparchus.analysis.UnivariateFunction;
23 import org.hipparchus.analysis.solvers.AllowedSolution;
24 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
25 import org.hipparchus.analysis.solvers.FieldBracketingNthOrderBrentSolver;
26 import org.hipparchus.analysis.solvers.UnivariateSolverUtils;
27 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
28 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
29 import org.hipparchus.geometry.euclidean.threed.Rotation;
30 import org.hipparchus.geometry.euclidean.threed.Vector3D;
31 import org.hipparchus.util.FastMath;
32 import org.orekit.bodies.CelestialBody;
33 import org.orekit.time.AbsoluteDate;
34 import org.orekit.time.FieldAbsoluteDate;
35 import org.orekit.utils.FieldPVCoordinates;
36 import org.orekit.utils.PVCoordinates;
37
38
39
40
41
42
43 public class L1TransformProvider implements TransformProvider {
44
45
46 private static final double RELATIVE_ACCURACY = 1e-14;
47
48
49 private static final double ABSOLUTE_ACCURACY = 1e-3;
50
51
52 private static final double FUNCTION_ACCURACY = 0;
53
54
55 private static final int MAX_ORDER = 5;
56
57
58 private static final int MAX_EVALUATIONS = 1000;
59
60
61 private static final long serialVersionUID = 20170824L;
62
63
64 private final Frame frame;
65
66
67 private final CelestialBody primaryBody;
68
69
70 private final CelestialBody secondaryBody;
71
72
73
74
75
76 public L1TransformProvider(final CelestialBody primaryBody, final CelestialBody secondaryBody) {
77 this.primaryBody = primaryBody;
78 this.secondaryBody = secondaryBody;
79 this.frame = primaryBody.getInertiallyOrientedFrame();
80 }
81
82
83 @Override
84 public Transform getTransform(final AbsoluteDate date) {
85 final PVCoordinates pv21 = secondaryBody.getPVCoordinates(date, frame);
86 final Vector3D translation = getL1(pv21.getPosition()).negate();
87 final Rotation rotation = new Rotation(pv21.getPosition(), pv21.getVelocity(),
88 Vector3D.PLUS_I, Vector3D.PLUS_J);
89 return new Transform(date, new Transform(date, translation), new Transform(date, rotation));
90 }
91
92
93 @Override
94 public StaticTransform getStaticTransform(final AbsoluteDate date) {
95 final PVCoordinates pv21 = secondaryBody.getPVCoordinates(date, frame);
96 final Vector3D translation = getL1(pv21.getPosition()).negate();
97 final Rotation rotation = new Rotation(pv21.getPosition(), pv21.getVelocity(),
98 Vector3D.PLUS_I, Vector3D.PLUS_J);
99 return StaticTransform.compose(
100 date,
101 StaticTransform.of(date, translation),
102 StaticTransform.of(date, rotation));
103 }
104
105
106 @Override
107 public <T extends CalculusFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date) {
108 final FieldPVCoordinates<T> pv21 = secondaryBody.getPVCoordinates(date, frame);
109 final FieldVector3D<T> translation = getL1(pv21.getPosition()).negate();
110 final Field<T> field = pv21.getPosition().getX().getField();
111 final FieldRotation<T> rotation = new FieldRotation<>(pv21.getPosition(), pv21.getVelocity(),
112 FieldVector3D.getPlusI(field),
113 FieldVector3D.getPlusJ(field));
114 return new FieldTransform<>(date,
115 new FieldTransform<>(date, translation),
116 new FieldTransform<>(date, rotation));
117 }
118
119
120 @Override
121 public <T extends CalculusFieldElement<T>> FieldStaticTransform<T> getStaticTransform(final FieldAbsoluteDate<T> date) {
122 final FieldPVCoordinates<T> pv21 = secondaryBody.getPVCoordinates(date, frame);
123 final FieldVector3D<T> translation = getL1(pv21.getPosition()).negate();
124 final FieldRotation<T> rotation = new FieldRotation<>(pv21.getPosition(), pv21.getVelocity(),
125 FieldVector3D.getPlusI(date.getField()), FieldVector3D.getPlusJ(date.getField()));
126 return FieldStaticTransform.compose(
127 date,
128 FieldStaticTransform.of(date, translation),
129 FieldStaticTransform.of(date, rotation));
130 }
131
132
133
134
135
136 private Vector3D getL1(final Vector3D primaryToSecondary) {
137
138
139 final double massRatio = secondaryBody.getGM() / primaryBody.getGM();
140
141
142 final double bigR = primaryToSecondary.getNorm();
143 final double baseR = bigR * (1 - FastMath.cbrt(massRatio / 3));
144
145
146 final UnivariateFunction l1Equation = r -> {
147 final double bigrminusR = bigR - r;
148 final double lhs = 1.0 / ( r * r );
149 final double rhs1 = massRatio / (bigrminusR * bigrminusR);
150 final double rhs2 = 1.0 / (bigR * bigR);
151 final double rhs3 = (1 + massRatio) * bigrminusR * rhs2 / bigR;
152 return lhs - (rhs1 + rhs2 - rhs3);
153 };
154 final double[] searchInterval = UnivariateSolverUtils.bracket(l1Equation,
155 baseR, 0, bigR,
156 0.01 * bigR, 1, MAX_EVALUATIONS);
157 final BracketingNthOrderBrentSolver solver =
158 new BracketingNthOrderBrentSolver(RELATIVE_ACCURACY,
159 ABSOLUTE_ACCURACY,
160 FUNCTION_ACCURACY,
161 MAX_ORDER);
162 final double r = solver.solve(MAX_EVALUATIONS, l1Equation,
163 searchInterval[0], searchInterval[1],
164 AllowedSolution.ANY_SIDE);
165
166
167 return new Vector3D(r / bigR, primaryToSecondary);
168
169 }
170
171
172
173
174
175
176 private <T extends CalculusFieldElement<T>> FieldVector3D<T>
177 getL1(final FieldVector3D<T> primaryToSecondary) {
178
179
180 final double massRatio = secondaryBody.getGM() / primaryBody.getGM();
181
182
183 final T bigR = primaryToSecondary.getNorm();
184 final T baseR = bigR.multiply(1 - FastMath.cbrt(massRatio / 3));
185
186
187 final CalculusFieldUnivariateFunction<T> l1Equation = r -> {
188 final T bigrminusR = bigR.subtract(r);
189 final T lhs = r.multiply(r).reciprocal();
190 final T rhs1 = bigrminusR.multiply(bigrminusR).reciprocal().multiply(massRatio);
191 final T rhs2 = bigR.multiply(bigR).reciprocal();
192 final T rhs3 = bigrminusR.multiply(rhs2).multiply(1 + massRatio).divide(bigR);
193 return lhs.subtract(rhs1.add(rhs2).add(rhs3));
194 };
195 final T zero = primaryToSecondary.getX().getField().getZero();
196 final T[] searchInterval = UnivariateSolverUtils.bracket(l1Equation,
197 baseR, zero, bigR.multiply(2),
198 bigR.multiply(0.01), zero.newInstance(1),
199 MAX_EVALUATIONS);
200
201
202 final T relativeAccuracy = zero.newInstance(RELATIVE_ACCURACY);
203 final T absoluteAccuracy = zero.newInstance(ABSOLUTE_ACCURACY);
204 final T functionAccuracy = zero.newInstance(FUNCTION_ACCURACY);
205
206 final FieldBracketingNthOrderBrentSolver<T> solver =
207 new FieldBracketingNthOrderBrentSolver<>(relativeAccuracy,
208 absoluteAccuracy,
209 functionAccuracy,
210 MAX_ORDER);
211 final T r = solver.solve(MAX_EVALUATIONS, l1Equation,
212 searchInterval[0], searchInterval[1],
213 AllowedSolution.ANY_SIDE);
214
215
216 return new FieldVector3D<>(r.divide(bigR), primaryToSecondary);
217
218 }
219
220 }