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