debug physics

2 files changed, 107 insertions, 1 deletions

diff --git a/src/sim/rb_sim/debug_render.rs b/src/sim/rb_sim/debug_render.rs
new file mode 100644
index 0000000..342fc5b
--- /dev/null
+++ b/src/sim/rb_sim/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_sim/mod.rs b/src/sim/rb_sim/mod.rs
index 8101afd..48e4282 100644
--- a/src/sim/rb_sim/mod.rs
+++ b/src/sim/rb_sim/mod.rs
@@ -1,3 +1,4 @@
+pub mod debug_render;
 pub mod rb_entity;
 
 use fxhash::FxHashMap;
@@ -12,10 +13,15 @@ use crate::{
     config::{CELLS_IN_CHUNK, CHUNK_SIZE, PHYSICS_DELTA_TIME, PIXELS_TO_METRES},
     sim::{
         cell::{cell::Cell, materials::MaterialId},
-        rb_sim::rb_entity::RbEntity,
+        rb_sim::{
+            debug_render::{DebugLineBuffer, DebugVertex},
+            rb_entity::RbEntity,
+        },
     },
 };
 
+pub use rapier2d::pipeline::DebugRenderMode;
+
 pub struct PhysicsManager {
     rigid_body_set: prelude::RigidBodySet,
     collider_set: prelude::ColliderSet,
@@ -27,6 +33,7 @@ pub struct PhysicsManager {
     impulse_joint_set: prelude::ImpulseJointSet,
     multibody_joint_set: prelude::MultibodyJointSet,
     ccd_solver: prelude::CCDSolver,
+    debug_render_pipeline: prelude::DebugRenderPipeline,
 }
 
 impl PhysicsManager {
@@ -47,6 +54,14 @@ impl PhysicsManager {
             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(),
+            ),
         }
     }
 }
@@ -55,6 +70,7 @@ pub struct RbSimManager {
     physics_manager: PhysicsManager,
     pub rb_entities: FxHashMap<u32, RbEntity>,
     next_id: u32,
+    debug_line_buffer: DebugLineBuffer,
 }
 
 impl RbSimManager {
@@ -77,6 +93,24 @@ impl RbSimManager {
         );
     }
 
+    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,
         cells: Box<[Cell; CELLS_IN_CHUNK]>,
@@ -175,6 +209,7 @@ impl RbSimManager {
             physics_manager: PhysicsManager::new(),
             rb_entities: FxHashMap::default(),
             next_id: 0,
+            debug_line_buffer: DebugLineBuffer::default(),
         }
     }
 }