play probe data

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<V3> for M3x3 {
         )
     }
 }
+impl MulAssign for M3x3 {
+    fn mul_assign(&mut self, rhs: Self) {
+        *self = *self * rhs;
+    }
+}

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<R: Renderer> window::App<R> 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<R: Renderer> window::App<R> 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))
+        let mut probe_rot = Model::new();
-            .rotate_by_m3x3(M3x3::new_rotate_x(PI))
+        probe_rot
-            .rotate_by_m3x3(M3x3::from_v3_rot(self.rot))
+            .add_obj(&probe_obj, Color::RGB(100, 160, 100))
-            .scale(V3(0.2, 0.2, 0.2))
+            .rotate_by_m3x3(M3x3::new_rotate_x(PI * 1.5))
-            .translate(V3(1.2, 0.0, 1.0));
+            .rotate_by_m3x3(M3x3::new_rotate_y(PI))
-        scene.draw_model(probe);
+            .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<R: Renderer> window::App<R> 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<Measurement>,
+    idx: usize,
+    timer_us: i64,
+}
+
+impl MotionDevice {
+    pub fn new(measurements: Vec<Measurement>) -> Self {
+        Self {
+            measurements,
+            idx: 0,
+            timer_us: 0,
+        }
+    }
+
+    pub fn update<F: FnMut(&Measurement, f64) -> ()>(&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::<Measurement>::new();
+    for entry in reader.deserialize::<Measurement>() {
+        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::<Measurement>() {
-        let entry: Measurement = entry.unwrap();
-        // println!("{:?}", entry);
-    }
-
     window.run(&mut app);
 }

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 WIDTH: f64 = 1920.0;
-pub static HEIGHT: f64 = 720.0;
+pub static HEIGHT: f64 = 1080.0;
 
 pub trait App<R: Renderer> {
     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<SdlWindow>,
+    font: Box<Font<'static>>,
 }
 
 impl Window {
     pub fn new() -> Self {
-        let sdl_context = sdl3::init().unwrap();
+        let sdl_cx = sdl3::init().unwrap();
-        let video_subsystem = sdl_context.video().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<Self>) {
-        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<V2> for FPoint {