nyx_space/od/groundpnt/
ground_dynamics.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use crate::dynamics::{Dynamics, DynamicsError};
20use crate::od::groundpnt::GroundAsset;
21use anise::prelude::Almanac;
22use nalgebra::allocator::Allocator;
23use nalgebra::{Const, DefaultAllocator, Matrix6, OMatrix, OVector, Vector6};
24use std::sync::Arc;
25
26#[derive(Clone)]
27pub struct GroundDynamics {}
28
29impl Dynamics for GroundDynamics {
30    type StateType = GroundAsset;
31    type HyperdualSize = Const<6>;
32
33    fn eom(
34        &self,
35        _delta_t: f64,
36        _state_vec: &OVector<f64, <Self::StateType as crate::State>::VecLength>,
37        state_ctx: &Self::StateType,
38        _almanac: Arc<Almanac>,
39    ) -> Result<OVector<f64, <Self::StateType as crate::State>::VecLength>, DynamicsError>
40    where
41        DefaultAllocator: Allocator<<Self::StateType as crate::State>::VecLength>,
42    {
43        let d_x = Vector6::from_iterator([
44            state_ctx.latitude_rate_deg_s,
45            state_ctx.longitude_rate_deg_s,
46            state_ctx.height_rate_km_s,
47            0.0,
48            0.0,
49            0.0,
50        ]);
51
52        Ok(OVector::<f64, Const<42>>::from_iterator(
53            d_x.iter()
54                .chain(OVector::<f64, Const<36>>::zeros().iter())
55                .cloned(),
56        ))
57    }
58
59    fn dual_eom(
60        &self,
61        _delta_t: f64,
62        osc: &Self::StateType,
63        _almanac: Arc<Almanac>,
64    ) -> Result<
65        (
66            OVector<f64, <Self::StateType as crate::State>::Size>,
67            OMatrix<
68                f64,
69                <Self::StateType as crate::State>::Size,
70                <Self::StateType as crate::State>::Size,
71            >,
72        ),
73        DynamicsError,
74    >
75    where
76        DefaultAllocator: Allocator<Self::HyperdualSize>
77            + Allocator<<Self::StateType as crate::State>::Size>
78            + Allocator<
79                <Self::StateType as crate::State>::Size,
80                <Self::StateType as crate::State>::Size,
81            >,
82        nalgebra::Owned<f64, Self::HyperdualSize>: Copy,
83    {
84        // The math is very simple here, so we're skipping the hyperdual represenation.
85        let dx = Vector6::new(
86            osc.latitude_rate_deg_s,
87            osc.longitude_rate_deg_s,
88            osc.height_rate_km_s,
89            0.0,
90            0.0,
91            0.0,
92        );
93
94        // A-matrix for kinematic (no acceleration) model:
95        // d/dt[pos] = vel  →  ∂(pos_dot)/∂vel = I₃
96        // d/dt[vel] = 0    →  all velocity partials are zero
97        let mut grad = Matrix6::zeros();
98        grad[(0, 3)] = 1.0;
99        grad[(1, 4)] = 1.0;
100        grad[(2, 5)] = 1.0;
101
102        Ok((dx, grad))
103    }
104}
nyx_space/od/groundpnt/ground_dynamics.rs

nyx_space/od/groundpnt/
ground_dynamics.rs
