use core::cell::OnceCell;
use crate::basic_types::Vector3WithYaw;
use crate::parsing::{read_i16, read_u16};
use crate::poly::{MAX_POLY_COEFFS, Poly, Poly4D};
pub(crate) fn num_coords(bits: u8) -> usize {
1 << (bits & 0x03)
}
pub(crate) fn coords_length(header: u8) -> usize {
[0, 2, 4, 6]
.into_iter()
.map(|shift| 2 * (num_coords(header >> shift) - 1))
.sum()
}
#[derive(Clone, Debug)]
pub struct TrajectorySegment {
start_time_msec: u32,
end_time_msec: u32,
duration_msec: u32,
start_time_sec: f32,
end_time_sec: f32,
duration_sec: f32,
start: Vector3WithYaw,
end: Vector3WithYaw,
points: [[f32; MAX_POLY_COEFFS]; 3],
num_points: [usize; 3],
poly: OnceCell<Poly4D>,
dpoly: OnceCell<Poly4D>,
ddpoly: OnceCell<Poly4D>,
}
impl PartialEq for TrajectorySegment {
fn eq(&self, other: &Self) -> bool {
self.start_time_msec == other.start_time_msec
&& self.end_time_msec == other.end_time_msec
&& self.duration_msec == other.duration_msec
&& self.start_time_sec == other.start_time_sec
&& self.end_time_sec == other.end_time_sec
&& self.duration_sec == other.duration_sec
&& self.start == other.start
&& self.end == other.end
&& self.points == other.points
&& self.num_points == other.num_points
}
}
impl TrajectorySegment {
pub(crate) fn hold(start_time_msec: u32, position: Vector3WithYaw) -> Self {
let mut points = [[0.0; MAX_POLY_COEFFS]; 3];
points[0][0] = position.x;
points[1][0] = position.y;
points[2][0] = position.z;
Self {
start_time_msec,
end_time_msec: u32::MAX,
duration_msec: u32::MAX - start_time_msec,
start_time_sec: msec_to_sec(start_time_msec),
end_time_sec: f32::INFINITY,
duration_sec: f32::INFINITY,
start: position,
end: position,
points,
num_points: [1; 3],
poly: OnceCell::from(Poly4D::constant(position)),
dpoly: OnceCell::new(),
ddpoly: OnceCell::new(),
}
}
pub(crate) fn decode(
bytes: &[u8],
start_time_msec: u32,
start: Vector3WithYaw,
scale: f32,
) -> Option<(Self, usize)> {
let (&header, _) = bytes.split_first()?;
let duration_msec = u32::from(read_u16(bytes, 1)?);
let end_time_msec = start_time_msec.saturating_add(duration_msec);
let mut offset = 3;
let mut points = [[0.0; MAX_POLY_COEFFS]; 3];
let mut num_points = [1; 3];
let mut end = start;
let starts = [start.x, start.y, start.z];
let ends = [&mut end.x, &mut end.y, &mut end.z];
for (axis, ((points, count), end)) in
points.iter_mut().zip(&mut num_points).zip(ends).enumerate()
{
let n = num_coords(header >> (2 * axis));
*count = n;
let (first, rest) = points.split_first_mut()?;
*first = starts.get(axis).copied().unwrap_or_default();
for point in rest.iter_mut().take(n - 1) {
*point = f32::from(read_i16(bytes, offset)?) * scale;
offset += 2;
}
*end = points.get(n - 1).copied().unwrap_or_default();
}
offset += 2 * (num_coords(header >> 6) - 1);
if offset > bytes.len() {
return None;
}
let segment = Self {
start_time_msec,
end_time_msec,
duration_msec,
start_time_sec: msec_to_sec(start_time_msec),
end_time_sec: msec_to_sec(end_time_msec),
duration_sec: msec_to_sec(duration_msec),
start,
end,
points,
num_points,
poly: OnceCell::new(),
dpoly: OnceCell::new(),
ddpoly: OnceCell::new(),
};
Some((segment, offset))
}
#[must_use]
pub const fn start_time_msec(&self) -> u32 {
self.start_time_msec
}
#[must_use]
pub const fn end_time_msec(&self) -> u32 {
self.end_time_msec
}
#[must_use]
pub const fn duration_msec(&self) -> u32 {
self.duration_msec
}
#[must_use]
pub const fn start_time_sec(&self) -> f32 {
self.start_time_sec
}
#[must_use]
pub const fn end_time_sec(&self) -> f32 {
self.end_time_sec
}
#[must_use]
pub const fn duration_sec(&self) -> f32 {
self.duration_sec
}
#[must_use]
pub const fn start(&self) -> Vector3WithYaw {
self.start
}
#[must_use]
pub const fn end(&self) -> Vector3WithYaw {
self.end
}
pub(crate) fn control_points(&self, axis: usize) -> &[f32] {
let count = self.num_points.get(axis).copied().unwrap_or(0);
self.points
.get(axis)
.and_then(|points| points.get(..count))
.unwrap_or_default()
}
pub fn poly(&self) -> &Poly4D {
let (points, num_points, yaw) = (&self.points, &self.num_points, self.start.yaw);
self.poly.get_or_init(|| {
let axis = |index: usize| {
let points = points.get(index).map_or(&[][..], |points| {
let count = num_points.get(index).copied().unwrap_or(1);
points.get(..count).unwrap_or(points)
});
Poly::bezier(1.0, points)
};
Poly4D {
x: axis(0),
y: axis(1),
z: axis(2),
yaw: Poly::constant(yaw),
}
})
}
pub fn dpoly(&self) -> &Poly4D {
self.dpoly.get_or_init(|| self.derivative_of(*self.poly()))
}
pub fn ddpoly(&self) -> &Poly4D {
self.ddpoly
.get_or_init(|| self.derivative_of(*self.dpoly()))
}
fn derivative_of(&self, poly: Poly4D) -> Poly4D {
let mut result = poly;
result.deriv();
if self.duration_sec.abs() > 1.0e-6 {
result.scale(1.0 / self.duration_sec);
}
result
}
}
#[expect(
clippy::cast_precision_loss,
reason = "the conversion of the C library"
)]
pub(crate) fn msec_to_sec(msec: u32) -> f32 {
msec as f32 / 1000.0
}
#[cfg(test)]
mod tests {
use crate::basic_types::Vector3WithYaw;
use crate::trajectory::{TrajectoryBuilder, TrajectoryPlayer};
#[test]
fn equality_ignores_the_calculated_polynomials() {
let mut builder = TrajectoryBuilder::new(1).unwrap();
builder
.set_start_position(Vector3WithYaw::default())
.unwrap();
let target = Vector3WithYaw {
x: 1000.0,
..Vector3WithYaw::default()
};
builder.append_line(target, 1000).unwrap();
let trajectory = builder.build().unwrap();
let player = TrajectoryPlayer::new(&trajectory);
let fresh = player.current_segment().clone();
let used = player.current_segment();
used.ddpoly();
assert_eq!(*used, fresh);
}
}