From f3d1e599421ae5cdb66b4e5166966f95c6446142 Mon Sep 17 00:00:00 2001 From: sfja Date: Tue, 7 Apr 2026 12:37:53 +0200 Subject: [PATCH] play probe data --- simul/src/m3x3.rs | 10 +++- simul/src/main.rs | 141 ++++++++++++++++++++++++++++++++++++++------ simul/src/window.rs | 52 +++++++++++++--- 3 files changed, 177 insertions(+), 26 deletions(-) diff --git a/simul/src/m3x3.rs b/simul/src/m3x3.rs index 0ea5c8a..3c85fe1 100644 --- a/simul/src/m3x3.rs +++ b/simul/src/m3x3.rs @@ -1,4 +1,4 @@ -use std::ops::Mul; +use std::ops::{Mul, MulAssign}; use crate::v3::V3; @@ -9,6 +9,9 @@ impl M3x3 { pub fn init(v: f64) -> Self { Self([[v; 3]; 3]) } + pub fn identity() -> Self { + Self([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) + } pub fn from_v3_rot(rot: V3) -> Self { M3x3::new_rotate_x(rot.0) * M3x3::new_rotate_y(rot.1) * M3x3::new_rotate_z(rot.2) @@ -72,3 +75,8 @@ impl Mul for M3x3 { ) } } +impl MulAssign for M3x3 { + fn mul_assign(&mut self, rhs: Self) { + *self = *self * rhs; + } +} diff --git a/simul/src/main.rs b/simul/src/main.rs index 64c033f..2b0cc23 100644 --- a/simul/src/main.rs +++ b/simul/src/main.rs @@ -52,22 +52,50 @@ impl AssetStore { pub struct App { assets: AssetStore, rot: V3, + motion_dev: MotionDevice, + dev_rot_rot: M3x3, + dev_accel_rot: M3x3, } impl App { - pub fn new() -> Self { + pub fn new(motion_dev: MotionDevice) -> Self { Self { assets: AssetStore::new(), rot: V3::init(0.0), + motion_dev, + dev_rot_rot: M3x3::identity(), + dev_accel_rot: M3x3::identity(), } } } +fn calculate_accel_axis_angle(axis: f64, tangent0: f64, tangent1: f64) -> f64 { + (axis / (tangent0 * tangent0 + tangent1 * tangent1).sqrt()).atan() +} + impl window::App for App { fn update(&mut self, delta_time: std::time::Duration) { self.rot.0 += PI * 2.0 * delta_time.as_secs_f64() * 0.2; self.rot.1 += PI * 2.0 * delta_time.as_secs_f64() * 0.2; // self.rot.2 += PI * 2.0 * delta_time.as_secs_f64() * 0.2; + + self.motion_dev + .update(delta_time.as_micros() as i64, |m, dt| { + self.dev_rot_rot *= M3x3::new_rotate_x(m.rotation().0 * dt); + self.dev_rot_rot *= M3x3::new_rotate_y(m.rotation().1 * dt); + self.dev_rot_rot *= M3x3::new_rotate_z(m.rotation().2 * dt); + + self.dev_accel_rot = M3x3::new_rotate_x(calculate_accel_axis_angle( + m.accel().0, + m.accel().1, + m.accel().2, + )); + self.dev_accel_rot *= M3x3::new_rotate_y(calculate_accel_axis_angle( + m.accel().1, + m.accel().0, + m.accel().2, + )); + }); } fn render(&self, r: &mut R) { @@ -81,15 +109,26 @@ impl window::App for App { .translate(V3(0.0, 0.0, 1.0)); scene.draw_model(cube); - let mut probe = Model::new(); let probe_obj = self.assets.get("assets/probe.obj").unwrap(); - probe - .add_obj(&probe_obj, Color::RGB(165, 125, 165)) - .rotate_by_m3x3(M3x3::new_rotate_x(PI)) - .rotate_by_m3x3(M3x3::from_v3_rot(self.rot)) - .scale(V3(0.2, 0.2, 0.2)) - .translate(V3(1.2, 0.0, 1.0)); - scene.draw_model(probe); + + let mut probe_rot = Model::new(); + probe_rot + .add_obj(&probe_obj, Color::RGB(100, 160, 100)) + .rotate_by_m3x3(M3x3::new_rotate_x(PI * 1.5)) + .rotate_by_m3x3(M3x3::new_rotate_y(PI)) + .rotate_by_m3x3(self.dev_rot_rot) + .scale(V3(0.15, 0.15, 0.15)) + .translate(V3(1.5, -0.8, 1.0)); + scene.draw_model(probe_rot); + let mut probe_accel = Model::new(); + probe_accel + .add_obj(&probe_obj, Color::RGB(160, 100, 100)) + .rotate_by_m3x3(M3x3::new_rotate_x(PI * 1.5)) + .rotate_by_m3x3(M3x3::new_rotate_y(PI)) + .rotate_by_m3x3(self.dev_accel_rot) + .scale(V3(0.15, 0.15, 0.15)) + .translate(V3(1.0, -0.8, 1.0)); + scene.draw_model(probe_accel); scene.render( r, @@ -97,13 +136,30 @@ impl window::App for App { M3x3::from_v3_rot(V3(0.0, 0.0, 0.0)), V3(0.0, 0.0, 0.0), ); + + let mut draw_v3 = |name: &str, v: V3, pos: V2| { + r.draw_text( + format!("{}: [{: >7.2}, {: >7.2}, {: >7.2}]", name, v.0, v.1, v.2).as_str(), + pos + V2(0.002, -0.002), + Color::BLACK, + ); + r.draw_text( + format!("{}: [{: >7.2}, {: >7.2}, {: >7.2}]", name, v.0, v.1, v.2).as_str(), + pos, + Color::WHITE, + ); + }; + + let m = self.motion_dev.current(); + draw_v3("acceleration", m.accel(), V2(-0.99, -0.5)); + draw_v3(" rotation", m.rotation(), V2(-0.99, -0.53)); } fn event(&mut self, event: Event) {} } #[derive(Debug, Deserialize)] -struct Measurement { +pub struct Measurement { time_delta_us: i64, accel_x: f64, accel_y: f64, @@ -113,17 +169,68 @@ struct Measurement { rotation_z: f64, } +impl Measurement { + pub fn accel(&self) -> V3 { + V3(self.accel_x, self.accel_y, self.accel_z) + } + + pub fn rotation(&self) -> V3 { + V3(self.rotation_x, self.rotation_y, self.rotation_z) + } +} + +pub struct MotionDevice { + measurements: Vec, + idx: usize, + timer_us: i64, +} + +impl MotionDevice { + pub fn new(measurements: Vec) -> Self { + Self { + measurements, + idx: 0, + timer_us: 0, + } + } + + pub fn update ()>(&mut self, delta_time_us: i64, mut func: F) { + self.timer_us += delta_time_us; + while self.timer_us >= self.measurements[self.idx].time_delta_us { + func( + &self.measurements[self.idx], + self.measurements[self.idx].time_delta_us as f64 / 1_000_000.0, + ); + self.timer_us -= self.measurements[self.idx].time_delta_us; + self.step(); + } + } + + fn step(&mut self) { + self.idx += 1; + if self.idx >= self.measurements.len() { + self.idx = 0; + } + } + + pub fn current(&self) -> &Measurement { + &self.measurements[self.idx] + } +} + fn main() { + let mut reader = csv::Reader::from_path("assets/inputs_moving.csv").unwrap(); + let mut measurements = Vec::::new(); + for entry in reader.deserialize::() { + let entry: Measurement = entry.unwrap(); + measurements.push(entry); + } + let motion_dev = MotionDevice::new(measurements); + let mut window = Window::new(); - let mut app = App::new(); + let mut app = App::new(motion_dev); app.assets.load("assets/cube.obj"); app.assets.load("assets/probe.obj"); - let mut reader = csv::Reader::from_path("assets/inputs_still.csv").unwrap(); - for entry in reader.deserialize::() { - let entry: Measurement = entry.unwrap(); - // println!("{:?}", entry); - } - window.run(&mut app); } diff --git a/simul/src/window.rs b/simul/src/window.rs index c7d47cc..b4c17e1 100644 --- a/simul/src/window.rs +++ b/simul/src/window.rs @@ -3,7 +3,8 @@ use std::time::{Duration, Instant}; use sdl3::{ Sdl, event::Event as SdlEvent, - render::{Canvas, FPoint, FRect, Vertex, VertexIndices}, + render::{Canvas, FPoint, FRect, TextureQuery, Vertex, VertexIndices}, + ttf::Font, video::Window as SdlWindow, }; @@ -11,8 +12,8 @@ pub use sdl3::{keyboard::Keycode, pixels::Color}; use crate::{tri2::Tri2, v2::V2}; -pub static WIDTH: f64 = 1280.0; -pub static HEIGHT: f64 = 720.0; +pub static WIDTH: f64 = 1920.0; +pub static HEIGHT: f64 = 1080.0; pub trait App { fn update(&mut self, delta_time: Duration); @@ -24,6 +25,7 @@ pub trait Renderer { fn draw_rect(&mut self, pos: V2, size: V2, color: Color); fn draw_line(&mut self, from: V2, to: V2, color: Color); fn draw_triangles(&mut self, tris: &[Tri2], color: Color); + fn draw_text(&mut self, text: &str, pos: V2, color: Color); } pub enum Event { @@ -32,14 +34,17 @@ pub enum Event { } pub struct Window { - sdl_context: Sdl, + sdl_cx: Sdl, canvas: Canvas, + font: Box>, } impl Window { pub fn new() -> Self { - let sdl_context = sdl3::init().unwrap(); - let video_subsystem = sdl_context.video().unwrap(); + let sdl_cx = sdl3::init().unwrap(); + let ttf_cx = sdl3::ttf::init().unwrap(); + + let video_subsystem = sdl_cx.video().unwrap(); let window = video_subsystem .window("Game", WIDTH as u32, HEIGHT as u32) .position_centered() @@ -47,18 +52,25 @@ impl Window { .unwrap(); let mut canvas = window.into_canvas(); + let font = Box::new( + ttf_cx + .load_font("/usr/share/fonts/TTF/FiraCode-Medium.ttf", 20.0) + .unwrap(), + ); + canvas.set_draw_color(Color::BLACK); canvas.clear(); canvas.present(); Self { - sdl_context, + sdl_cx, canvas, + font, } } pub fn run(&mut self, app: &mut impl App) { - let mut event_pump = self.sdl_context.event_pump().unwrap(); + let mut event_pump = self.sdl_cx.event_pump().unwrap(); let mut time_before = Instant::now(); let time_per_frame = Duration::from_secs_f64(1.0 / 60.0); @@ -151,6 +163,30 @@ impl Renderer for Window { .render_geometry(&vertices, None, VertexIndices::Sequential) .unwrap(); } + + fn draw_text(&mut self, text: &str, pos: V2, color: Color) { + let texture_creator = self.canvas.texture_creator(); + + let surface = self.font.render(text).blended(color).unwrap(); + let texture = texture_creator + .create_texture_from_surface(&surface) + .unwrap(); + + let TextureQuery { width, height, .. } = texture.query(); + + let pos = self.point_w2s(pos); + + self.canvas.copy( + &texture, + None, + FRect { + x: pos.0 as _, + y: pos.1 as _, + w: width as _, + h: height as _, + }, + ); + } } impl From for FPoint {