lox_orbits/
ground.rs

1/*
2 * Copyright (c) 2024. Helge Eichhorn and the LOX contributors
3 *
4 * This Source Code Form is subject to the terms of the Mozilla Public
5 * License, v. 2.0. If a copy of the MPL was not distributed with this
6 * file, you can obtain one at https://mozilla.org/MPL/2.0/.
7 */
8
9use std::f64::consts::FRAC_PI_2;
10
11use crate::frames::{DynFrame, Iau, Icrf, TryRotateTo};
12use crate::propagators::Propagator;
13use crate::states::{DynState, State};
14use crate::trajectories::{DynTrajectory, Trajectory, TrajectoryError};
15use glam::{DMat3, DVec3};
16use lox_bodies::{DynOrigin, RotationalElements, Spheroid, TrySpheroid};
17use lox_math::types::units::Radians;
18use lox_time::time_scales::TryToScale;
19use lox_time::time_scales::{Tdb, TimeScale};
20use lox_time::ut1::DeltaUt1TaiProvider;
21use lox_time::{DynTime, Time};
22use thiserror::Error;
23
24#[derive(Clone, Debug)]
25pub struct Observables {
26    azimuth: Radians,
27    elevation: Radians,
28    range: f64,
29    range_rate: f64,
30}
31
32impl Observables {
33    pub fn new(azimuth: Radians, elevation: Radians, range: f64, range_rate: f64) -> Self {
34        Observables {
35            azimuth,
36            elevation,
37            range,
38            range_rate,
39        }
40    }
41    pub fn azimuth(&self) -> Radians {
42        self.azimuth
43    }
44
45    pub fn elevation(&self) -> Radians {
46        self.elevation
47    }
48
49    pub fn range(&self) -> f64 {
50        self.range
51    }
52
53    pub fn range_rate(&self) -> f64 {
54        self.range_rate
55    }
56}
57
58#[derive(Clone, Debug)]
59pub struct GroundLocation<B: TrySpheroid> {
60    longitude: f64,
61    latitude: f64,
62    altitude: f64,
63    body: B,
64}
65
66pub type DynGroundLocation = GroundLocation<DynOrigin>;
67
68impl<B: Spheroid> GroundLocation<B> {
69    pub fn new(longitude: f64, latitude: f64, altitude: f64, body: B) -> Self {
70        GroundLocation {
71            longitude,
72            latitude,
73            altitude,
74            body,
75        }
76    }
77}
78
79impl DynGroundLocation {
80    pub fn with_dynamic(
81        longitude: f64,
82        latitude: f64,
83        altitude: f64,
84        body: DynOrigin,
85    ) -> Result<Self, &'static str> {
86        if body.try_equatorial_radius().is_err() {
87            return Err("no spheroid");
88        }
89        Ok(GroundLocation {
90            longitude,
91            latitude,
92            altitude,
93            body,
94        })
95    }
96}
97
98impl<B: TrySpheroid> GroundLocation<B> {
99    pub fn origin(&self) -> B
100    where
101        B: Clone,
102    {
103        self.body.clone()
104    }
105
106    pub fn longitude(&self) -> f64 {
107        self.longitude
108    }
109
110    pub fn latitude(&self) -> f64 {
111        self.latitude
112    }
113
114    pub fn altitude(&self) -> f64 {
115        self.altitude
116    }
117
118    fn equatorial_radius(&self) -> f64 {
119        self.body
120            .try_equatorial_radius()
121            .expect("equatorial radius should be available")
122    }
123
124    fn flattening(&self) -> f64 {
125        self.body
126            .try_flattening()
127            .expect("flattening should be available")
128    }
129
130    pub fn body_fixed_position(&self) -> DVec3 {
131        let alt = self.altitude;
132        let (lon_sin, lon_cos) = self.longitude.sin_cos();
133        let (lat_sin, lat_cos) = self.latitude.sin_cos();
134        let f = self.flattening();
135        let r_eq = self.equatorial_radius();
136        let e = (2.0 * f - f.powi(2)).sqrt();
137        let c = r_eq / (1.0 - e.powi(2) * lat_sin.powi(2)).sqrt();
138        let s = c * (1.0 - e.powi(2));
139        let r_delta = (c + alt) * lat_cos;
140        let r_kappa = (s + alt) * lat_sin;
141        DVec3::new(r_delta * lon_cos, r_delta * lon_sin, r_kappa)
142    }
143
144    pub fn rotation_to_topocentric(&self) -> DMat3 {
145        let rot1 = DMat3::from_rotation_z(self.longitude()).transpose();
146        let rot2 = DMat3::from_rotation_y(FRAC_PI_2 - self.latitude()).transpose();
147        rot2 * rot1
148    }
149
150    pub fn observables<T: TimeScale + Clone>(&self, state: State<T, B, Iau<B>>) -> Observables
151    where
152        B: RotationalElements + Clone,
153    {
154        let rot = self.rotation_to_topocentric();
155        let position = rot * (state.position() - self.body_fixed_position());
156        let velocity = rot * state.velocity();
157        let range = position.length();
158        let range_rate = position.dot(velocity) / range;
159        let elevation = (position.z / range).asin();
160        let azimuth = position.y.atan2(-position.x);
161        Observables {
162            azimuth,
163            elevation,
164            range,
165            range_rate,
166        }
167    }
168
169    pub fn observables_dyn(&self, state: DynState) -> Observables {
170        let rot = self.rotation_to_topocentric();
171        let position = rot * (state.position() - self.body_fixed_position());
172        let velocity = rot * state.velocity();
173        let range = position.length();
174        let range_rate = position.dot(velocity) / range;
175        let elevation = (position.z / range).asin();
176        let azimuth = position.y.atan2(-position.x);
177        Observables {
178            azimuth,
179            elevation,
180            range,
181            range_rate,
182        }
183    }
184}
185
186#[derive(Debug, Error)]
187pub enum GroundPropagatorError {
188    #[error("frame transformation error: {0}")]
189    FrameTransformation(String),
190    #[error(transparent)]
191    Trajectory(#[from] TrajectoryError),
192}
193
194pub struct GroundPropagator<B: TrySpheroid, P> {
195    location: GroundLocation<B>,
196    // FIXME: We should not take ownership of the provider here
197    provider: Option<P>,
198}
199
200pub type DynGroundPropagator<P> = GroundPropagator<DynOrigin, P>;
201
202impl<B, P> GroundPropagator<B, P>
203where
204    B: Spheroid,
205{
206    pub fn new(location: GroundLocation<B>, provider: Option<P>) -> Self {
207        GroundPropagator { location, provider }
208    }
209}
210
211impl<P: DeltaUt1TaiProvider> DynGroundPropagator<P> {
212    pub fn with_dynamic(location: DynGroundLocation, provider: Option<P>) -> Self {
213        GroundPropagator { location, provider }
214    }
215
216    pub fn propagate_dyn(&self, time: DynTime) -> Result<DynState, GroundPropagatorError> {
217        let s = State::new(
218            time,
219            self.location.body_fixed_position(),
220            DVec3::ZERO,
221            self.location.body,
222            DynFrame::Iau(self.location.body),
223        );
224        let rot = s
225            .reference_frame()
226            .try_rotation(DynFrame::Icrf, time, self.provider.as_ref())
227            .map_err(|err| GroundPropagatorError::FrameTransformation(err.to_string()))?;
228        let (r1, v1) = rot.rotate_state(s.position(), s.velocity());
229        Ok(State::new(time, r1, v1, self.location.body, DynFrame::Icrf))
230    }
231
232    pub(crate) fn propagate_all_dyn(
233        &self,
234        times: impl IntoIterator<Item = DynTime>,
235    ) -> Result<DynTrajectory, GroundPropagatorError> {
236        let mut states = vec![];
237        for time in times {
238            let state = self.propagate_dyn(time)?;
239            states.push(state);
240        }
241        Ok(Trajectory::new(&states)?)
242    }
243}
244
245impl<T, O, P> Propagator<T, O, Icrf> for GroundPropagator<O, P>
246where
247    T: TimeScale + TryToScale<Tdb, P> + Clone,
248    O: Spheroid + RotationalElements + Clone,
249{
250    type Error = GroundPropagatorError;
251
252    fn propagate(&self, time: Time<T>) -> Result<State<T, O, Icrf>, Self::Error> {
253        let s = State::new(
254            time.clone(),
255            self.location.body_fixed_position(),
256            DVec3::ZERO,
257            self.location.body.clone(),
258            Iau(self.location.body.clone()),
259        );
260        let rot = s
261            .reference_frame()
262            .try_rotation(Icrf, time.clone(), self.provider.as_ref())
263            .map_err(|err| GroundPropagatorError::FrameTransformation(err.to_string()))?;
264        let (r1, v1) = rot.rotate_state(s.position(), s.velocity());
265        Ok(State::new(time, r1, v1, self.location.body.clone(), Icrf))
266    }
267}
268
269#[cfg(test)]
270mod tests {
271    use float_eq::assert_float_eq;
272
273    use lox_bodies::Earth;
274    use lox_math::assert_close;
275    use lox_math::is_close::IsClose;
276    use lox_time::utc::Utc;
277    use lox_time::{Time, time, utc};
278
279    use super::*;
280
281    #[test]
282    fn test_ground_location_to_body_fixed() {
283        let longitude = -4.3676f64.to_radians();
284        let latitude = 40.4527f64.to_radians();
285        let location = GroundLocation::new(longitude, latitude, 0.0, Earth);
286        let expected = DVec3::new(4846.130017870638, -370.1328551351891, 4116.364272747229);
287        assert_close!(location.body_fixed_position(), expected);
288    }
289
290    #[test]
291    fn test_ground_location_rotation_to_topocentric() {
292        let longitude = -4.3676f64.to_radians();
293        let latitude = 40.4527f64.to_radians();
294        let location = GroundLocation::new(longitude, latitude, 0.0, Earth);
295        let rot = location.rotation_to_topocentric();
296        let expected = DMat3::from_cols(
297            DVec3::new(0.6469358921661584, 0.07615519584215287, 0.7587320591443464),
298            DVec3::new(
299                -0.049411020334552434,
300                0.9970959763965771,
301                -0.05794967578213965,
302            ),
303            DVec3::new(-0.7609418522440956, 0.0, 0.6488200809957448),
304        );
305        assert_close!(rot.x_axis, expected.x_axis);
306        assert_close!(rot.y_axis, expected.y_axis);
307        assert_close!(rot.z_axis, expected.z_axis);
308    }
309
310    #[test]
311    fn test_ground_location_observables() {
312        let longitude = -4f64.to_radians();
313        let latitude = 41f64.to_radians();
314        let location = GroundLocation::new(longitude, latitude, 0.0, Earth);
315        let position = DVec3::new(3359.927, -2398.072, 5153.0);
316        let velocity = DVec3::new(5.0657, 5.485, -0.744);
317        let time = time!(Tdb, 2012, 7, 1).unwrap();
318        let state = State::new(time, position, velocity, Earth, Iau(Earth));
319        let observables = location.observables(state);
320        let expected_range = 2707.7;
321        let expected_range_rate = -7.16;
322        let expected_azimuth = -53.418f64.to_radians();
323        let expected_elevation = -7.077f64.to_radians();
324        assert_float_eq!(observables.range, expected_range, rel <= 1e-2);
325        assert_float_eq!(observables.range_rate, expected_range_rate, rel <= 1e-2);
326        assert_float_eq!(observables.azimuth, expected_azimuth, rel <= 1e-2);
327        assert_float_eq!(observables.elevation, expected_elevation, rel <= 1e-2);
328    }
329
330    #[test]
331    fn test_ground_propagator() {
332        let longitude = -4.3676f64.to_radians();
333        let latitude = 40.4527f64.to_radians();
334        let location = GroundLocation::new(longitude, latitude, 0.0, Earth);
335        let propagator = GroundPropagator::new(location, None::<()>);
336        let time = utc!(2022, 1, 31, 23).unwrap().to_time();
337        let expected = DVec3::new(-1765.9535510583582, 4524.585984442561, 4120.189198495323);
338        let state = propagator.propagate(time).unwrap();
339        assert_close!(state.position(), expected);
340    }
341}
lox_orbits/ground.rs

lox_orbits/
ground.rs
