From bf44097156c993c897dc1f48931697e9b993d33b Mon Sep 17 00:00:00 2001 From: "Matheus T. dos Santos" Date: Fri, 8 Aug 2025 20:45:09 -0300 Subject: [PATCH 1/5] refactor: improve cart and state code readability --- Cargo.toml | 10 ++-- src/cart.rs | 146 ++++++++++++++++++++++++++++++++++----------------- src/state.rs | 45 ++++++++-------- 3 files changed, 127 insertions(+), 74 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 8b0d629..1a5fa39 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,8 +8,8 @@ egui-macroquad = "0.15.0" macroquad = "0.3.25" [profile.release] -opt-level = 'z' # Optimize for size -lto = true # Enable link-time optimization -codegen-units = 1 # Reduce number of codegen units to increase optimizations -panic = 'abort' # Abort on panic -strip = true # Strip symbols from binary* \ No newline at end of file +opt-level = 'z' # Optimize for size +lto = true # Enable link-time optimization +codegen-units = 1 # Reduce number of codegen units to increase optimizations +panic = 'abort' # Abort on panic +strip = true # Strip symbols from binary* diff --git a/src/cart.rs b/src/cart.rs index 5d0d90a..a592e52 100644 --- a/src/cart.rs +++ b/src/cart.rs @@ -56,9 +56,9 @@ impl Default for Cart { m, M, l: 1., - g: 9.80665, + g: Self::GRAVITY, F: 0., - Fclamp: 400., + Fclamp: Self::MAX_FORCE, Finp: 20., int: 0., error: 0., @@ -72,8 +72,8 @@ impl Default for Cart { m1, m2, m3, - pid: (40., 8., 2.5), - steps: 5, + pid: (Self::KP, Self::KI, Self::KD), + steps: Self::STEP_SIZE, enable: true, integrator: Integrator::default(), camera: CameraDynamics::default(), @@ -82,81 +82,131 @@ impl Default for Cart { } impl Cart { + const GRAVITY: f64 = 9.80665; + const MAX_FORCE: f64 = 400.; + const KP: f64 = 40.; + const KI: f64 = 8.; + const KD: f64 = 2.5; + const STEP_SIZE: i32 = 5; + + fn update_force(&mut self) { + if self.enable { + self.F = (10. + * (self.error * self.pid.0 + self.int * self.pid.1 - self.state.w * self.pid.2)) + .clamp(-self.Fclamp, self.Fclamp); + } else { + self.F = 0.; + } + } + + fn process_input(&mut self) { + if is_key_down(KeyCode::Left) { + self.F = -self.Finp; + self.int = 0. + } else if is_key_down(KeyCode::Right) { + self.F = self.Finp; + self.int = 0. + } + } + pub fn update(&mut self, dt: f64) { self.camera.update(self.state.x, self.state.v, dt); + let steps = if dt > 0.02 { ((self.steps * 60) as f64 * dt) as i32 } else { self.steps }; + let dt = dt / steps as f64; for _ in 0..steps { self.error = PI - self.state.th; self.int += self.error * dt; - self.F = 0.; - if self.enable { - self.F = (10. - * (self.error * self.pid.0 + self.int * self.pid.1 - - self.state.w * self.pid.2)) - .clamp(-self.Fclamp, self.Fclamp); - } - if is_key_down(KeyCode::Left) { - self.F = -self.Finp; - self.int = 0. - } else if is_key_down(KeyCode::Right) { - self.F = self.Finp; - self.int = 0. - } + + self.update_force(); + + self.process_input(); + let k1 = self.process_state(self.state); - if self.integrator == Integrator::Euler { - self.state.update(k1, dt); - continue; + match self.integrator { + Integrator::Euler => { + self.state = self.state.next_state(k1, dt); + } + Integrator::RungeKutta4 => { + let k2 = self.process_state(self.state.next_state(k1, dt * 0.5)); + let k3 = self.process_state(self.state.next_state(k2, dt * 0.5)); + let k4 = self.process_state(self.state.next_state(k3, dt)); + + let k_avg = ( + (k1.x + 2.0 * k2.x + 2.0 * k3.x + k4.x) / 6.0, + (k1.v + 2.0 * k2.v + 2.0 * k3.v + k4.v) / 6.0, + (k1.th + 2.0 * k2.th + 2.0 * k3.th + k4.th) / 6.0, + (k1.w + 2.0 * k2.w + 2.0 * k3.w + k4.w) / 6.0, + ) + .into(); + + self.state = self.state.next_state(k_avg, dt); + } } - let k2 = self.process_state(self.state.after(k1, dt * 0.5)); - let k3 = self.process_state(self.state.after(k2, dt * 0.5)); - let k4 = self.process_state(self.state.after(k3, dt)); - - let k_avg = ( - (k1.0 + 2.0 * k2.0 + 2.0 * k3.0 + k4.0) / 6.0, - (k1.1 + 2.0 * k2.1 + 2.0 * k3.1 + k4.1) / 6.0, - (k1.2 + 2.0 * k2.2 + 2.0 * k3.2 + k4.2) / 6.0, - (k1.3 + 2.0 * k2.3 + 2.0 * k3.3 + k4.3) / 6.0, - ); - self.state.update(k_avg, dt); } } - pub fn process_state(&self, state: State) -> (f64, f64, f64, f64) { - let (_, v, w, th) = state.unpack(); + #[inline(always)] + fn compute_d(&self, th: f64) -> f64 { + let c = th.cos(); + + // d = (m2 * m1 * l^2) - (m3 * l * cos(th))^2; + self.m2 * self.l * self.l * self.m1 - self.m3 * self.m3 * self.l * self.l * c * c + } + + #[inline(always)] + fn compute_f2(&self, th: f64, v: f64, w: f64) -> f64 { + let (s, c) = (th.sin(), th.cos()); + + // f2 = -(m3)^2 * l^2 * w^2 * sin(th) * cos(th) + // + m3 * l * b1 * v * cos(th) + // - m1 * (m3 * g * l * sin(th) + self.b2 * w); + -self.m3 * self.m3 * self.l * self.l * w * w * s * c + self.m3 * self.l * self.b1 * v * c + - self.m1 * (self.m3 * self.g * self.l * s + self.b2 * w) + } + #[inline(always)] + fn compute_f4(&self, th: f64, v: f64, w: f64) -> f64 { let (s, c) = (th.sin(), th.cos()); - let d = self.m2 * self.l * self.l * self.m1 - self.m3 * self.m3 * self.l * self.l * c * c; - let f2 = -self.m3 * self.m3 * self.l * self.l * w * w * s * c - + self.m3 * self.l * self.b1 * v * c - - self.m1 * (self.m3 * self.g * self.l * s + self.b2 * w); - let f4 = self.m2 * self.m3 * self.l * self.l * self.l * w * w * s + + // f4 = m2 * m3 * l^3 * w^2 * sin(th) + // - m2 * l^2 * b1 * v + // + m3^2 * l^2 * g * sin(th) * cos(th) + // + m3 * l * b2 * w * cos(th); + self.m2 * self.m3 * self.l * self.l * self.l * w * w * s - self.m2 * self.l * self.l * self.b1 * v + self.m3 * self.m3 * self.l * self.l * self.g * s * c - + self.m3 * self.l * self.b2 * w * c; - - // returns (vdot, v, wdot, w) - ( - (f4 + self.m2 * self.l * self.l * self.F) / d, - v, - (f2 - self.m3 * self.l * c * self.F) / d, - w, - ) + + self.m3 * self.l * self.b2 * w * c + } + + pub fn process_state(&self, State { v, w, th, .. }: State) -> State { + let d = self.compute_d(th); + let f2 = self.compute_f2(th, v, w); + let f4 = self.compute_f4(th, v, w); + + let v_dot = (f4 + self.m2 * self.l * self.l * self.F) / d; + let w_dot = (f2 - self.m3 * self.l * th.cos() * self.F) / d; + + (v_dot, v, w_dot, w).into() } pub fn get_potential_energy(&self) -> f64 { // with respect to ground -self.m3 * self.g * self.l * self.state.th.cos() } + pub fn get_kinetic_energy(&self) -> f64 { + // (m1 * v^2) / 2 + (m2 * (w * l)^2) / 2 + m3 * v * w * l * cos(th) 0.5 * self.m1 * self.state.v * self.state.v + 0.5 * self.m2 * self.state.w * self.state.w * self.l * self.l + self.m3 * self.state.v * self.state.w * self.l * self.state.th.cos() } + pub fn get_total_energy(&self) -> f64 { self.get_potential_energy() + self.get_kinetic_energy() } diff --git a/src/state.rs b/src/state.rs index 625dcf7..3b3aed6 100644 --- a/src/state.rs +++ b/src/state.rs @@ -1,40 +1,43 @@ use std::f64::consts::PI; #[derive(Clone, Copy, PartialEq)] - pub struct State { pub x: f64, pub v: f64, - pub w: f64, pub th: f64, + pub w: f64, } impl Default for State { fn default() -> Self { - Self::from(0.0, 0.0, 0.0, PI + 0.5) + Self { + x: 0.0, + v: 0.0, + th: PI + 0.5, // Initial angle offset + w: 0.0, + } } } -impl State { - pub fn from(x: f64, v: f64, w: f64, th: f64) -> Self { - State { x, v, w, th } - } - - pub fn update(&mut self, (vdot, v, wdot, w): (f64, f64, f64, f64), dt: f64) { - self.w += wdot * dt; - self.th += w * dt; - self.th = (self.th % (2. * PI) + 2. * PI) % (2. * PI); - self.v += vdot * dt; - self.x += v * dt; +impl From<(f64, f64, f64, f64)> for State { + fn from(tuple: (f64, f64, f64, f64)) -> Self { + Self { + x: tuple.0, + v: tuple.1, + th: tuple.2, + w: tuple.3, + } } +} - pub fn after(&self, (vdot, v, wdot, w): (f64, f64, f64, f64), dt: f64) -> State { - let mut new_state = self.clone(); - new_state.update((vdot, v, wdot, w), dt); - new_state - } +impl State { + pub fn next_state(mut self, state: State, dt: f64) -> State { + self.v += state.x * dt; + self.x += state.v * dt; - pub fn unpack(&self) -> (f64, f64, f64, f64) { - (self.x, self.v, self.w, self.th) + self.w += state.th * dt; + self.th += state.w * dt; + self.th = (self.th % (2. * PI) + 2. * PI) % (2. * PI); + self } } From 29d749bf486ee84211edfc08cc87d96d342921fd Mon Sep 17 00:00:00 2001 From: "Matheus T. dos Santos" Date: Fri, 8 Aug 2025 21:45:51 -0300 Subject: [PATCH 2/5] refactor: extract physics simulation from cart --- src/cart.rs | 264 ++++++++++++++++++++++++++++------------------------ src/main.rs | 16 ++-- src/ui.rs | 20 ++-- 3 files changed, 165 insertions(+), 135 deletions(-) diff --git a/src/cart.rs b/src/cart.rs index a592e52..61aa2f8 100644 --- a/src/cart.rs +++ b/src/cart.rs @@ -1,25 +1,17 @@ #![allow(non_snake_case)] -use std::f64::consts::PI; - +use crate::{camera::CameraDynamics, state::State}; use macroquad::prelude::*; +use std::f64::consts::PI; -use crate::{camera::CameraDynamics, state::State}; #[derive(PartialEq, Eq)] pub enum Integrator { Euler, RungeKutta4, } -impl Default for Integrator { - fn default() -> Self { - Self::RungeKutta4 - } -} - #[derive(PartialEq)] pub struct Cart { - pub F: f64, pub Fclamp: f64, pub Finp: f64, pub ui_scale: f32, @@ -27,22 +19,28 @@ pub struct Cart { pub pid: (f64, f64, f64), pub error: f64, pub int: f64, - pub state: State, pub integrator: Integrator, pub steps: i32, pub m: f64, pub M: f64, pub mw: f64, pub ml: f64, + pub R: f64, + pub camera: CameraDynamics, + pub physics: CartPhysics, +} + +#[derive(PartialEq)] +pub struct CartPhysics { + pub F: f64, + pub m1: f64, + pub m2: f64, + pub m3: f64, pub l: f64, pub b1: f64, pub b2: f64, - pub R: f64, - pub camera: CameraDynamics, - g: f64, - m1: f64, - m2: f64, - m3: f64, + pub g: f64, + pub state: State, } impl Default for Cart { @@ -55,34 +53,31 @@ impl Default for Cart { Cart { m, M, - l: 1., - g: Self::GRAVITY, - F: 0., Fclamp: Self::MAX_FORCE, Finp: 20., int: 0., error: 0., R: 0.1, - state: State::default(), - b1: 0.01, - b2: 0.005, ui_scale: 0.3, mw, ml, - m1, - m2, - m3, pid: (Self::KP, Self::KI, Self::KD), steps: Self::STEP_SIZE, enable: true, integrator: Integrator::default(), camera: CameraDynamics::default(), + physics: CartPhysics::new(m1, m2, m3), } } } +impl Default for Integrator { + fn default() -> Self { + Self::RungeKutta4 + } +} + impl Cart { - const GRAVITY: f64 = 9.80665; const MAX_FORCE: f64 = 400.; const KP: f64 = 40.; const KI: f64 = 8.; @@ -91,26 +86,56 @@ impl Cart { fn update_force(&mut self) { if self.enable { - self.F = (10. - * (self.error * self.pid.0 + self.int * self.pid.1 - self.state.w * self.pid.2)) + self.physics.F = (10. + * (self.error * self.pid.0 + self.int * self.pid.1 + - self.physics.state.w * self.pid.2)) .clamp(-self.Fclamp, self.Fclamp); } else { - self.F = 0.; + self.physics.F = 0.; } } fn process_input(&mut self) { if is_key_down(KeyCode::Left) { - self.F = -self.Finp; + self.physics.F = -self.Finp; self.int = 0. } else if is_key_down(KeyCode::Right) { - self.F = self.Finp; + self.physics.F = self.Finp; self.int = 0. } } + fn integrate(&mut self, dt: f64) { + let k1 = self.physics.simulate(self.physics.state); + match self.integrator { + Integrator::Euler => { + self.physics.state = self.physics.state.next_state(k1, dt); + } + Integrator::RungeKutta4 => { + let k2 = self + .physics + .simulate(self.physics.state.next_state(k1, dt * 0.5)); + let k3 = self + .physics + .simulate(self.physics.state.next_state(k2, dt * 0.5)); + let k4 = self.physics.simulate(self.physics.state.next_state(k3, dt)); + + let k_avg = ( + (k1.x + 2.0 * k2.x + 2.0 * k3.x + k4.x) / 6.0, + (k1.v + 2.0 * k2.v + 2.0 * k3.v + k4.v) / 6.0, + (k1.th + 2.0 * k2.th + 2.0 * k3.th + k4.th) / 6.0, + (k1.w + 2.0 * k2.w + 2.0 * k3.w + k4.w) / 6.0, + ) + .into(); + + self.physics.state = self.physics.state.next_state(k_avg, dt); + } + } + } + pub fn update(&mut self, dt: f64) { - self.camera.update(self.state.x, self.state.v, dt); + self.camera + .update(self.physics.state.x, self.physics.state.v, dt); let steps = if dt > 0.02 { ((self.steps * 60) as f64 * dt) as i32 @@ -120,97 +145,17 @@ impl Cart { let dt = dt / steps as f64; for _ in 0..steps { - self.error = PI - self.state.th; + self.error = PI - self.physics.state.th; self.int += self.error * dt; self.update_force(); self.process_input(); - let k1 = self.process_state(self.state); - match self.integrator { - Integrator::Euler => { - self.state = self.state.next_state(k1, dt); - } - Integrator::RungeKutta4 => { - let k2 = self.process_state(self.state.next_state(k1, dt * 0.5)); - let k3 = self.process_state(self.state.next_state(k2, dt * 0.5)); - let k4 = self.process_state(self.state.next_state(k3, dt)); - - let k_avg = ( - (k1.x + 2.0 * k2.x + 2.0 * k3.x + k4.x) / 6.0, - (k1.v + 2.0 * k2.v + 2.0 * k3.v + k4.v) / 6.0, - (k1.th + 2.0 * k2.th + 2.0 * k3.th + k4.th) / 6.0, - (k1.w + 2.0 * k2.w + 2.0 * k3.w + k4.w) / 6.0, - ) - .into(); - - self.state = self.state.next_state(k_avg, dt); - } - } + self.integrate(dt); } } - #[inline(always)] - fn compute_d(&self, th: f64) -> f64 { - let c = th.cos(); - - // d = (m2 * m1 * l^2) - (m3 * l * cos(th))^2; - self.m2 * self.l * self.l * self.m1 - self.m3 * self.m3 * self.l * self.l * c * c - } - - #[inline(always)] - fn compute_f2(&self, th: f64, v: f64, w: f64) -> f64 { - let (s, c) = (th.sin(), th.cos()); - - // f2 = -(m3)^2 * l^2 * w^2 * sin(th) * cos(th) - // + m3 * l * b1 * v * cos(th) - // - m1 * (m3 * g * l * sin(th) + self.b2 * w); - -self.m3 * self.m3 * self.l * self.l * w * w * s * c + self.m3 * self.l * self.b1 * v * c - - self.m1 * (self.m3 * self.g * self.l * s + self.b2 * w) - } - - #[inline(always)] - fn compute_f4(&self, th: f64, v: f64, w: f64) -> f64 { - let (s, c) = (th.sin(), th.cos()); - - // f4 = m2 * m3 * l^3 * w^2 * sin(th) - // - m2 * l^2 * b1 * v - // + m3^2 * l^2 * g * sin(th) * cos(th) - // + m3 * l * b2 * w * cos(th); - self.m2 * self.m3 * self.l * self.l * self.l * w * w * s - - self.m2 * self.l * self.l * self.b1 * v - + self.m3 * self.m3 * self.l * self.l * self.g * s * c - + self.m3 * self.l * self.b2 * w * c - } - - pub fn process_state(&self, State { v, w, th, .. }: State) -> State { - let d = self.compute_d(th); - let f2 = self.compute_f2(th, v, w); - let f4 = self.compute_f4(th, v, w); - - let v_dot = (f4 + self.m2 * self.l * self.l * self.F) / d; - let w_dot = (f2 - self.m3 * self.l * th.cos() * self.F) / d; - - (v_dot, v, w_dot, w).into() - } - - pub fn get_potential_energy(&self) -> f64 { - // with respect to ground - -self.m3 * self.g * self.l * self.state.th.cos() - } - - pub fn get_kinetic_energy(&self) -> f64 { - // (m1 * v^2) / 2 + (m2 * (w * l)^2) / 2 + m3 * v * w * l * cos(th) - 0.5 * self.m1 * self.state.v * self.state.v - + 0.5 * self.m2 * self.state.w * self.state.w * self.l * self.l - + self.m3 * self.state.v * self.state.w * self.l * self.state.th.cos() - } - - pub fn get_total_energy(&self) -> f64 { - self.get_potential_energy() + self.get_kinetic_energy() - } - pub fn display( &self, back_color: Color, @@ -220,11 +165,11 @@ impl Cart { depth: f32, ) { draw_line(-length, -depth, length, -depth, thickness, color); - let x = (self.state.x - self.camera.y) as f32 * self.ui_scale; + let x = (self.physics.state.x - self.camera.y) as f32 * self.ui_scale; let R = self.R as f32 * self.ui_scale; let (c, s) = ( - (self.state.x / self.R).cos() as f32, - (self.state.x / self.R).sin() as f32, + (self.physics.state.x / self.R).cos() as f32, + (self.physics.state.x / self.R).sin() as f32, ); let ticks = (9. / self.ui_scale) as i32; @@ -279,8 +224,11 @@ impl Cart { color, ); - let (c, s) = ((self.state.th).cos() as f32, (self.state.th).sin() as f32); - let l = self.l as f32 * self.ui_scale; + let (c, s) = ( + (self.physics.state.th).cos() as f32, + (self.physics.state.th).sin() as f32, + ); + let l = self.physics.l as f32 * self.ui_scale; // pendulum draw_line( x, @@ -294,3 +242,79 @@ impl Cart { draw_circle(x, -depth + 2. * R + h, 0.01, color); } } + +impl CartPhysics { + pub fn new(m1: f64, m2: f64, m3: f64) -> Self { + Self { + F: 0., + m1, + m2, + m3, + l: 1., + b1: 0.01, + b2: 0.005, + g: 9.80665, + state: State::default(), + } + } + + #[inline(always)] + fn compute_d(&self, th: f64) -> f64 { + let c = th.cos(); + + // d = (m2 * m1 * l^2) - (m3 * l * cos(th))^2; + self.m2 * self.l * self.l * self.m1 - self.m3 * self.m3 * self.l * self.l * c * c + } + + #[inline(always)] + fn compute_f2(&self, th: f64, v: f64, w: f64) -> f64 { + let (s, c) = (th.sin(), th.cos()); + + // f2 = -(m3)^2 * l^2 * w^2 * sin(th) * cos(th) + // + m3 * l * b1 * v * cos(th) + // - m1 * (m3 * g * l * sin(th) + self.b2 * w); + -self.m3 * self.m3 * self.l * self.l * w * w * s * c + self.m3 * self.l * self.b1 * v * c + - self.m1 * (self.m3 * self.g * self.l * s + self.b2 * w) + } + + #[inline(always)] + fn compute_f4(&self, th: f64, v: f64, w: f64) -> f64 { + let (s, c) = (th.sin(), th.cos()); + + // f4 = m2 * m3 * l^3 * w^2 * sin(th) + // - m2 * l^2 * b1 * v + // + m3^2 * l^2 * g * sin(th) * cos(th) + // + m3 * l * b2 * w * cos(th); + self.m2 * self.m3 * self.l * self.l * self.l * w * w * s + - self.m2 * self.l * self.l * self.b1 * v + + self.m3 * self.m3 * self.l * self.l * self.g * s * c + + self.m3 * self.l * self.b2 * w * c + } + + pub fn simulate(&self, State { v, w, th, .. }: State) -> State { + let d = self.compute_d(th); + let f2 = self.compute_f2(th, v, w); + let f4 = self.compute_f4(th, v, w); + + let v_dot = (f4 + self.m2 * self.l * self.l * self.F) / d; + let w_dot = (f2 - self.m3 * self.l * th.cos() * self.F) / d; + + (v_dot, v, w_dot, w).into() + } + + pub fn get_potential_energy(&self) -> f64 { + // with respect to ground + -self.m3 * self.g * self.l * self.state.th.cos() + } + + pub fn get_kinetic_energy(&self) -> f64 { + // (m1 * v^2) / 2 + (m2 * (w * l)^2) / 2 + m3 * v * w * l * cos(th) + 0.5 * self.m1 * self.state.v * self.state.v + + 0.5 * self.m2 * self.state.w * self.state.w * self.l * self.l + + self.m3 * self.state.v * self.state.w * self.l * self.state.th.cos() + } + + pub fn get_total_energy(&self) -> f64 { + self.get_potential_energy() + self.get_kinetic_energy() + } +} diff --git a/src/main.rs b/src/main.rs index 9f7fe0c..1fd5a02 100644 --- a/src/main.rs +++ b/src/main.rs @@ -54,8 +54,8 @@ async fn main() { if get_time() > 0. { cart.update(get_frame_time() as f64); } - forceplt.update([cart.F].to_vec()); - forceplt1.update([cart.int, -cart.state.w, cart.error].to_vec()); + forceplt.update([cart.physics.F].to_vec()); + forceplt1.update([cart.int, -cart.physics.state.w, cart.error].to_vec()); clear_background(back_color); draw_blue_grid(grid, SKYBLUE, 0.001, 3, 0.003); @@ -64,16 +64,16 @@ async fn main() { draw_speedometer( &format!( "Angular Velocity ({}) {:.2}", - if cart.state.w.is_sign_negative() { + if cart.physics.state.w.is_sign_negative() { "-" } else { "+" }, - cart.state.w.abs() + cart.physics.state.w.abs() ), vec2(0., screen_height() / screen_width() - 0.75 * grid), 0.08, - cart.state.w as f32, + cart.physics.state.w as f32, 9., 0.8, font, @@ -83,16 +83,16 @@ async fn main() { draw_speedometer( &format!( "Cart Velocity ({}) {:.2}", - if cart.state.v.is_sign_negative() { + if cart.physics.state.v.is_sign_negative() { "-" } else { "+" }, - cart.state.v.abs() + cart.physics.state.v.abs() ), vec2(0., screen_height() / screen_width() - 1.75 * grid), 0.08, - cart.state.v as f32, + cart.physics.state.v as f32, 20., 0.8, font, diff --git a/src/ui.rs b/src/ui.rs index 08dd72f..e1dd1ea 100644 --- a/src/ui.rs +++ b/src/ui.rs @@ -247,7 +247,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl }); ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.b1) + DragValue::new(&mut cart.physics.b1) .clamp_range(0.0..=0.5) .speed(0.0002) .custom_formatter(|x, _| format!("{:.3}", x)), @@ -256,7 +256,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl }); ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.l) + DragValue::new(&mut cart.physics.l) .clamp_range(0.1..=10.) .speed(0.05), ); @@ -290,7 +290,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl }); ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.b2) + DragValue::new(&mut cart.physics.b2) .clamp_range(0.0..=0.5) .speed(0.0002) .custom_formatter(|x, _| format!("{:.3}", x)), @@ -326,11 +326,17 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl // .title_bar(false) .show(ctx, |ui| { ui.with_layout(Layout::top_down(Align::Center), |ui| { - ui.label(format!("System Energy: {:.2}", cart.get_total_energy())); - ui.label(format!("Kinetic Energy: {:.2}", cart.get_kinetic_energy())); + ui.label(format!( + "System Energy: {:.2}", + cart.physics.get_total_energy() + )); + ui.label(format!( + "Kinetic Energy: {:.2}", + cart.physics.get_kinetic_energy() + )); ui.label(format!( "Potential Energy: {:.2}", - cart.get_potential_energy() + cart.physics.get_potential_energy() )); ui.separator(); ui.horizontal(|ui| { @@ -367,7 +373,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl }, ); if ui.button("Reset").clicked() { - cart.state = State::default(); + cart.physics.state = State::default(); cart.int = 0.; cart.camera = CameraDynamics::default(); }; From 752ca79d190af5d3fa8c7b524e2c7310d887a739 Mon Sep 17 00:00:00 2001 From: "Matheus T. dos Santos" Date: Fri, 8 Aug 2025 22:16:51 -0300 Subject: [PATCH 3/5] refactor: split graphics logic from cart to ui --- src/cart.rs | 209 ++++++++++++++++++++++++++++------------------------ src/main.rs | 9 ++- src/ui.rs | 14 ++-- 3 files changed, 127 insertions(+), 105 deletions(-) diff --git a/src/cart.rs b/src/cart.rs index 61aa2f8..6e0c57b 100644 --- a/src/cart.rs +++ b/src/cart.rs @@ -14,20 +14,14 @@ pub enum Integrator { pub struct Cart { pub Fclamp: f64, pub Finp: f64, - pub ui_scale: f32, pub enable: bool, pub pid: (f64, f64, f64), pub error: f64, pub int: f64, pub integrator: Integrator, pub steps: i32, - pub m: f64, - pub M: f64, - pub mw: f64, - pub ml: f64, - pub R: f64, - pub camera: CameraDynamics, pub physics: CartPhysics, + pub ui: CartUI, } #[derive(PartialEq)] @@ -43,6 +37,17 @@ pub struct CartPhysics { pub state: State, } +#[derive(PartialEq)] +pub struct CartUI { + pub ui_scale: f32, + pub m: f64, + pub M: f64, + pub mw: f64, + pub ml: f64, + pub R: f64, + pub camera: CameraDynamics, +} + impl Default for Cart { fn default() -> Self { let (M, m, ml, mw) = (5., 0.5, 1., 1.); @@ -51,22 +56,16 @@ impl Default for Cart { let m3 = m + ml / 2.; Cart { - m, - M, Fclamp: Self::MAX_FORCE, Finp: 20., int: 0., error: 0., - R: 0.1, - ui_scale: 0.3, - mw, - ml, pid: (Self::KP, Self::KI, Self::KD), steps: Self::STEP_SIZE, enable: true, integrator: Integrator::default(), - camera: CameraDynamics::default(), physics: CartPhysics::new(m1, m2, m3), + ui: CartUI::new(m, M, mw, ml), } } } @@ -134,7 +133,8 @@ impl Cart { } pub fn update(&mut self, dt: f64) { - self.camera + self.ui + .camera .update(self.physics.state.x, self.physics.state.v, dt); let steps = if dt > 0.02 { @@ -155,6 +155,96 @@ impl Cart { self.integrate(dt); } } +} + +impl CartPhysics { + pub fn new(m1: f64, m2: f64, m3: f64) -> Self { + Self { + F: 0., + m1, + m2, + m3, + l: 1., + b1: 0.01, + b2: 0.005, + g: 9.80665, + state: State::default(), + } + } + + #[inline(always)] + fn compute_d(&self, th: f64) -> f64 { + let c = th.cos(); + + // d = (m2 * m1 * l^2) - (m3 * l * cos(th))^2; + self.m2 * self.l * self.l * self.m1 - self.m3 * self.m3 * self.l * self.l * c * c + } + + #[inline(always)] + fn compute_f2(&self, th: f64, v: f64, w: f64) -> f64 { + let (s, c) = (th.sin(), th.cos()); + + // f2 = -(m3)^2 * l^2 * w^2 * sin(th) * cos(th) + // + m3 * l * b1 * v * cos(th) + // - m1 * (m3 * g * l * sin(th) + self.b2 * w); + -self.m3 * self.m3 * self.l * self.l * w * w * s * c + self.m3 * self.l * self.b1 * v * c + - self.m1 * (self.m3 * self.g * self.l * s + self.b2 * w) + } + + #[inline(always)] + fn compute_f4(&self, th: f64, v: f64, w: f64) -> f64 { + let (s, c) = (th.sin(), th.cos()); + + // f4 = m2 * m3 * l^3 * w^2 * sin(th) + // - m2 * l^2 * b1 * v + // + m3^2 * l^2 * g * sin(th) * cos(th) + // + m3 * l * b2 * w * cos(th); + self.m2 * self.m3 * self.l * self.l * self.l * w * w * s + - self.m2 * self.l * self.l * self.b1 * v + + self.m3 * self.m3 * self.l * self.l * self.g * s * c + + self.m3 * self.l * self.b2 * w * c + } + + pub fn simulate(&self, State { v, w, th, .. }: State) -> State { + let d = self.compute_d(th); + let f2 = self.compute_f2(th, v, w); + let f4 = self.compute_f4(th, v, w); + + let v_dot = (f4 + self.m2 * self.l * self.l * self.F) / d; + let w_dot = (f2 - self.m3 * self.l * th.cos() * self.F) / d; + + (v_dot, v, w_dot, w).into() + } + + pub fn get_potential_energy(&self) -> f64 { + // with respect to ground + -self.m3 * self.g * self.l * self.state.th.cos() + } + + pub fn get_kinetic_energy(&self) -> f64 { + // (m1 * v^2) / 2 + (m2 * (w * l)^2) / 2 + m3 * v * w * l * cos(th) + 0.5 * self.m1 * self.state.v * self.state.v + + 0.5 * self.m2 * self.state.w * self.state.w * self.l * self.l + + self.m3 * self.state.v * self.state.w * self.l * self.state.th.cos() + } + + pub fn get_total_energy(&self) -> f64 { + self.get_potential_energy() + self.get_kinetic_energy() + } +} + +impl CartUI { + pub fn new(m: f64, M: f64, mw: f64, ml: f64) -> Self { + Self { + ui_scale: 0.3, + m, + M, + mw, + ml, + R: 0.1, + camera: CameraDynamics::default(), + } + } pub fn display( &self, @@ -163,13 +253,14 @@ impl Cart { thickness: f32, length: f32, depth: f32, + physics: &CartPhysics, ) { draw_line(-length, -depth, length, -depth, thickness, color); - let x = (self.physics.state.x - self.camera.y) as f32 * self.ui_scale; + let x = (physics.state.x - self.camera.y) as f32 * self.ui_scale; let R = self.R as f32 * self.ui_scale; let (c, s) = ( - (self.physics.state.x / self.R).cos() as f32, - (self.physics.state.x / self.R).sin() as f32, + (physics.state.x / self.R).cos() as f32, + (physics.state.x / self.R).sin() as f32, ); let ticks = (9. / self.ui_scale) as i32; @@ -225,10 +316,10 @@ impl Cart { ); let (c, s) = ( - (self.physics.state.th).cos() as f32, - (self.physics.state.th).sin() as f32, + (physics.state.th).cos() as f32, + (physics.state.th).sin() as f32, ); - let l = self.physics.l as f32 * self.ui_scale; + let l = physics.l as f32 * self.ui_scale; // pendulum draw_line( x, @@ -242,79 +333,3 @@ impl Cart { draw_circle(x, -depth + 2. * R + h, 0.01, color); } } - -impl CartPhysics { - pub fn new(m1: f64, m2: f64, m3: f64) -> Self { - Self { - F: 0., - m1, - m2, - m3, - l: 1., - b1: 0.01, - b2: 0.005, - g: 9.80665, - state: State::default(), - } - } - - #[inline(always)] - fn compute_d(&self, th: f64) -> f64 { - let c = th.cos(); - - // d = (m2 * m1 * l^2) - (m3 * l * cos(th))^2; - self.m2 * self.l * self.l * self.m1 - self.m3 * self.m3 * self.l * self.l * c * c - } - - #[inline(always)] - fn compute_f2(&self, th: f64, v: f64, w: f64) -> f64 { - let (s, c) = (th.sin(), th.cos()); - - // f2 = -(m3)^2 * l^2 * w^2 * sin(th) * cos(th) - // + m3 * l * b1 * v * cos(th) - // - m1 * (m3 * g * l * sin(th) + self.b2 * w); - -self.m3 * self.m3 * self.l * self.l * w * w * s * c + self.m3 * self.l * self.b1 * v * c - - self.m1 * (self.m3 * self.g * self.l * s + self.b2 * w) - } - - #[inline(always)] - fn compute_f4(&self, th: f64, v: f64, w: f64) -> f64 { - let (s, c) = (th.sin(), th.cos()); - - // f4 = m2 * m3 * l^3 * w^2 * sin(th) - // - m2 * l^2 * b1 * v - // + m3^2 * l^2 * g * sin(th) * cos(th) - // + m3 * l * b2 * w * cos(th); - self.m2 * self.m3 * self.l * self.l * self.l * w * w * s - - self.m2 * self.l * self.l * self.b1 * v - + self.m3 * self.m3 * self.l * self.l * self.g * s * c - + self.m3 * self.l * self.b2 * w * c - } - - pub fn simulate(&self, State { v, w, th, .. }: State) -> State { - let d = self.compute_d(th); - let f2 = self.compute_f2(th, v, w); - let f4 = self.compute_f4(th, v, w); - - let v_dot = (f4 + self.m2 * self.l * self.l * self.F) / d; - let w_dot = (f2 - self.m3 * self.l * th.cos() * self.F) / d; - - (v_dot, v, w_dot, w).into() - } - - pub fn get_potential_energy(&self) -> f64 { - // with respect to ground - -self.m3 * self.g * self.l * self.state.th.cos() - } - - pub fn get_kinetic_energy(&self) -> f64 { - // (m1 * v^2) / 2 + (m2 * (w * l)^2) / 2 + m3 * v * w * l * cos(th) - 0.5 * self.m1 * self.state.v * self.state.v - + 0.5 * self.m2 * self.state.w * self.state.w * self.l * self.l - + self.m3 * self.state.v * self.state.w * self.l * self.state.th.cos() - } - - pub fn get_total_energy(&self) -> f64 { - self.get_potential_energy() + self.get_kinetic_energy() - } -} diff --git a/src/main.rs b/src/main.rs index 1fd5a02..d466d11 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,7 +60,14 @@ async fn main() { clear_background(back_color); draw_blue_grid(grid, SKYBLUE, 0.001, 3, 0.003); - cart.display(back_color, WHITE, 0.006, 6. * grid, 3. * grid); + cart.ui.display( + back_color, + WHITE, + 0.006, + 6. * grid, + 3. * grid, + &cart.physics, + ); draw_speedometer( &format!( "Angular Velocity ({}) {:.2}", diff --git a/src/ui.rs b/src/ui.rs index e1dd1ea..658a041 100644 --- a/src/ui.rs +++ b/src/ui.rs @@ -231,7 +231,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl cols[0].with_layout(Layout::top_down(Align::Max), |ui| { ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.M) + DragValue::new(&mut cart.ui.M) .clamp_range(0.0..=100.) .speed(0.05), ); @@ -239,7 +239,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl }); ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.ml) + DragValue::new(&mut cart.ui.ml) .clamp_range(0.0..=100.) .speed(0.05), ); @@ -274,7 +274,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl cols[1].with_layout(Layout::top_down(Align::Max), |ui| { ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.m) + DragValue::new(&mut cart.ui.m) .clamp_range(0.0..=100.) .speed(0.05), ); @@ -282,7 +282,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl }); ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.mw) + DragValue::new(&mut cart.ui.mw) .clamp_range(0.0..=100.) .speed(0.05), ); @@ -299,7 +299,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl }); ui.horizontal(|ui| { ui.add( - DragValue::new(&mut cart.R) + DragValue::new(&mut cart.ui.R) .clamp_range(0.0..=1.) .speed(0.005), ); @@ -355,7 +355,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl .text("Steps / Frame"), ); ui.add( - Slider::new(&mut cart.ui_scale, 0.03..=0.6) + Slider::new(&mut cart.ui.ui_scale, 0.03..=0.6) .custom_formatter(|n, _| format!("{:.2}", n / 0.3)) .custom_parser(|s| s.parse::().map(|v| v * 0.3).ok()) .text("Draw Scale"), @@ -375,7 +375,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl if ui.button("Reset").clicked() { cart.physics.state = State::default(); cart.int = 0.; - cart.camera = CameraDynamics::default(); + cart.ui.camera = CameraDynamics::default(); }; }) }); From dd86172728c3d0b2d99b2a56827bad6a24ffa44e Mon Sep 17 00:00:00 2001 From: "Matheus T. dos Santos" Date: Fri, 8 Aug 2025 23:01:41 -0300 Subject: [PATCH 4/5] refactor: replace custom PID to PID from aule-rs --- Cargo.toml | 2 ++ src/cart.rs | 35 +++++++++++++++-------------------- src/main.rs | 9 ++++++++- src/ui.rs | 8 ++++---- 4 files changed, 29 insertions(+), 25 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 1a5fa39..fac6b97 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,6 +6,8 @@ edition = "2021" [dependencies] egui-macroquad = "0.15.0" macroquad = "0.3.25" +aule = { git = "https://github.com/matheuswhite/aule-rs" } +ndarray = "0.16.1" [profile.release] opt-level = 'z' # Optimize for size diff --git a/src/cart.rs b/src/cart.rs index 6e0c57b..cbbf5d7 100644 --- a/src/cart.rs +++ b/src/cart.rs @@ -1,8 +1,9 @@ #![allow(non_snake_case)] use crate::{camera::CameraDynamics, state::State}; +use aule::prelude::*; use macroquad::prelude::*; -use std::f64::consts::PI; +use std::{f64::consts::PI, time::Duration}; #[derive(PartialEq, Eq)] pub enum Integrator { @@ -15,9 +16,7 @@ pub struct Cart { pub Fclamp: f64, pub Finp: f64, pub enable: bool, - pub pid: (f64, f64, f64), - pub error: f64, - pub int: f64, + pub pid: PID, pub integrator: Integrator, pub steps: i32, pub physics: CartPhysics, @@ -58,9 +57,7 @@ impl Default for Cart { Cart { Fclamp: Self::MAX_FORCE, Finp: 20., - int: 0., - error: 0., - pid: (Self::KP, Self::KI, Self::KD), + pid: PID::new(Self::KP, Self::KI, Self::KD), steps: Self::STEP_SIZE, enable: true, integrator: Integrator::default(), @@ -78,17 +75,15 @@ impl Default for Integrator { impl Cart { const MAX_FORCE: f64 = 400.; - const KP: f64 = 40.; - const KI: f64 = 8.; - const KD: f64 = 2.5; + const KP: f32 = 40.; + const KI: f32 = 8.; + const KD: f32 = 2.5; const STEP_SIZE: i32 = 5; - fn update_force(&mut self) { + fn update_force(&mut self, error: Signal) { if self.enable { - self.physics.F = (10. - * (self.error * self.pid.0 + self.int * self.pid.1 - - self.physics.state.w * self.pid.2)) - .clamp(-self.Fclamp, self.Fclamp); + let pid_output = self.pid.output(error).value as f64; + self.physics.F = (10. * pid_output).clamp(-self.Fclamp, self.Fclamp); } else { self.physics.F = 0.; } @@ -97,10 +92,10 @@ impl Cart { fn process_input(&mut self) { if is_key_down(KeyCode::Left) { self.physics.F = -self.Finp; - self.int = 0. + self.pid.clear_integral(); } else if is_key_down(KeyCode::Right) { self.physics.F = self.Finp; - self.int = 0. + self.pid.clear_integral(); } } @@ -145,10 +140,10 @@ impl Cart { let dt = dt / steps as f64; for _ in 0..steps { - self.error = PI - self.physics.state.th; - self.int += self.error * dt; + let error = PI - self.physics.state.th; + let error = (error as f32, Duration::from_secs_f64(dt)).into(); - self.update_force(); + self.update_force(error); self.process_input(); diff --git a/src/main.rs b/src/main.rs index d466d11..cde285f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -55,7 +55,14 @@ async fn main() { cart.update(get_frame_time() as f64); } forceplt.update([cart.physics.F].to_vec()); - forceplt1.update([cart.int, -cart.physics.state.w, cart.error].to_vec()); + forceplt1.update( + [ + cart.pid.integral() as f64, + -cart.physics.state.w, + cart.pid.error() as f64, + ] + .to_vec(), + ); clear_background(back_color); draw_blue_grid(grid, SKYBLUE, 0.001, 3, 0.003); diff --git a/src/ui.rs b/src/ui.rs index 658a041..91fa343 100644 --- a/src/ui.rs +++ b/src/ui.rs @@ -210,17 +210,17 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl .show(ctx, |ui| { ui.with_layout(Layout::top_down(Align::RIGHT), |ui| { ui.add( - Slider::new(&mut cart.pid.0, 0.0..=150.0) + Slider::new(cart.pid.kp_mut(), 0.0..=150.0) .drag_value_speed(0.2) .text("P"), ); ui.add( - Slider::new(&mut cart.pid.1, 0.0..=100.0) + Slider::new(cart.pid.ki_mut(), 0.0..=100.0) .drag_value_speed(0.1) .text("I"), ); ui.add( - Slider::new(&mut cart.pid.2, 0.0..=40.) + Slider::new(cart.pid.kd_mut(), 0.0..=40.) .drag_value_speed(0.04) .text("D"), ); @@ -374,7 +374,7 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl ); if ui.button("Reset").clicked() { cart.physics.state = State::default(); - cart.int = 0.; + cart.pid.clear_integral(); cart.ui.camera = CameraDynamics::default(); }; }) From 2c811965d7ea3e1d21bbfaf9c7679119a3d3d1df Mon Sep 17 00:00:00 2001 From: "Matheus T. dos Santos" Date: Sun, 10 Aug 2025 19:47:17 -0300 Subject: [PATCH 5/5] refactor: replace custom integrator by aule integrators --- src/cart.rs | 57 +++++++++++++++++++++++++--------------------------- src/state.rs | 29 +++++++++++++++++++++++++- src/ui.rs | 10 ++++++--- 3 files changed, 62 insertions(+), 34 deletions(-) diff --git a/src/cart.rs b/src/cart.rs index cbbf5d7..2397607 100644 --- a/src/cart.rs +++ b/src/cart.rs @@ -3,10 +3,11 @@ use crate::{camera::CameraDynamics, state::State}; use aule::prelude::*; use macroquad::prelude::*; -use std::{f64::consts::PI, time::Duration}; +use ndarray::Array2; +use std::{f64::consts::PI, mem::swap, time::Duration}; #[derive(PartialEq, Eq)] -pub enum Integrator { +pub enum IntegratorKind { Euler, RungeKutta4, } @@ -17,7 +18,7 @@ pub struct Cart { pub Finp: f64, pub enable: bool, pub pid: PID, - pub integrator: Integrator, + pub integrator: IntegratorKind, pub steps: i32, pub physics: CartPhysics, pub ui: CartUI, @@ -60,16 +61,16 @@ impl Default for Cart { pid: PID::new(Self::KP, Self::KI, Self::KD), steps: Self::STEP_SIZE, enable: true, - integrator: Integrator::default(), + integrator: IntegratorKind::default(), physics: CartPhysics::new(m1, m2, m3), ui: CartUI::new(m, M, mw, ml), } } } -impl Default for Integrator { +impl Default for IntegratorKind { fn default() -> Self { - Self::RungeKutta4 + Self::Euler } } @@ -100,31 +101,15 @@ impl Cart { } fn integrate(&mut self, dt: f64) { - let k1 = self.physics.simulate(self.physics.state); - match self.integrator { - Integrator::Euler => { - self.physics.state = self.physics.state.next_state(k1, dt); - } - Integrator::RungeKutta4 => { - let k2 = self - .physics - .simulate(self.physics.state.next_state(k1, dt * 0.5)); - let k3 = self - .physics - .simulate(self.physics.state.next_state(k2, dt * 0.5)); - let k4 = self.physics.simulate(self.physics.state.next_state(k3, dt)); - - let k_avg = ( - (k1.x + 2.0 * k2.x + 2.0 * k3.x + k4.x) / 6.0, - (k1.v + 2.0 * k2.v + 2.0 * k3.v + k4.v) / 6.0, - (k1.th + 2.0 * k2.th + 2.0 * k3.th + k4.th) / 6.0, - (k1.w + 2.0 * k2.w + 2.0 * k3.w + k4.w) / 6.0, - ) - .into(); - - self.physics.state = self.physics.state.next_state(k_avg, dt); - } + let old_state: Array2 = self.physics.state.into(); + let dt = Duration::from_secs_f64(dt); + + self.physics.state = match self.integrator { + IntegratorKind::Euler => Euler::integrate(old_state, dt, &self.physics), + IntegratorKind::RungeKutta4 => RK4::integrate(old_state, dt, &self.physics), } + .into(); + self.physics.state.th = (self.physics.state.th % (2. * PI) + 2. * PI) % (2. * PI); } pub fn update(&mut self, dt: f64) { @@ -228,6 +213,18 @@ impl CartPhysics { } } +impl StateEstimation for CartPhysics { + fn estimate(&self, state: Array2) -> Array2 { + let state: State = state.into(); + + let mut next_state = self.simulate(state); + swap(&mut next_state.x, &mut next_state.v); + swap(&mut next_state.th, &mut next_state.w); + + Array2::from(next_state) + } +} + impl CartUI { pub fn new(m: f64, M: f64, mw: f64, ml: f64) -> Self { Self { diff --git a/src/state.rs b/src/state.rs index 3b3aed6..bf86aa9 100644 --- a/src/state.rs +++ b/src/state.rs @@ -1,6 +1,7 @@ +use ndarray::Array2; use std::f64::consts::PI; -#[derive(Clone, Copy, PartialEq)] +#[derive(Clone, Copy, PartialEq, Debug)] pub struct State { pub x: f64, pub v: f64, @@ -41,3 +42,29 @@ impl State { self } } + +impl From> for State { + fn from(array: Array2) -> Self { + Self { + x: array[[0, 0]] as f64, + v: array[[1, 0]] as f64, + th: array[[2, 0]] as f64, + w: array[[3, 0]] as f64, + } + } +} + +impl From for Array2 { + fn from(state: State) -> Self { + Array2::from_shape_vec( + (4, 1), + vec![ + state.x as f32, + state.v as f32, + state.th as f32, + state.w as f32, + ], + ) + .unwrap() + } +} diff --git a/src/ui.rs b/src/ui.rs index 91fa343..5cff4d9 100644 --- a/src/ui.rs +++ b/src/ui.rs @@ -340,11 +340,15 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl )); ui.separator(); ui.horizontal(|ui| { - ui.label("Integrator: "); - ui.selectable_value(&mut cart.integrator, cart::Integrator::Euler, "Euler"); + ui.label("IntegratorKind: "); ui.selectable_value( &mut cart.integrator, - cart::Integrator::RungeKutta4, + cart::IntegratorKind::Euler, + "Euler", + ); + ui.selectable_value( + &mut cart.integrator, + cart::IntegratorKind::RungeKutta4, "Runge-Kutta⁴", ); });