deke_types/
path.rs

1use ndarray::Array2;
2use num_traits::Float;
3
4use crate::{DekeResult, SRobotQ};
5
6pub type RobotPath<T = f32> = Array2<T>;
7
8/// Statically-sized robot path backed by `Vec<SRobotQ<N, T>>`.
9///
10/// SRobotPath is guaranteed 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, F: Float = f32> {
13    first: SRobotQ<N, F>,
14    last: SRobotQ<N, F>,
15    waypoints: Vec<SRobotQ<N, F>>,
16}
17
18impl<const N: usize, F: Float> SRobotPath<N, F> {
19    pub fn try_new(waypoints: Vec<SRobotQ<N, F>>) -> 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(
31        first: SRobotQ<N, F>,
32        last: SRobotQ<N, F>,
33        middle: Vec<SRobotQ<N, F>>,
34    ) -> Self {
35        let mut waypoints = Vec::with_capacity(middle.len() + 2);
36        waypoints.push(first);
37        waypoints.extend(middle);
38        waypoints.push(last);
39        Self {
40            first,
41            last,
42            waypoints,
43        }
44    }
45
46    pub fn from_two(start: SRobotQ<N, F>, goal: SRobotQ<N, F>) -> Self {
47        Self {
48            first: start,
49            last: goal,
50            waypoints: vec![start, goal],
51        }
52    }
53
54    pub fn len(&self) -> usize {
55        self.waypoints.len()
56    }
57
58    pub fn is_empty(&self) -> bool {
59        self.waypoints.is_empty()
60    }
61
62    pub fn get(&self, index: usize) -> Option<&SRobotQ<N, F>> {
63        self.waypoints.get(index)
64    }
65
66    pub fn first(&self) -> &SRobotQ<N, F> {
67        &self.first
68    }
69
70    pub fn last(&self) -> &SRobotQ<N, F> {
71        &self.last
72    }
73
74    pub fn iter(&self) -> std::slice::Iter<'_, SRobotQ<N, F>> {
75        self.waypoints.iter()
76    }
77
78    pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, SRobotQ<N, F>> {
79        self.waypoints.iter_mut()
80    }
81
82    pub fn segments(&self) -> impl Iterator<Item = (&SRobotQ<N, F>, &SRobotQ<N, F>)> {
83        self.waypoints.windows(2).map(|w| (&w[0], &w[1]))
84    }
85
86    pub fn push(&mut self, q: SRobotQ<N, F>) {
87        self.waypoints.push(q);
88    }
89
90    pub fn pop(&mut self) -> Option<SRobotQ<N, F>> {
91        if self.waypoints.len() > 2 {
92            let popped = self.waypoints.pop();
93            if let Some(p) = popped {
94                if let Some(last) = self.waypoints.last() {
95                    self.last = *last;
96                }
97                Some(p)
98            } else {
99                None
100            }
101        } else {
102            None
103        }
104    }
105
106    pub fn truncate(&mut self, len: usize) {
107        if len < 2 {
108            return;
109        }
110        self.waypoints.truncate(len);
111        if let Some(last) = self.waypoints.last() {
112            self.last = *last;
113        }
114    }
115
116    pub fn reverse(&mut self) {
117        self.waypoints.reverse();
118        if let Some(first) = self.waypoints.first() {
119            self.first = *first;
120        }
121        if let Some(last) = self.waypoints.last() {
122            self.last = *last;
123        }
124    }
125
126    pub fn reversed(&self) -> Self {
127        let mut wps = self.clone();
128        wps.reverse();
129        wps
130    }
131
132    pub fn arc_length(&self) -> F {
133        self.segments()
134            .map(|(a, b)| a.distance(b))
135            .fold(F::zero(), |acc, d| acc + d)
136    }
137
138    pub fn segment_lengths(&self) -> Vec<F> {
139        self.segments().map(|(a, b)| a.distance(b)).collect()
140    }
141
142    pub fn cumulative_lengths(&self) -> Vec<F> {
143        let mut cum = Vec::with_capacity(self.len());
144        let mut total = F::zero();
145        cum.push(F::zero());
146        for (a, b) in self.segments() {
147            total = total + a.distance(b);
148            cum.push(total);
149        }
150        cum
151    }
152
153    pub fn max_segment_length(&self) -> F {
154        self.segments()
155            .map(|(a, b)| a.distance(b))
156            .fold(F::zero(), |a, b| a.max(b))
157    }
158
159    pub fn max_joint_step(&self) -> F {
160        self.segments()
161            .map(|(a, b)| (*a - *b).linf_norm())
162            .fold(F::zero(), |a, b| a.max(b))
163    }
164
165    pub fn sample(&self, t: F) -> Option<SRobotQ<N, F>> {
166        let n = self.len();
167        if n < 2 {
168            return if n == 1 {
169                Some(self.waypoints[0])
170            } else {
171                None
172            };
173        }
174
175        let t = t.max(F::zero()).min(F::one());
176        let total = self.arc_length();
177        if total == F::zero() {
178            return Some(self.waypoints[0]);
179        }
180
181        let target = t * total;
182        let mut accumulated = F::zero();
183
184        for (a, b) in self.segments() {
185            let seg_len = a.distance(b);
186            if accumulated + seg_len >= target {
187                let local_t = if seg_len > F::zero() {
188                    (target - accumulated) / seg_len
189                } else {
190                    F::zero()
191                };
192                return Some(a.interpolate(b, local_t));
193            }
194            accumulated = accumulated + seg_len;
195        }
196
197        Some(self.last)
198    }
199
200    pub fn densify(&self, max_dist: F) -> Self {
201        if self.len() < 2 {
202            return self.clone();
203        }
204
205        let mut out = Vec::new();
206        out.push(self.waypoints[0]);
207
208        for (a, b) in self.segments() {
209            let d = a.distance(b);
210            let steps = (d / max_dist).ceil().max(F::one()).to_usize().unwrap_or(1);
211            for i in 1..=steps {
212                let t = F::from(i).unwrap() / F::from(steps).unwrap();
213                out.push(a.interpolate(b, t));
214            }
215        }
216
217        Self {
218            first: self.first,
219            last: self.last,
220            waypoints: out,
221        }
222    }
223
224    /// Removes consecutive duplicate waypoints within `tol` distance of each other.
225    pub fn deduplicate(&self, tol: F) -> Self {
226        let mut out = Vec::with_capacity(self.len());
227        out.push(self.waypoints[0]);
228        for q in &self.waypoints[1..] {
229            if out.last().unwrap().distance(q) > tol {
230                out.push(*q);
231            }
232        }
233        if out.len() < 2 {
234            out.push(self.last);
235        }
236        Self {
237            first: out[0],
238            last: out[out.len() - 1],
239            waypoints: out,
240        }
241    }
242
243    pub fn simplify(&self, tol: F) -> Self {
244        if self.len() <= 2 {
245            return self.clone();
246        }
247
248        let mut keep = vec![true; self.len()];
249        srdp_mark(&self.waypoints, 0, self.len() - 1, tol, &mut keep);
250
251        let waypoints = self
252            .waypoints
253            .iter()
254            .enumerate()
255            .filter(|(i, _)| keep[*i])
256            .map(|(_, q)| *q)
257            .collect();
258
259        Self {
260            first: self.first,
261            last: self.last,
262            waypoints,
263        }
264    }
265
266    pub fn to_robot_path(&self) -> RobotPath<F> {
267        let n = self.waypoints.len();
268        let mut arr = RobotPath::zeros((n, N));
269        for (i, q) in self.waypoints.iter().enumerate() {
270            arr.row_mut(i).assign(&ndarray::ArrayView1::from(&q.0[..]));
271        }
272        arr
273    }
274}
275
276impl<const N: usize, T: Float> std::ops::Index<usize> for SRobotPath<N, T> {
277    type Output = SRobotQ<N, T>;
278    #[inline]
279    fn index(&self, i: usize) -> &SRobotQ<N, T> {
280        &self.waypoints[i]
281    }
282}
283
284impl<const N: usize, T: Float> std::ops::IndexMut<usize> for SRobotPath<N, T> {
285    #[inline]
286    fn index_mut(&mut self, i: usize) -> &mut SRobotQ<N, T> {
287        &mut self.waypoints[i]
288    }
289}
290
291impl<const N: usize, T: Float> IntoIterator for SRobotPath<N, T> {
292    type Item = SRobotQ<N, T>;
293    type IntoIter = std::vec::IntoIter<SRobotQ<N, T>>;
294
295    fn into_iter(self) -> Self::IntoIter {
296        self.waypoints.into_iter()
297    }
298}
299
300impl<'a, const N: usize, T: Float> IntoIterator for &'a SRobotPath<N, T> {
301    type Item = &'a SRobotQ<N, T>;
302    type IntoIter = std::slice::Iter<'a, SRobotQ<N, T>>;
303
304    fn into_iter(self) -> Self::IntoIter {
305        self.waypoints.iter()
306    }
307}
308
309impl<const N: usize, T: Float> AsRef<[SRobotQ<N, T>]> for SRobotPath<N, T> {
310    fn as_ref(&self) -> &[SRobotQ<N, T>] {
311        &self.waypoints
312    }
313}
314
315impl<const N: usize, T: Float> TryFrom<Vec<SRobotQ<N, T>>> for SRobotPath<N, T> {
316    type Error = crate::DekeError;
317
318    fn try_from(waypoints: Vec<SRobotQ<N, T>>) -> Result<Self, Self::Error> {
319        Self::try_new(waypoints)
320    }
321}
322
323impl<const N: usize, T: Float> TryFrom<&[SRobotQ<N, T>]> for SRobotPath<N, T> {
324    type Error = crate::DekeError;
325
326    fn try_from(waypoints: &[SRobotQ<N, T>]) -> Result<Self, Self::Error> {
327        Self::try_new(waypoints.to_vec())
328    }
329}
330
331impl<const N: usize, T: Float> TryFrom<&Vec<SRobotQ<N, T>>> for SRobotPath<N, T> {
332    type Error = crate::DekeError;
333
334    fn try_from(waypoints: &Vec<SRobotQ<N, T>>) -> Result<Self, Self::Error> {
335        Self::try_new(waypoints.clone())
336    }
337}
338
339impl<const N: usize, T: Float> From<SRobotPath<N, T>> for RobotPath<T> {
340    fn from(sp: SRobotPath<N, T>) -> Self {
341        sp.to_robot_path()
342    }
343}
344
345impl<const N: usize, T: Float> From<&SRobotPath<N, T>> for RobotPath<T> {
346    fn from(sp: &SRobotPath<N, T>) -> Self {
347        sp.to_robot_path()
348    }
349}
350
351impl<const N: usize, T: Float> TryFrom<RobotPath<T>> for SRobotPath<N, T> {
352    type Error = crate::DekeError;
353
354    fn try_from(arr: RobotPath<T>) -> Result<Self, Self::Error> {
355        if arr.ncols() != N {
356            return Err(crate::DekeError::ShapeMismatch {
357                expected: N,
358                found: arr.ncols(),
359            });
360        }
361        let mut waypoints = Vec::with_capacity(arr.nrows());
362        for row in arr.rows() {
363            let mut q = [T::zero(); N];
364            for (j, &v) in row.iter().enumerate() {
365                q[j] = v;
366            }
367            waypoints.push(SRobotQ(q));
368        }
369        Self::try_new(waypoints)
370    }
371}
372
373impl<const N: usize, F: Float> TryFrom<&RobotPath<F>> for SRobotPath<N, F> {
374    type Error = crate::DekeError;
375
376    fn try_from(arr: &RobotPath<F>) -> Result<Self, Self::Error> {
377        if arr.ncols() != N {
378            return Err(crate::DekeError::ShapeMismatch {
379                expected: N,
380                found: arr.ncols(),
381            });
382        }
383        let mut waypoints = Vec::with_capacity(arr.nrows());
384        for row in arr.rows() {
385            let mut q = [F::zero(); N];
386            for (j, &v) in row.iter().enumerate() {
387                q[j] = v;
388            }
389            waypoints.push(SRobotQ(q));
390        }
391        Self::try_new(waypoints)
392    }
393}
394
395impl<const N: usize> From<SRobotPath<N, f32>> for SRobotPath<N, f64> {
396    fn from(path: SRobotPath<N, f32>) -> Self {
397        Self {
398            first: path.first.into(),
399            last: path.last.into(),
400            waypoints: path.waypoints.into_iter().map(Into::into).collect(),
401        }
402    }
403}
404
405impl<const N: usize> From<SRobotPath<N, f64>> for SRobotPath<N, f32> {
406    fn from(path: SRobotPath<N, f64>) -> Self {
407        Self {
408            first: path.first.into(),
409            last: path.last.into(),
410            waypoints: path.waypoints.into_iter().map(Into::into).collect(),
411        }
412    }
413}
414
415impl<const N: usize> From<SRobotPath<N, f32>> for RobotPath<f64> {
416    fn from(sp: SRobotPath<N, f32>) -> Self {
417        let n = sp.waypoints.len();
418        let mut arr = RobotPath::zeros((n, N));
419        for (i, q) in sp.waypoints.iter().enumerate() {
420            for (j, &v) in q.0.iter().enumerate() {
421                arr[[i, j]] = v as f64;
422            }
423        }
424        arr
425    }
426}
427
428impl<const N: usize> From<SRobotPath<N, f64>> for RobotPath<f32> {
429    fn from(sp: SRobotPath<N, f64>) -> Self {
430        let n = sp.waypoints.len();
431        let mut arr = RobotPath::zeros((n, N));
432        for (i, q) in sp.waypoints.iter().enumerate() {
433            for (j, &v) in q.0.iter().enumerate() {
434                arr[[i, j]] = v as f32;
435            }
436        }
437        arr
438    }
439}
440
441impl<const N: usize> TryFrom<RobotPath<f64>> for SRobotPath<N, f32> {
442    type Error = crate::DekeError;
443
444    fn try_from(arr: RobotPath<f64>) -> Result<Self, Self::Error> {
445        if arr.ncols() != N {
446            return Err(crate::DekeError::ShapeMismatch {
447                expected: N,
448                found: arr.ncols(),
449            });
450        }
451        let mut waypoints = Vec::with_capacity(arr.nrows());
452        for row in arr.rows() {
453            let mut q = [0.0f32; N];
454            for (j, &v) in row.iter().enumerate() {
455                q[j] = v as f32;
456            }
457            waypoints.push(SRobotQ(q));
458        }
459        Self::try_new(waypoints)
460    }
461}
462
463impl<const N: usize> TryFrom<RobotPath<f32>> for SRobotPath<N, f64> {
464    type Error = crate::DekeError;
465
466    fn try_from(arr: RobotPath<f32>) -> Result<Self, Self::Error> {
467        if arr.ncols() != N {
468            return Err(crate::DekeError::ShapeMismatch {
469                expected: N,
470                found: arr.ncols(),
471            });
472        }
473        let mut waypoints = Vec::with_capacity(arr.nrows());
474        for row in arr.rows() {
475            let mut q = [0.0f64; N];
476            for (j, &v) in row.iter().enumerate() {
477                q[j] = v as f64;
478            }
479            waypoints.push(SRobotQ(q));
480        }
481        Self::try_new(waypoints)
482    }
483}
484
485#[cfg(feature = "valuable")]
486mod valuable_impl {
487    use super::SRobotPath;
488    use ::valuable::{
489        Fields, NamedField, NamedValues, StructDef, Structable, Valuable, Value, Visit,
490    };
491
492    const FIELDS: &[NamedField<'static>] = &[
493        NamedField::new("first"),
494        NamedField::new("last"),
495        NamedField::new("waypoints"),
496    ];
497
498    macro_rules! impl_valuable {
499        ($f:ty) => {
500            impl<const N: usize> Valuable for SRobotPath<N, $f> {
501                #[inline]
502                fn as_value(&self) -> Value<'_> {
503                    Value::Structable(self)
504                }
505                fn visit(&self, visit: &mut dyn Visit) {
506                    let values = [
507                        self.first.as_value(),
508                        self.last.as_value(),
509                        self.waypoints.as_value(),
510                    ];
511                    visit.visit_named_fields(&NamedValues::new(FIELDS, &values));
512                }
513            }
514
515            impl<const N: usize> Structable for SRobotPath<N, $f> {
516                fn definition(&self) -> StructDef<'_> {
517                    StructDef::new_static("SRobotPath", Fields::Named(FIELDS))
518                }
519            }
520        };
521    }
522
523    impl_valuable!(f32);
524    impl_valuable!(f64);
525}
526
527fn srdp_mark<const N: usize, F: Float>(
528    pts: &[SRobotQ<N, F>],
529    start: usize,
530    end: usize,
531    tol: F,
532    keep: &mut [bool],
533) {
534    if end <= start + 1 {
535        return;
536    }
537
538    let seg = pts[end] - pts[start];
539    let seg_len_sq = seg.norm_squared();
540
541    let mut max_dist = F::zero();
542    let mut max_idx = start;
543
544    let near_zero: F = F::from(1e-30).unwrap_or_else(F::zero);
545
546    for i in (start + 1)..end {
547        let v = pts[i] - pts[start];
548        let dist = if seg_len_sq < near_zero {
549            v.norm()
550        } else {
551            let t = (v.dot(&seg) / seg_len_sq).max(F::zero()).min(F::one());
552            (v - seg * t).norm()
553        };
554        if dist > max_dist {
555            max_dist = dist;
556            max_idx = i;
557        }
558    }
559
560    if max_dist > tol {
561        keep[max_idx] = true;
562        srdp_mark(pts, start, max_idx, tol, keep);
563        srdp_mark(pts, max_idx, end, tol, keep);
564    } else {
565        keep[(start + 1)..end].fill(false);
566    }
567}
deke_types/path.rs

deke_types/
path.rs
