use core::ops::Deref;
use super::Trajectory;
use super::segment::TrajectorySegment;
use crate::basic_types::Vector3WithYaw;
#[derive(Clone, Debug, PartialEq)]
pub struct TrajectoryPlayerState {
start: usize,
length: usize,
segment: TrajectorySegment,
}
impl TrajectoryPlayerState {
pub(crate) const fn start(&self) -> usize {
self.start
}
pub(crate) const fn length(&self) -> usize {
self.length
}
#[must_use]
pub const fn segment(&self) -> &TrajectorySegment {
&self.segment
}
}
#[derive(Clone, Debug)]
pub struct TrajectoryPlayer<T> {
trajectory: T,
state: TrajectoryPlayerState,
}
impl<'a, T: Deref<Target = Trajectory<'a>>> TrajectoryPlayer<T> {
pub fn new(trajectory: T) -> Self {
let state = Self::state_at(
&trajectory,
trajectory.header_length(),
0,
trajectory.start(),
);
Self { trajectory, state }
}
pub fn trajectory(&self) -> &Trajectory<'a> {
&self.trajectory
}
pub fn rewind(&mut self) {
self.state = Self::state_at(
&self.trajectory,
self.trajectory.header_length(),
0,
self.trajectory.start(),
);
}
pub fn build_next_segment(&mut self) {
let segment = &self.state.segment;
self.state = Self::state_at(
&self.trajectory,
self.state.start + self.state.length,
segment.end_time_msec(),
segment.end(),
);
}
#[must_use]
pub const fn current_segment(&self) -> &TrajectorySegment {
&self.state.segment
}
#[must_use]
pub fn has_more_segments(&self) -> bool {
self.state.length > 0
}
pub fn position_at(&mut self, t: f32) -> Vector3WithYaw {
let rel_t = self.seek_to_time(t);
self.state.segment.poly().eval(rel_t)
}
pub fn velocity_at(&mut self, t: f32) -> Vector3WithYaw {
let rel_t = self.seek_to_time(t);
self.state.segment.dpoly().eval(rel_t)
}
pub fn acceleration_at(&mut self, t: f32) -> Vector3WithYaw {
let rel_t = self.seek_to_time(t);
self.state.segment.ddpoly().eval(rel_t)
}
pub fn total_duration_msec(&mut self) -> u32 {
let mut result: u32 = 0;
self.rewind();
while self.has_more_segments() {
result = result.saturating_add(self.state.segment.duration_msec());
self.build_next_segment();
}
result
}
#[must_use]
pub fn save_state(&self) -> TrajectoryPlayerState {
self.state.clone()
}
pub fn restore_state(&mut self, state: &TrajectoryPlayerState) {
self.state.clone_from(state);
}
pub(crate) fn seek_to_time(&mut self, t: f32) -> f32 {
let t = if t <= 0.0 { 0.0 } else { t };
loop {
let segment = &self.state.segment;
if segment.start_time_sec() > t {
self.rewind();
} else if segment.end_time_sec() < t {
self.build_next_segment();
} else if !t.is_finite() {
return 1.0;
} else if segment.duration_sec().abs() > 1.0e-6 {
return (t - segment.start_time_sec()) / segment.duration_sec();
} else {
return 0.5;
}
}
}
fn state_at(
trajectory: &Trajectory<'_>,
offset: usize,
start_time_msec: u32,
start: Vector3WithYaw,
) -> TrajectoryPlayerState {
let decoded = if trajectory.scale() == 0.0 {
None
} else {
trajectory.bytes().get(offset..).and_then(|bytes| {
TrajectorySegment::decode(bytes, start_time_msec, start, trajectory.scale())
})
};
match decoded {
Some((segment, length)) => TrajectoryPlayerState {
start: offset,
length,
segment,
},
None => TrajectoryPlayerState {
start: offset,
length: 0,
segment: TrajectorySegment::hold(start_time_msec, start),
},
}
}
}