use super::SmoothError;
use sightloom_core::{Point, Rect, TrackId};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TrajectorySample {
pub frame_index: u64,
pub bbox: Rect,
pub confidence: f32,
}
#[derive(Clone, Debug)]
pub struct TrajectoryHistory<const N: usize> {
track_id: TrackId,
samples: [Option<TrajectorySample>; N],
head: usize,
len: usize,
}
impl<const N: usize> TrajectoryHistory<N> {
#[must_use]
pub const fn new(track_id: TrackId) -> Self {
Self {
track_id,
samples: [None; N],
head: 0,
len: 0,
}
}
#[must_use]
pub const fn track_id(&self) -> TrackId {
self.track_id
}
#[must_use]
pub const fn len(&self) -> usize {
self.len
}
#[must_use]
pub const fn is_empty(&self) -> bool {
self.len == 0
}
pub fn push(&mut self, sample: TrajectorySample) -> Result<(), SmoothError> {
if N == 0 {
return Err(SmoothError::InvalidConfig);
}
self.samples[self.head] = Some(sample);
self.head = (self.head + 1) % N;
if self.len < N {
self.len += 1;
}
Ok(())
}
pub fn copy_chronological(
&self,
output: &mut [TrajectorySample],
) -> Result<usize, SmoothError> {
if output.len() < self.len {
return Err(SmoothError::InsufficientCapacity);
}
let start = if self.len < N { 0 } else { self.head };
for (offset, slot) in output.iter_mut().enumerate().take(self.len) {
let index = (start + offset) % N;
let Some(sample) = self.samples[index] else {
return Err(SmoothError::InsufficientCapacity);
};
*slot = sample;
}
Ok(self.len)
}
#[must_use]
pub fn velocity(&self) -> Option<Point> {
if self.len < 2 {
return None;
}
let last = self.nth_from_end(0)?;
let prev = self.nth_from_end(1)?;
let df = last.frame_index.saturating_sub(prev.frame_index).max(1) as f32;
let c0 = prev.bbox.center();
let c1 = last.bbox.center();
Point::new((c1.x() - c0.x()) / df, (c1.y() - c0.y()) / df).ok()
}
#[must_use]
pub fn acceleration(&self) -> Option<Point> {
if self.len < 3 {
return None;
}
let s0 = self.nth_from_end(2)?;
let s1 = self.nth_from_end(1)?;
let s2 = self.nth_from_end(0)?;
let d01 = s1.frame_index.saturating_sub(s0.frame_index).max(1) as f32;
let d12 = s2.frame_index.saturating_sub(s1.frame_index).max(1) as f32;
let v01x = (s1.bbox.center().x() - s0.bbox.center().x()) / d01;
let v01y = (s1.bbox.center().y() - s0.bbox.center().y()) / d01;
let v12x = (s2.bbox.center().x() - s1.bbox.center().x()) / d12;
let v12y = (s2.bbox.center().y() - s1.bbox.center().y()) / d12;
let dt = ((d01 + d12) * 0.5).max(1.0);
Point::new((v12x - v01x) / dt, (v12y - v01y) / dt).ok()
}
#[must_use]
pub fn jitter(&self) -> f32 {
if self.len < 2 {
return 0.0;
}
let mut total = 0.0_f32;
let mut count = 0_u32;
for offset in 0..(self.len - 1) {
let a = self.nth_from_end(self.len - 1 - offset);
let b = self.nth_from_end(self.len - 2 - offset);
if let (Some(a), Some(b)) = (a, b) {
let dx = a.bbox.center().x() - b.bbox.center().x();
let dy = a.bbox.center().y() - b.bbox.center().y();
let ax = if dx < 0.0 { -dx } else { dx };
let ay = if dy < 0.0 { -dy } else { dy };
total += ax + ay;
count += 1;
}
}
if count == 0 {
0.0
} else {
total / count as f32
}
}
fn nth_from_end(&self, n: usize) -> Option<TrajectorySample> {
if n >= self.len {
return None;
}
let start = if self.len < N { 0 } else { self.head };
let index = (start + self.len - 1 - n) % N;
self.samples[index]
}
}