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