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 <T extends CalculusFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date) {
95 final FieldPVCoordinates<T> pv21 = secondaryBody.getPVCoordinates(date, frame);
96 final FieldVector3D<T> translation = getL1(pv21.getPosition()).negate();
97 final Field<T> field = pv21.getPosition().getX().getField();
98 final FieldRotation<T> rotation = new FieldRotation<>(pv21.getPosition(), pv21.getVelocity(),
99 FieldVector3D.getPlusI(field),
100 FieldVector3D.getPlusJ(field));
101 return new FieldTransform<>(date,
102 new FieldTransform<>(date, translation),
103 new FieldTransform<>(date, rotation));
104 }
105
106
107
108
109
110 private Vector3D getL1(final Vector3D primaryToSecondary) {
111
112
113 final double massRatio = secondaryBody.getGM() / primaryBody.getGM();
114
115
116 final double bigR = primaryToSecondary.getNorm();
117 final double baseR = bigR * (1 - FastMath.cbrt(massRatio / 3));
118
119
120 final UnivariateFunction l1Equation = r -> {
121 final double bigrminusR = bigR - r;
122 final double lhs = 1.0 / ( r * r );
123 final double rhs1 = massRatio / (bigrminusR * bigrminusR);
124 final double rhs2 = 1.0 / (bigR * bigR);
125 final double rhs3 = (1 + massRatio) * bigrminusR * rhs2 / bigR;
126 return lhs - (rhs1 + rhs2 - rhs3);
127 };
128 final double[] searchInterval = UnivariateSolverUtils.bracket(l1Equation,
129 baseR, 0, bigR,
130 0.01 * bigR, 1, MAX_EVALUATIONS);
131 final BracketingNthOrderBrentSolver solver =
132 new BracketingNthOrderBrentSolver(RELATIVE_ACCURACY,
133 ABSOLUTE_ACCURACY,
134 FUNCTION_ACCURACY,
135 MAX_ORDER);
136 final double r = solver.solve(MAX_EVALUATIONS, l1Equation,
137 searchInterval[0], searchInterval[1],
138 AllowedSolution.ANY_SIDE);
139
140
141 return new Vector3D(r / bigR, primaryToSecondary);
142
143 }
144
145
146
147
148
149
150 private <T extends CalculusFieldElement<T>> FieldVector3D<T>
151 getL1(final FieldVector3D<T> primaryToSecondary) {
152
153
154 final double massRatio = secondaryBody.getGM() / primaryBody.getGM();
155
156
157 final T bigR = primaryToSecondary.getNorm();
158 final T baseR = bigR.multiply(1 - FastMath.cbrt(massRatio / 3));
159
160
161 final CalculusFieldUnivariateFunction<T> l1Equation = r -> {
162 final T bigrminusR = bigR.subtract(r);
163 final T lhs = r.multiply(r).reciprocal();
164 final T rhs1 = bigrminusR.multiply(bigrminusR).reciprocal().multiply(massRatio);
165 final T rhs2 = bigR.multiply(bigR).reciprocal();
166 final T rhs3 = bigrminusR.multiply(rhs2).multiply(1 + massRatio).divide(bigR);
167 return lhs.subtract(rhs1.add(rhs2).add(rhs3));
168 };
169 final T zero = primaryToSecondary.getX().getField().getZero();
170 final T[] searchInterval = UnivariateSolverUtils.bracket(l1Equation,
171 baseR, zero, bigR.multiply(2),
172 bigR.multiply(0.01), zero.add(1),
173 MAX_EVALUATIONS);
174
175
176 final T relativeAccuracy = zero.add(RELATIVE_ACCURACY);
177 final T absoluteAccuracy = zero.add(ABSOLUTE_ACCURACY);
178 final T functionAccuracy = zero.add(FUNCTION_ACCURACY);
179
180 final FieldBracketingNthOrderBrentSolver<T> solver =
181 new FieldBracketingNthOrderBrentSolver<>(relativeAccuracy,
182 absoluteAccuracy,
183 functionAccuracy,
184 MAX_ORDER);
185 final T r = solver.solve(MAX_EVALUATIONS, l1Equation,
186 searchInterval[0], searchInterval[1],
187 AllowedSolution.ANY_SIDE);
188
189
190 return new FieldVector3D<>(r.divide(bigR), primaryToSecondary);
191
192 }
193
194 }