Skip to main content

skybrush/trajectory/
mod.rs

1//! Trajectories of drones.
2//!
3//! A trajectory is kept in its encoded form, as in the body of a trajectory
4//! block of a show file, and decoded one segment at a time by a
5//! [`TrajectoryPlayer`]. A trajectory read from a show file in memory borrows
6//! its bytes from the show file.
7
8mod builder;
9mod edit;
10mod player;
11mod segment;
12mod stats;
13
14use alloc::borrow::Cow;
15use alloc::vec::Vec;
16use core::{error, fmt};
17
18pub use builder::{TrajectoryBuilder, TrajectoryBuilderError};
19pub use edit::LandingError;
20pub use player::{TrajectoryPlayer, TrajectoryPlayerState};
21pub use segment::TrajectorySegment;
22pub use stats::{
23    LandingStats, TakeoffStats, TrajectoryStatComponents, TrajectoryStats,
24    TrajectoryStatsCalculator, TrajectoryStatsError,
25};
26
27use crate::basic_types::{BoundingBox, Interval, Vector3WithYaw};
28use crate::formats::binary::{BinaryBlockType, BinaryFile, BinaryFileError};
29use crate::parsing::read_i16;
30use crate::poly::Poly;
31use segment::{coords_length, msec_to_sec};
32
33/// The length of the header of a trajectory block: a flags byte and four
34/// 16-bit start coordinates, the last one retired (it held the start yaw of
35/// the legacy embedded yaw track).
36const HEADER_LENGTH: usize = 9;
37
38/// Why a trajectory could not be read.
39#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
40#[non_exhaustive]
41pub enum TrajectoryError {
42    /// The body of the trajectory block is malformed: it is shorter than the
43    /// header, or a segment does not fit in it.
44    Corrupted,
45    /// The show file could not be read.
46    File(BinaryFileError),
47}
48
49impl fmt::Display for TrajectoryError {
50    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
51        match self {
52            Self::Corrupted => f.write_str("the trajectory is malformed"),
53            Self::File(_) => f.write_str("the show file could not be read"),
54        }
55    }
56}
57
58impl error::Error for TrajectoryError {
59    fn source(&self) -> Option<&(dyn error::Error + 'static)> {
60        match self {
61            Self::Corrupted => None,
62            Self::File(err) => Some(err),
63        }
64    }
65}
66
67impl From<BinaryFileError> for TrajectoryError {
68    fn from(err: BinaryFileError) -> Self {
69        Self::File(err)
70    }
71}
72
73/// The trajectory of a drone.
74///
75/// The bytes of the trajectory are borrowed when it is read from a show file
76/// in memory, and owned when they were built or modified.
77///
78/// Cutting a borrowed trajectory at the boundary of a segment narrows the
79/// borrowed bytes. Other modifications (cutting within a segment, replacing
80/// the end with a landing) copy the bytes that are kept, like the C library
81/// does. An alternative that would save that memory: keep borrowing the unmodified segments, and
82/// store the few modified or appended segments at the end inline, in a small
83/// fixed buffer. A cut rewrites at most one segment and a landing appends one,
84/// so this would need no allocation, at the cost of a more complex player.
85#[derive(Clone, Debug, PartialEq)]
86pub struct Trajectory<'a> {
87    /// The header and the segments, as in the body of a trajectory block.
88    bytes: Cow<'a, [u8]>,
89    /// The start position of the trajectory.
90    start: Vector3WithYaw,
91    /// The scale of the coordinates: each unit is this many millimeters.
92    scale: f32,
93    /// The length of the header in `bytes`.
94    header_length: usize,
95}
96
97impl Default for Trajectory<'static> {
98    fn default() -> Self {
99        Self::new()
100    }
101}
102
103impl Trajectory<'static> {
104    /// An empty trajectory at the origin.
105    #[must_use]
106    pub const fn new() -> Self {
107        Self {
108            bytes: Cow::Owned(Vec::new()),
109            start: Vector3WithYaw {
110                x: 0.0,
111                y: 0.0,
112                z: 0.0,
113                yaw: 0.0,
114            },
115            scale: 1.0,
116            header_length: 0,
117        }
118    }
119
120    /// A trajectory that owns the given body of a trajectory block.
121    ///
122    /// # Errors
123    ///
124    /// Returns [`TrajectoryError::Corrupted`] if the body is malformed.
125    pub fn from_vec(bytes: Vec<u8>) -> Result<Self, TrajectoryError> {
126        Self::from_cow(Cow::Owned(bytes))
127    }
128}
129
130impl<'a> Trajectory<'a> {
131    /// A trajectory that borrows the given body of a trajectory block.
132    ///
133    /// # Errors
134    ///
135    /// Returns [`TrajectoryError::Corrupted`] if the body is malformed.
136    pub fn from_buffer(bytes: &'a [u8]) -> Result<Self, TrajectoryError> {
137        Self::from_cow(Cow::Borrowed(bytes))
138    }
139
140    /// The trajectory of a show file, borrowing its bytes from the show file,
141    /// or `None` if the show file has no trajectory.
142    ///
143    /// # Errors
144    ///
145    /// Returns an error if the trajectory block is truncated or malformed.
146    pub fn from_binary_file(file: &BinaryFile<'a>) -> Result<Option<Self>, TrajectoryError> {
147        file.find_first_block_by_type(BinaryBlockType::TRAJECTORY)?
148            .map(|block| Self::from_buffer(block.body))
149            .transpose()
150    }
151
152    fn from_cow(bytes: Cow<'a, [u8]>) -> Result<Self, TrajectoryError> {
153        validate(&bytes)?;
154        let scale = f32::from(scale_of(bytes.first().copied().unwrap_or_default()));
155        // The fourth start coordinate belongs to the legacy yaw track and is
156        // ignored
157        let coordinate = |offset| f32::from(read_i16(&bytes, offset).unwrap_or_default()) * scale;
158        let start = Vector3WithYaw {
159            x: coordinate(1),
160            y: coordinate(3),
161            z: coordinate(5),
162            yaw: 0.0,
163        };
164        Ok(Self {
165            bytes,
166            start,
167            scale,
168            header_length: HEADER_LENGTH,
169        })
170    }
171
172    /// The trajectory with its bytes owned, copying them if they are
173    /// borrowed.
174    #[must_use]
175    pub fn into_owned(self) -> Trajectory<'static> {
176        Trajectory {
177            bytes: Cow::Owned(self.bytes.into_owned()),
178            start: self.start,
179            scale: self.scale,
180            header_length: self.header_length,
181        }
182    }
183
184    /// The bytes of the trajectory, as in the body of a trajectory block.
185    #[must_use]
186    pub fn bytes(&self) -> &[u8] {
187        &self.bytes
188    }
189
190    /// Whether the trajectory is empty: it has no bytes, or its scale is zero.
191    #[must_use]
192    pub fn is_empty(&self) -> bool {
193        self.bytes.first().is_none_or(|&flags| scale_of(flags) == 0)
194    }
195
196    /// The scale of the coordinates: each unit is this many millimeters.
197    #[must_use]
198    pub const fn scale(&self) -> f32 {
199        self.scale
200    }
201
202    /// The start position of the trajectory, as in its header.
203    pub(crate) const fn start(&self) -> Vector3WithYaw {
204        self.start
205    }
206
207    pub(crate) const fn header_length(&self) -> usize {
208        self.header_length
209    }
210
211    /// The position at time zero.
212    #[must_use]
213    pub fn start_position(&self) -> Vector3WithYaw {
214        TrajectoryPlayer::new(self).position_at(0.0)
215    }
216
217    /// The position after the end of the trajectory.
218    #[must_use]
219    pub fn end_position(&self) -> Vector3WithYaw {
220        TrajectoryPlayer::new(self).position_at(f32::INFINITY)
221    }
222
223    /// The total duration of the trajectory, in milliseconds.
224    #[must_use]
225    pub fn total_duration_msec(&self) -> u32 {
226        TrajectoryPlayer::new(self).total_duration_msec()
227    }
228
229    /// The total duration of the trajectory, in seconds.
230    #[must_use]
231    pub fn total_duration_sec(&self) -> f32 {
232        msec_to_sec(self.total_duration_msec())
233    }
234
235    /// The segment that contains the given time, in seconds, with its
236    /// derivatives calculated, and the relative time within the segment.
237    #[must_use]
238    pub fn segment_at(&self, time_sec: f32) -> (TrajectoryPlayerState, f32) {
239        let mut player = TrajectoryPlayer::new(self);
240        let rel_time = player.seek_to_time(time_sec);
241        let segment = player.current_segment();
242        segment.dpoly();
243        segment.ddpoly();
244        (player.save_state(), rel_time)
245    }
246
247    /// The axis-aligned bounding box of the segments of the trajectory. The
248    /// intervals are empty (from infinity to minus infinity) if the trajectory
249    /// has no segments.
250    #[must_use]
251    pub fn axis_aligned_bounding_box(&self) -> BoundingBox {
252        let empty = Interval {
253            min: f32::INFINITY,
254            max: f32::NEG_INFINITY,
255        };
256        let mut result = BoundingBox {
257            x: empty,
258            y: empty,
259            z: empty,
260        };
261        let mut player = TrajectoryPlayer::new(self);
262        while player.has_more_segments() {
263            let poly = *player.current_segment().poly();
264            for (interval, poly) in [
265                (&mut result.x, &poly.x),
266                (&mut result.y, &poly.y),
267                (&mut result.z, &poly.z),
268            ] {
269                let extrema = extrema_of(poly);
270                if extrema.min < interval.min {
271                    interval.min = extrema.min;
272                }
273                if extrema.max > interval.max {
274                    interval.max = extrema.max;
275                }
276            }
277            player.build_next_segment();
278        }
279        result
280    }
281}
282
283/// The minimum and maximum of a polynomial of a segment on `[0, 1]`. Above
284/// degree 3, where the extrema are not implemented, the minimum and maximum
285/// of its control points are used instead, which bound a Bézier curve.
286fn extrema_of(poly: &Poly) -> Interval {
287    poly.extrema()
288        .unwrap_or_else(|_| control_point_bounds(poly))
289}
290
291/// The minimum and maximum of the Bézier control points of a polynomial on
292/// `[0, 1]`.
293fn control_point_bounds(poly: &Poly) -> Interval {
294    let coeffs = poly.coeffs();
295    let n = coeffs.len().saturating_sub(1);
296    let mut result = Interval {
297        min: f32::INFINITY,
298        max: f32::NEG_INFINITY,
299    };
300    // The k-th control point is the sum of binomial(k, i) / binomial(n, i)
301    // times the i-th coefficient, over i from 0 to k
302    for k in 0..=n {
303        let mut point = 0.0;
304        for (i, &coeff) in coeffs.iter().enumerate().take(k + 1) {
305            point += binomial(k, i) / binomial(n, i) * coeff;
306        }
307        if point < result.min {
308            result.min = point;
309        }
310        if point > result.max {
311            result.max = point;
312        }
313    }
314    result
315}
316
317#[expect(
318    clippy::cast_precision_loss,
319    reason = "binomials of numbers up to the maximum degree are small"
320)]
321fn binomial(n: usize, k: usize) -> f32 {
322    let mut result: u32 = 1;
323    for i in 0..k.min(n) {
324        let (Ok(numerator), Ok(denominator)) = (u32::try_from(n - i), u32::try_from(i + 1)) else {
325            return f32::NAN;
326        };
327        result = result.saturating_mul(numerator) / denominator;
328    }
329    result as f32
330}
331
332/// The scale in the flags byte of a trajectory: its lowest 7 bits. The highest
333/// bit marked the legacy embedded yaw track and is ignored.
334const fn scale_of(flags: u8) -> u8 {
335    flags & 0x7f
336}
337
338/// Checks that the body of a trajectory block is well-formed: it has a
339/// complete header, and every segment fits in it. Segments are not checked if
340/// the scale is zero, since the trajectory is empty then.
341fn validate(bytes: &[u8]) -> Result<(), TrajectoryError> {
342    if bytes.len() < HEADER_LENGTH {
343        return Err(TrajectoryError::Corrupted);
344    }
345    if bytes.first().is_some_and(|&flags| scale_of(flags) == 0) {
346        return Ok(());
347    }
348    let mut rest = bytes.get(HEADER_LENGTH..).unwrap_or_default();
349    while let Some(&header) = rest.first() {
350        if rest.len() < 3 {
351            return Err(TrajectoryError::Corrupted);
352        }
353        rest = rest
354            .get(3 + coords_length(header)..)
355            .ok_or(TrajectoryError::Corrupted)?;
356    }
357    Ok(())
358}