Skip to main content

vibe/physics/
trajectory.rs

1//! WorldLine trajectory & temporal persistence (W2).
2
3use crate::value::{Pose, WorldLine};
4
5/// A waypoint on a worldline — an instant plus the pose at that instant.
6#[derive(Debug, Clone, PartialEq)]
7pub struct Waypoint {
8    /// Unix seconds for this waypoint.
9    pub t_secs: i64,
10    /// Nanoseconds within the second.
11    pub t_nanos: u32,
12    /// The pose at this instant.
13    pub pose: Pose,
14}
15
16impl Waypoint {
17    /// Creates a new `Waypoint`.
18    pub fn new(t_secs: i64, t_nanos: u32, pose: Pose) -> Self {
19        Self {
20            t_secs,
21            t_nanos,
22            pose,
23        }
24    }
25
26    /// Total time in nanoseconds.
27    pub fn total_nanos(&self) -> i128 {
28        self.t_secs as i128 * 1_000_000_000 + self.t_nanos as i128
29    }
30}
31
32/// A worldline trajectory — the time-like self of a continuant (W2).
33#[derive(Debug, Clone)]
34pub struct WorldLineTrajectory {
35    /// The worldline identity.
36    pub worldline: WorldLine,
37    /// Waypoints ordered by time.
38    pub waypoints: Vec<Waypoint>,
39}
40
41impl WorldLineTrajectory {
42    /// Creates a new `WorldLineTrajectory`.
43    pub fn new(worldline: WorldLine) -> Self {
44        Self {
45            worldline,
46            waypoints: Vec::new(),
47        }
48    }
49
50    /// Add a waypoint. Waypoints are kept sorted by time.
51    pub fn add(&mut self, waypoint: Waypoint) -> &mut Self {
52        let insert_pos = self
53            .waypoints
54            .partition_point(|w| w.total_nanos() < waypoint.total_nanos());
55        self.waypoints.insert(insert_pos, waypoint);
56        self
57    }
58
59    /// Get the pose at a given time by linear interpolation.
60    /// Returns None if the time is outside the waypoint range.
61    pub fn pose_at(&self, t_secs: i64, t_nanos: u32) -> Option<Pose> {
62        if self.waypoints.is_empty() {
63            return None;
64        }
65        let target = t_secs as i128 * 1_000_000_000 + t_nanos as i128;
66
67        // Before first waypoint.
68        let first = self.waypoints.first()?;
69        let first_t = first.total_nanos();
70        if target <= first_t {
71            return Some(first.pose.clone());
72        }
73
74        // After last waypoint.
75        let last = self.waypoints.last()?;
76        let last_t = last.total_nanos();
77        if target >= last_t {
78            return Some(last.pose.clone());
79        }
80
81        // Find bracketing waypoints.
82        let idx = self.waypoints.partition_point(|w| w.total_nanos() < target);
83        let (w0, w1) = (&self.waypoints[idx - 1], &self.waypoints[idx]);
84        let t0 = w0.total_nanos();
85        let t1 = w1.total_nanos();
86        let alpha = (target - t0) as f64 / (t1 - t0) as f64;
87
88        // Linear interpolation of position.
89        let pos: Vec<f64> = w0
90            .pose
91            .position
92            .iter()
93            .zip(w1.pose.position.iter())
94            .map(|(a, b)| a + (b - a) * alpha)
95            .collect();
96        Some(Pose {
97            position: pos,
98            orientation: w0.pose.orientation.clone(), // hold orientation
99            frame: w0.pose.frame.clone(),
100        })
101    }
102
103    /// Number of waypoints.
104    pub fn len(&self) -> usize {
105        self.waypoints.len()
106    }
107
108    /// Is the trajectory empty?
109    pub fn is_empty(&self) -> bool {
110        self.waypoints.is_empty()
111    }
112
113    /// Total time span in nanoseconds (last - first).
114    pub fn span_nanos(&self) -> Option<i128> {
115        if self.waypoints.len() < 2 {
116            return None;
117        }
118        let first = self.waypoints.first()?.total_nanos();
119        let last = self.waypoints.last()?.total_nanos();
120        Some(last - first)
121    }
122}