use alloc::borrow::Cow;
use alloc::collections::TryReserveError;
use alloc::vec::Vec;
use core::{error, fmt};
use libm::{floorf, hypotf};
use super::{HEADER_LENGTH, Trajectory, TrajectoryError, msec_to_sec};
use crate::basic_types::Vector3WithYaw;
use crate::motion::{cubic_bezier_from_velocity_constraints, travel_velocity_for_distance};
use crate::poly::MAX_POLY_COEFFS;
const MAX_DURATION_MSEC: u32 = 60_000;
const LINEAR: u8 = 0x01;
const BEZIER: u8 = 0x02;
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum TrajectoryBuilderError {
InvalidScale,
StartPositionAlreadyUsed,
ZeroDuration,
DurationTooLong,
CoordinateOutOfRange,
OutOfMemory,
}
impl fmt::Display for TrajectoryBuilderError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
f.write_str(match self {
Self::InvalidScale => "the scale must be between 1 and 127",
Self::StartPositionAlreadyUsed => {
"the start position cannot be set after a segment was added"
}
Self::ZeroDuration => "the duration of the segment is zero",
Self::DurationTooLong => "the duration of the segment is longer than 60 seconds",
Self::CoordinateOutOfRange => "a coordinate is out of the range of the scale",
Self::OutOfMemory => "out of memory",
})
}
}
impl error::Error for TrajectoryBuilderError {}
impl From<TryReserveError> for TrajectoryBuilderError {
fn from(_: TryReserveError) -> Self {
Self::OutOfMemory
}
}
#[derive(Debug)]
pub struct TrajectoryBuilder {
bytes: Vec<u8>,
last_position: Vector3WithYaw,
scale: f32,
}
impl TrajectoryBuilder {
pub fn new(scale: u8) -> Result<Self, TrajectoryBuilderError> {
if scale == 0 || scale > 127 {
return Err(TrajectoryBuilderError::InvalidScale);
}
let mut bytes = Vec::new();
bytes.try_reserve(HEADER_LENGTH)?;
bytes.push(scale);
bytes.resize(HEADER_LENGTH, 0);
Ok(Self {
bytes,
last_position: Vector3WithYaw::default(),
scale: f32::from(scale),
})
}
pub fn from_trajectory(
trajectory: Trajectory<'_>,
last_position: Option<Vector3WithYaw>,
) -> Result<Self, TrajectoryBuilderError> {
let last_position = last_position.unwrap_or_else(|| trajectory.end_position());
let scale = trajectory.scale();
Ok(Self {
bytes: try_into_owned(trajectory.bytes)?,
last_position,
scale,
})
}
#[must_use]
pub const fn last_position(&self) -> Vector3WithYaw {
self.last_position
}
pub fn set_start_position(
&mut self,
start: Vector3WithYaw,
) -> Result<(), TrajectoryBuilderError> {
if self.bytes.len() != HEADER_LENGTH {
return Err(TrajectoryBuilderError::StartPositionAlreadyUsed);
}
let coordinates = [
scale_coordinate(start.x, self.scale)?,
scale_coordinate(start.y, self.scale)?,
scale_coordinate(start.z, self.scale)?,
0,
];
let header = self.bytes.get_mut(1..HEADER_LENGTH).into_iter().flatten();
let encoded = coordinates.into_iter().flat_map(i16::to_le_bytes);
for (slot, byte) in header.zip(encoded) {
*slot = byte;
}
self.last_position = Vector3WithYaw { yaw: 0.0, ..start };
Ok(())
}
pub fn append_cubic_bezier(
&mut self,
control1: Vector3WithYaw,
control2: Vector3WithYaw,
target: Vector3WithYaw,
duration_msec: u32,
) -> Result<(), TrajectoryBuilderError> {
let segment = EncodedSegment::cubic_bezier(
self.last_position,
[control1, control2, target],
duration_msec,
self.scale,
)?;
self.append(&segment, target)
}
pub fn append_line(
&mut self,
target: Vector3WithYaw,
duration_msec: u32,
) -> Result<(), TrajectoryBuilderError> {
self.all_or_nothing(|builder| builder.append_line_unchecked(target, duration_msec))
}
pub fn hold_position_for(&mut self, duration_msec: u32) -> Result<(), TrajectoryBuilderError> {
self.all_or_nothing(|builder| {
let mut remaining = duration_msec;
while remaining > 0 {
let current = remaining.min(MAX_DURATION_MSEC);
remaining -= current;
builder.append_line_unchecked(builder.last_position, current)?;
}
Ok(())
})
}
pub fn move_to_in_time(
&mut self,
target: Vector3WithYaw,
duration_msec: u32,
max_acceleration: f32,
) -> Result<(), TrajectoryBuilderError> {
self.all_or_nothing(|builder| {
builder.move_to_in_time_unchecked(target, duration_msec, max_acceleration)
})
}
pub fn build(self) -> Result<Trajectory<'static>, TrajectoryError> {
Trajectory::from_vec(self.bytes)
}
fn all_or_nothing(
&mut self,
operation: impl FnOnce(&mut Self) -> Result<(), TrajectoryBuilderError>,
) -> Result<(), TrajectoryBuilderError> {
let (length, last_position) = (self.bytes.len(), self.last_position);
let result = operation(self);
if result.is_err() {
self.bytes.truncate(length);
self.last_position = last_position;
}
result
}
fn append(
&mut self,
segment: &EncodedSegment,
target: Vector3WithYaw,
) -> Result<(), TrajectoryBuilderError> {
self.bytes.try_reserve(segment.as_bytes().len())?;
self.bytes.extend_from_slice(segment.as_bytes());
self.last_position = Vector3WithYaw { yaw: 0.0, ..target };
Ok(())
}
fn append_line_unchecked(
&mut self,
target: Vector3WithYaw,
duration_msec: u32,
) -> Result<(), TrajectoryBuilderError> {
if duration_msec > MAX_DURATION_MSEC {
let last = self.last_position;
let midpoint = (last + target) / 2.0;
let half = duration_msec / 2;
self.append_line_unchecked(midpoint, half)?;
return self.append_line_unchecked(target, duration_msec - half);
}
let segment = EncodedSegment::line(self.last_position, target, duration_msec, self.scale)?;
self.append(&segment, target)
}
fn move_to_in_time_unchecked(
&mut self,
target: Vector3WithYaw,
duration_msec: u32,
max_acceleration: f32,
) -> Result<(), TrajectoryBuilderError> {
if duration_msec == 0 {
return Err(TrajectoryBuilderError::ZeroDuration);
}
if !max_acceleration.is_finite() || max_acceleration <= 0.0 || duration_msec < 3 {
return self.append_line_unchecked(target, duration_msec);
}
let start = self.last_position;
let distance = hypotf(
hypotf(target.x - start.x, target.y - start.y),
target.z - start.z,
);
let duration_sec = msec_to_sec(duration_msec);
let mut travel_speed =
travel_velocity_for_distance(distance, duration_sec, max_acceleration);
let mut cruise_sec = duration_sec - 2.0 * travel_speed / max_acceleration;
if cruise_sec < 0.0 {
cruise_sec = 0.0;
travel_speed = 2.0 * distance / duration_sec;
}
let mut accel_sec = (duration_sec - cruise_sec) / 2.0;
let mut accel_msec = float_to_msec(accel_sec * 1000.0);
if accel_msec < 1 {
accel_msec = 1;
accel_sec = msec_to_sec(accel_msec);
}
let rest = Vector3WithYaw {
yaw: (target.yaw - start.yaw) / duration_sec,
..Vector3WithYaw::default()
};
let travel_velocity =
(travel_speed * (target.xyz() - start.xyz()) / distance).with_yaw(rest.yaw);
let offset =
(travel_velocity.xyz() * accel_sec / 2.0).with_yaw(travel_velocity.yaw * accel_sec);
let cruise_start = start + offset;
self.append_smooth_curve(
(start, rest),
(cruise_start, travel_velocity),
accel_sec,
accel_msec,
)?;
let cruise_end = target - offset;
if let Some(cruise_msec) = duration_msec.checked_sub(accel_msec.saturating_mul(2))
&& cruise_msec > 0
{
self.append_line_unchecked(cruise_end, cruise_msec)?;
}
self.append_smooth_curve(
(cruise_end, travel_velocity),
(target, rest),
accel_sec,
accel_msec,
)
}
fn append_smooth_curve(
&mut self,
(start, start_velocity): (Vector3WithYaw, Vector3WithYaw),
(end, end_velocity): (Vector3WithYaw, Vector3WithYaw),
duration_sec: f32,
duration_msec: u32,
) -> Result<(), TrajectoryBuilderError> {
let (control1, control2) = cubic_bezier_from_velocity_constraints(
start,
start_velocity,
end,
end_velocity,
duration_sec,
)
.ok_or(TrajectoryBuilderError::ZeroDuration)?;
let segment = EncodedSegment::cubic_bezier(
self.last_position,
[control1, control2, end],
duration_msec,
self.scale,
)?;
self.append(&segment, end)
}
}
pub(super) fn try_into_owned(bytes: Cow<'_, [u8]>) -> Result<Vec<u8>, TryReserveError> {
match bytes {
Cow::Owned(bytes) => Ok(bytes),
Cow::Borrowed(bytes) => {
let mut result = Vec::new();
result.try_reserve_exact(bytes.len())?;
result.extend_from_slice(bytes);
Ok(result)
}
}
}
pub(super) fn scale_coordinate(coordinate: f32, scale: f32) -> Result<i16, TrajectoryBuilderError> {
let scaled = floorf(coordinate / scale);
if scaled >= f32::from(i16::MIN) && scaled <= f32::from(i16::MAX) {
#[expect(
clippy::cast_possible_truncation,
reason = "an integer in the range of i16"
)]
Ok(scaled as i16)
} else {
Err(TrajectoryBuilderError::CoordinateOutOfRange)
}
}
#[expect(
clippy::cast_possible_truncation,
clippy::cast_sign_loss,
reason = "truncated as in the C library; out of range values saturate"
)]
pub(super) fn float_to_msec(msec: f32) -> u32 {
msec as u32
}
const MAX_SEGMENT_LENGTH: usize = 3 + 2 * 3 * (MAX_POLY_COEFFS - 1);
#[derive(Clone, Copy, Debug)]
pub(super) struct EncodedSegment {
bytes: [u8; MAX_SEGMENT_LENGTH],
len: usize,
}
impl EncodedSegment {
pub(super) fn new(header: u8, duration_msec: u16) -> Self {
let mut result = Self {
bytes: [0; MAX_SEGMENT_LENGTH],
len: 1,
};
result.bytes[0] = header;
result.push(duration_msec.to_le_bytes());
result
}
#[expect(
clippy::float_cmp,
reason = "an axis is left out only if it stays exactly where it is"
)]
fn line(
start: Vector3WithYaw,
target: Vector3WithYaw,
duration_msec: u32,
scale: f32,
) -> Result<Self, TrajectoryBuilderError> {
let mut result = Self::new(0, check_duration(duration_msec)?);
let axes = [
(start.x, target.x),
(start.y, target.y),
(start.z, target.z),
];
for (axis, (start, target)) in axes.into_iter().enumerate() {
if start != target {
result.bytes[0] |= LINEAR << (2 * axis);
result.push_coordinate(target, scale)?;
}
}
Ok(result)
}
#[expect(
clippy::float_cmp,
reason = "an axis is left out only if it stays exactly where it is"
)]
pub(super) fn cubic_bezier(
start: Vector3WithYaw,
points: [Vector3WithYaw; 3],
duration_msec: u32,
scale: f32,
) -> Result<Self, TrajectoryBuilderError> {
let mut result = Self::new(0, check_duration(duration_msec)?);
let [control1, control2, target] = points;
let axes = [
[start.x, control1.x, control2.x, target.x],
[start.y, control1.y, control2.y, target.y],
[start.z, control1.z, control2.z, target.z],
];
for (axis, [start, control1, control2, target]) in axes.into_iter().enumerate() {
if start != control1 || control1 != control2 || control2 != target {
result.bytes[0] |= BEZIER << (2 * axis);
for coordinate in [control1, control2, target] {
result.push_coordinate(coordinate, scale)?;
}
}
}
Ok(result)
}
pub(super) fn push_coordinate(
&mut self,
coordinate: f32,
scale: f32,
) -> Result<(), TrajectoryBuilderError> {
self.push(scale_coordinate(coordinate, scale)?.to_le_bytes());
Ok(())
}
fn push(&mut self, bytes: [u8; 2]) {
if let Some(slot) = self.bytes.get_mut(self.len..self.len + 2) {
slot.copy_from_slice(&bytes);
self.len += 2;
}
}
pub(super) fn as_bytes(&self) -> &[u8] {
self.bytes.get(..self.len).unwrap_or(&self.bytes)
}
}
fn check_duration(duration_msec: u32) -> Result<u16, TrajectoryBuilderError> {
if duration_msec == 0 {
Err(TrajectoryBuilderError::ZeroDuration)
} else if duration_msec > MAX_DURATION_MSEC {
Err(TrajectoryBuilderError::DurationTooLong)
} else {
u16::try_from(duration_msec).map_err(|_| TrajectoryBuilderError::DurationTooLong)
}
}
#[cfg(test)]
mod tests {
use super::*;
fn point(x: f32, y: f32, z: f32) -> Vector3WithYaw {
Vector3WithYaw { x, y, z, yaw: 0.0 }
}
#[test]
fn a_failed_start_position_changes_nothing() {
let mut builder = TrajectoryBuilder::new(1).unwrap();
builder.set_start_position(point(1.0, 2.0, 3.0)).unwrap();
let bytes = builder.bytes.clone();
assert_eq!(
builder.set_start_position(point(5.0, f32::NAN, 0.0)),
Err(TrajectoryBuilderError::CoordinateOutOfRange)
);
assert_eq!(builder.bytes, bytes);
assert_eq!(builder.last_position(), point(1.0, 2.0, 3.0));
}
#[test]
fn a_failed_split_line_changes_nothing() {
let mut builder = TrajectoryBuilder::new(1).unwrap();
builder.set_start_position(point(0.0, 0.0, 0.0)).unwrap();
assert_eq!(
builder.append_line(point(60_000.0, 0.0, 0.0), 120_000),
Err(TrajectoryBuilderError::CoordinateOutOfRange)
);
assert_eq!(builder.bytes.len(), HEADER_LENGTH);
assert_eq!(builder.last_position(), point(0.0, 0.0, 0.0));
}
#[test]
fn coordinates_are_rounded_down() {
assert_eq!(scale_coordinate(25.0, 10.0), Ok(2));
assert_eq!(scale_coordinate(-25.0, 10.0), Ok(-3));
assert_eq!(scale_coordinate(327_670.0, 10.0), Ok(i16::MAX));
assert_eq!(
scale_coordinate(327_680.0, 10.0),
Err(TrajectoryBuilderError::CoordinateOutOfRange)
);
assert_eq!(
scale_coordinate(f32::INFINITY, 1.0),
Err(TrajectoryBuilderError::CoordinateOutOfRange)
);
}
}