trajectory/
cubic_spline.rs

Help
1use super::traits::*;
2use super::utils::*;
3use num_traits::Float;
4
5fn convert<T>(val: f64) -> T
6where
7    T: Float,
8{
9    T::from(val).unwrap()
10}
11
12#[derive(Debug)]
13pub struct CubicSpline<T>
14where
15    T: Float,
16{
17    times: Vec<T>,
18    points: Vec<Vec<T>>,
19    a: Vec<Vec<T>>,
20    b: Vec<Vec<T>>,
21    c: Vec<Vec<T>>,
22    d: Vec<Vec<T>>,
23}
24
25// from https://en.wikipedia.org/wiki/Spline_(mathematics)
26impl<T> CubicSpline<T>
27where
28    T: Float,
29{
30    #[allow(clippy::many_single_char_names, clippy::just_underscores_and_digits)]
31    pub fn new(times: Vec<T>, points: Vec<Vec<T>>) -> Option<Self> {
32        if !is_inputs_valid(&times, &points) {
33            return None;
34        }
35        let dim = points[0].len();
36        let _0: T = T::zero();
37        let _1: T = T::one();
38        let _2: T = convert(2.0);
39        let _3: T = convert(3.0);
40
41        // x_i = times[i]
42        // y_i = points[i][j]
43        let n = points.len() - 1;
44
45        // size of a is n+1
46        let a = points.clone();
47        // size of h is n
48        let mut h = vec![_0; n];
49        for i in 0..n {
50            h[i] = times[i + 1] - times[i];
51        }
52        let mut alpha = vec![vec![_0; dim]; n];
53        for i in 1..n {
54            for j in 0..dim {
55                alpha[i][j] = (_3 / h[i] * (a[i + 1][j] - a[i][j]))
56                    - (_3 / h[i - 1] * (a[i][j] - a[i - 1][j]));
57            }
58        }
59        let mut b = vec![vec![_0; dim]; n];
60        let mut c = vec![vec![_0; dim]; n + 1];
61        let mut d = vec![vec![_0; dim]; n];
62        let mut l = vec![_1; n + 1];
63        let mut m = vec![_0; n + 1];
64        let mut z = vec![vec![_0; dim]; n + 1];
65        for i in 1..n {
66            l[i] = _2 * (times[i + 1] - times[i - 1]) - h[i - 1] * m[i - 1];
67            m[i] = h[i] / l[i];
68            for j in 0..dim {
69                z[i][j] = (alpha[i][j] - h[i - 1] * z[i - 1][j]) / l[i];
70            }
71        }
72        l[n] = _1;
73        for j_ in 1..n + 1 {
74            let j = n - j_;
75            for k in 0..dim {
76                c[j][k] = z[j][k] - m[j] * c[j + 1][k];
77            }
78            for k in 0..dim {
79                b[j][k] =
80                    (a[j + 1][k] - a[j][k]) / h[j] - (h[j] * (c[j + 1][k] + _2 * c[j][k]) / _3);
81            }
82            for k in 0..dim {
83                d[j][k] = (c[j + 1][k] - c[j][k]) / (_3 * h[j]);
84            }
85        }
86        Some(Self {
87            times,
88            points,
89            a,
90            b,
91            c,
92            d,
93        })
94    }
95}
96
97impl<T> Trajectory for CubicSpline<T>
98where
99    T: Float,
100{
101    type Point = Vec<T>;
102    type Time = T;
103
104    fn position(&self, t: T) -> Option<Vec<T>> {
105        if t < self.times[0] {
106            return None;
107        }
108        let dim = self.points[0].len();
109        for i in 0..(self.points.len() - 1) {
110            if t >= self.times[i] && t <= self.times[i + 1] {
111                let mut pt = vec![T::zero(); dim];
112                let dx = t - self.times[i];
113                for (j, point_iter) in pt.iter_mut().enumerate() {
114                    *point_iter = self.a[i][j]
115                        + self.b[i][j] * dx
116                        + self.c[i][j] * dx * dx
117                        + self.d[i][j] * dx * dx * dx;
118                }
119                return Some(pt);
120            }
121        }
122        None
123    }
124
125    fn velocity(&self, t: T) -> Option<Vec<T>> {
126        if t < self.times[0] {
127            return None;
128        }
129        let dim = self.points[0].len();
130        for i in 0..(self.points.len() - 1) {
131            if t >= self.times[i] && t <= self.times[i + 1] {
132                let mut pt = vec![T::zero(); dim];
133                let dx = t - self.times[i];
134                for (j, point_iter) in pt.iter_mut().enumerate() {
135                    *point_iter = self.b[i][j]
136                        + convert::<T>(2.0) * self.c[i][j] * dx
137                        + convert::<T>(3.0) * self.d[i][j] * dx * dx;
138                }
139                return Some(pt);
140            }
141        }
142        None
143    }
144
145    fn acceleration(&self, t: T) -> Option<Vec<T>> {
146        if t < self.times[0] {
147            return None;
148        }
149        let dim = self.points[0].len();
150        for i in 0..(self.points.len() - 1) {
151            if t >= self.times[i] && t <= self.times[i + 1] {
152                let mut pt = vec![T::zero(); dim];
153                let dx = t - self.times[i];
154                for (j, point_iter) in pt.iter_mut().enumerate() {
155                    *point_iter =
156                        convert::<T>(2.0) * self.c[i][j] + convert::<T>(6.0) * self.d[i][j] * dx;
157                }
158                return Some(pt);
159            }
160        }
161        None
162    }
163}
164
165#[cfg(test)]
166mod tests {
167    use super::*;
168    #[test]
169    fn test_spline() {
170        let times = vec![0.0, 1.0, 3.0, 4.0];
171        let points = vec![
172            vec![0.0, -1.0],
173            vec![2.0, -3.0],
174            vec![3.0, 3.0],
175            vec![1.0, 5.0],
176        ];
177        let ip = CubicSpline::new(times, points).unwrap();
178        for i in 0..400 {
179            let t = i as f64 * 0.01f64;
180            let p = ip.position(t).unwrap();
181            println!("{t} {} {}", p[0], p[1]);
182        }
183    }
184
185    #[test]
186    fn test_velocity() {
187        let times = vec![0.0, 1.0, 3.0, 4.0];
188        let points = vec![
189            vec![0.0, -1.0],
190            vec![2.0, -3.0],
191            vec![3.0, 3.0],
192            vec![1.0, 5.0],
193        ];
194        let ip = CubicSpline::new(times, points).unwrap();
195        for i in 0..400 {
196            let t = i as f64 * 0.01f64;
197            let p = ip.velocity(t).unwrap();
198            println!("{t} {} {}", p[0], p[1]);
199        }
200    }
201
202    #[test]
203    fn test_acceleration() {
204        let times = vec![0.0, 1.0, 3.0, 4.0];
205        let points = vec![
206            vec![0.0, -1.0],
207            vec![2.0, -3.0],
208            vec![3.0, 3.0],
209            vec![1.0, 5.0],
210        ];
211        let ip = CubicSpline::new(times, points).unwrap();
212        for i in 0..400 {
213            let t = i as f64 * 0.01f64;
214            let p = ip.acceleration(t).unwrap();
215            println!("{t} {} {}", p[0], p[1]);
216        }
217    }
218}
trajectory/cubic_spline.rs

trajectory/
cubic_spline.rs
