rrtk/
state.rs

1// SPDX-License-Identifier: BSD-3-Clause
2// Copyright 2024 UxuginPython
3use crate::*;
4///A one-dimensional motion state with position, velocity, and acceleration.
5#[derive(Clone, Copy, Debug, Default, PartialEq)]
6pub struct State {
7    ///Where you are. This should be in millimeters.
8    pub position: f32,
9    ///How fast you're going. This should be in millimeters per second.
10    pub velocity: f32,
11    ///How fast how fast you're going's changing. This should be in millimeters per second squared.
12    pub acceleration: f32,
13}
14impl State {
15    ///Constructor for [`State`] using [`Quantity`] objects for position, velocity, and acceleration.
16    pub const fn new(position: Quantity, velocity: Quantity, acceleration: Quantity) -> Self {
17        position.unit.assert_eq_assume_ok(&MILLIMETER);
18        velocity.unit.assert_eq_assume_ok(&MILLIMETER_PER_SECOND);
19        acceleration
20            .unit
21            .assert_eq_assume_ok(&MILLIMETER_PER_SECOND_SQUARED);
22        State {
23            position: position.value,
24            velocity: velocity.value,
25            acceleration: acceleration.value,
26        }
27    }
28    ///Constructor for [`State`] using raw [`f32`]s for position, velocity, and acceleration.
29    pub const fn new_raw(position: f32, velocity: f32, acceleration: f32) -> Self {
30        State {
31            position: position,
32            velocity: velocity,
33            acceleration: acceleration,
34        }
35    }
36    ///Calculate the future state assuming a constant acceleration.
37    pub fn update(&mut self, delta_time: Time) {
38        let delta_time = Quantity::from(delta_time);
39        let old_acceleration = self.get_acceleration();
40        let old_velocity = self.get_velocity();
41        let old_position = self.get_position();
42        let new_velocity = old_velocity + delta_time * old_acceleration;
43        let new_position = old_position
44            + delta_time * (old_velocity + new_velocity) / Quantity::dimensionless(2.0);
45        self.position = new_position.value;
46        self.velocity = new_velocity.value;
47    }
48    ///Set the acceleration with a [`Quantity`]. With dimension checking enabled, sets the
49    ///acceleration and returns [`Ok`] if the argument's [`Unit`] is correct, otherwise leaves it
50    ///unchanged and returns [`Err`]. With dimension checking disabled, always sets the acceleration
51    ///to the [`Quantity`]'s value and returns [`Ok`], ignoring the [`Unit`].
52    pub const fn set_constant_acceleration(&mut self, acceleration: Quantity) -> Result<(), ()> {
53        if acceleration
54            .unit
55            .eq_assume_true(&MILLIMETER_PER_SECOND_SQUARED)
56        {
57            self.acceleration = acceleration.value;
58            Ok(())
59        } else {
60            Err(())
61        }
62    }
63    ///Set the acceleration with an [`f32`] of millimeters per second squared.
64    #[inline]
65    pub const fn set_constant_acceleration_raw(&mut self, acceleration: f32) {
66        self.acceleration = acceleration;
67    }
68    ///Set the velocity to a given value with a [`Quantity`], and set acceleration to zero. With
69    ///dimension checking enabled, sets the velocity and acceleration and returns [`Ok`] if the
70    ///argument's [`Unit`] is correct, otherwise leaves them unchanged and returns [`Err`]. With
71    ///dimension checking disabled, ignores the [`Unit`] and always sets velocity and acceleration
72    ///and returns [`Ok`].
73    pub const fn set_constant_velocity(&mut self, velocity: Quantity) -> Result<(), ()> {
74        if velocity.unit.eq_assume_true(&MILLIMETER_PER_SECOND) {
75            self.acceleration = 0.0;
76            self.velocity = velocity.value;
77            Ok(())
78        } else {
79            Err(())
80        }
81    }
82    ///Set the velocity to a given value with an [`f32`] of millimeters per second, and set acceleration to zero.
83    #[inline]
84    pub const fn set_constant_velocity_raw(&mut self, velocity: f32) {
85        self.acceleration = 0.0;
86        self.velocity = velocity;
87    }
88    ///Set the position to a given value with a [`Quantity`], and set velocity and acceleration to
89    ///zero. With dimension checking enabled, sets the position, velocity, and acceleration and
90    ///returns [`Ok`] if the argument's [`Unit`] is correct, otherwise leaves them unchanged and
91    ///returns [`Err`]. With dimension checking disabled, always sets the position, velocity, and
92    ///acceleration and returns [`Ok`], ignoring the [`Unit`].
93    pub const fn set_constant_position(&mut self, position: Quantity) -> Result<(), ()> {
94        if position.unit.eq_assume_true(&MILLIMETER) {
95            self.acceleration = 0.0;
96            self.velocity = 0.0;
97            self.position = position.value;
98            Ok(())
99        } else {
100            Err(())
101        }
102    }
103    ///Set the position to a given value with an [`f32`] of millimeters, and set velocity and acceleration to zero.
104    #[inline]
105    pub const fn set_constant_position_raw(&mut self, position: f32) {
106        self.acceleration = 0.0;
107        self.velocity = 0.0;
108        self.position = position;
109    }
110    ///Get the position as a [`Quantity`].
111    #[inline]
112    pub const fn get_position(&self) -> Quantity {
113        Quantity::new(self.position, MILLIMETER)
114    }
115    ///Get the velocity as a [`Quantity`].
116    #[inline]
117    pub const fn get_velocity(&self) -> Quantity {
118        Quantity::new(self.velocity, MILLIMETER_PER_SECOND)
119    }
120    ///Get the acceleration as a [`Quantity`].
121    #[inline]
122    pub const fn get_acceleration(&self) -> Quantity {
123        Quantity::new(self.acceleration, MILLIMETER_PER_SECOND_SQUARED)
124    }
125    ///State contains a position, velocity, and acceleration. This gets the respective field of a
126    ///given position derivative.
127    pub fn get_value(&self, position_derivative: PositionDerivative) -> Quantity {
128        match position_derivative {
129            PositionDerivative::Position => self.get_position(),
130            PositionDerivative::Velocity => self.get_velocity(),
131            PositionDerivative::Acceleration => self.get_acceleration(),
132        }
133    }
134}
135impl Neg for State {
136    type Output = Self;
137    fn neg(self) -> Self {
138        State::new_raw(-self.position, -self.velocity, -self.acceleration)
139    }
140}
141impl Add for State {
142    type Output = Self;
143    fn add(self, other: State) -> Self {
144        State::new_raw(
145            self.position + other.position,
146            self.velocity + other.velocity,
147            self.acceleration + other.acceleration,
148        )
149    }
150}
151impl Sub for State {
152    type Output = Self;
153    fn sub(self, other: State) -> Self {
154        State::new_raw(
155            self.position - other.position,
156            self.velocity - other.velocity,
157            self.acceleration - other.acceleration,
158        )
159    }
160}
161impl Mul<f32> for State {
162    type Output = Self;
163    fn mul(self, coef: f32) -> Self {
164        State::new_raw(
165            self.position * coef,
166            self.velocity * coef,
167            self.acceleration * coef,
168        )
169    }
170}
171impl Div<f32> for State {
172    type Output = Self;
173    fn div(self, dvsr: f32) -> Self {
174        State::new_raw(
175            self.position / dvsr,
176            self.velocity / dvsr,
177            self.acceleration / dvsr,
178        )
179    }
180}
181impl AddAssign for State {
182    fn add_assign(&mut self, other: State) {
183        *self = *self + other;
184    }
185}
186impl SubAssign for State {
187    fn sub_assign(&mut self, other: State) {
188        *self = *self - other;
189    }
190}
191impl MulAssign<f32> for State {
192    fn mul_assign(&mut self, coef: f32) {
193        *self = *self * coef;
194    }
195}
196impl DivAssign<f32> for State {
197    fn div_assign(&mut self, dvsr: f32) {
198        *self = *self / dvsr;
199    }
200}
rrtk/state.rs

rrtk/
state.rs
