melvin_ob/flight_control/orbit/
closed_orbit.rs

1use super::orbit_base::OrbitBase;
2use crate::util::{Vec2D, VecAxis};
3use crate::imaging::CameraAngle;
4use crate::{fatal, warn};
5use bincode::{error::EncodeError, config::{Configuration, Fixint, LittleEndian}};
6use bitvec::{
7    bitbox,
8    order::Lsb0,
9    prelude::{BitBox, BitRef},
10};
11use fixed::types::I32F32;
12use std::env;
13use strum_macros::Display;
14
15/// Represents a single segment of the orbit path between two points.
16/// Used to model transitions across map boundaries and detect deviations from the orbit.
17#[derive(serde::Serialize, serde::Deserialize, Debug, Clone)]
18pub(super) struct OrbitSegment {
19    /// Starting position of the orbit segment.
20    start: Vec2D<I32F32>,
21    /// Ending position of the orbit segment.
22    end: Vec2D<I32F32>,
23    /// Delta vector from start to end.
24    delta: Vec2D<I32F32>,
25}
26
27impl OrbitSegment {
28    /// Creates a new orbit segment from two positions.
29    fn new(start: Vec2D<I32F32>, end: Vec2D<I32F32>) -> Self {
30        let delta = end - start;
31        Self { start, end, delta }
32    }
33
34    /// Returns the start point of the segment.
35    pub(crate) fn start(&self) -> &Vec2D<I32F32> { &self.start }
36    /// Returns the end point of the segment.
37    pub(crate) fn end(&self) -> &Vec2D<I32F32> { &self.end }
38    /// Computes the absolute vector distance from a point to the segment midpoint.
39    fn get_proj_dist(&self, pos: &Vec2D<I32F32>) -> (VecAxis, I32F32) {
40        let (t_x, t_y) = self.tx_tys(pos);
41
42        if t_x.is_negative() || t_x > I32F32::ONE || t_y.is_negative() || t_y > I32F32::ONE {
43            return (VecAxis::X, I32F32::MAX);
44        }
45
46        let proj_x = self.start.x() + self.delta.x() * t_y;
47        let proj_y = self.start.y() + self.delta.y() * t_x;
48
49        let deviation_x = proj_x - pos.x();
50        let deviation_y = proj_y - pos.y();
51
52        if deviation_x.abs() < deviation_y.abs() {
53            (VecAxis::X, deviation_x)
54        } else {
55            (VecAxis::Y, deviation_y)
56        }
57    }
58    /// Returns the projected deviation along the dominant axis from a given position to the segment.
59    fn tx_tys(&self, pos: &Vec2D<I32F32>) -> (I32F32, I32F32) {
60        let t_x = if self.delta.x().abs() > I32F32::DELTA {
61            (pos.x() - self.start.x()) / self.delta.x()
62        } else {
63            I32F32::ZERO
64        };
65
66        let t_y = if self.delta.y().abs() > I32F32::DELTA {
67            (pos.y() - self.start.y()) / self.delta.y()
68        } else {
69            I32F32::ZERO
70        };
71        (t_x, t_y)
72    }
73
74    /// Returns the shortest distance to the [`OrbitSegment`]
75    fn get_abs_dist(&self, pos: &Vec2D<I32F32>) -> Vec2D<I32F32> {
76        let (t_x, t_y) = self.tx_tys(pos);
77        let t_x_pos = *self.start() + self.delta * t_x;
78        let t_y_pos = *self.start() + self.delta * t_y;
79        let midpoint = (t_x_pos + t_y_pos) / I32F32::from_num(2);
80        pos.to(&midpoint)
81    }
82}
83
84/// Represents a closed orbit with a fixed period, image time information, and completion status.
85#[derive(serde::Serialize, serde::Deserialize, Debug)]
86pub struct ClosedOrbit {
87    /// The base configuration and parameters of the orbit.
88    base_orbit: OrbitBase,
89    /// The period of the orbit defined as a tuple:
90    /// - First element represents the total orbit time.
91    /// - Second and third element represent the x/y-period respectively.
92    period: (I32F32, I32F32, I32F32),
93    /// Maximum time interval between images that ensures proper coverage of the orbit.
94    max_image_dt: I32F32,
95    /// A bitvector indicating the completion status of orbit segments.
96    done: BitBox<usize, Lsb0>,
97    /// A vector containing all of the orbits segments.
98    segments: Vec<OrbitSegment>,
99}
100
101/// Represents possible errors that can occur when creating or verifying an orbit.
102#[derive(Debug, Display)]
103pub enum OrbitUsabilityError {
104    /// Indicates that the orbit is not closed (i.e., does not have a finite period).
105    OrbitNotClosed,
106    /// Indicates that the orbit does not have sufficient overlap to image properly.
107    OrbitNotEnoughOverlap,
108}
109
110impl ClosedOrbit {
111    /// ENV Var marking that the orbit configuration should be exported
112    const EXPORT_ORBIT_ENV: &'static str = "EXPORT_ORBIT";
113    /// ENV Var marking that it should be tried to import the orbit configuration
114    const TRY_IMPORT_ENV: &'static str = "TRY_IMPORT_ORBIT";
115    /// File were the orbit should be serialized to/deserialized from
116    const DEF_FILEPATH: &'static str = "orbit.bin";
117    /// Creates a new [`ClosedOrbit`] instance using a given [`OrbitBase`] and [`CameraAngle`].
118    ///
119    /// # Arguments
120    /// - `try_orbit`: The base orbit to initialize the closed orbit.
121    /// - `lens`: The camera lens angle used to determine image overlaps.
122    ///
123    /// # Returns
124    /// - `Ok(ClosedOrbit)` if the orbit is closed and sufficient overlap exists.
125    /// - `Err(OrbitUsabilityError)` if the orbit doesn't meet the requirements.
126    #[allow(clippy::cast_sign_loss, clippy::cast_possible_truncation)]
127    pub fn new(base_orbit: OrbitBase, lens: CameraAngle) -> Result<Self, OrbitUsabilityError> {
128        match base_orbit.period() {
129            None => Err(OrbitUsabilityError::OrbitNotClosed),
130            Some(period) => match base_orbit.max_image_dt(lens, period) {
131                None => Err(OrbitUsabilityError::OrbitNotEnoughOverlap),
132                Some(max_image_dt) => {
133                    let segments = Self::compute_segments(base_orbit.fp(), base_orbit.vel());
134                    let done = bitbox![usize, Lsb0; 0; period.0.to_num::<usize>()];
135                    Ok(Self { base_orbit, period, max_image_dt, done, segments })
136                }
137            },
138        }
139    }
140
141    /// Clears all completion tracking for the orbit.
142    pub fn clear_done(&mut self) {
143        self.done.fill(false);
144    }
145
146    /// Tries to import a previously serialized orbit if environment variable `TRY_IMPORT_ORBIT=1`.
147    pub fn try_from_env() -> Option<Self> {
148        if env::var(Self::TRY_IMPORT_ENV).is_ok_and(|s| s == "1") {
149            Self::import_from(Self::DEF_FILEPATH).ok()
150        } else {
151            None
152        }        
153    }
154
155    /// Tries to export the current orbit to disk if `EXPORT_ORBIT=1` is set in the environment.
156    pub fn try_export_default(&self) {
157        if env::var(Self::EXPORT_ORBIT_ENV).is_ok_and(|s| s == "1") {
158            self.export_to(Self::DEF_FILEPATH).unwrap_or_else(|e| {
159                warn!("Failed to export orbit: {}", e);
160            });
161        }
162    }
163
164    /// Deserializes a saved orbit from disk.
165    fn import_from(filename: &'static str) -> Result<Self, std::io::Error> {
166        let mut file = std::fs::OpenOptions::new().read(true).open(filename)?;
167        bincode::serde::decode_from_std_read(&mut file, Self::get_serde_config()).map_err(|e| {
168            fatal!("Failed to import orbit from {}: {}", filename, e);
169        })
170    }
171
172    /// Serializes the orbit to a given file path using fixed-size encoding.
173    fn export_to(&self, filename: &'static str) -> Result<(), EncodeError> {
174        let mut file = std::fs::OpenOptions::new()
175            .create(true)
176            .write(true)
177            .truncate(true)
178            .open(filename)
179            .unwrap();
180        bincode::serde::encode_into_std_write(self, &mut file, Self::get_serde_config())?;
181        Ok(())
182    }
183
184    /// Returns a `bincode` serialization config with little-endian fixed-width layout.
185    fn get_serde_config() -> Configuration<LittleEndian, Fixint> {
186        bincode::config::standard().with_little_endian().with_fixed_int_encoding()
187    }
188
189    /// Computes all forward-wrapped orbit segments.
190    fn compute_segments(base_point: &Vec2D<I32F32>, vel: &Vec2D<I32F32>) -> Vec<OrbitSegment> {
191        let mut segments = Vec::new();
192
193        let mut current_point = base_point.project_overboundary_bw(vel);
194        let mut visited_points = Vec::new();
195
196        loop {
197            let min = visited_points
198                .iter()
199                .map(|p| (p, current_point.euclid_distance(p)))
200                .min_by(|&(_, dist1), &(_, dist2)| dist1.cmp(&dist2))
201                .map(|(p, _)| p);
202
203            if let Some(min_point) = min {
204                if current_point.euclid_distance(min_point) < 2 * vel.abs() {
205                    break;
206                }
207            } else {
208                visited_points.push(current_point);
209            }
210
211            let next_point = current_point.project_overboundary_fw(vel);
212            segments.push(OrbitSegment::new(current_point, next_point));
213            // wrap next_point back onto the plane
214            current_point = next_point.wrap_around_map().project_overboundary_bw(vel);
215        }
216        segments
217    }
218
219    /// Returns an iterator that reorders the `done` bitvector sequence based on a specified shift.
220    ///
221    /// # Arguments
222    /// - `shift_start`: The starting index of the reordering shift.
223    /// - `shift_end`: The ending index of the reordering shift.
224    ///
225    /// # Returns
226    /// - An iterator over the reordered `done` bitvector segment.
227    ///
228    /// # Panics
229    /// - If `shift_start` or `shift_end` exceed the length of the bitvector.
230    pub fn get_p_t_reordered(
231        &self,
232        shift_start: usize,
233        shift_end: usize,
234    ) -> Box<dyn Iterator<Item = BitRef> + '_> {
235        assert!(
236            shift_start < self.done.len() && shift_end <= self.done.len(),
237            "[FATAL] Shift is larger than the orbit length"
238        );
239        Box::new(
240            self.done[shift_start..]
241                .iter()
242                .chain(self.done[..shift_start].iter())
243                .rev()
244                .skip(shift_end),
245        )
246    }
247
248    /// Marks a specified range of orbit segments as completed in the `done` bitvector.
249    ///
250    /// # Arguments
251    /// - `first_i`: The first index of the range to mark as completed.
252    /// - `last_i`: The last index of the range to mark as completed.
253    pub fn mark_done(&mut self, first_i: usize, last_i: usize) {
254        self.done
255            .as_mut_bitslice()
256            .get_mut(first_i..=last_i)
257            .unwrap()
258            .iter_mut()
259            .for_each(|mut b| *b = true);
260    }
261
262    pub fn get_closest_deviation(&self, pos: Vec2D<I32F32>) -> (VecAxis, I32F32) {
263        self.segments
264            .iter()
265            .map(|seg| seg.get_proj_dist(&pos))
266            .min_by(|a, b| a.1.abs().cmp(&b.1.abs()))
267            .unwrap()
268    }
269
270    /// Returns the maximum image time interval for the orbit.
271    ///
272    /// # Returns
273    /// - `I32F32` representing the maximum imaging time interval.
274    pub fn max_image_dt(&self) -> I32F32 { self.max_image_dt }
275
276    /// Returns a reference to the base orbit configuration.
277    ///
278    /// # Returns
279    /// - A reference to the associated `OrbitBase`.
280    pub fn base_orbit_ref(&self) -> &OrbitBase { &self.base_orbit }
281
282    /// Returns the period tuple of the closed orbit.
283    ///
284    /// # Returns
285    /// - A tuple `(I32F32, I32F32, I32F32)` representing the orbit's period.
286    pub fn period(&self) -> (I32F32, I32F32, I32F32) { self.period }
287
288    /// Checks whether the specified position on the map will be visited during the orbit.
289    ///
290    /// # Arguments
291    /// - `pos`: The position to check.
292    ///
293    /// # Returns
294    /// - `true`: If the position will be visited during the orbit.
295    /// - `false`: Otherwise.
296    pub fn will_visit(&self, pos: Vec2D<I32F32>) -> bool {
297        self.segments
298            .iter()
299            .map(|seg| seg.get_abs_dist(&pos))
300            .min_by(|a, b| a.abs().cmp(&b.abs()))
301            .unwrap()
302            .abs()
303            < I32F32::lit("1.0")
304    }
305
306    /// Returns the done-vector index corresponding to the current position, if on the orbit.
307    pub fn get_i(&self, pos: Vec2D<I32F32>) -> Option<usize> {
308        if self.will_visit(pos) {
309            let step = *self.base_orbit.vel();
310            let step_abs = step.abs();
311            let mut i_pos = *self.base_orbit.fp();
312            for i in 0..self.period.0.to_num::<usize>() {
313                let mut dx_abs = i_pos.euclid_distance(&pos);
314                if dx_abs < step_abs * 2 {
315                    let mut next = (i_pos + step).wrap_around_map();
316                    let mut add_i = 0;
317                    while next.wrap_around_map().euclid_distance(&pos) < dx_abs {
318                        add_i += 1;
319                        next = (next + step).wrap_around_map();
320                        dx_abs = next.euclid_distance(&pos);
321                    }
322                    return Some(i + add_i);
323                }
324                i_pos = (i_pos + step).wrap_around_map();
325            }
326        }
327        None
328    }
329
330    /// Returns a reference to all orbit segments.
331    pub(super) fn segments(&self) -> &Vec<OrbitSegment> { &self.segments }
332    
333    /// Calculates the coverage from the done - bitmap
334    pub fn get_coverage(&self) -> I32F32 {
335        let zeros = I32F32::from_num(self.done.count_zeros());
336        let length = I32F32::from_num(self.done.len());
337        zeros / length
338    }
339}
melvin_ob/flight_control/orbit/closed_orbit.rs

melvin_ob/flight_control/orbit/
closed_orbit.rs
