sim manager refactor

4 files changed, 447 insertions, 0 deletions

diff --git a/src/sim/rb_manager/debug_ops.rs b/src/sim/rb_manager/debug_ops.rs
new file mode 100644
index 0000000..cb56256
--- /dev/null
+++ b/src/sim/rb_manager/debug_ops.rs
@@ -0,0 +1,48 @@
+use glam::{ivec2, vec2};
+
+use crate::sim::{
+    cell::{cell::Cell, materials::MaterialId},
+    rb_manager::RbManager,
+};
+
+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 RbManager {
+    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_manager/debug_render.rs b/src/sim/rb_manager/debug_render.rs
new file mode 100644
index 0000000..342fc5b
--- /dev/null
+++ b/src/sim/rb_manager/debug_render.rs
@@ -0,0 +1,71 @@
+use rapier2d::pipeline::{DebugColor, DebugRenderBackend, DebugRenderObject};
+
+use crate::config::PIXELS_TO_METRES;
+
+#[repr(C)]
+#[derive(Copy, Clone, bytemuck::Pod, bytemuck::Zeroable)]
+pub struct DebugVertex {
+    pub position: [f32; 2],
+    pub color: [f32; 4],
+}
+
+#[derive(Default)]
+pub struct DebugLineBuffer {
+    pub vertices: Vec<DebugVertex>,
+}
+
+impl DebugRenderBackend for DebugLineBuffer {
+    fn draw_line(
+        &mut self,
+        _object: DebugRenderObject,
+        a: rapier2d::math::Vector,
+        b: rapier2d::math::Vector,
+        color: DebugColor,
+    ) {
+        let color = hsla_to_linear_rgba(color);
+        self.vertices.push(DebugVertex {
+            position: [a.x * PIXELS_TO_METRES, a.y * PIXELS_TO_METRES],
+            color,
+        });
+        self.vertices.push(DebugVertex {
+            position: [b.x * PIXELS_TO_METRES, b.y * PIXELS_TO_METRES],
+            color,
+        });
+    }
+}
+
+fn hsla_to_linear_rgba(hsla: DebugColor) -> [f32; 4] {
+    let [hue, saturation, lightness, alpha] = hsla;
+
+    let hue = hue.rem_euclid(360.0) / 60.0;
+    let saturation = saturation.clamp(0.0, 1.0);
+    let lightness = lightness.clamp(0.0, 1.0);
+
+    let chroma = (1.0 - (2.0 * lightness - 1.0).abs()) * saturation;
+    let second = chroma * (1.0 - (hue % 2.0 - 1.0).abs());
+    let (r, g, b) = match hue as u32 {
+        0 => (chroma, second, 0.0),
+        1 => (second, chroma, 0.0),
+        2 => (0.0, chroma, second),
+        3 => (0.0, second, chroma),
+        4 => (second, 0.0, chroma),
+        _ => (chroma, 0.0, second),
+    };
+    let m = lightness - chroma / 2.0;
+
+    [
+        srgb_to_linear(r + m),
+        srgb_to_linear(g + m),
+        srgb_to_linear(b + m),
+        alpha.clamp(0.0, 1.0),
+    ]
+}
+
+fn srgb_to_linear(channel: f32) -> f32 {
+    let channel = channel.clamp(0.0, 1.0);
+    if channel <= 0.04045 {
+        channel / 12.92
+    } else {
+        ((channel + 0.055) / 1.055).powf(2.4)
+    }
+}
diff --git a/src/sim/rb_manager/mod.rs b/src/sim/rb_manager/mod.rs
new file mode 100644
index 0000000..2c64f12
--- /dev/null
+++ b/src/sim/rb_manager/mod.rs
@@ -0,0 +1,289 @@
+pub mod debug_ops;
+pub mod debug_render;
+pub mod rb_entity;
+
+use fxhash::FxHashMap;
+use glam::Vec2;
+use rapier2d::{dynamics, geometry, glamx::vec2, prelude};
+
+use crate::{
+    config::{CHUNK_SIZE, PHYSICS_DELTA_TIME, PIXELS_TO_METRES},
+    sim::{
+        cell::cell::Cell,
+        cell_manager::chunk::Chunk,
+        lib::marching_squares::{Marchable, marching_squares_vertex_trace},
+        rb_manager::{
+            debug_render::{DebugLineBuffer, DebugVertex},
+            rb_entity::RbEntity,
+        },
+    },
+};
+
+pub use rapier2d::pipeline::DebugRenderMode;
+
+pub struct PhysicsManager {
+    rigid_body_set: prelude::RigidBodySet,
+    collider_set: prelude::ColliderSet,
+    physics_pipeline: prelude::PhysicsPipeline,
+    integration_parameters: prelude::IntegrationParameters,
+    island_manager: prelude::IslandManager,
+    broad_phase: prelude::DefaultBroadPhase,
+    narrow_phase: prelude::NarrowPhase,
+    impulse_joint_set: prelude::ImpulseJointSet,
+    multibody_joint_set: prelude::MultibodyJointSet,
+    ccd_solver: prelude::CCDSolver,
+    debug_render_pipeline: prelude::DebugRenderPipeline,
+}
+
+impl PhysicsManager {
+    pub fn new() -> Self {
+        PhysicsManager {
+            rigid_body_set: prelude::RigidBodySet::new(),
+            collider_set: prelude::ColliderSet::new(),
+            physics_pipeline: prelude::PhysicsPipeline::new(),
+            integration_parameters: prelude::IntegrationParameters {
+                // 20 pixels <-> 1 meter
+                length_unit: PIXELS_TO_METRES,
+                dt: PHYSICS_DELTA_TIME,
+                ..prelude::IntegrationParameters::default()
+            },
+            island_manager: prelude::IslandManager::new(),
+            broad_phase: prelude::DefaultBroadPhase::new(),
+            narrow_phase: prelude::NarrowPhase::new(),
+            impulse_joint_set: prelude::ImpulseJointSet::new(),
+            multibody_joint_set: prelude::MultibodyJointSet::new(),
+            ccd_solver: prelude::CCDSolver::new(),
+            debug_render_pipeline: prelude::DebugRenderPipeline::new(
+                prelude::DebugRenderStyle {
+                    sleep_color_multiplier: [1.0; 4],
+                    sleep_eligible_color_multiplier: [1.0; 4],
+                    ..prelude::DebugRenderStyle::default()
+                },
+                prelude::DebugRenderMode::default(),
+            ),
+        }
+    }
+}
+
+pub struct RbManager {
+    chunk_colliders: FxHashMap<(i32, i32), geometry::ColliderHandle>,
+    pub rb_entities: FxHashMap<u32, RbEntity>,
+
+    physics_manager: PhysicsManager,
+    next_id: u32,
+    debug_line_buffer: DebugLineBuffer,
+}
+
+impl RbManager {
+    pub fn tick(&mut self, _delta_time: f32) {
+        let gravity = vec2(0.0, 9.81);
+
+        self.physics_manager.physics_pipeline.step(
+            gravity,
+            &self.physics_manager.integration_parameters,
+            &mut self.physics_manager.island_manager,
+            &mut self.physics_manager.broad_phase,
+            &mut self.physics_manager.narrow_phase,
+            &mut self.physics_manager.rigid_body_set,
+            &mut self.physics_manager.collider_set,
+            &mut self.physics_manager.impulse_joint_set,
+            &mut self.physics_manager.multibody_joint_set,
+            &mut self.physics_manager.ccd_solver,
+            &(),
+            &(),
+        );
+    }
+
+    pub fn debug_render(&mut self, mode: DebugRenderMode) -> &[DebugVertex] {
+        puffin::profile_function!();
+
+        let physics = &mut self.physics_manager;
+        self.debug_line_buffer.vertices.clear();
+        physics.debug_render_pipeline.mode = mode;
+        physics.debug_render_pipeline.render(
+            &mut self.debug_line_buffer,
+            &physics.rigid_body_set,
+            &physics.collider_set,
+            &physics.impulse_joint_set,
+            &physics.multibody_joint_set,
+            &physics.narrow_phase,
+        );
+
+        &self.debug_line_buffer.vertices
+    }
+
+    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 mut entity: RbEntity = RbEntity {
+            id,
+            cells,
+            width: w,
+            height: h,
+            rb: rb_handle,
+            collider: None,
+        };
+
+        let collider = self
+            .convex_hull_collider_from_marchable(&entity, None)
+            .build();
+        let collider_handle = self.physics_manager.collider_set.insert_with_parent(
+            collider,
+            rb_handle,
+            &mut self.physics_manager.rigid_body_set,
+        );
+
+        entity.collider = Some(collider_handle);
+
+        self.rb_entities.insert(id, entity);
+
+        id
+    }
+
+    pub fn update_rb_entity(&mut self, entity_id: u32) {
+        let entity = self.rb_entities.get(&entity_id).unwrap();
+        let new_collider = self
+            .convex_hull_collider_from_marchable(entity, None)
+            .build();
+
+        self.physics_manager.collider_set.remove(
+            entity.collider.unwrap(),
+            &mut self.physics_manager.island_manager,
+            &mut self.physics_manager.rigid_body_set,
+            false,
+        );
+
+        let new_collider_handle = self.physics_manager.collider_set.insert_with_parent(
+            new_collider,
+            entity.rb,
+            &mut self.physics_manager.rigid_body_set,
+        );
+
+        let entity = self.rb_entities.get_mut(&entity_id).unwrap();
+        entity.collider = Some(new_collider_handle);
+    }
+
+    pub fn destroy_rb_entity(&mut self, entity_id: u32) {
+        if let Some(entity) = self.rb_entities.get(&entity_id) {
+            self.physics_manager.rigid_body_set.remove(
+                entity.rb,
+                &mut self.physics_manager.island_manager,
+                &mut self.physics_manager.collider_set,
+                &mut self.physics_manager.impulse_joint_set,
+                &mut self.physics_manager.multibody_joint_set,
+                true,
+            );
+            self.rb_entities.remove(&entity_id);
+        }
+    }
+
+    pub fn get_rb_entity_transform(&self, entity_id: u32) -> Option<(f32, f32, f32, f32)> {
+        let entity = self.rb_entities.get(&entity_id);
+        match entity {
+            Some(entity) => {
+                let rb = self.physics_manager.rigid_body_set.get(entity.rb);
+                rb.map(|rb| {
+                    let position = rb.position();
+                    let angle = position.rotation.angle();
+                    (
+                        position.translation.x * PIXELS_TO_METRES,
+                        position.translation.y * PIXELS_TO_METRES,
+                        angle.cos(),
+                        angle.sin(),
+                    )
+                })
+            }
+            None => return None,
+        }
+    }
+
+    fn polyline_from_marchable(
+        &self,
+        marchable: &impl Marchable,
+        minimum_verts: Option<u32>,
+    ) -> (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();
+
+        let minimum_verts = minimum_verts.unwrap_or(3);
+
+        for poly in polys {
+            let v = vertices.len() as u32;
+            let p = poly.len() as u32;
+            if p < minimum_verts {
+                continue;
+            }
+            for i in 0..p {
+                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,
+        minimum_verts: Option<u32>,
+    ) -> geometry::ColliderBuilder {
+        let (vertices, indices) = self.polyline_from_marchable(marchable, minimum_verts);
+
+        geometry::ColliderBuilder::polyline(vertices, Some(indices))
+    }
+
+    fn convex_hull_collider_from_marchable(
+        &self,
+        marchable: &impl Marchable,
+        minimum_verts: Option<u32>,
+    ) -> geometry::ColliderBuilder {
+        let (vertices, indices) = self.polyline_from_marchable(marchable, minimum_verts);
+        geometry::ColliderBuilder::convex_decomposition(&vertices, &indices)
+    }
+
+    pub fn upsert_chunk_collider(&mut self, cx: i32, cy: i32, chunk: &Chunk) {
+        puffin::profile_function!();
+        let collider = self
+            .polyline_collider_from_marchable(chunk, Some(20))
+            .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)) {
+            self.physics_manager.collider_set.remove(
+                handle,
+                &mut self.physics_manager.island_manager,
+                &mut self.physics_manager.rigid_body_set,
+                false,
+            );
+        }
+
+        let handle = self.physics_manager.collider_set.insert(collider);
+        self.chunk_colliders.insert((cx, cy), handle);
+    }
+
+    pub fn new() -> Self {
+        RbManager {
+            rb_entities: FxHashMap::default(),
+            chunk_colliders: FxHashMap::default(),
+
+            physics_manager: PhysicsManager::new(),
+            next_id: 0,
+            debug_line_buffer: DebugLineBuffer::default(),
+        }
+    }
+}
diff --git a/src/sim/rb_manager/rb_entity.rs b/src/sim/rb_manager/rb_entity.rs
new file mode 100644
index 0000000..18a6693
--- /dev/null
+++ b/src/sim/rb_manager/rb_entity.rs
@@ -0,0 +1,39 @@
+use rapier2d::prelude;
+
+use crate::sim::{
+    cell::{cell::Cell, materials::MaterialId},
+    lib::marching_squares::Marchable,
+};
+
+pub struct RbEntity {
+    pub id: u32,
+    pub width: i32,
+    pub height: i32,
+    pub cells: Vec<Cell>,
+    pub rb: prelude::RigidBodyHandle,
+    pub collider: Option<prelude::ColliderHandle>,
+}
+
+impl RbEntity {
+    #[inline]
+    pub fn get_cell_at_local_position(&self, x: u8, y: u8) -> Cell {
+        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 * 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)
+    }
+}