marching rigid bodies

7 files changed, 160 insertions, 99 deletions

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)
     }
 }