skybrush/trajectory/
mod.rs1mod 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
33const HEADER_LENGTH: usize = 9;
37
38#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
40#[non_exhaustive]
41pub enum TrajectoryError {
42 Corrupted,
45 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#[derive(Clone, Debug, PartialEq)]
86pub struct Trajectory<'a> {
87 bytes: Cow<'a, [u8]>,
89 start: Vector3WithYaw,
91 scale: f32,
93 header_length: usize,
95}
96
97impl Default for Trajectory<'static> {
98 fn default() -> Self {
99 Self::new()
100 }
101}
102
103impl Trajectory<'static> {
104 #[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 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 pub fn from_buffer(bytes: &'a [u8]) -> Result<Self, TrajectoryError> {
137 Self::from_cow(Cow::Borrowed(bytes))
138 }
139
140 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 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 #[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 #[must_use]
186 pub fn bytes(&self) -> &[u8] {
187 &self.bytes
188 }
189
190 #[must_use]
192 pub fn is_empty(&self) -> bool {
193 self.bytes.first().is_none_or(|&flags| scale_of(flags) == 0)
194 }
195
196 #[must_use]
198 pub const fn scale(&self) -> f32 {
199 self.scale
200 }
201
202 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 #[must_use]
213 pub fn start_position(&self) -> Vector3WithYaw {
214 TrajectoryPlayer::new(self).position_at(0.0)
215 }
216
217 #[must_use]
219 pub fn end_position(&self) -> Vector3WithYaw {
220 TrajectoryPlayer::new(self).position_at(f32::INFINITY)
221 }
222
223 #[must_use]
225 pub fn total_duration_msec(&self) -> u32 {
226 TrajectoryPlayer::new(self).total_duration_msec()
227 }
228
229 #[must_use]
231 pub fn total_duration_sec(&self) -> f32 {
232 msec_to_sec(self.total_duration_msec())
233 }
234
235 #[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 #[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
283fn extrema_of(poly: &Poly) -> Interval {
287 poly.extrema()
288 .unwrap_or_else(|_| control_point_bounds(poly))
289}
290
291fn 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 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
332const fn scale_of(flags: u8) -> u8 {
335 flags & 0x7f
336}
337
338fn 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}