marching rigid bodies

1 files changed, 32 insertions, 24 deletions

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