vibe/physics/
trajectory.rs1use crate::value::{Pose, WorldLine};
4
5#[derive(Debug, Clone, PartialEq)]
7pub struct Waypoint {
8 pub t_secs: i64,
10 pub t_nanos: u32,
12 pub pose: Pose,
14}
15
16impl Waypoint {
17 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 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#[derive(Debug, Clone)]
34pub struct WorldLineTrajectory {
35 pub worldline: WorldLine,
37 pub waypoints: Vec<Waypoint>,
39}
40
41impl WorldLineTrajectory {
42 pub fn new(worldline: WorldLine) -> Self {
44 Self {
45 worldline,
46 waypoints: Vec::new(),
47 }
48 }
49
50 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 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 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 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 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 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(), frame: w0.pose.frame.clone(),
100 })
101 }
102
103 pub fn len(&self) -> usize {
105 self.waypoints.len()
106 }
107
108 pub fn is_empty(&self) -> bool {
110 self.waypoints.is_empty()
111 }
112
113 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}