1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import java.lang.reflect.Array;
20
21 import org.hipparchus.CalculusFieldElement;
22 import org.hipparchus.Field;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.util.FastMath;
25 import org.hipparchus.util.FieldSinCos;
26 import org.orekit.utils.FieldPVCoordinates;
27
28
29
30
31
32
33
34
35
36
37 public class FieldKeplerianParametersConverter<T extends CalculusFieldElement<T>> {
38
39
40 private final T mu;
41
42
43
44
45
46 public FieldKeplerianParametersConverter(final T mu) {
47 this.mu = mu;
48 }
49
50
51
52
53
54
55
56 public FieldKeplerianParameters<T> toParameters(final FieldPVCoordinates<T> cartesian,
57 final PositionAngleType positionAngleType) {
58
59 final Field<T> field = cartesian.getPosition().getX().getField();
60 final FieldVector3D<T> plusK = FieldVector3D.getPlusK(field);
61
62
63 final FieldVector3D<T> momentum = cartesian.getMomentum();
64 final T m2 = momentum.getNorm2Sq();
65
66 final T i = FieldVector3D.angle(momentum, plusK);
67
68 final T raan = FieldVector3D.crossProduct(plusK, momentum).getAlpha();
69
70 final FieldVector3D<T> pvP = cartesian.getPosition();
71 final FieldVector3D<T> pvV = cartesian.getVelocity();
72
73 final T r2 = pvP.getNorm2Sq();
74 final T r = r2.sqrt();
75 final T v2 = pvV.getNorm2Sq();
76 final T rV2OnMu = r.multiply(v2).divide(mu);
77
78
79 final T a = r.divide(rV2OnMu.negate().add(2.0));
80 final T muA = a.multiply(mu);
81
82
83 final T e;
84 final T eccentricAnomaly;
85 final PositionAngleType intermediateType = PositionAngleType.ECCENTRIC;
86 if (a.getReal() > 0.) {
87
88 final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
89 final T eCE = rV2OnMu.subtract(1);
90 e = (eSE.multiply(eSE).add(eCE.multiply(eCE))).sqrt();
91 eccentricAnomaly = eSE.atan2(eCE);
92 } else {
93
94 final T eSH = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
95 final T eCH = rV2OnMu.subtract(1);
96 e = (m2.negate().divide(muA).add(1)).sqrt();
97 eccentricAnomaly = (eCH.add(eSH)).divide(eCH.subtract(eSH)).log().divide(2);
98 }
99
100
101 final FieldVector3D<T> node = new FieldVector3D<>(raan, field.getZero());
102 final T px = FieldVector3D.dotProduct(pvP, node);
103 final T py = FieldVector3D.dotProduct(pvP, FieldVector3D.crossProduct(momentum, node)).divide(m2.sqrt());
104 final T trueAnomaly = FieldKeplerianAnomalyUtility.convertAnomaly(intermediateType, eccentricAnomaly, e, PositionAngleType.TRUE);
105 final T pa = py.atan2(px).subtract(trueAnomaly);
106
107 return switch (positionAngleType) {
108 case PositionAngleType.MEAN -> {
109 final T meanAnomaly = FieldKeplerianAnomalyUtility.convertAnomaly(intermediateType, eccentricAnomaly, e, positionAngleType);
110 yield new FieldKeplerianParameters<>(a, e, i, pa, raan, meanAnomaly, positionAngleType);
111 }
112 case PositionAngleType.TRUE -> new FieldKeplerianParameters<>(a, e, i, pa, raan, trueAnomaly, positionAngleType);
113 case PositionAngleType.ECCENTRIC -> new FieldKeplerianParameters<>(a, e, i, pa, raan, eccentricAnomaly, positionAngleType);
114 };
115 }
116
117
118
119
120
121
122 public FieldPVCoordinates<T> toCartesian(final FieldKeplerianParameters<T> elements) {
123 final FieldVector3D<T> position;
124 final FieldVector3D<T> velocity;
125 final FieldVector3D<T>[] axes = referenceAxes(elements.i(), elements.pa(), elements.raan());
126
127 final T e = elements.e();
128 final T a = elements.a();
129 if (elements.a().getReal() > 0.) {
130
131 final T uME2 = e.negate().add(1).multiply(e.add(1));
132 final T s1Me2 = uME2.sqrt();
133 final FieldKeplerianParameters<T> elementsWithEccentricAnomaly = elements.withPositionAngleType(PositionAngleType.ECCENTRIC);
134 final T eccentricAnomaly = elementsWithEccentricAnomaly.anomaly();
135 final FieldSinCos<T> scE = FastMath.sinCos(eccentricAnomaly);
136 final T cosE = scE.cos();
137 final T sinE = scE.sin();
138
139
140 final T x = a.multiply(cosE.subtract(e));
141 final T y = a.multiply(sinE).multiply(s1Me2);
142 final T factor = FastMath.sqrt(mu.divide(a)).divide(e.negate().multiply(cosE).add(1));
143 final T xDot = sinE.negate().multiply(factor);
144 final T yDot = cosE.multiply(s1Me2).multiply(factor);
145
146 position = new FieldVector3D<>(x, axes[0], y, axes[1]);
147 velocity = new FieldVector3D<>(xDot, axes[0], yDot, axes[1]);
148
149 } else {
150
151
152
153 final FieldKeplerianParameters<T> elementsWithTrueAnomaly = elements.withPositionAngleType(PositionAngleType.TRUE);
154 final FieldSinCos<T> scV = FastMath.sinCos(elementsWithTrueAnomaly.anomaly());
155 final T sinV = scV.sin();
156 final T cosV = scV.cos();
157 final T f = a.multiply(e.square().negate().add(1));
158 final T posFactor = f.divide(e.multiply(cosV).add(1));
159 final T velFactor = FastMath.sqrt(mu.divide(f));
160
161 position = new FieldVector3D<>(posFactor.multiply(cosV), axes[0], posFactor.multiply(sinV), axes[1]);
162 velocity = new FieldVector3D<>(velFactor.multiply(sinV).negate(), axes[0], velFactor.multiply(e.add(cosV)), axes[1]);
163
164 }
165 return new FieldPVCoordinates<>(position, velocity);
166 }
167
168
169
170
171
172
173
174
175 static <W extends CalculusFieldElement<W>> FieldVector3D<W>[] referenceAxes(final W i, final W pa, final W raan) {
176
177 final FieldSinCos<W> scRaan = FastMath.sinCos(raan);
178 final FieldSinCos<W> scPa = FastMath.sinCos(pa);
179 final FieldSinCos<W> scI = FastMath.sinCos(i);
180 final W cosRaan = scRaan.cos();
181 final W sinRaan = scRaan.sin();
182 final W cosPa = scPa.cos();
183 final W sinPa = scPa.sin();
184 final W cosI = scI.cos();
185 final W sinI = scI.sin();
186 final W crcp = cosRaan.multiply(cosPa);
187 final W crsp = cosRaan.multiply(sinPa);
188 final W srcp = sinRaan.multiply(cosPa);
189 final W srsp = sinRaan.multiply(sinPa);
190
191
192 @SuppressWarnings("unchecked")
193 final FieldVector3D<W>[] axes = (FieldVector3D<W>[]) Array.newInstance(FieldVector3D.class, 2);
194 axes[0] = new FieldVector3D<>(crcp.subtract(cosI.multiply(srsp)), srcp.add(cosI.multiply(crsp)), sinI.multiply(sinPa));
195 axes[1] = new FieldVector3D<>(crsp.add(cosI.multiply(srcp)).negate(), cosI.multiply(crcp).subtract(srsp), sinI.multiply(cosPa));
196
197 return axes;
198 }
199 }