trajectory/
linear.rs

Help
1use super::traits::*;
2use super::utils::*;
3use num_traits::Float;
4
5#[derive(Debug)]
6pub struct Linear<T>
7where
8    T: Float,
9{
10    times: Vec<T>,
11    points: Vec<Vec<T>>,
12}
13
14impl<T> Linear<T>
15where
16    T: Float,
17{
18    pub fn new(times: Vec<T>, points: Vec<Vec<T>>) -> Option<Self> {
19        if !is_inputs_valid(&times, &points) {
20            return None;
21        }
22        Some(Self { times, points })
23    }
24}
25
26impl<T> Trajectory for Linear<T>
27where
28    T: Float,
29{
30    type Point = Vec<T>;
31    type Time = T;
32
33    fn position(&self, t: T) -> Option<Vec<T>> {
34        if t < self.times[0] {
35            return None;
36        }
37        for i in 0..(self.points.len() - 1) {
38            let t0 = self.times[i];
39            let t1 = self.times[i + 1];
40            if t >= t0 && t <= t1 {
41                let mut pt = self.points[i].clone();
42                let p0 = &self.points[i];
43                let p1 = &self.points[i + 1];
44                for j in 0..p0.len() {
45                    pt[j] = p0[j] + (p1[j] - p0[j]) / (t1 - t0) * (t - t0);
46                }
47                return Some(pt);
48            }
49        }
50        None
51    }
52
53    fn velocity(&self, t: T) -> Option<Vec<T>> {
54        if t < self.times[0] {
55            return None;
56        }
57        let dim = self.points[0].len();
58
59        for i in 0..(self.points.len() - 1) {
60            let t0 = self.times[i];
61            let t1 = self.times[i + 1];
62            if t >= t0 && t <= t1 {
63                let mut pt = vec![T::zero(); dim];
64                let p0 = &self.points[i];
65                let p1 = &self.points[i + 1];
66                for j in 0..p0.len() {
67                    pt[j] = (p1[j] - p0[j]) / (t1 - t0);
68                }
69                return Some(pt);
70            }
71        }
72        None
73    }
74
75    fn acceleration(&self, t: T) -> Option<Vec<T>> {
76        if t < self.times[0] {
77            return None;
78        }
79        let dim = self.points[0].len();
80        for tm in &self.times {
81            if t == *tm {
82                return None;
83            }
84        }
85        Some(vec![T::zero(); dim])
86    }
87}
88
89#[cfg(test)]
90mod tests {
91    use super::*;
92    use assert_approx_eq::assert_approx_eq;
93
94    #[test]
95    fn test_linear() {
96        let times = vec![0.0, 1.0, 3.0, 4.0];
97        let points = vec![
98            vec![0.0, -1.0],
99            vec![2.0, -3.0],
100            vec![3.0, 3.0],
101            vec![1.0, 5.0],
102        ];
103        let ip = Linear::new(times, points).unwrap();
104        let p0 = ip.position(-0.5);
105        assert!(p0.is_none());
106        let p1 = ip.position(0.5).unwrap();
107        assert_approx_eq!(p1[0], 1.0);
108        assert_approx_eq!(p1[1], -2.0);
109        let p2 = ip.position(1.0).unwrap();
110        assert_approx_eq!(p2[0], 2.0);
111        assert_approx_eq!(p2[1], -3.0);
112        let p3 = ip.position(3.0).unwrap();
113        assert_approx_eq!(p3[0], 3.0);
114        assert_approx_eq!(p3[1], 3.0);
115        let p4 = ip.position(3.5).unwrap();
116        assert_approx_eq!(p4[0], 2.0);
117        assert_approx_eq!(p4[1], 4.0);
118    }
119}
trajectory/linear.rs

trajectory/
linear.rs
