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