1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

use super::traits::*;
use super::utils::*;
use num_traits::Float;

#[derive(Debug)]
pub struct Linear<T>
where
    T: Float,
{
    times: Vec<T>,
    points: Vec<Vec<T>>,
}

impl<T> Linear<T>
where
    T: Float,
{
    pub fn new(times: Vec<T>, points: Vec<Vec<T>>) -> Option<Self> {
        if !is_inputs_valid(&times, &points) {
            return None;
        }
        Some(Self { times, points })
    }
}

impl<T> Trajectory for Linear<T>
where
    T: Float,
{
    type Point = Vec<T>;
    type Time = T;

    fn position(&self, t: T) -> Option<Vec<T>> {
        if t < self.times[0] {
            return None;
        }
        for i in 0..(self.points.len() - 1) {
            let t0 = self.times[i];
            let t1 = self.times[i + 1];
            if t >= t0 && t <= t1 {
                let mut pt = self.points[i].clone();
                let p0 = &self.points[i];
                let p1 = &self.points[i + 1];
                for j in 0..p0.len() {
                    pt[j] = p0[j] + (p1[j] - p0[j]) / (t1 - t0) * (t - t0);
                }
                return Some(pt);
            }
        }
        None
    }

    fn velocity(&self, t: T) -> Option<Vec<T>> {
        if t < self.times[0] {
            return None;
        }
        let dim = self.points[0].len();

        for i in 0..(self.points.len() - 1) {
            let t0 = self.times[i];
            let t1 = self.times[i + 1];
            if t >= t0 && t <= t1 {
                let mut pt = vec![T::zero(); dim];
                let p0 = &self.points[i];
                let p1 = &self.points[i + 1];
                for j in 0..p0.len() {
                    pt[j] = (p1[j] - p0[j]) / (t1 - t0);
                }
                return Some(pt);
            }
        }
        None
    }

    fn acceleration(&self, t: T) -> Option<Vec<T>> {
        if t < self.times[0] {
            return None;
        }
        let dim = self.points[0].len();
        for tm in &self.times {
            if t == *tm {
                return None;
            }
        }
        Some(vec![T::zero(); dim])
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use assert_approx_eq::assert_approx_eq;

    #[test]
    fn test_linear() {
        let times = vec![0.0, 1.0, 3.0, 4.0];
        let points = vec![
            vec![0.0, -1.0],
            vec![2.0, -3.0],
            vec![3.0, 3.0],
            vec![1.0, 5.0],
        ];
        let ip = Linear::new(times, points).unwrap();
        let p0 = ip.position(-0.5);
        assert!(p0.is_none());
        let p1 = ip.position(0.5).unwrap();
        assert_approx_eq!(p1[0], 1.0);
        assert_approx_eq!(p1[1], -2.0);
        let p2 = ip.position(1.0).unwrap();
        assert_approx_eq!(p2[0], 2.0);
        assert_approx_eq!(p2[1], -3.0);
        let p3 = ip.position(3.0).unwrap();
        assert_approx_eq!(p3[0], 3.0);
        assert_approx_eq!(p3[1], 3.0);
        let p4 = ip.position(3.5).unwrap();
        assert_approx_eq!(p4[0], 2.0);
        assert_approx_eq!(p4[1], 4.0);
    }
}