motion_planning/
lib.rs

1use core::ops::{Add, Mul};
2use std::vec::Vec;
3
4pub mod hermite;
5use hermite::{h_5, h_5p, h_5pp};
6
7pub mod vec;
8
9#[derive(Debug, PartialEq)]
10pub struct Pose<V> {
11	pub position: V,
12	pub velocity: V,
13	pub acceleration: V,
14}
15
16#[derive(Debug, PartialEq)]
17pub struct Segment<'a, V>(f64, &'a Pose<V>, &'a Pose<V>);
18
19pub trait Trajectory<V> {
20	fn get_segment(&self, t: f64) -> Option<Segment<V>>;
21	fn position_at(&self, t: f64) -> Option<V>;
22	fn velocity_at(&self, t: f64) -> Option<V>;
23	fn acceleration_at(&self, t: f64) -> Option<V>;
24}
25
26impl<V> Trajectory<V> for Vec<Pose<V>>
27where
28	V: Add<V, Output = V> + Copy + Mul<f64, Output = V>,
29{
30	fn get_segment(&self, t: f64) -> Option<Segment<V>> {
31		let length = self.len();
32
33		// If our container (Vec) has length 0, we cannot find a segment!.
34		if let 0 = length {
35			return None;
36		}
37
38		// `t` ranges from `0.` to `length * 1.`;
39
40		let prec_idx = t.floor() as usize;
41		let succ_idx = t.ceil() as usize;
42
43		let prec: &Pose<V> = &self[prec_idx];
44		let succ: &Pose<V> = &self[succ_idx];
45
46		let t = t.fract();
47
48		assert!(0.0_f64 <= t && t <= 1.0_f64, "{} not in [0., 1.]", t);
49
50		Some(Segment(t, prec, succ))
51	}
52
53	fn position_at(&self, t: f64) -> Option<V> {
54		let anchors = self.get_segment(t);
55
56		if let Some(Segment(t, prec, succ)) = anchors {
57			let p0 = &prec.position;
58			let v0 = &prec.velocity;
59			let a0 = &prec.acceleration;
60
61			let p1 = &succ.position;
62			let v1 = &succ.velocity;
63			let a1 = &succ.acceleration;
64
65			let h05 = h_5(t, 0);
66			let h15 = h_5(t, 1);
67			let h25 = h_5(t, 2);
68			let h35 = h_5(t, 3);
69			let h45 = h_5(t, 4);
70			let h55 = h_5(t, 5);
71
72			return Some(
73				(*p0 * h05) + (*v0 * h15) + (*a0 * h25) + (*a1 * h35) + (*v1 * h45) + (*p1 * h55),
74			);
75		}
76
77		None
78	}
79
80	fn velocity_at(&self, t: f64) -> Option<V> {
81		let anchors = self.get_segment(t);
82
83		if let Some(Segment(t, prec, succ)) = anchors {
84			let p0 = &prec.position;
85			let v0 = &prec.velocity;
86			let a0 = &prec.acceleration;
87
88			let p1 = &succ.position;
89			let v1 = &succ.velocity;
90			let a1 = &succ.acceleration;
91
92			let h05p = h_5p(t, 0);
93			let h15p = h_5p(t, 1);
94			let h25p = h_5p(t, 2);
95			let h35p = h_5p(t, 3);
96			let h45p = h_5p(t, 4);
97			let h55p = h_5p(t, 5);
98
99			return Some(
100				(*p0 * h05p) + (*v0 * h15p) + (*a0 * h25p) + (*a1 * h35p) + (*v1 * h45p) + (*p1 * h55p),
101			);
102		}
103
104		None
105	}
106
107	fn acceleration_at(&self, t: f64) -> Option<V> {
108		let anchors = self.get_segment(t);
109
110		if let Some(Segment(t, prec, succ)) = anchors {
111			let p0 = &prec.position;
112			let v0 = &prec.velocity;
113			let a0 = &prec.acceleration;
114
115			let p1 = &succ.position;
116			let v1 = &succ.velocity;
117			let a1 = &succ.acceleration;
118
119			let h05pp = h_5pp(t, 0);
120			let h15pp = h_5pp(t, 1);
121			let h25pp = h_5pp(t, 2);
122			let h35pp = h_5pp(t, 3);
123			let h45pp = h_5pp(t, 4);
124			let h55pp = h_5pp(t, 5);
125
126			return Some(
127				(*p0 * h05pp)
128					+ (*v0 * h15pp)
129					+ (*a0 * h25pp)
130					+ (*a1 * h35pp)
131					+ (*v1 * h45pp)
132					+ (*p1 * h55pp),
133			);
134		}
135
136		None
137	}
138}
139
140#[cfg(test)]
141mod tests;
142
143#[cfg(test)]
144#[macro_export]
145macro_rules! assert_f64_roughly_eq {
146	($left:expr, $right:expr) => {
147		assert!(($right - $left).abs() < f64::EPSILON)
148	};
149}
motion_planning/lib.rs

motion_planning/
lib.rs
