marching rigid bodies

9 files changed, 183 insertions, 120 deletions

diff --git a/src/main.rs b/src/main.rs
index 586b456..bc616ca 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,8 +4,6 @@ mod renderer;
 mod sim;
 
 use futures::executor;
-use fxhash::FxHashMap;
-use glam::Vec2;
 use rand::random_range;
 use std::{collections::VecDeque, sync::Arc, time::Instant};
 use winit::{
@@ -21,12 +19,12 @@ use winit::{
 
 use crate::{
     camera::Camera,
-    config::{CHUNK_SIZE, PHYSICS_DELTA_TIME, PHYSICS_FPS, SIM_FPS, WINDOW_TITLE},
+    config::{PHYSICS_DELTA_TIME, PHYSICS_FPS, SIM_FPS, WINDOW_TITLE},
     renderer::RendererState,
     sim::{
         cell::{cell::Cell, materials::MaterialId},
         cell_sim::{sim::sim_tick, world::World},
-        rb_sim::{DebugRenderMode, RbSimManager},
+        rb_sim::{DebugRenderMode, RbSimManager, debug_ops::DebugOperator},
         write_rb_entity_to_world,
     },
 };
@@ -115,11 +113,11 @@ impl App {
         }
 
         if self.input.trigger_test_2
-            && let Some((cx, cy)) = self.input.last_mouse_chunk_pos
-            && let Some(world) = &self.world
+            && let Some(lm) = self.input.last_mouse_world_pos
             && let Some(rbsm) = &mut self.rb_sim_manager
         {
             self.input.trigger_test_2 = false;
+            rbsm.test_spawn_ball(lm.0, lm.1, self.config.dropper_material);
         }
 
         // --TEST DRAWING--
@@ -182,10 +180,15 @@ impl App {
                 for (lx, ly, x, y) in cells_written {
                     // update the entity
                     // TODO we should skip cells that weren't changed?
+                    // TODO optimize
                     let new_local_cell = world.get_cell_from_game_position(x, y).unwrap();
-                    // if new_local_cell.material != MaterialId::Void && !new_local_cell.rb() {
-                    //     panic!("Someone swapped into this rb's cell!");
-                    // }
+                    if new_local_cell.material != MaterialId::Void && !new_local_cell.rb() {
+                        panic!(
+                            "Someone swapped into this rb's cell! ({x}, {y}, {m:#?}, {f})",
+                            m = new_local_cell.material,
+                            f = new_local_cell.flags
+                        );
+                    }
                     rb_entity.set_cell_at_local_position(lx, ly, new_local_cell);
                     // update the world
                     world.set_cell_from_game_position(x, y, Cell::void(), false);
@@ -261,7 +264,7 @@ impl Default for App {
                 use_threading: true,
                 brush_radius: 10.0,
                 brush_material: MaterialId::Sand,
-                dropper_material: MaterialId::Sand,
+                dropper_material: MaterialId::Wood,
                 debug_render: true,
                 debug_render_mode: DebugRenderMode::default(),
             },
@@ -283,9 +286,6 @@ impl ApplicationHandler for App {
 
         self.window = Some(window.clone());
         self.renderer_state = Some(executor::block_on(RendererState::new(window.clone())));
-        if let Some(rbsm) = &mut self.rb_sim_manager {
-            rbsm.test();
-        }
 
         let size = window.inner_size();
         self.camera = Some(Camera::new((size.width as i32, size.height as i32)));
diff --git a/src/renderer/mod.rs b/src/renderer/mod.rs
index 7dd1802..2676a9c 100644
--- a/src/renderer/mod.rs
+++ b/src/renderer/mod.rs
@@ -13,7 +13,7 @@ use crate::{
     sim::{
         cell::materials::MaterialId,
         cell_sim::world::World,
-        rb_sim::{RbSimManager, debug_render::DebugVertex},
+        rb_sim::{RbSimManager, debug_render::DebugVertex, rb_entity::RbEntity},
     },
 };
 
@@ -557,8 +557,8 @@ impl RendererState {
             self.renderer_rb_entities.drain();
             for entity in rb_sim_manager.rb_entities.values() {
                 // TODO add "needs texture update"?
-                for (byte, cell) in cell_buffer.iter_mut().zip(entity.cells.iter()) {
-                    *byte = cell.material as u8;
+                for i in 0..entity.cells.len() {
+                    cell_buffer[i] = entity.cells[i].material as u8;
                 }
 
                 let next_slot = CHUNK_SLOTS + self.renderer_rb_entities.len();
@@ -569,7 +569,7 @@ impl RendererState {
 
                 self.queue.write_buffer(
                     &self.cell_buffer,
-                    (slot * CELLS_IN_CHUNK) as u64,
+                    (slot * CHUNK_SIZE as usize * CHUNK_SIZE as usize) as u64,
                     &cell_buffer,
                 );
             }
@@ -603,13 +603,15 @@ impl RendererState {
             }
 
             for (id, slot) in &self.renderer_rb_entities {
-                if let Some((x, y, cos, sin)) = rb_sim_manager.get_rb_entity_transform(*id) {
+                if let Some(&RbEntity { width, height, .. }) = rb_sim_manager.rb_entities.get(id)
+                    && let Some((x, y, cos, sin)) = rb_sim_manager.get_rb_entity_transform(*id)
+                {
                     instances.push(Instance {
                         centre: [x, y],
                         cos_sin: [cos, sin],
-                        half_size,
-                        dims,
-                        cell_offset: (slot * CELLS_IN_CHUNK) as u32,
+                        half_size: [width as f32 / 2.0, height as f32 / 2.0],
+                        dims: [width as u32, height as u32],
+                        cell_offset: (slot * CHUNK_SIZE as usize * CHUNK_SIZE as usize) as u32,
                         _padding: 0,
                     });
                 }
diff --git a/src/sim/cell/cell.rs b/src/sim/cell/cell.rs
index c4b0a8f..d567893 100644
--- a/src/sim/cell/cell.rs
+++ b/src/sim/cell/cell.rs
@@ -31,7 +31,7 @@ impl Cell {
 
     #[inline]
     pub fn rb(self) -> bool {
-        self.flags & Self::FLAG_RB == 1
+        (self.flags & Self::FLAG_RB) == Self::FLAG_RB
     }
     pub fn set_rb(&mut self, rb: bool) {
         if rb {
diff --git a/src/sim/cell_sim/chunk.rs b/src/sim/cell_sim/chunk.rs
index 8c3acb4..c7d619b 100644
--- a/src/sim/cell_sim/chunk.rs
+++ b/src/sim/cell_sim/chunk.rs
@@ -39,4 +39,7 @@ impl Marchable for Chunk {
             self.get_cell_at_local_position(x as u8, y as u8).material != MaterialId::Void
         }
     }
+    fn size(&self) -> (i32, i32) {
+        (CHUNK_SIZE, CHUNK_SIZE)
+    }
 }
diff --git a/src/sim/lib/marching_squares.rs b/src/sim/lib/marching_squares.rs
index 41d50ee..9df3dfa 100644
--- a/src/sim/lib/marching_squares.rs
+++ b/src/sim/lib/marching_squares.rs
@@ -43,6 +43,7 @@ fn derive_type(tl: bool, tr: bool, br: bool, bl: bool) -> usize {
 
 pub trait Marchable {
     fn occupied(&self, x: i32, y: i32) -> bool;
+    fn size(&self) -> (i32, i32);
 }
 
 pub fn compute_types(marchable: &impl Marchable, w: i32, h: i32) -> Vec<u8> {
diff --git a/src/sim/mod.rs b/src/sim/mod.rs
index 56a8276..c73a373 100644
--- a/src/sim/mod.rs
+++ b/src/sim/mod.rs
@@ -20,39 +20,47 @@ pub fn write_rb_entity_to_world(
     if let Some(rb_entity) = rb_sim_manager.rb_entities.get(&rb_entity_id)
         && let Some((rb_x, rb_y, cos, sin)) = rb_sim_manager.get_rb_entity_transform(rb_entity_id)
     {
-        let half_size = CHUNK_SIZE as f32 / 2.0;
+        let (half_size_x, half_size_y) =
+            (rb_entity.width as f32 / 2.0, rb_entity.height as f32 / 2.0);
         // half-extent of the rotated grid's axis-aligned bounding box, plus a cell of margin
-        let radius = half_size * (cos.abs() + sin.abs()) + 1.0;
+        let (radius_x, radius_y) = (
+            half_size_x * (cos.abs() + sin.abs()) + 1.0,
+            half_size_y * (cos.abs() + sin.abs()) + 1.0,
+        );
 
-        let world_xl = (rb_x - radius).floor() as i32;
-        let world_xu = (rb_x + radius).ceil() as i32;
-        let world_yl = (rb_y - radius).floor() as i32;
-        let world_yu = (rb_y + radius).ceil() as i32;
+        let world_xl = (rb_x - radius_x).floor() as i32;
+        let world_xu = (rb_x + radius_x).ceil() as i32;
+        let world_yl = (rb_y - radius_y).floor() as i32;
+        let world_yu = (rb_y + radius_y).ceil() as i32;
 
         for world_x in world_xl..=world_xu {
             for world_y in world_yl..=world_yu {
-                // same as shader
-                let d = (world_x as f32 + 0.5 - rb_x, world_y as f32 + 0.5 - rb_y);
-                let q = (d.0.floor() + 0.5, d.1.floor() + 0.5);
-                let (lx, ly) = (
-                    (q.0 * cos + q.1 * sin + half_size).floor() as i32,
-                    (-q.0 * sin + q.1 * cos + half_size).floor() as i32,
-                );
+                if let Some(cur_world_cell) = world.get_cell_from_game_position(world_x, world_y)
+                    && cur_world_cell.material == MaterialId::Void
+                {
+                    // same as shader
+                    let d = (world_x as f32 + 0.5 - rb_x, world_y as f32 + 0.5 - rb_y);
+                    let q = (d.0.floor() + 0.5, d.1.floor() + 0.5);
+                    let (lx, ly) = (
+                        (q.0 * cos + q.1 * sin + half_size_x).floor() as i32,
+                        (-q.0 * sin + q.1 * cos + half_size_y).floor() as i32,
+                    );
 
-                if lx < 0 || ly < 0 || lx >= CHUNK_SIZE || ly >= CHUNK_SIZE {
-                    continue;
-                }
+                    if lx < 0 || ly < 0 || lx >= rb_entity.width || ly >= rb_entity.height {
+                        continue;
+                    }
 
-                let mut cell = rb_entity.get_cell_at_local_position(lx as u8, ly as u8);
-                if cell.material == MaterialId::Void {
-                    continue;
-                }
+                    let mut cell = rb_entity.get_cell_at_local_position(lx as u8, ly as u8);
+                    if cell.material == MaterialId::Void {
+                        continue;
+                    }
 
-                cell.match_parity(seqno);
+                    cell.match_parity(seqno);
 
-                // TODO: OPTIMIZE!!
-                world.set_cell_from_game_position(world_x, world_y, cell, false);
-                cells_written.push((lx as u8, ly as u8, world_x, world_y));
+                    // TODO: OPTIMIZE!!
+                    world.set_cell_from_game_position(world_x, world_y, cell, false);
+                    cells_written.push((lx as u8, ly as u8, world_x, world_y));
+                }
             }
         }
     }
diff --git a/src/sim/rb_sim/debug_ops.rs b/src/sim/rb_sim/debug_ops.rs
new file mode 100644
index 0000000..d6852da
--- /dev/null
+++ b/src/sim/rb_sim/debug_ops.rs
@@ -0,0 +1,48 @@
+use glam::{ivec2, vec2};
+
+use crate::sim::{
+    cell::{cell::Cell, materials::MaterialId},
+    rb_sim::RbSimManager,
+};
+
+pub trait DebugOperator {
+    fn test_spawn_box(&mut self, x: f32, y: f32, material: MaterialId) -> ();
+    fn test_spawn_ball(&mut self, x: f32, y: f32, material: MaterialId) -> ();
+}
+
+impl DebugOperator for RbSimManager {
+    fn test_spawn_box(&mut self, x: f32, y: f32, material: MaterialId) {
+        let w = 10;
+        let h = 10;
+        let mut test_cells = vec![Cell::void(); (w * h) as usize];
+
+        for x in 0..w {
+            for y in 0..h {
+                let cell_idx = x + y * w;
+                test_cells[cell_idx as usize] = Cell::from_material(material);
+                test_cells[cell_idx as usize].set_rb(true);
+            }
+        }
+
+        self.create_rb_entity(vec2(x, y), test_cells, w, h);
+    }
+
+    fn test_spawn_ball(&mut self, x: f32, y: f32, material: MaterialId) {
+        let r = 5;
+        let w = r * 2;
+        let h = r * 2;
+        let mut test_cells = vec![Cell::void(); (w * h) as usize];
+
+        for x in 0..w {
+            for y in 0..h {
+                let cell_idx = x + y * w;
+                if ivec2(x, y).distance_squared(ivec2(w / 2, h / 2)) < r.pow(2) {
+                    test_cells[cell_idx as usize] = Cell::from_material(material);
+                    test_cells[cell_idx as usize].set_rb(true);
+                }
+            }
+        }
+
+        self.create_rb_entity(vec2(x, y), test_cells, w, h);
+    }
+}
diff --git a/src/sim/rb_sim/mod.rs b/src/sim/rb_sim/mod.rs
index b79de86..d986962 100644
--- a/src/sim/rb_sim/mod.rs
+++ b/src/sim/rb_sim/mod.rs
@@ -1,21 +1,17 @@
+pub mod debug_ops;
 pub mod debug_render;
 pub mod rb_entity;
 
 use fxhash::FxHashMap;
 use glam::Vec2;
-use rapier2d::{
-    dynamics::{self},
-    geometry::{self, ColliderHandle},
-    glamx::vec2,
-    math, prelude,
-};
+use rapier2d::{dynamics, geometry, glamx::vec2, prelude};
 
 use crate::{
-    config::{CELLS_IN_CHUNK, CHUNK_SIZE, PHYSICS_DELTA_TIME, PIXELS_TO_METRES},
+    config::{CHUNK_SIZE, PHYSICS_DELTA_TIME, PIXELS_TO_METRES},
     sim::{
-        cell::{cell::Cell, materials::MaterialId},
+        cell::cell::Cell,
         cell_sim::chunk::Chunk,
-        lib::marching_squares::marching_squares_vertex_trace,
+        lib::marching_squares::{Marchable, marching_squares_vertex_trace},
         rb_sim::{
             debug_render::{DebugLineBuffer, DebugVertex},
             rb_entity::RbEntity,
@@ -70,7 +66,7 @@ impl PhysicsManager {
 }
 
 pub struct RbSimManager {
-    chunk_colliders: FxHashMap<(i32, i32), ColliderHandle>,
+    chunk_colliders: FxHashMap<(i32, i32), geometry::ColliderHandle>,
     pub rb_entities: FxHashMap<u32, RbEntity>,
 
     physics_manager: PhysicsManager,
@@ -116,20 +112,33 @@ impl RbSimManager {
         &self.debug_line_buffer.vertices
     }
 
-    pub fn create_rb_entity(
-        &mut self,
-        cells: Box<[Cell; CELLS_IN_CHUNK]>,
-        rb_parent: prelude::RigidBodyHandle,
-    ) {
+    pub fn create_rb_entity(&mut self, position: Vec2, cells: Vec<Cell>, w: i32, h: i32) -> u32 {
         let id = self.next_id;
         self.next_id += 1;
+
+        let rb = dynamics::RigidBodyBuilder::dynamic()
+            .translation(position / PIXELS_TO_METRES)
+            .build();
+        let rb_handle = self.physics_manager.rigid_body_set.insert(rb);
+
         let entity: RbEntity = RbEntity {
             id,
             cells,
-            rb_parent,
+            rb_parent: rb_handle,
+            width: w,
+            height: h,
         };
 
+        let collider = self.convex_hull_collider_from_marchable(&entity).build();
+
         self.rb_entities.insert(id, entity);
+        self.physics_manager.collider_set.insert_with_parent(
+            collider,
+            rb_handle,
+            &mut self.physics_manager.rigid_body_set,
+        );
+
+        id
     }
 
     pub fn destroy_rb_entity(&mut self, entity_id: u32) {
@@ -166,8 +175,9 @@ impl RbSimManager {
         }
     }
 
-    fn collider_from_chunk(&self, position: Vec2, chunk: &Chunk) -> geometry::Collider {
-        let polys = marching_squares_vertex_trace(chunk, CHUNK_SIZE, CHUNK_SIZE);
+    fn polyline_from_marchable(&self, marchable: &impl Marchable) -> (Vec<Vec2>, Vec<[u32; 2]>) {
+        let (w, h) = marchable.size();
+        let polys = marching_squares_vertex_trace(marchable, w, h);
         let mut vertices = Vec::new();
         let mut indices = Vec::new();
 
@@ -178,20 +188,40 @@ impl RbSimManager {
                 continue;
             }
             for i in 0..p {
-                vertices.push(poly[i as usize] / PIXELS_TO_METRES);
+                vertices.push(
+                    (poly[i as usize] - vec2((w as f32 - 1.0) / 2.0, (h as f32 - 1.0) / 2.0))
+                        / PIXELS_TO_METRES,
+                );
                 indices.push([v + i, v + (i + 1) % p]);
             }
         }
 
+        (vertices, indices)
+    }
+
+    fn polyline_collider_from_marchable(
+        &self,
+        marchable: &impl Marchable,
+    ) -> geometry::ColliderBuilder {
+        let (vertices, indices) = self.polyline_from_marchable(marchable);
+
         geometry::ColliderBuilder::polyline(vertices, Some(indices))
-            .translation(position / PIXELS_TO_METRES)
-            .build()
+    }
+
+    fn convex_hull_collider_from_marchable(
+        &self,
+        marchable: &impl Marchable,
+    ) -> geometry::ColliderBuilder {
+        let (vertices, indices) = self.polyline_from_marchable(marchable);
+        geometry::ColliderBuilder::convex_decomposition(&vertices, &indices)
     }
 
     pub fn upsert_chunk_collider(&mut self, cx: i32, cy: i32, chunk: &Chunk) {
-        let collider = self.collider_from_chunk(
-            Vec2::new((cx * CHUNK_SIZE) as f32, (cy * CHUNK_SIZE) as f32),
-            chunk,
+        let collider = self.polyline_collider_from_marchable(chunk).translation(
+            vec2(
+                (cx as f32 + 0.5) * CHUNK_SIZE as f32,
+                (cy as f32 + 0.5) * CHUNK_SIZE as f32,
+            ) / PIXELS_TO_METRES,
         );
 
         if let Some(handle) = self.chunk_colliders.remove(&(cx, cy)) {
@@ -207,49 +237,6 @@ impl RbSimManager {
         self.chunk_colliders.insert((cx, cy), handle);
     }
 
-    pub fn test(&mut self) {
-        /* Create the ground. */
-        let collider =
-            geometry::ColliderBuilder::cuboid(100.0 / PIXELS_TO_METRES, 8.0 / PIXELS_TO_METRES)
-                .position(math::Pose2::from_translation(vec2(
-                    0 as f32,
-                    CHUNK_SIZE as f32 / PIXELS_TO_METRES,
-                )))
-                .build();
-        self.physics_manager.collider_set.insert(collider);
-    }
-
-    pub fn test_spawn_box(&mut self, x: f32, y: f32, material: MaterialId) {
-        /* Create the bouncing ball. */
-        let rigid_body = dynamics::RigidBodyBuilder::dynamic()
-            .translation(math::Vector::new(
-                x / PIXELS_TO_METRES,
-                y / PIXELS_TO_METRES,
-            ))
-            .build();
-        let collider =
-            geometry::ColliderBuilder::cuboid(5.0 / PIXELS_TO_METRES, 5.0 / PIXELS_TO_METRES)
-                .build();
-        let ball_body_handle = self.physics_manager.rigid_body_set.insert(rigid_body);
-        self.physics_manager.collider_set.insert_with_parent(
-            collider,
-            ball_body_handle,
-            &mut self.physics_manager.rigid_body_set,
-        );
-
-        let mut test_cells = Box::new([Cell::void(); CELLS_IN_CHUNK]);
-
-        for x in CHUNK_SIZE / 2 - 5..CHUNK_SIZE / 2 + 5 {
-            for y in CHUNK_SIZE / 2 - 5..CHUNK_SIZE / 2 + 5 {
-                let cell_idx = x + y * CHUNK_SIZE;
-                test_cells[cell_idx as usize] = Cell::from_material(material);
-                test_cells[cell_idx as usize].set_rb(true);
-            }
-        }
-
-        self.create_rb_entity(test_cells, ball_body_handle);
-    }
-
     pub fn new() -> Self {
         RbSimManager {
             rb_entities: FxHashMap::default(),
diff --git a/src/sim/rb_sim/rb_entity.rs b/src/sim/rb_sim/rb_entity.rs
index 50243bc..08813f6 100644
--- a/src/sim/rb_sim/rb_entity.rs
+++ b/src/sim/rb_sim/rb_entity.rs
@@ -1,24 +1,38 @@
 use rapier2d::prelude;
 
-use crate::{
-    config::{CELLS_IN_CHUNK, CHUNK_SIZE},
-    sim::cell::cell::Cell,
+use crate::sim::{
+    cell::{cell::Cell, materials::MaterialId},
+    lib::marching_squares::Marchable,
 };
 
 pub struct RbEntity {
     pub id: u32,
-    // TODO resizable
-    pub cells: Box<[Cell; CELLS_IN_CHUNK]>,
+    pub width: i32,
+    pub height: i32,
+    pub cells: Vec<Cell>,
     pub rb_parent: prelude::RigidBodyHandle,
 }
 
 impl RbEntity {
     #[inline]
     pub fn get_cell_at_local_position(&self, x: u8, y: u8) -> Cell {
-        self.cells[x as usize + y as usize * CHUNK_SIZE as usize]
+        self.cells[x as usize + y as usize * self.width as usize]
     }
     #[inline]
     pub fn set_cell_at_local_position(&mut self, x: u8, y: u8, cell: Cell) {
-        self.cells[x as usize + y as usize * CHUNK_SIZE as usize] = cell;
+        self.cells[x as usize + y as usize * self.width as usize] = cell;
+    }
+}
+
+impl Marchable for RbEntity {
+    fn occupied(&self, x: i32, y: i32) -> bool {
+        if x < 0 || x >= self.width || y < 0 || y >= self.height {
+            false
+        } else {
+            self.get_cell_at_local_position(x as u8, y as u8).material != MaterialId::Void
+        }
+    }
+    fn size(&self) -> (i32, i32) {
+        (self.width, self.height)
     }
 }