Take round shapes into account in 2D debug render

3 files changed, 35 insertions, 14 deletions

diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index 748530f..06dff5d 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -299,10 +299,20 @@ impl MultibodyJointSet {
     }
 
     /// Gets a mutable reference to the multibody identified by its `handle`.
+    pub fn get_mut(&mut self, handle: MultibodyJointHandle) -> Option<(&mut Multibody, usize)> {
+        let link = self.rb2mb.get(handle.0)?;
+        let multibody = self.multibodies.get_mut(link.multibody.0)?;
+        Some((multibody, link.id))
+    }
+
+    /// Gets a mutable reference to the multibody identified by its `handle`.
+    ///
+    /// This method will bypass any modification-detection automatically done by the MultibodyJointSet.
     pub fn get_mut_internal(
         &mut self,
         handle: MultibodyJointHandle,
     ) -> Option<(&mut Multibody, usize)> {
+        // TODO: modification tracking?
         let link = self.rb2mb.get(handle.0)?;
         let multibody = self.multibodies.get_mut(link.multibody.0)?;
         Some((multibody, link.id))
diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
index 36593ce..1c929c2 100644
--- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
+++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
@@ -285,17 +285,20 @@ impl DebugRenderPipeline {
              * Round shapes.
              */
             TypedShape::RoundCuboid(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                let vtx = s.to_polyline(self.style.border_subdivisions);
+                backend.draw_line_strip(object, &vtx, pos, &Vector::repeat(1.0), color, true)
             }
             TypedShape::RoundTriangle(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                // TODO: take roundness into account.
+                self.render_shape(object, backend, &s.inner_shape, pos, color)
             }
-            // TypedShape::RoundTriMesh(s) => self.render_shape(backend, &s.base_shape, pos, color),
+            // TypedShape::RoundTriMesh(s) => self.render_shape(backend, &s.inner_shape, pos, color),
             // TypedShape::RoundHeightField(s) => {
-            //     self.render_shape(backend, &s.base_shape, pos, color)
+            //     self.render_shape(backend, &s.inner_shape, pos, color)
             // }
             TypedShape::RoundConvexPolygon(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                let vtx = s.to_polyline(self.style.border_subdivisions);
+                backend.draw_line_strip(object, &vtx, pos, &Vector::repeat(1.0), color, true)
             }
             TypedShape::Custom(_) => {}
         }
@@ -326,7 +329,6 @@ impl DebugRenderPipeline {
                 let (vtx, idx) = &self.instances[&TypeId::of::<Cuboid>()];
                 backend.draw_polyline(object, vtx, idx, pos, &(s.half_extents * 2.0), color)
             }
-            #[cfg(feature = "dim3")]
             TypedShape::Capsule(s) => {
                 let (vtx, idx) = s.to_outline(self.style.subdivisions);
                 backend.draw_polyline(object, &vtx, &idx, pos, &Vector::repeat(1.0), color)
@@ -426,23 +428,30 @@ impl DebugRenderPipeline {
              * Round shapes.
              */
             TypedShape::RoundCuboid(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                let (vtx, idx) = s.to_outline(self.style.border_subdivisions);
+                backend.draw_polyline(object, &vtx, &idx, pos, &Vector::repeat(1.0), color)
             }
             TypedShape::RoundTriangle(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                // TODO: take roundness into account.
+                self.render_shape(object, backend, &s.inner_shape, pos, color)
             }
-            // TypedShape::RoundTriMesh(s) => self.render_shape(object, backend, &s.base_shape, pos, color),
+            // TypedShape::RoundTriMesh(s) => self.render_shape(object, backend, &s.inner_shape, pos, color),
             // TypedShape::RoundHeightField(s) => {
-            //     self.render_shape(object, backend, &s.base_shape, pos, color)
+            //     self.render_shape(object, backend, &s.inner_shape, pos, color)
             // }
             TypedShape::RoundCylinder(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                let (vtx, idx) =
+                    s.to_outline(self.style.subdivisions, self.style.border_subdivisions);
+                backend.draw_polyline(object, &vtx, &idx, pos, &Vector::repeat(1.0), color)
             }
             TypedShape::RoundCone(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                let (vtx, idx) =
+                    s.to_outline(self.style.subdivisions, self.style.border_subdivisions);
+                backend.draw_polyline(object, &vtx, &idx, pos, &Vector::repeat(1.0), color)
             }
             TypedShape::RoundConvexPolyhedron(s) => {
-                self.render_shape(object, backend, &s.base_shape, pos, color)
+                let (vtx, idx) = s.to_outline(self.style.border_subdivisions);
+                backend.draw_polyline(object, &vtx, &idx, pos, &Vector::repeat(1.0), color)
             }
             TypedShape::Custom(_) => {}
         }
diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs
index 9d4b6cc..5401596 100644
--- a/src/pipeline/debug_render_pipeline/debug_render_style.rs
+++ b/src/pipeline/debug_render_pipeline/debug_render_style.rs
@@ -3,9 +3,10 @@
 /// The default colors are provided in HSLA (Hue Saturation Lightness Alpha) format.
 pub type DebugColor = [f32; 4];
 
-#[derive(Clone, Debug, PartialEq)]
+#[derive(Copy, Clone, Debug, PartialEq)]
 pub struct DebugRenderStyle {
     pub subdivisions: u32,
+    pub border_subdivisions: u32,
     pub collider_dynamic_color: DebugColor,
     pub collider_fixed_color: DebugColor,
     pub collider_kinematic_color: DebugColor,
@@ -22,6 +23,7 @@ impl Default for DebugRenderStyle {
     fn default() -> Self {
         Self {
             subdivisions: 20,
+            border_subdivisions: 5,
             collider_dynamic_color: [340.0, 1.0, 0.3, 1.0],
             collider_kinematic_color: [20.0, 1.0, 0.3, 1.0],
             collider_fixed_color: [30.0, 1.0, 0.4, 1.0],