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.last)
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 {
209            first: self.first,
210            last: self.last,
211            waypoints: out,
212        }
213    }
214
215    pub fn simplify(&self, tol: f32) -> Self {
216        if self.len() <= 2 {
217            return self.clone();
218        }
219
220        let mut keep = vec![true; self.len()];
221        srdp_mark(&self.waypoints, 0, self.len() - 1, tol, &mut keep);
222
223        let waypoints = self
224            .waypoints
225            .iter()
226            .enumerate()
227            .filter(|(i, _)| keep[*i])
228            .map(|(_, q)| *q)
229            .collect();
230
231        Self {
232            first: self.first,
233            last: self.last,
234            waypoints,
235        }
236    }
237
238    pub fn to_robot_path(&self) -> RobotPath {
239        let n = self.waypoints.len();
240        let mut arr = RobotPath::zeros((n, N));
241        for (i, q) in self.waypoints.iter().enumerate() {
242            arr.row_mut(i).assign(&ndarray::ArrayView1::from(&q.0));
243        }
244        arr
245    }
246}
247
248impl<const N: usize> std::ops::Index<usize> for SRobotPath<N> {
249    type Output = SRobotQ<N>;
250    #[inline]
251    fn index(&self, i: usize) -> &SRobotQ<N> {
252        &self.waypoints[i]
253    }
254}
255
256impl<const N: usize> std::ops::IndexMut<usize> for SRobotPath<N> {
257    #[inline]
258    fn index_mut(&mut self, i: usize) -> &mut SRobotQ<N> {
259        &mut self.waypoints[i]
260    }
261}
262
263impl<const N: usize> IntoIterator for SRobotPath<N> {
264    type Item = SRobotQ<N>;
265    type IntoIter = std::vec::IntoIter<SRobotQ<N>>;
266
267    fn into_iter(self) -> Self::IntoIter {
268        self.waypoints.into_iter()
269    }
270}
271
272impl<'a, const N: usize> IntoIterator for &'a SRobotPath<N> {
273    type Item = &'a SRobotQ<N>;
274    type IntoIter = std::slice::Iter<'a, SRobotQ<N>>;
275
276    fn into_iter(self) -> Self::IntoIter {
277        self.waypoints.iter()
278    }
279}
280
281impl<const N: usize> AsRef<[SRobotQ<N>]> for SRobotPath<N> {
282    fn as_ref(&self) -> &[SRobotQ<N>] {
283        &self.waypoints
284    }
285}
286
287impl<const N: usize> TryFrom<Vec<SRobotQ<N>>> for SRobotPath<N> {
288    type Error = crate::DekeError;
289
290    fn try_from(waypoints: Vec<SRobotQ<N>>) -> Result<Self, Self::Error> {
291        Self::new(waypoints)
292    }
293}
294
295impl<const N: usize> TryFrom<&[SRobotQ<N>]> for SRobotPath<N> {
296    type Error = crate::DekeError;
297
298    fn try_from(waypoints: &[SRobotQ<N>]) -> Result<Self, Self::Error> {
299        Self::new(waypoints.to_vec())
300    }
301}
302
303impl<const N: usize> TryFrom<&Vec<SRobotQ<N>>> for SRobotPath<N> {
304    type Error = crate::DekeError;
305
306    fn try_from(waypoints: &Vec<SRobotQ<N>>) -> Result<Self, Self::Error> {
307        Self::new(waypoints.clone())
308    }
309}
310
311impl<const N: usize> From<SRobotPath<N>> for RobotPath {
312    fn from(sp: SRobotPath<N>) -> Self {
313        sp.to_robot_path()
314    }
315}
316
317impl<const N: usize> From<&SRobotPath<N>> for RobotPath {
318    fn from(sp: &SRobotPath<N>) -> Self {
319        sp.to_robot_path()
320    }
321}
322
323impl<const N: usize> TryFrom<RobotPath> for SRobotPath<N> {
324    type Error = crate::DekeError;
325
326    fn try_from(arr: RobotPath) -> Result<Self, Self::Error> {
327        if arr.ncols() != N {
328            return Err(crate::DekeError::ShapeMismatch {
329                expected: N,
330                found: arr.ncols(),
331            });
332        }
333        let mut waypoints = Vec::with_capacity(arr.nrows());
334        for row in arr.rows() {
335            let mut q = [0.0f32; N];
336            for (j, &v) in row.iter().enumerate() {
337                q[j] = v;
338            }
339            waypoints.push(SRobotQ(q));
340        }
341        Self::new(waypoints)
342    }
343}
344
345impl<const N: usize> TryFrom<&RobotPath> for SRobotPath<N> {
346    type Error = crate::DekeError;
347
348    fn try_from(arr: &RobotPath) -> Result<Self, Self::Error> {
349        if arr.ncols() != N {
350            return Err(crate::DekeError::ShapeMismatch {
351                expected: N,
352                found: arr.ncols(),
353            });
354        }
355        let mut waypoints = Vec::with_capacity(arr.nrows());
356        for row in arr.rows() {
357            let mut q = [0.0f32; N];
358            for (j, &v) in row.iter().enumerate() {
359                q[j] = v;
360            }
361            waypoints.push(SRobotQ(q));
362        }
363        Self::new(waypoints)
364    }
365}
366
367fn srdp_mark<const N: usize>(
368    pts: &[SRobotQ<N>],
369    start: usize,
370    end: usize,
371    tol: f32,
372    keep: &mut [bool],
373) {
374    if end <= start + 1 {
375        return;
376    }
377
378    let seg = pts[end] - pts[start];
379    let seg_len_sq = seg.norm_squared();
380
381    let mut max_dist = 0.0f32;
382    let mut max_idx = start;
383
384    for i in (start + 1)..end {
385        let v = pts[i] - pts[start];
386        let dist = if seg_len_sq < 1e-30 {
387            v.norm()
388        } else {
389            let t = (v.dot(&seg) / seg_len_sq).clamp(0.0, 1.0);
390            (v - seg * t).norm()
391        };
392        if dist > max_dist {
393            max_dist = dist;
394            max_idx = i;
395        }
396    }
397
398    if max_dist > tol {
399        keep[max_idx] = true;
400        srdp_mark(pts, start, max_idx, tol, keep);
401        srdp_mark(pts, max_idx, end, tol, keep);
402    } else {
403        for k in (start + 1)..end {
404            keep[k] = false;
405        }
406    }
407}
deke_types/path.rs

deke_types/
path.rs
