1use anise::errors::OrientationSnafu;
20use anise::prelude::Almanac;
21use snafu::ResultExt;
22
23use crate::cosmic::{AstroPhysicsSnafu, Frame, Orbit};
24use crate::dynamics::AccelModel;
25use crate::io::gravity::HarmonicsMem;
26use crate::linalg::{DMatrix, Matrix3, Vector3, Vector4, U7};
27use hyperdual::linalg::norm;
28use hyperdual::{hyperspace_from_vector, Float, OHyperdual};
29use std::cmp::min;
30use std::fmt;
31use std::sync::Arc;
32
33use super::{DynamicsAlmanacSnafu, DynamicsAstroSnafu, DynamicsError};
34
35#[derive(Clone)]
36pub struct Harmonics {
37 compute_frame: Frame,
38 stor: HarmonicsMem,
39 a_nm: DMatrix<f64>,
40 b_nm: DMatrix<f64>,
41 c_nm: DMatrix<f64>,
42 vr01: DMatrix<f64>,
43 vr11: DMatrix<f64>,
44 a_nm_h: DMatrix<OHyperdual<f64, U7>>,
45 b_nm_h: DMatrix<OHyperdual<f64, U7>>,
46 c_nm_h: DMatrix<OHyperdual<f64, U7>>,
47 vr01_h: DMatrix<OHyperdual<f64, U7>>,
48 vr11_h: DMatrix<OHyperdual<f64, U7>>,
49}
50
51impl Harmonics {
52 pub fn from_stor(compute_frame: Frame, stor: HarmonicsMem) -> Arc<Self> {
54 let degree_np2 = stor.max_degree_n() + 2;
55 let mut a_nm = DMatrix::from_element(degree_np2 + 1, degree_np2 + 1, 0.0);
56 let mut b_nm = DMatrix::from_element(degree_np2, degree_np2, 0.0);
57 let mut c_nm = DMatrix::from_element(degree_np2, degree_np2, 0.0);
58 let mut vr01 = DMatrix::from_element(degree_np2, degree_np2, 0.0);
59 let mut vr11 = DMatrix::from_element(degree_np2, degree_np2, 0.0);
60
61 a_nm[(0, 0)] = 1.0;
63 for n in 1..=degree_np2 {
64 let nf64 = n as f64;
65 a_nm[(n, n)] = (1.0 + 1.0 / (2.0 * nf64)).sqrt() * a_nm[(n - 1, n - 1)];
67 }
68
69 for n in 0..degree_np2 {
71 for m in 0..degree_np2 {
72 let nf64 = n as f64;
73 let mf64 = m as f64;
74 c_nm[(n, m)] = (((2.0 * nf64 + 1.0) * (nf64 + mf64 - 1.0) * (nf64 - mf64 - 1.0))
76 / ((nf64 - mf64) * (nf64 + mf64) * (2.0 * nf64 - 3.0)))
77 .sqrt();
78
79 b_nm[(n, m)] = (((2.0 * nf64 + 1.0) * (2.0 * nf64 - 1.0))
80 / ((nf64 + mf64) * (nf64 - mf64)))
81 .sqrt();
82
83 vr01[(n, m)] = ((nf64 - mf64) * (nf64 + mf64 + 1.0)).sqrt();
84 vr11[(n, m)] = (((2.0 * nf64 + 1.0) * (nf64 + mf64 + 2.0) * (nf64 + mf64 + 1.0))
85 / (2.0 * nf64 + 3.0))
86 .sqrt();
87
88 if m == 0 {
89 vr01[(n, m)] /= 2.0_f64.sqrt();
90 vr11[(n, m)] /= 2.0_f64.sqrt();
91 }
92 }
93 }
94
95 let mut a_nm_h =
97 DMatrix::from_element(degree_np2 + 1, degree_np2 + 1, OHyperdual::from(0.0));
98 let mut b_nm_h = DMatrix::from_element(degree_np2, degree_np2, OHyperdual::from(0.0));
99 let mut c_nm_h = DMatrix::from_element(degree_np2, degree_np2, OHyperdual::from(0.0));
100 let mut vr01_h = DMatrix::from_element(degree_np2, degree_np2, OHyperdual::from(0.0));
101 let mut vr11_h = DMatrix::from_element(degree_np2, degree_np2, OHyperdual::from(0.0));
102
103 a_nm_h[(0, 0)] = OHyperdual::from(1.0);
105 for n in 1..=degree_np2 {
106 a_nm_h[(n, n)] = OHyperdual::from(a_nm[(n, n)]);
108 }
109
110 for n in 0..degree_np2 {
112 for m in 0..degree_np2 {
113 vr01_h[(n, m)] = OHyperdual::from(vr01[(n, m)]);
114 vr11_h[(n, m)] = OHyperdual::from(vr11[(n, m)]);
115 b_nm_h[(n, m)] = OHyperdual::from(b_nm[(n, m)]);
116 c_nm_h[(n, m)] = OHyperdual::from(c_nm[(n, m)]);
117 }
118 }
119
120 Arc::new(Self {
121 compute_frame,
122 stor,
123 a_nm,
124 b_nm,
125 c_nm,
126 vr01,
127 vr11,
128 a_nm_h,
129 b_nm_h,
130 c_nm_h,
131 vr01_h,
132 vr11_h,
133 })
134 }
135}
136
137impl fmt::Display for Harmonics {
138 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
139 write!(
140 f,
141 "{} gravity field {}x{} (order x degree)",
142 self.compute_frame,
143 self.stor.max_order_m(),
144 self.stor.max_degree_n(),
145 )
146 }
147}
148
149impl AccelModel for Harmonics {
150 fn eom(&self, osc: &Orbit, almanac: Arc<Almanac>) -> Result<Vector3<f64>, DynamicsError> {
151 let state = almanac
153 .transform_to(*osc, self.compute_frame, None)
154 .context(DynamicsAlmanacSnafu {
155 action: "transforming into gravity field frame",
156 })?;
157
158 let r_ = state.rmag_km();
160 let s_ = state.radius_km.x / r_;
161 let t_ = state.radius_km.y / r_;
162 let u_ = state.radius_km.z / r_;
163 let max_degree = self.stor.max_degree_n(); let max_order = self.stor.max_order_m(); let mut a_nm = self.a_nm.clone();
168
169 a_nm[(1, 0)] = u_ * 3.0f64.sqrt();
171 for n in 1..=max_degree + 1 {
172 let nf64 = n as f64;
173 a_nm[(n + 1, n)] = (2.0 * nf64 + 3.0).sqrt() * u_ * a_nm[(n, n)];
175 }
176
177 for m in 0..=max_order + 1 {
178 for n in (m + 2)..=max_degree + 1 {
179 let hm_idx = (n, m);
180 a_nm[hm_idx] = u_ * self.b_nm[hm_idx] * a_nm[(n - 1, m)]
181 - self.c_nm[hm_idx] * a_nm[(n - 2, m)];
182 }
183 }
184
185 let mut r_m = Vec::with_capacity(min(max_degree, max_order) + 1);
187 let mut i_m = Vec::with_capacity(min(max_degree, max_order) + 1);
188
189 r_m.push(1.0);
190 i_m.push(0.0);
191
192 for m in 1..=min(max_degree, max_order) {
193 r_m.push(s_ * r_m[m - 1] - t_ * i_m[m - 1]);
194 i_m.push(s_ * i_m[m - 1] + t_ * r_m[m - 1]);
195 }
196
197 let eq_radius_km = self
198 .compute_frame
199 .mean_equatorial_radius_km()
200 .context(AstroPhysicsSnafu)
201 .context(DynamicsAstroSnafu)?;
202
203 let mu_km3_s2 = self
204 .compute_frame
205 .mu_km3_s2()
206 .context(AstroPhysicsSnafu)
207 .context(DynamicsAstroSnafu)?;
208
209 let rho = eq_radius_km / r_;
210 let mut rho_np1 = mu_km3_s2 / r_ * rho;
211 let mut accel4: Vector4<f64> = Vector4::zeros();
212
213 for n in 1..max_degree {
214 let mut sum: Vector4<f64> = Vector4::zeros();
215 rho_np1 *= rho;
216
217 for m in 0..=min(n, max_order) {
218 let (c_val, s_val) = self.stor.cs_nm(n, m);
219 let d_ = (c_val * r_m[m] + s_val * i_m[m]) * 2.0.sqrt();
220 let e_ = if m == 0 {
221 0.0
222 } else {
223 (c_val * r_m[m - 1] + s_val * i_m[m - 1]) * 2.0.sqrt()
224 };
225 let f_ = if m == 0 {
226 0.0
227 } else {
228 (s_val * r_m[m - 1] - c_val * i_m[m - 1]) * 2.0.sqrt()
229 };
230
231 sum.x += (m as f64) * a_nm[(n, m)] * e_;
232 sum.y += (m as f64) * a_nm[(n, m)] * f_;
233 sum.z += self.vr01[(n, m)] * a_nm[(n, m + 1)] * d_;
234 sum.w -= self.vr11[(n, m)] * a_nm[(n + 1, m + 1)] * d_;
235 }
236 let rr = rho_np1 / eq_radius_km;
237 accel4 += rr * sum;
238 }
239 let accel = Vector3::new(
240 accel4.x + accel4.w * s_,
241 accel4.y + accel4.w * t_,
242 accel4.z + accel4.w * u_,
243 );
244 let dcm = almanac
248 .rotate(self.compute_frame, osc.frame, osc.epoch)
249 .context(OrientationSnafu {
250 action: "transform state dcm",
251 })
252 .context(DynamicsAlmanacSnafu {
253 action: "transforming into gravity field frame",
254 })?;
255
256 Ok(dcm.rot_mat * accel)
257 }
258
259 fn dual_eom(
260 &self,
261 osc: &Orbit,
262 almanac: Arc<Almanac>,
263 ) -> Result<(Vector3<f64>, Matrix3<f64>), DynamicsError> {
264 let state = almanac
266 .transform_to(*osc, self.compute_frame, None)
267 .context(DynamicsAlmanacSnafu {
268 action: "transforming into gravity field frame",
269 })?;
270
271 let radius: Vector3<OHyperdual<f64, U7>> = hyperspace_from_vector(&state.radius_km);
272
273 let r_ = norm(&radius);
275 let s_ = radius[0] / r_;
276 let t_ = radius[1] / r_;
277 let u_ = radius[2] / r_;
278 let max_degree = self.stor.max_degree_n(); let max_order = self.stor.max_order_m(); let mut a_nm = self.a_nm_h.clone();
283
284 a_nm[(1, 0)] = u_ * 3.0f64.sqrt();
286 for n in 1..=max_degree + 1 {
287 let nf64 = n as f64;
288 a_nm[(n + 1, n)] = OHyperdual::from((2.0 * nf64 + 3.0).sqrt()) * u_ * a_nm[(n, n)];
290 }
291
292 for m in 0..=max_order + 1 {
293 for n in (m + 2)..=max_degree + 1 {
294 let hm_idx = (n, m);
295 a_nm[hm_idx] = u_ * self.b_nm_h[hm_idx] * a_nm[(n - 1, m)]
296 - self.c_nm_h[hm_idx] * a_nm[(n - 2, m)];
297 }
298 }
299
300 let mut r_m = Vec::with_capacity(min(max_degree, max_order) + 1);
302 let mut i_m = Vec::with_capacity(min(max_degree, max_order) + 1);
303
304 r_m.push(OHyperdual::<f64, U7>::from(1.0));
305 i_m.push(OHyperdual::<f64, U7>::from(0.0));
306
307 for m in 1..=min(max_degree, max_order) {
308 r_m.push(s_ * r_m[m - 1] - t_ * i_m[m - 1]);
309 i_m.push(s_ * i_m[m - 1] + t_ * r_m[m - 1]);
310 }
311
312 let real_eq_radius_km = self
313 .compute_frame
314 .mean_equatorial_radius_km()
315 .context(AstroPhysicsSnafu)
316 .context(DynamicsAstroSnafu)?;
317
318 let real_mu_km3_s2 = self
319 .compute_frame
320 .mu_km3_s2()
321 .context(AstroPhysicsSnafu)
322 .context(DynamicsAstroSnafu)?;
323
324 let eq_radius = OHyperdual::<f64, U7>::from(real_eq_radius_km);
325 let rho = eq_radius / r_;
326 let mut rho_np1 = OHyperdual::<f64, U7>::from(real_mu_km3_s2) / r_ * rho;
327
328 let mut a0 = OHyperdual::<f64, U7>::from(0.0);
329 let mut a1 = OHyperdual::<f64, U7>::from(0.0);
330 let mut a2 = OHyperdual::<f64, U7>::from(0.0);
331 let mut a3 = OHyperdual::<f64, U7>::from(0.0);
332 let sqrt2 = OHyperdual::<f64, U7>::from(2.0.sqrt());
333
334 for n in 1..max_degree {
335 let mut sum0 = OHyperdual::from(0.0);
336 let mut sum1 = OHyperdual::from(0.0);
337 let mut sum2 = OHyperdual::from(0.0);
338 let mut sum3 = OHyperdual::from(0.0);
339 rho_np1 *= rho;
340
341 for m in 0..=min(n, max_order) {
342 let (c_valf64, s_valf64) = self.stor.cs_nm(n, m);
343 let c_val = OHyperdual::<f64, U7>::from(c_valf64);
344 let s_val = OHyperdual::<f64, U7>::from(s_valf64);
345
346 let d_ = (c_val * r_m[m] + s_val * i_m[m]) * sqrt2;
347 let e_ = if m == 0 {
348 OHyperdual::from(0.0)
349 } else {
350 (c_val * r_m[m - 1] + s_val * i_m[m - 1]) * sqrt2
351 };
352 let f_ = if m == 0 {
353 OHyperdual::from(0.0)
354 } else {
355 (s_val * r_m[m - 1] - c_val * i_m[m - 1]) * sqrt2
356 };
357
358 sum0 += OHyperdual::from(m as f64) * a_nm[(n, m)] * e_;
359 sum1 += OHyperdual::from(m as f64) * a_nm[(n, m)] * f_;
360 sum2 += self.vr01_h[(n, m)] * a_nm[(n, m + 1)] * d_;
361 sum3 += self.vr11_h[(n, m)] * a_nm[(n + 1, m + 1)] * d_;
362 }
363 let rr = rho_np1 / eq_radius;
364 a0 += rr * sum0;
365 a1 += rr * sum1;
366 a2 += rr * sum2;
367 a3 -= rr * sum3;
368 }
369
370 let dcm = almanac
371 .rotate(self.compute_frame, osc.frame, osc.epoch)
372 .context(OrientationSnafu {
373 action: "transform state dcm",
374 })
375 .context(DynamicsAlmanacSnafu {
376 action: "transforming into gravity field frame",
377 })?
378 .rot_mat;
379
380 let mut dcm_d = Matrix3::<OHyperdual<f64, U7>>::zeros();
382 for i in 0..3 {
383 for j in 0..3 {
384 dcm_d[(i, j)] = OHyperdual::from_fn(|k| {
385 if k == 0 {
386 dcm[(i, j)]
387 } else if i + 1 == k {
388 1.0
389 } else {
390 0.0
391 }
392 })
393 }
394 }
395
396 let accel = dcm_d * Vector3::new(a0 + a3 * s_, a1 + a3 * t_, a2 + a3 * u_);
397 let mut dx = Vector3::zeros();
399 let mut grad = Matrix3::zeros();
400 for i in 0..3 {
401 dx[i] += accel[i].real();
402 for j in 1..4 {
404 grad[(i, j - 1)] += accel[i][j];
405 }
406 }
407 Ok((dx, grad))
408 }
409}