geomath/
trajectory.rs

1//!
2//! A trajectory is a circular buffer containing vectors. It can be seen as the trajectory
3//! of a moving point, in this case, the vectors are the position vectors of the point.
4//!
5//! **Note :** Use the method `push` to add a point to a trajectory.
6//! ## Purpose
7//! This abstraction aims represent only the last points of a complete trajectory.
8//! It's main purpose is to allow drawing a trajectory consistently without allocating
9//! new space each time a point is added.
10//!
11//! ## Conventions
12//! * All the trajectories have the same fixed size
13//! * Vector space operations can be performed between trajectories
14//! * Translation can be performed with a vector using `+` and `-` operators
15//! * Scaling can be performed using `*` and `/` operators
16//! * When adding a point to a trajectory, the oldest point is erased
17//!
18
19use std::fmt;
20use std::fmt::Debug;
21use std::ops::{Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Sub, SubAssign};
22
23use crate::prelude::{Initializer, Reset};
24use crate::vector::{Vector2, Vector3, Vector4};
25use crate::trajectory::consts::*;
26
27/// 2D trajectory
28pub type Trajectory2 = Trajectory<Vector2>;
29
30/// 3D trajectory
31pub type Trajectory3 = Trajectory<Vector3>;
32
33/// 4D trajectory
34pub type Trajectory4 = Trajectory<Vector4>;
35
36
37/// constant trajectories
38pub mod consts {
39    use super::*;
40    use crate::vector;
41
42
43    /// Size of all the trajectories
44    pub const TRAJECTORY_SIZE: usize = 256;
45
46    /// 2D zero trajectory
47    pub const ZEROS_2: Trajectory2 = Trajectory2 {
48        positions: [vector::consts::ZEROS_2; TRAJECTORY_SIZE],
49        index: 0,
50    };
51
52    /// 3D zero trajectory
53    pub const ZEROS_3: Trajectory3 = Trajectory3 {
54        positions: [vector::consts::ZEROS_3; TRAJECTORY_SIZE],
55        index: 0,
56    };
57
58    /// 4D zero trajectory
59    pub const ZEROS_4: Trajectory4 = Trajectory4 {
60        positions: [vector::consts::ZEROS_4; TRAJECTORY_SIZE],
61        index: 0,
62    };
63}
64
65/// Generic structure and documentation for trajectories
66#[derive(Copy, Clone)]
67pub struct Trajectory<T> {
68    positions: [T; TRAJECTORY_SIZE],
69    index: usize,
70}
71
72impl<T> From<T> for Trajectory<T> where
73    T: Copy + Clone
74{
75    fn from(position: T) -> Self {
76        Trajectory::new([position; TRAJECTORY_SIZE], 0)
77    }
78}
79
80impl<T> From<[T; TRAJECTORY_SIZE]> for Trajectory<T> where
81    T: Copy + Clone {
82    fn from(positions: [T; TRAJECTORY_SIZE]) -> Self {
83        Trajectory::new(positions, 0)
84    }
85}
86
87impl<T> Trajectory<T> where
88    T: Copy + Clone {
89
90    /// Construct a trajectory with given positions and index of the oldest position in the array
91    #[inline]
92    pub fn new(positions: [T; TRAJECTORY_SIZE], index: usize) -> Trajectory<T> {
93        Trajectory { positions, index }
94    }
95
96    /// Add a vector to the trajectory
97    #[inline]
98    pub fn push(&mut self, position: &T) {
99        self.positions[self.index] = *position;
100        self.index = self.index_offset(1);
101    }
102
103    /// Get the positions vectors of the trajectory as array
104    #[inline]
105    pub fn positions(&self) -> &[T; TRAJECTORY_SIZE] { &self.positions }
106
107    /// Get the position vector at index. The 0 index is the oldest position vector
108    #[inline]
109    pub fn position(&self, i: usize) -> &T {
110        &self.positions[self.index_offset(i)]
111    }
112
113    /// Same as `position` but with a mutable reference
114    #[inline]
115    pub fn position_mut(&mut self, i: usize) -> &mut T {
116        &mut self.positions[self.index_offset(i)]
117    }
118
119    /// Get the lastly added position vector
120    pub fn last(&self) -> &T {
121        &self.positions[self.index_offset(TRAJECTORY_SIZE - 1)]
122    }
123
124    /// Same as `last` but with a mutable reference
125    pub fn last_mut(&mut self) -> &mut T {
126        &mut self.positions[self.index_offset(TRAJECTORY_SIZE - 1)]
127    }
128
129    #[inline]
130    fn index_offset(&self, i: usize) -> usize {
131        (i + self.index) % TRAJECTORY_SIZE
132    }
133}
134
135impl<T> Initializer for Trajectory<T> where
136    T: Initializer + Copy + Clone {
137    #[inline]
138    fn zeros() -> Self {
139        Trajectory::new([T::zeros(); TRAJECTORY_SIZE], 0)
140    }
141
142    #[inline]
143    fn ones() -> Self {
144        Trajectory::new([T::ones(); TRAJECTORY_SIZE], 0)
145    }
146}
147
148impl<T> Reset<T> for Trajectory<T> where
149    T: Reset<T> + Copy + Clone {
150    #[inline]
151    fn reset0(&mut self) -> &mut Self {
152        for position in self.positions.iter_mut() {
153            position.reset0();
154        }
155        self
156    }
157
158    #[inline]
159    fn reset1(&mut self) -> &mut Self {
160        for position in self.positions.iter_mut() {
161            position.reset1();
162        }
163        self
164    }
165
166    #[inline]
167    fn reset(&mut self, val: &T) -> &mut Self {
168        for pos in self.positions.iter_mut() {
169            pos.reset(val);
170        }
171        self
172    }
173}
174
175impl<T> Debug for Trajectory<T> where
176    T: Debug + Copy + Clone {
177    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> {
178        let mut buffer = String::new();
179        for i in TRAJECTORY_SIZE - 32..TRAJECTORY_SIZE {
180            buffer += format!("\n{:?}", self.positions[self.index_offset(i)]).as_str();
181        }
182        write!(f, "{}", buffer)
183    }
184}
185
186impl<T> Add<Trajectory<T>> for Trajectory<T> where
187    T: AddAssign<T> + Copy + Clone {
188    type Output = Trajectory<T>;
189    #[inline]
190    fn add(self, rhs: Trajectory<T>) -> Self::Output {
191        let mut ret = self;
192        ret += rhs;
193        ret
194    }
195}
196
197impl<T> Add<T> for Trajectory<T> where
198    T: AddAssign<T> + Copy + Clone {
199    type Output = Trajectory<T>;
200    #[inline]
201    fn add(self, rhs: T) -> Self::Output {
202        let mut ret = self;
203        ret += rhs;
204        ret
205    }
206}
207
208impl<T> AddAssign<Trajectory<T>> for Trajectory<T> where
209    T: AddAssign<T> + Copy + Clone {
210    #[inline]
211    fn add_assign(&mut self, rhs: Trajectory<T>) {
212        for i in 0..TRAJECTORY_SIZE {
213            self.positions[self.index_offset(i)] += rhs.positions[rhs.index_offset(i)];
214        }
215    }
216}
217
218impl<T> AddAssign<T> for Trajectory<T> where
219    T: AddAssign<T> + Copy + Clone {
220    #[inline]
221    fn add_assign(&mut self, rhs: T) {
222        for i in 0..TRAJECTORY_SIZE {
223            self.positions[i] += rhs;
224        }
225    }
226}
227
228impl<T> Sub<Trajectory<T>> for Trajectory<T> where
229    T: SubAssign<T> + Copy + Clone {
230    type Output = Trajectory<T>;
231    #[inline]
232    fn sub(self, rhs: Trajectory<T>) -> Self::Output {
233        let mut ret = self;
234        ret -= rhs;
235        ret
236    }
237}
238
239impl<T> Sub<T> for Trajectory<T> where
240    T: SubAssign<T> + Copy + Clone {
241    type Output = Trajectory<T>;
242    #[inline]
243    fn sub(self, rhs: T) -> Self::Output {
244        let mut ret = self;
245        ret -= rhs;
246        ret
247    }
248}
249
250impl<T> SubAssign<Trajectory<T>> for Trajectory<T> where
251    T: SubAssign<T> + Copy + Clone {
252    #[inline]
253    fn sub_assign(&mut self, rhs: Trajectory<T>) {
254        for i in 0..TRAJECTORY_SIZE {
255            self.positions[self.index_offset(i)] -= rhs.positions[rhs.index_offset(i)];
256        }
257    }
258}
259
260impl<T> SubAssign<T> for Trajectory<T> where
261    T: SubAssign<T> + Copy + Clone {
262    #[inline]
263    fn sub_assign(&mut self, rhs: T) {
264        for i in 0..TRAJECTORY_SIZE {
265            self.positions[i] -= rhs;
266        }
267    }
268}
269
270impl<T> Mul<f64> for Trajectory<T> where
271    T: MulAssign<f64> + Copy + Clone {
272    type Output = Trajectory<T>;
273    #[inline]
274    fn mul(self, rhs: f64) -> Self::Output {
275        let mut ret = self;
276        ret *= rhs;
277        ret
278    }
279}
280
281impl<T> MulAssign<f64> for Trajectory<T> where
282    T: MulAssign<f64> + Copy + Clone {
283    #[inline]
284    fn mul_assign(&mut self, rhs: f64) {
285        for i in 0..TRAJECTORY_SIZE {
286            self.positions[i] *= rhs;
287        }
288    }
289}
290
291impl<T> Div<f64> for Trajectory<T> where
292    T: DivAssign<f64> + Copy + Clone {
293    type Output = Trajectory<T>;
294    #[inline]
295    fn div(self, rhs: f64) -> Self::Output {
296        let mut ret = self;
297        ret /= rhs;
298        ret
299    }
300}
301
302impl<T> DivAssign<f64> for Trajectory<T> where
303    T: DivAssign<f64> + Copy + Clone {
304    #[inline]
305    fn div_assign(&mut self, rhs: f64) {
306        for i in 0..TRAJECTORY_SIZE {
307            self.positions[i] /= rhs;
308        }
309    }
310}
311
312impl<T> Index<usize> for Trajectory<T> where
313    T: Copy + Clone {
314    type Output = T;
315    #[inline]
316    fn index(&self, index: usize) -> &Self::Output {
317        &self.positions[self.index_offset(index)]
318    }
319}
320
321impl<T> IndexMut<usize> for Trajectory<T> where
322    T: Copy + Clone {
323    #[inline]
324    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
325        &mut self.positions[self.index_offset(index)]
326    }
327}
geomath/trajectory.rs

geomath/
trajectory.rs
