Packages/MoonriseKit/Sources/SkyEngine/SGP4.swift
232 lines · 10376 bytes
1import Foundation
2
3/// General perturbations orbital elements in CelesTrak's OMM JSON form.
4public struct OrbitalElements: Codable, Sendable, Hashable {
5 public var OBJECT_NAME: String
6 public var NORAD_CAT_ID: Int
7 /// UTC, ISO 8601 without a zone, e.g. "2026-10-18T03:28:15.076416".
8 public var EPOCH: String
9 /// Revolutions per day.
10 public var MEAN_MOTION: Double
11 public var ECCENTRICITY: Double
12 /// Degrees.
13 public var INCLINATION: Double
14 public var RA_OF_ASC_NODE: Double
15 public var ARG_OF_PERICENTER: Double
16 public var MEAN_ANOMALY: Double
17 public var BSTAR: Double
18
19 public init(name: String, norad: Int, epoch: String, meanMotion: Double, eccentricity: Double, inclination: Double, raan: Double, argPerigee: Double, meanAnomaly: Double, bstar: Double) {
20 OBJECT_NAME = name; NORAD_CAT_ID = norad; EPOCH = epoch; MEAN_MOTION = meanMotion; ECCENTRICITY = eccentricity
21 INCLINATION = inclination; RA_OF_ASC_NODE = raan; ARG_OF_PERICENTER = argPerigee; MEAN_ANOMALY = meanAnomaly; BSTAR = bstar
22 }
23
24 public var epochDate: Date? {
25 let parts = EPOCH.split(separator: ".")
26 guard let whole = try? Date(String(parts[0]) + "Z", strategy: .iso8601) else { return nil }
27 let frac = parts.count > 1 ? Double("0." + parts[1]) ?? 0 : 0
28 return whole.addingTimeInterval(frac)
29 }
30}
31
32/// Near-Earth SGP4 (Vallado et al. 2006, "Revisiting Spacetrack Report #3"), WGS-72 constants.
33/// Deep-space orbits (period of 225 minutes or more) are not supported.
34public struct SGP4: Sendable {
35 // WGS-72
36 static let mu = 398600.8
37 static let re = 6378.135
38 static let xke = 60.0 / ((re * re * re) / mu).squareRoot()
39 static let j2 = 0.001082616
40 static let j3 = -0.00000253881
41 static let j4 = -0.00000165597
42 static let j3oj2 = j3 / j2
43 static let twoPi = 2 * Double.pi
44
45 public let epoch: Date
46 let ecco, inclo, nodeo, argpo, mo, bstar: Double
47 let noUnkozai, ao, con41, x1mth2, x7thm1, eta, cc1, cc4, cc5, d2, d3, d4, t2cof, t3cof, t4cof, t5cof: Double
48 let mdot, argpdot, nodedot, nodecf, omgcof, xmcof, xlcof, aycof, delmo, sinmao, cosio, sinio: Double
49 let isimp: Bool
50
51 public enum Failure: Error { case deepSpace, badEpoch }
52
53 public init(_ el: OrbitalElements) throws {
54 guard let epoch = el.epochDate else { throw Failure.badEpoch }
55 self.epoch = epoch
56 let r = Double.pi / 180
57 ecco = el.ECCENTRICITY
58 inclo = el.INCLINATION * r
59 nodeo = el.RA_OF_ASC_NODE * r
60 argpo = el.ARG_OF_PERICENTER * r
61 mo = el.MEAN_ANOMALY * r
62 bstar = el.BSTAR
63 let noKozai = el.MEAN_MOTION * Self.twoPi / 1440
64 let x2o3 = 2.0 / 3.0
65
66 // initl
67 let eccsq = ecco * ecco, omeosq = 1 - eccsq, rteosq = omeosq.squareRoot()
68 cosio = cos(inclo)
69 let cosio2 = cosio * cosio
70 let ak = pow(Self.xke / noKozai, x2o3)
71 let d1 = 0.75 * Self.j2 * (3 * cosio2 - 1) / (rteosq * omeosq)
72 var del = d1 / (ak * ak)
73 let adel = ak * (1 - del * del - del * (1.0 / 3.0 + 134 * del * del / 81))
74 del = d1 / (adel * adel)
75 noUnkozai = noKozai / (1 + del)
76 guard Self.twoPi / noUnkozai < 225 else { throw Failure.deepSpace }
77 ao = pow(Self.xke / noUnkozai, x2o3)
78 sinio = sin(inclo)
79 let po = ao * omeosq
80 let con42 = 1 - 5 * cosio2
81 con41 = -con42 - cosio2 - cosio2
82 let posq = po * po
83 let rp = ao * (1 - ecco)
84
85 // sgp4init
86 let ss = 78 / Self.re + 1
87 let qzms2t = pow((120 - 78) / Self.re, 4)
88 isimp = rp < 220 / Self.re + 1
89 var sfour = ss, qzms24 = qzms2t
90 let perige = (rp - 1) * Self.re
91 if perige < 156 {
92 sfour = perige < 98 ? 20 : perige - 78
93 qzms24 = pow((120 - sfour) / Self.re, 4)
94 sfour = sfour / Self.re + 1
95 }
96 let pinvsq = 1 / posq
97 let tsi = 1 / (ao - sfour)
98 eta = ao * ecco * tsi
99 let etasq = eta * eta, eeta = ecco * eta
100 let psisq = abs(1 - etasq)
101 let coef = qzms24 * pow(tsi, 4)
102 let coef1 = coef / pow(psisq, 3.5)
103 let cc2 = coef1 * noUnkozai * (ao * (1 + 1.5 * etasq + eeta * (4 + etasq)) + 0.375 * Self.j2 * tsi / psisq * con41 * (8 + 3 * etasq * (8 + etasq)))
104 cc1 = bstar * cc2
105 let cc3 = ecco > 1e-4 ? -2 * coef * tsi * Self.j3oj2 * noUnkozai * sinio / ecco : 0
106 x1mth2 = 1 - cosio2
107 cc4 = 2 * noUnkozai * coef1 * ao * omeosq * (eta * (2 + 0.5 * etasq) + ecco * (0.5 + 2 * etasq)
108 - Self.j2 * tsi / (ao * psisq) * (-3 * con41 * (1 - 2 * eeta + etasq * (1.5 - 0.5 * eeta)) + 0.75 * x1mth2 * (2 * etasq - eeta * (1 + etasq)) * cos(2 * argpo)))
109 cc5 = 2 * coef1 * ao * omeosq * (1 + 2.75 * (etasq + eeta) + eeta * etasq)
110 let cosio4 = cosio2 * cosio2
111 let temp1 = 1.5 * Self.j2 * pinvsq * noUnkozai
112 let temp2 = 0.5 * temp1 * Self.j2 * pinvsq
113 let temp3 = -0.46875 * Self.j4 * pinvsq * pinvsq * noUnkozai
114 mdot = noUnkozai + 0.5 * temp1 * rteosq * con41 + 0.0625 * temp2 * rteosq * (13 - 78 * cosio2 + 137 * cosio4)
115 argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * (7 - 114 * cosio2 + 395 * cosio4) + temp3 * (3 - 36 * cosio2 + 49 * cosio4)
116 let xhdot1 = -temp1 * cosio
117 nodedot = xhdot1 + (0.5 * temp2 * (4 - 19 * cosio2) + 2 * temp3 * (3 - 7 * cosio2)) * cosio
118 omgcof = bstar * cc3 * cos(argpo)
119 xmcof = ecco > 1e-4 ? -x2o3 * coef * bstar / eeta : 0
120 nodecf = 3.5 * omeosq * xhdot1 * cc1
121 t2cof = 1.5 * cc1
122 xlcof = -0.25 * Self.j3oj2 * sinio * (3 + 5 * cosio) / (abs(cosio + 1) > 1.5e-12 ? 1 + cosio : 1.5e-12)
123 aycof = -0.5 * Self.j3oj2 * sinio
124 delmo = pow(1 + eta * cos(mo), 3)
125 sinmao = sin(mo)
126 x7thm1 = 7 * cosio2 - 1
127 if !isimp {
128 let cc1sq = cc1 * cc1
129 d2 = 4 * ao * tsi * cc1sq
130 let temp = d2 * tsi * cc1 / 3
131 d3 = (17 * ao + sfour) * temp
132 d4 = 0.5 * temp * ao * tsi * (221 * ao + 31 * sfour) * cc1
133 t3cof = d2 + 2 * cc1sq
134 t4cof = 0.25 * (3 * d3 + cc1 * (12 * d2 + 10 * cc1sq))
135 t5cof = 0.2 * (3 * d4 + 12 * cc1 * d3 + 6 * d2 * d2 + 15 * cc1sq * (2 * d2 + cc1sq))
136 } else {
137 d2 = 0; d3 = 0; d4 = 0; t3cof = 0; t4cof = 0; t5cof = 0
138 }
139 }
140
141 /// Position (km) and velocity (km/s) in the TEME frame, `minutes` after epoch.
142 public func propagate(minutes t: Double) -> (r: SIMD3<Double>, v: SIMD3<Double>)? {
143 let x2o3 = 2.0 / 3.0
144 let xmdf = mo + mdot * t
145 let argpdf = argpo + argpdot * t
146 let nodedf = nodeo + nodedot * t
147 var argpm = argpdf, mm = xmdf
148 let t2 = t * t
149 var nodem = nodedf + nodecf * t2
150 var tempa = 1 - cc1 * t
151 var tempe = bstar * cc4 * t
152 var templ = t2cof * t2
153 if !isimp {
154 let delomg = omgcof * t
155 let delm = xmcof * (pow(1 + eta * cos(xmdf), 3) - delmo)
156 let temp = delomg + delm
157 mm = xmdf + temp
158 argpm = argpdf - temp
159 let t3 = t2 * t, t4 = t3 * t
160 tempa = tempa - d2 * t2 - d3 * t3 - d4 * t4
161 tempe = tempe + bstar * cc5 * (sin(mm) - sinmao)
162 templ = templ + t3cof * t3 + t4 * (t4cof + t * t5cof)
163 }
164 let am = pow(Self.xke / noUnkozai, x2o3) * tempa * tempa
165 let nm = Self.xke / pow(am, 1.5)
166 var em = ecco - tempe
167 guard em < 1, am >= 0.95 else { return nil }
168 if em < 1e-6 { em = 1e-6 }
169 mm += noUnkozai * templ
170 var xlm = mm + argpm + nodem
171 nodem = fmod(nodem, Self.twoPi)
172 argpm = fmod(argpm, Self.twoPi)
173 xlm = fmod(xlm, Self.twoPi)
174 mm = fmod(xlm - argpm - nodem, Self.twoPi)
175
176 // Long-period periodics
177 let axnl = em * cos(argpm)
178 var temp = 1 / (am * (1 - em * em))
179 let aynl = em * sin(argpm) + temp * aycof
180 let xl = mm + argpm + nodem + temp * xlcof * axnl
181
182 // Kepler's equation
183 let u = fmod(xl - nodem, Self.twoPi)
184 var eo1 = u, tem5 = 9999.9, ktr = 1
185 var sineo1 = 0.0, coseo1 = 0.0
186 while abs(tem5) >= 1e-12 && ktr <= 10 {
187 sineo1 = sin(eo1); coseo1 = cos(eo1)
188 tem5 = 1 - coseo1 * axnl - sineo1 * aynl
189 tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5
190 if abs(tem5) >= 0.95 { tem5 = tem5 > 0 ? 0.95 : -0.95 }
191 eo1 += tem5
192 ktr += 1
193 }
194
195 // Short-period periodics
196 let ecose = axnl * coseo1 + aynl * sineo1
197 let esine = axnl * sineo1 - aynl * coseo1
198 let el2 = axnl * axnl + aynl * aynl
199 let pl = am * (1 - el2)
200 guard pl >= 0 else { return nil }
201 let rl = am * (1 - ecose)
202 let rdotl = am.squareRoot() * esine / rl
203 let rvdotl = pl.squareRoot() / rl
204 let betal = (1 - el2).squareRoot()
205 temp = esine / (1 + betal)
206 let sinu = am / rl * (sineo1 - aynl - axnl * temp)
207 let cosu = am / rl * (coseo1 - axnl + aynl * temp)
208 var su = atan2(sinu, cosu)
209 let sin2u = (cosu + cosu) * sinu
210 let cos2u = 1 - 2 * sinu * sinu
211 temp = 1 / pl
212 let temp1 = 0.5 * Self.j2 * temp
213 let temp2 = temp1 * temp
214 let mrt = rl * (1 - 1.5 * temp2 * betal * con41) + 0.5 * temp1 * x1mth2 * cos2u
215 su -= 0.25 * temp2 * x7thm1 * sin2u
216 let xnode = nodem + 1.5 * temp2 * cosio * sin2u
217 let xinc = inclo + 1.5 * temp2 * cosio * sinio * cos2u
218 let mvt = rdotl - nm * temp1 * x1mth2 * sin2u / Self.xke
219 let rvdot = rvdotl + nm * temp1 * (x1mth2 * cos2u + 1.5 * con41) / Self.xke
220
221 let sinsu = sin(su), cossu = cos(su), snod = sin(xnode), cnod = cos(xnode), sini = sin(xinc), cosi = cos(xinc)
222 let xmx = -snod * cosi, xmy = cnod * cosi
223 let uv = SIMD3(xmx * sinsu + cnod * cossu, xmy * sinsu + snod * cossu, sini * sinsu)
224 let vv = SIMD3(xmx * cossu - cnod * sinsu, xmy * cossu - snod * sinsu, sini * cossu)
225 let vkmpersec = Self.re * Self.xke / 60
226 return (mrt * uv * Self.re, (mvt * uv + rvdot * vv) * vkmpersec)
227 }
228
229 public func position(at date: Date) -> SIMD3<Double>? {
230 propagate(minutes: date.timeIntervalSince(epoch) / 60)?.r
231 }
232}