deke_types/
path.rs

1use ndarray::Array2;
2
3use crate::{DekeResult, SRobotQ};
4
5/// Dynamically-sized robot path as a 2D array (rows = waypoints, cols = joints).
6pub type RobotPath = Array2<f32>;
7
8/// Statically-sized robot path backed by `Vec<SRobotQ<N>>`.
9/// 
10/// SRobotPath is guranteed to have at least 2 waypoints, so it always has a defined start and end configuration.
11#[derive(Debug, Clone)]
12pub struct SRobotPath<const N: usize> {
13    first: SRobotQ<N>,
14    last: SRobotQ<N>,
15    waypoints: Vec<SRobotQ<N>>,
16}
17
18impl<const N: usize> SRobotPath<N> {
19    pub fn new(waypoints: Vec<SRobotQ<N>>) -> DekeResult<Self> {
20        if waypoints.len() < 2 {
21            return Err(crate::DekeError::PathTooShort(waypoints.len()));
22        }
23        Ok(Self {
24            first: waypoints[0],
25            last: waypoints[waypoints.len() - 1],
26            waypoints,
27        })
28    }
29
30    pub fn new_prechecked(first: SRobotQ<N>, last: SRobotQ<N>, middle: Vec<SRobotQ<N>>) -> Self {
31        let mut waypoints = Vec::with_capacity(middle.len() + 2);
32        waypoints.push(first);
33        waypoints.extend(middle);
34        waypoints.push(last);
35        Self {
36            first,
37            last,
38            waypoints,
39        }
40     }
41
42    pub fn from_two(start: SRobotQ<N>, goal: SRobotQ<N>) -> Self {
43        Self {
44            first: start,
45            last: goal,
46            waypoints: vec![start, goal],
47        }
48    }
49
50    pub fn len(&self) -> usize {
51        self.waypoints.len()
52    }
53
54    pub fn get(&self, index: usize) -> Option<&SRobotQ<N>> {
55        self.waypoints.get(index)
56    }
57
58    pub fn first(&self) -> &SRobotQ<N> {
59        &self.first
60    }
61
62    pub fn last(&self) -> &SRobotQ<N> {
63        &self.last
64    }
65
66    pub fn iter(&self) -> std::slice::Iter<'_, SRobotQ<N>> {
67        self.waypoints.iter()
68    }
69
70    pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, SRobotQ<N>> {
71        self.waypoints.iter_mut()
72    }
73
74    pub fn segments(&self) -> impl Iterator<Item = (&SRobotQ<N>, &SRobotQ<N>)> {
75        self.waypoints.windows(2).map(|w| (&w[0], &w[1]))
76    }
77
78    pub fn push(&mut self, q: SRobotQ<N>) {
79        self.waypoints.push(q);
80    }
81
82    pub fn pop(&mut self) -> Option<SRobotQ<N>> {
83        if self.waypoints.len() > 2 {
84            let popped = self.waypoints.pop();
85            if let Some(p) = popped {
86                if let Some(last) = self.waypoints.last() {
87                    self.last = *last;
88                }
89                Some(p)
90            } else {
91                None
92            }
93
94        } else {
95            None
96        }
97    }
98
99    pub fn truncate(&mut self, len: usize) {
100        if len < 2 {
101            return;
102        }
103        self.waypoints.truncate(len);
104        if let Some(last) = self.waypoints.last() {
105            self.last = *last;
106        }
107    }
108
109    pub fn reverse(&mut self) {
110        self.waypoints.reverse();
111        if let Some(first) = self.waypoints.first() {
112            self.first = *first;
113        }
114        if let Some(last) = self.waypoints.last() {
115            self.last = *last;
116        }
117    }
118
119    pub fn reversed(&self) -> Self {
120        let mut wps = self.clone();
121        wps.reverse();
122        wps
123    }
124
125    pub fn arc_length(&self) -> f32 {
126        self.segments().map(|(a, b)| a.distance(b)).sum()
127    }
128
129    pub fn segment_lengths(&self) -> Vec<f32> {
130        self.segments().map(|(a, b)| a.distance(b)).collect()
131    }
132
133    pub fn cumulative_lengths(&self) -> Vec<f32> {
134        let mut cum = Vec::with_capacity(self.len());
135        let mut total = 0.0;
136        cum.push(0.0);
137        for (a, b) in self.segments() {
138            total += a.distance(b);
139            cum.push(total);
140        }
141        cum
142    }
143
144    pub fn max_segment_length(&self) -> f32 {
145        self.segments()
146            .map(|(a, b)| a.distance(b))
147            .fold(0.0, f32::max)
148    }
149
150    pub fn max_joint_step(&self) -> f32 {
151        self.segments()
152            .map(|(a, b)| (*a - *b).linf_norm())
153            .fold(0.0, f32::max)
154    }
155
156    pub fn sample(&self, t: f32) -> Option<SRobotQ<N>> {
157        let n = self.len();
158        if n < 2 {
159            return if n == 1 {
160                Some(self.waypoints[0])
161            } else {
162                None
163            };
164        }
165
166        let t = t.clamp(0.0, 1.0);
167        let total = self.arc_length();
168        if total == 0.0 {
169            return Some(self.waypoints[0]);
170        }
171
172        let target = t * total;
173        let mut accumulated = 0.0;
174
175        for (a, b) in self.segments() {
176            let seg_len = a.distance(b);
177            if accumulated + seg_len >= target {
178                let local_t = if seg_len > 0.0 {
179                    (target - accumulated) / seg_len
180                } else {
181                    0.0
182                };
183                return Some(a.interpolate(b, local_t));
184            }
185            accumulated += seg_len;
186        }
187
188        Some(*self.waypoints.last().unwrap())
189    }
190
191    pub fn densify(&self, max_dist: f32) -> Self {
192        if self.len() < 2 {
193            return self.clone();
194        }
195
196        let mut out = Vec::new();
197        out.push(self.waypoints[0]);
198
199        for (a, b) in self.segments() {
200            let d = a.distance(b);
201            let steps = (d / max_dist).ceil().max(1.0) as usize;
202            for i in 1..=steps {
203                let t = i as f32 / steps as f32;
204                out.push(a.interpolate(b, t));
205            }
206        }
207
208        Self::new(out).unwrap()
209    }
210
211    pub fn simplify(&self, tol: f32) -> Self {
212        if self.len() <= 2 {
213            return self.clone();
214        }
215
216        let mut keep = vec![true; self.len()];
217        srdp_mark(&self.waypoints, 0, self.len() - 1, tol, &mut keep);
218
219        let waypoints = self
220            .waypoints
221            .iter()
222            .enumerate()
223            .filter(|(i, _)| keep[*i])
224            .map(|(_, q)| *q)
225            .collect();
226
227        Self::new(waypoints).unwrap()
228    }
229
230    pub fn to_robot_path(&self) -> RobotPath {
231        let n = self.waypoints.len();
232        let mut arr = RobotPath::zeros((n, N));
233        for (i, q) in self.waypoints.iter().enumerate() {
234            arr.row_mut(i).assign(&ndarray::ArrayView1::from(&q.0));
235        }
236        arr
237    }
238}
239
240impl<const N: usize> std::ops::Index<usize> for SRobotPath<N> {
241    type Output = SRobotQ<N>;
242    #[inline]
243    fn index(&self, i: usize) -> &SRobotQ<N> {
244        &self.waypoints[i]
245    }
246}
247
248impl<const N: usize> std::ops::IndexMut<usize> for SRobotPath<N> {
249    #[inline]
250    fn index_mut(&mut self, i: usize) -> &mut SRobotQ<N> {
251        &mut self.waypoints[i]
252    }
253}
254
255impl<const N: usize> IntoIterator for SRobotPath<N> {
256    type Item = SRobotQ<N>;
257    type IntoIter = std::vec::IntoIter<SRobotQ<N>>;
258
259    fn into_iter(self) -> Self::IntoIter {
260        self.waypoints.into_iter()
261    }
262}
263
264impl<'a, const N: usize> IntoIterator for &'a SRobotPath<N> {
265    type Item = &'a SRobotQ<N>;
266    type IntoIter = std::slice::Iter<'a, SRobotQ<N>>;
267
268    fn into_iter(self) -> Self::IntoIter {
269        self.waypoints.iter()
270    }
271}
272
273impl<const N: usize> AsRef<[SRobotQ<N>]> for SRobotPath<N> {
274    fn as_ref(&self) -> &[SRobotQ<N>] {
275        &self.waypoints
276    }
277}
278
279impl<const N: usize> TryFrom<Vec<SRobotQ<N>>> for SRobotPath<N> {
280    type Error = crate::DekeError;
281
282    fn try_from(waypoints: Vec<SRobotQ<N>>) -> Result<Self, Self::Error> {
283        Self::new(waypoints)
284    }
285}
286
287impl<const N: usize> TryFrom<&[SRobotQ<N>]> for SRobotPath<N> {
288    type Error = crate::DekeError;
289
290    fn try_from(waypoints: &[SRobotQ<N>]) -> Result<Self, Self::Error> {
291        Self::new(waypoints.to_vec())
292    }
293}
294
295impl<const N: usize> TryFrom<&Vec<SRobotQ<N>>> for SRobotPath<N> {
296    type Error = crate::DekeError;
297
298    fn try_from(waypoints: &Vec<SRobotQ<N>>) -> Result<Self, Self::Error> {
299        Self::new(waypoints.clone())
300    }
301}
302
303impl<const N: usize> From<SRobotPath<N>> for RobotPath {
304    fn from(sp: SRobotPath<N>) -> Self {
305        sp.to_robot_path()
306    }
307}
308
309impl<const N: usize> From<&SRobotPath<N>> for RobotPath {
310    fn from(sp: &SRobotPath<N>) -> Self {
311        sp.to_robot_path()
312    }
313}
314
315impl<const N: usize> TryFrom<RobotPath> for SRobotPath<N> {
316    type Error = crate::DekeError;
317
318    fn try_from(arr: RobotPath) -> Result<Self, Self::Error> {
319        if arr.ncols() != N {
320            return Err(crate::DekeError::ShapeMismatch {
321                expected: N,
322                found: arr.ncols(),
323            });
324        }
325        let mut waypoints = Vec::with_capacity(arr.nrows());
326        for row in arr.rows() {
327            let mut q = [0.0f32; N];
328            q.copy_from_slice(row.as_slice().unwrap());
329            waypoints.push(SRobotQ(q));
330        }
331        Self::new(waypoints)
332    }
333}
334
335impl<const N: usize> TryFrom<&RobotPath> for SRobotPath<N> {
336    type Error = crate::DekeError;
337
338    fn try_from(arr: &RobotPath) -> Result<Self, Self::Error> {
339        if arr.ncols() != N {
340            return Err(crate::DekeError::ShapeMismatch {
341                expected: N,
342                found: arr.ncols(),
343            });
344        }
345        let mut waypoints = Vec::with_capacity(arr.nrows());
346        for row in arr.rows() {
347            let mut q = [0.0f32; N];
348            q.copy_from_slice(row.as_slice().unwrap());
349            waypoints.push(SRobotQ(q));
350        }
351        Self::new(waypoints)
352    }
353}
354
355fn srdp_mark<const N: usize>(
356    pts: &[SRobotQ<N>],
357    start: usize,
358    end: usize,
359    tol: f32,
360    keep: &mut [bool],
361) {
362    if end <= start + 1 {
363        return;
364    }
365
366    let seg = pts[end] - pts[start];
367    let seg_len_sq = seg.norm_squared();
368
369    let mut max_dist = 0.0f32;
370    let mut max_idx = start;
371
372    for i in (start + 1)..end {
373        let v = pts[i] - pts[start];
374        let dist = if seg_len_sq < 1e-30 {
375            v.norm()
376        } else {
377            let t = (v.dot(&seg) / seg_len_sq).clamp(0.0, 1.0);
378            (v - seg * t).norm()
379        };
380        if dist > max_dist {
381            max_dist = dist;
382            max_idx = i;
383        }
384    }
385
386    if max_dist > tol {
387        keep[max_idx] = true;
388        srdp_mark(pts, start, max_idx, tol, keep);
389        srdp_mark(pts, max_idx, end, tol, keep);
390    } else {
391        for k in (start + 1)..end {
392            keep[k] = false;
393        }
394    }
395}
deke_types/path.rs

deke_types/
path.rs
