1 files changed, 71 insertions, 0 deletions
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)
+    }
+}