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