Add a basic lines-based debug-renderer

crates/rapier2d-f64/Cargo.toml

@@ -28,6 +28,7 @@ simd-is-enabled = [ "vec_map" ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
 serde-serialize = [ "nalgebra/serde-serialize", "parry2d-f64/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "parry2d-f64/enhanced-determinism", "indexmap" ]
+debug-render = [ ]
 
 # Feature used for debugging only.
 debug-disable-legitimate-fe-exceptions = [ ]

crates/rapier2d/Cargo.toml

@@ -28,6 +28,7 @@ simd-is-enabled = [ "vec_map" ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
 serde-serialize = [ "nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "parry2d/enhanced-determinism", "indexmap" ]
+debug-render = [ "oorandom" ]
 
 # Feature used for debugging only.
 debug-disable-legitimate-fe-exceptions = [ ]
@@ -61,6 +62,8 @@ downcast-rs = "1.2"
 num-derive = "0.3"
 bitflags = "1"
 
+oorandom = { version = "11", optional = true }
+
 [dev-dependencies]
 bincode = "1"
 serde = { version = "1", features = [ "derive" ] }

crates/rapier3d-f64/Cargo.toml

@@ -28,6 +28,7 @@ simd-is-enabled = [ "vec_map" ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
 serde-serialize = [ "nalgebra/serde-serialize", "parry3d-f64/serde-serialize", "serde", "bit-vec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "parry3d-f64/enhanced-determinism" ]
+debug-render = []
 
 # Feature used for debugging only.
 debug-disable-legitimate-fe-exceptions = [ ]

crates/rapier3d/Cargo.toml

@@ -28,6 +28,7 @@ simd-is-enabled = [ "vec_map" ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
 serde-serialize = [ "nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "bit-vec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "parry3d/enhanced-determinism" ]
+debug-render = [ ]
 
 # Feature used for debugging only.
 debug-disable-legitimate-fe-exceptions = [ ]

crates/rapier_testbed2d-f64/Cargo.toml

@@ -40,6 +40,7 @@ md5 = "0.7"
 
 bevy_egui = "0.10"
 bevy_ecs = "0.6"
+bevy_prototype_debug_lines = "0.6"
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
@@ -54,4 +55,4 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
 package = "rapier2d-f64"
 path = "../rapier2d-f64"
 version = "0.12.0-alpha.1"
-features = [ "serde-serialize" ]
+features = [ "serde-serialize", "debug-render" ]

crates/rapier_testbed2d/Cargo.toml

@@ -40,6 +40,7 @@ md5 = "0.7"
 
 bevy_egui = "0.10"
 bevy_ecs = "0.6"
+bevy_prototype_debug_lines = "0.6"
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
@@ -54,4 +55,4 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
 package = "rapier2d"
 path = "../rapier2d"
 version = "0.12.0-alpha.1"
-features = [ "serde-serialize" ]
+features = [ "serde-serialize", "debug-render" ]

crates/rapier_testbed3d-f64/Cargo.toml

@@ -38,6 +38,7 @@ serde = { version = "1", features = [ "derive" ] }
 
 bevy_egui = "0.10"
 bevy_ecs = "0.6"
+bevy_prototype_debug_lines = { version = "0.6", features = [ "3d" ] }
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
@@ -52,4 +53,4 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
 package = "rapier3d-f64"
 path = "../rapier3d-f64"
 version = "0.12.0-alpha.1"
-features = [ "serde-serialize" ]
+features = [ "serde-serialize", "debug-render" ]

crates/rapier_testbed3d/Cargo.toml

@@ -42,6 +42,7 @@ serde = { version = "1", features = [ "derive" ] }
 
 bevy_egui = "0.10"
 bevy_ecs = "0.6"
+bevy_prototype_debug_lines = { version = "0.6", features = [ "3d" ] }
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
@@ -56,4 +57,4 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
 package = "rapier3d"
 path = "../rapier3d"
 version = "0.12.0-alpha.1"
-features = [ "serde-serialize" ]
+features = [ "serde-serialize", "debug-render" ]

examples3d/joints3.rs

@@ -578,33 +578,33 @@ fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
     let mut impulse_joints = ImpulseJointSet::new();
     let mut multibody_joints = MultibodyJointSet::new();
 
-    // create_prismatic_joints(
+    create_prismatic_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![20.0, 5.0, 0.0],
+        point![20.0, 5.0, 0.0],
-    //     4,
+        4,
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
-    // create_actuated_prismatic_joints(
+    create_actuated_prismatic_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![25.0, 5.0, 0.0],
+        point![25.0, 5.0, 0.0],
-    //     4,
+        4,
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
-    // create_revolute_joints(
+    create_revolute_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![20.0, 0.0, 0.0],
+        point![20.0, 0.0, 0.0],
-    //     3,
+        3,
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
     create_revolute_joints_with_limits(
         &mut bodies,
         &mut colliders,
@@ -613,57 +613,57 @@ fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
         point![34.0, 0.0, 0.0],
         use_articulations,
     );
-    // create_fixed_joints(
+    create_fixed_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![0.0, 10.0, 0.0],
+        point![0.0, 10.0, 0.0],
-    //     10,
+        10,
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
-    // create_actuated_revolute_joints(
+    create_actuated_revolute_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![20.0, 10.0, 0.0],
+        point![20.0, 10.0, 0.0],
-    //     6,
+        6,
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
-    // create_actuated_spherical_joints(
+    create_actuated_spherical_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![13.0, 10.0, 0.0],
+        point![13.0, 10.0, 0.0],
-    //     3,
+        3,
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
-    // create_spherical_joints(
+    create_spherical_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     15,
+        15,
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
-    // create_spherical_joints_with_limits(
+    create_spherical_joints_with_limits(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![-5.0, 0.0, 0.0],
+        point![-5.0, 0.0, 0.0],
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
-    // create_coupled_joints(
+    create_coupled_joints(
-    //     &mut bodies,
+        &mut bodies,
-    //     &mut colliders,
+        &mut colliders,
-    //     &mut impulse_joints,
+        &mut impulse_joints,
-    //     &mut multibody_joints,
+        &mut multibody_joints,
-    //     point![0.0, 20.0, 0.0],
+        point![0.0, 20.0, 0.0],
-    //     use_articulations,
+        use_articulations,
-    // );
+    );
 
     /*
      * Set up the testbed.

src/pipeline/debug_render_pipeline/debug_render_backend.rs

@@ -0,0 +1,66 @@
+use crate::dynamics::{
+    ImpulseJoint, ImpulseJointHandle, Multibody, MultibodyLink, RigidBody, RigidBodyHandle,
+};
+use crate::geometry::Collider;
+use crate::math::{Isometry, Point, Real, Vector};
+use crate::prelude::{ColliderHandle, MultibodyJointHandle};
+use na::Scale;
+
+#[derive(Copy, Clone)]
+pub enum DebugRenderObject<'a> {
+    RigidBody(RigidBodyHandle, &'a RigidBody),
+    Collider(ColliderHandle, &'a Collider),
+    ImpulseJoint(ImpulseJointHandle, &'a ImpulseJoint),
+    MultibodyJoint(MultibodyJointHandle, &'a Multibody, &'a MultibodyLink),
+    Other,
+}
+
+pub trait DebugRenderBackend {
+    fn draw_line(
+        &mut self,
+        object: DebugRenderObject,
+        a: Point<Real>,
+        b: Point<Real>,
+        color: [f32; 4],
+    );
+
+    fn draw_polyline(
+        &mut self,
+        object: DebugRenderObject,
+        vertices: &[Point<Real>],
+        indices: &[[u32; 2]],
+        transform: &Isometry<Real>,
+        scale: &Vector<Real>,
+        color: [f32; 4],
+    ) {
+        for idx in indices {
+            let a = transform * (Scale::from(*scale) * vertices[idx[0] as usize]);
+            let b = transform * (Scale::from(*scale) * vertices[idx[1] as usize]);
+            self.draw_line(object, a, b, color);
+        }
+    }
+
+    fn draw_line_strip(
+        &mut self,
+        object: DebugRenderObject,
+        vertices: &[Point<Real>],
+        transform: &Isometry<Real>,
+        scale: &Vector<Real>,
+        color: [f32; 4],
+        closed: bool,
+    ) {
+        for vtx in vertices.windows(2) {
+            let a = transform * (Scale::from(*scale) * vtx[0]);
+            let b = transform * (Scale::from(*scale) * vtx[1]);
+            self.draw_line(object, a, b, color);
+        }
+
+        if closed {
+            if vertices.len() > 2 {
+                let a = transform * (Scale::from(*scale) * vertices[0]);
+                let b = transform * (Scale::from(*scale) * vertices.last().unwrap());
+                self.draw_line(object, a, b, color);
+            }
+        }
+    }
+}

src/pipeline/debug_render_pipeline/debug_render_pipeline.rs

@@ -0,0 +1,450 @@
+use super::{outlines, DebugRenderBackend};
+use crate::dynamics::{
+    GenericJoint, ImpulseJointSet, MultibodyJointSet, RigidBodySet, RigidBodyType,
+};
+use crate::geometry::{Ball, ColliderSet, Cuboid, Shape, TypedShape};
+#[cfg(feature = "dim3")]
+use crate::geometry::{Cone, Cylinder};
+use crate::math::{Isometry, Point, Real, Vector, DIM};
+use crate::pipeline::debug_render_pipeline::debug_render_backend::DebugRenderObject;
+use crate::pipeline::debug_render_pipeline::DebugRenderStyle;
+use crate::utils::WBasis;
+use std::any::TypeId;
+use std::collections::HashMap;
+
+bitflags::bitflags! {
+    pub struct DebugRenderMode: u32 {
+        const COLLIDER_SHAPES = 1 << 0;
+        const RIGID_BODY_AXES = 1 << 1;
+        const MULTIBODY_JOINTS = 1 << 2;
+        const IMPULSE_JOINTS = 1 << 3;
+    }
+}
+
+pub struct DebugRenderPipeline {
+    #[cfg(feature = "dim2")]
+    instances: HashMap<TypeId, Vec<Point<Real>>>,
+    #[cfg(feature = "dim3")]
+    instances: HashMap<TypeId, (Vec<Point<Real>>, Vec<[u32; 2]>)>,
+    pub style: DebugRenderStyle,
+    pub mode: DebugRenderMode,
+}
+
+impl Default for DebugRenderPipeline {
+    fn default() -> Self {
+        Self::render_all(DebugRenderStyle::default())
+    }
+}
+
+impl DebugRenderPipeline {
+    pub fn new(style: DebugRenderStyle, mode: DebugRenderMode) -> Self {
+        Self {
+            instances: outlines::instances(style.subdivisions),
+            style,
+            mode,
+        }
+    }
+
+    pub fn render_all(style: DebugRenderStyle) -> Self {
+        Self::new(style, DebugRenderMode::all())
+    }
+
+    pub fn render(
+        &mut self,
+        backend: &mut impl DebugRenderBackend,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
+    ) {
+        self.render_bodies(backend, bodies);
+        self.render_colliders(backend, bodies, colliders);
+        self.render_joints(backend, bodies, impulse_joints, multibody_joints);
+    }
+
+    pub fn render_joints(
+        &mut self,
+        backend: &mut impl DebugRenderBackend,
+        bodies: &RigidBodySet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
+    ) {
+        let mut render_joint = |body1,
+                                body2,
+                                data: &GenericJoint,
+                                mut anchor_color: [f32; 4],
+                                mut separation_color: [f32; 4],
+                                object| {
+            if let (Some(rb1), Some(rb2)) = (bodies.get(body1), bodies.get(body2)) {
+                let coeff = if (rb1.is_fixed() || rb1.is_sleeping())
+                    && (rb2.is_fixed() || rb2.is_sleeping())
+                {
+                    self.style.sleep_color_multiplier
+                } else {
+                    [1.0; 4]
+                };
+
+                let frame1 = rb1.position() * data.local_frame1;
+                let frame2 = rb2.position() * data.local_frame2;
+
+                let a = *rb1.translation();
+                let b = frame1.translation.vector;
+                let c = frame2.translation.vector;
+                let d = *rb2.translation();
+
+                for k in 0..4 {
+                    anchor_color[k] *= coeff[k];
+                    separation_color[k] *= coeff[k];
+                }
+
+                backend.draw_line(object, a.into(), b.into(), anchor_color);
+                backend.draw_line(object, b.into(), c.into(), separation_color);
+                backend.draw_line(object, c.into(), d.into(), anchor_color);
+            }
+        };
+
+        if self.mode.contains(DebugRenderMode::IMPULSE_JOINTS) {
+            for (handle, joint) in impulse_joints.iter() {
+                let anc_color = self.style.impulse_joint_anchor_color;
+                let sep_color = self.style.impulse_joint_separation_color;
+                let object = DebugRenderObject::ImpulseJoint(handle, joint);
+                render_joint(
+                    joint.body1,
+                    joint.body2,
+                    &joint.data,
+                    anc_color,
+                    sep_color,
+                    object,
+                );
+            }
+        }
+
+        if self.mode.contains(DebugRenderMode::MULTIBODY_JOINTS) {
+            for (handle, multibody, link) in multibody_joints.iter() {
+                let anc_color = self.style.multibody_joint_anchor_color;
+                let sep_color = self.style.multibody_joint_separation_color;
+                let parent = multibody.link(link.parent_id().unwrap()).unwrap();
+                let object = DebugRenderObject::MultibodyJoint(handle, multibody, link);
+                render_joint(
+                    parent.rigid_body_handle(),
+                    link.rigid_body_handle(),
+                    &link.joint.data,
+                    anc_color,
+                    sep_color,
+                    object,
+                );
+            }
+        }
+    }
+
+    pub fn render_bodies(&mut self, backend: &mut impl DebugRenderBackend, bodies: &RigidBodySet) {
+        for (handle, rb) in bodies.iter() {
+            let object = DebugRenderObject::RigidBody(handle, rb);
+
+            if self.style.rigid_body_axes_length != 0.0
+                && self.mode.contains(DebugRenderMode::RIGID_BODY_AXES)
+            {
+                let basis = rb.rotation().to_rotation_matrix().into_inner();
+                let coeff = if rb.is_sleeping() {
+                    self.style.sleep_color_multiplier
+                } else {
+                    [1.0; 4]
+                };
+                let colors = [
+                    [0.0 * coeff[0], 1.0 * coeff[1], 0.25 * coeff[2], coeff[3]],
+                    [120.0 * coeff[0], 1.0 * coeff[1], 0.1 * coeff[2], coeff[3]],
+                    [240.0 * coeff[0], 1.0 * coeff[1], 0.2 * coeff[2], coeff[3]],
+                ];
+                let com = rb.mprops.world_com;
+
+                for k in 0..DIM {
+                    let axis = basis.column(k) * self.style.rigid_body_axes_length;
+                    backend.draw_line(object, com, com + axis, colors[k]);
+                }
+            }
+        }
+    }
+
+    pub fn render_colliders(
+        &mut self,
+        backend: &mut impl DebugRenderBackend,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+    ) {
+        if self.mode.contains(DebugRenderMode::COLLIDER_SHAPES) {
+            for (h, co) in colliders.iter() {
+                let object = DebugRenderObject::Collider(h, co);
+                let color = if let Some(parent) = co.parent().and_then(|p| bodies.get(p)) {
+                    let coeff = if parent.is_sleeping() {
+                        self.style.sleep_color_multiplier
+                    } else {
+                        [1.0; 4]
+                    };
+                    let c = match parent.body_type {
+                        RigidBodyType::Fixed => self.style.collider_fixed_color,
+                        RigidBodyType::Dynamic => self.style.collider_dynamic_color,
+                        RigidBodyType::KinematicPositionBased
+                        | RigidBodyType::KinematicVelocityBased => {
+                            self.style.collider_kinematic_color
+                        }
+                    };
+
+                    [
+                        c[0] * coeff[0],
+                        c[1] * coeff[1],
+                        c[2] * coeff[2],
+                        c[3] * coeff[3],
+                    ]
+                } else {
+                    self.style.collider_parentless_color
+                };
+
+                self.render_shape(object, backend, co.shape(), co.position(), color)
+            }
+        }
+    }
+
+    #[cfg(feature = "dim2")]
+    fn render_shape(
+        &mut self,
+        object: DebugRenderObject,
+        backend: &mut impl DebugRenderBackend,
+        shape: &dyn Shape,
+        pos: &Isometry<Real>,
+        color: [f32; 4],
+    ) {
+        match shape.as_typed_shape() {
+            TypedShape::Ball(s) => {
+                let vtx = &self.instances[&TypeId::of::<Ball>()];
+                backend.draw_line_strip(
+                    object,
+                    vtx,
+                    pos,
+                    &Vector::repeat(s.radius * 2.0),
+                    color,
+                    true,
+                )
+            }
+            TypedShape::Cuboid(s) => {
+                let vtx = &self.instances[&TypeId::of::<Cuboid>()];
+                backend.draw_line_strip(object, vtx, pos, &(s.half_extents * 2.0), color, true)
+            }
+            TypedShape::Capsule(s) => {
+                let vtx = s.to_polyline(self.style.subdivisions);
+                backend.draw_line_strip(object, &vtx, pos, &Vector::repeat(1.0), color, true)
+            }
+            TypedShape::Segment(s) => backend.draw_line_strip(
+                object,
+                &[s.a, s.b],
+                pos,
+                &Vector::repeat(1.0),
+                color,
+                false,
+            ),
+            TypedShape::Triangle(s) => backend.draw_line_strip(
+                object,
+                &[s.a, s.b, s.c],
+                pos,
+                &Vector::repeat(1.0),
+                color,
+                true,
+            ),
+            TypedShape::TriMesh(s) => {
+                for tri in s.triangles() {
+                    self.render_shape(object, backend, &tri, pos, color)
+                }
+            }
+            TypedShape::Polyline(s) => backend.draw_polyline(
+                object,
+                s.vertices(),
+                s.indices(),
+                pos,
+                &Vector::repeat(1.0),
+                color,
+            ),
+            TypedShape::HalfSpace(s) => {
+                let basis = s.normal.orthonormal_basis()[0];
+                let a = Point::from(basis) * 10_000.0;
+                let b = Point::from(basis) * -10_000.0;
+                backend.draw_line_strip(object, &[a, b], pos, &Vector::repeat(1.0), color, false)
+            }
+            TypedShape::HeightField(s) => {
+                for seg in s.segments() {
+                    self.render_shape(object, backend, &seg, pos, color)
+                }
+            }
+            TypedShape::Compound(s) => {
+                for (sub_pos, shape) in s.shapes() {
+                    self.render_shape(object, backend, &**shape, &(pos * sub_pos), color)
+                }
+            }
+            TypedShape::ConvexPolygon(s) => {
+                backend.draw_line_strip(object, s.points(), pos, &Vector::repeat(1.0), color, true)
+            }
+            /*
+             * Round shapes.
+             */
+            TypedShape::RoundCuboid(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            TypedShape::RoundTriangle(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            // TypedShape::RoundTriMesh(s) => self.render_shape(backend, &s.base_shape, pos, color),
+            // TypedShape::RoundHeightField(s) => {
+            //     self.render_shape(backend, &s.base_shape, pos, color)
+            // }
+            TypedShape::RoundConvexPolygon(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            TypedShape::Custom(_) => {}
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    fn render_shape(
+        &mut self,
+        object: DebugRenderObject,
+        backend: &mut impl DebugRenderBackend,
+        shape: &dyn Shape,
+        pos: &Isometry<Real>,
+        color: [f32; 4],
+    ) {
+        match shape.as_typed_shape() {
+            TypedShape::Ball(s) => {
+                let (vtx, idx) = &self.instances[&TypeId::of::<Ball>()];
+                backend.draw_polyline(
+                    object,
+                    vtx,
+                    idx,
+                    pos,
+                    &Vector::repeat(s.radius * 2.0),
+                    color,
+                )
+            }
+            TypedShape::Cuboid(s) => {
+                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)
+            }
+            TypedShape::Segment(s) => backend.draw_polyline(
+                object,
+                &[s.a, s.b],
+                &[[0, 1]],
+                pos,
+                &Vector::repeat(1.0),
+                color,
+            ),
+            TypedShape::Triangle(s) => backend.draw_line_strip(
+                object,
+                &[s.a, s.b, s.c],
+                pos,
+                &Vector::repeat(1.0),
+                color,
+                true,
+            ),
+            TypedShape::TriMesh(s) => {
+                for tri in s.triangles() {
+                    self.render_shape(object, backend, &tri, pos, color)
+                }
+            }
+            TypedShape::Polyline(s) => backend.draw_polyline(
+                object,
+                s.vertices(),
+                s.indices(),
+                pos,
+                &Vector::repeat(1.0),
+                color,
+            ),
+            TypedShape::HalfSpace(s) => {
+                let basis = s.normal.orthonormal_basis();
+                let a = Point::from(basis[0]) * 10_000.0;
+                let b = Point::from(basis[0]) * -10_000.0;
+                let c = Point::from(basis[1]) * 10_000.0;
+                let d = Point::from(basis[1]) * -10_000.0;
+                backend.draw_polyline(
+                    object,
+                    &[a, b, c, d],
+                    &[[0, 1], [2, 3]],
+                    pos,
+                    &Vector::repeat(1.0),
+                    color,
+                )
+            }
+            TypedShape::HeightField(s) => {
+                for tri in s.triangles() {
+                    self.render_shape(object, backend, &tri, pos, color)
+                }
+            }
+            TypedShape::Compound(s) => {
+                for (sub_pos, shape) in s.shapes() {
+                    self.render_shape(object, backend, &**shape, &(pos * sub_pos), color)
+                }
+            }
+            TypedShape::ConvexPolyhedron(s) => {
+                let indices: Vec<_> = s
+                    .edges()
+                    .iter()
+                    .map(|e| [e.vertices.x, e.vertices.y])
+                    .collect();
+                backend.draw_polyline(
+                    object,
+                    s.points(),
+                    &indices,
+                    pos,
+                    &Vector::repeat(1.0),
+                    color,
+                )
+            }
+            TypedShape::Cylinder(s) => {
+                let (vtx, idx) = &self.instances[&TypeId::of::<Cylinder>()];
+                backend.draw_polyline(
+                    object,
+                    vtx,
+                    idx,
+                    pos,
+                    &(Vector::new(s.radius, s.half_height, s.radius) * 2.0),
+                    color,
+                )
+            }
+            TypedShape::Cone(s) => {
+                let (vtx, idx) = &self.instances[&TypeId::of::<Cone>()];
+                backend.draw_polyline(
+                    object,
+                    vtx,
+                    idx,
+                    pos,
+                    &(Vector::new(s.radius, s.half_height, s.radius) * 2.0),
+                    color,
+                )
+            }
+            /*
+             * Round shapes.
+             */
+            TypedShape::RoundCuboid(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            TypedShape::RoundTriangle(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            // TypedShape::RoundTriMesh(s) => self.render_shape(object, backend, &s.base_shape, pos, color),
+            // TypedShape::RoundHeightField(s) => {
+            //     self.render_shape(object, backend, &s.base_shape, pos, color)
+            // }
+            TypedShape::RoundCylinder(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            TypedShape::RoundCone(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            TypedShape::RoundConvexPolyhedron(s) => {
+                self.render_shape(object, backend, &s.base_shape, pos, color)
+            }
+            TypedShape::Custom(_) => {}
+        }
+    }
+}

src/pipeline/debug_render_pipeline/debug_render_style.rs

@@ -0,0 +1,37 @@
+/// A color for debug-rendering.
+///
+/// The default colors are provided in HSLA (Hue Saturation Lightness Alpha) format.
+pub type DebugColor = [f32; 4];
+
+#[derive(Clone, Debug, PartialEq)]
+pub struct DebugRenderStyle {
+    pub subdivisions: u32,
+    pub collider_dynamic_color: DebugColor,
+    pub collider_fixed_color: DebugColor,
+    pub collider_kinematic_color: DebugColor,
+    pub collider_parentless_color: DebugColor,
+    pub impulse_joint_anchor_color: DebugColor,
+    pub impulse_joint_separation_color: DebugColor,
+    pub multibody_joint_anchor_color: DebugColor,
+    pub multibody_joint_separation_color: DebugColor,
+    pub sleep_color_multiplier: [f32; 4],
+    pub rigid_body_axes_length: f32,
+}
+
+impl Default for DebugRenderStyle {
+    fn default() -> Self {
+        Self {
+            subdivisions: 20,
+            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],
+            collider_parentless_color: [30.0, 1.0, 0.4, 1.0],
+            impulse_joint_anchor_color: [240.0, 0.5, 0.4, 1.0],
+            impulse_joint_separation_color: [0.0, 0.5, 0.4, 1.0],
+            multibody_joint_anchor_color: [300.0, 1.0, 0.4, 1.0],
+            multibody_joint_separation_color: [0.0, 1.0, 0.4, 1.0],
+            sleep_color_multiplier: [1.0, 1.0, 0.2, 1.0],
+            rigid_body_axes_length: 0.5,
+        }
+    }
+}

src/pipeline/debug_render_pipeline/mod.rs

@@ -0,0 +1,8 @@
+pub use self::debug_render_backend::{DebugRenderBackend, DebugRenderObject};
+pub use self::debug_render_pipeline::{DebugRenderMode, DebugRenderPipeline};
+pub use self::debug_render_style::{DebugColor, DebugRenderStyle};
+
+mod debug_render_backend;
+mod debug_render_pipeline;
+mod debug_render_style;
+pub(self) mod outlines;

src/pipeline/debug_render_pipeline/outlines.rs

@@ -0,0 +1,36 @@
+use crate::geometry::{Ball, Cuboid};
+#[cfg(feature = "dim3")]
+use crate::geometry::{Cone, Cylinder};
+use crate::math::{Point, Real, Vector};
+use std::any::TypeId;
+use std::collections::HashMap;
+
+#[cfg(feature = "dim2")]
+pub fn instances(nsubdivs: u32) -> HashMap<TypeId, Vec<Point<Real>>> {
+    let mut result = HashMap::new();
+    result.insert(
+        TypeId::of::<Cuboid>(),
+        Cuboid::new(Vector::repeat(0.5)).to_polyline(),
+    );
+    result.insert(TypeId::of::<Ball>(), Ball::new(0.5).to_polyline(nsubdivs));
+    result
+}
+
+#[cfg(feature = "dim3")]
+pub fn instances(nsubdivs: u32) -> HashMap<TypeId, (Vec<Point<Real>>, Vec<[u32; 2]>)> {
+    let mut result = HashMap::new();
+    result.insert(
+        TypeId::of::<Cuboid>(),
+        Cuboid::new(Vector::repeat(0.5)).to_outline(),
+    );
+    result.insert(TypeId::of::<Ball>(), Ball::new(0.5).to_outline(nsubdivs));
+    result.insert(
+        TypeId::of::<Cone>(),
+        Cone::new(0.5, 0.5).to_outline(nsubdivs),
+    );
+    result.insert(
+        TypeId::of::<Cylinder>(),
+        Cylinder::new(0.5, 0.5).to_outline(nsubdivs),
+    );
+    result
+}

src/pipeline/mod.rs

@@ -6,9 +6,18 @@ pub use physics_hooks::{ActiveHooks, ContactModificationContext, PairFilterConte
 pub use physics_pipeline::PhysicsPipeline;
 pub use query_pipeline::{QueryPipeline, QueryPipelineMode};
 
+#[cfg(feature = "debug-render")]
+pub use self::debug_render_pipeline::{
+    DebugColor, DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline,
+    DebugRenderStyle,
+};
+
 mod collision_pipeline;
 mod event_handler;
 mod physics_hooks;
 mod physics_pipeline;
 mod query_pipeline;
 mod user_changes;
+
+#[cfg(feature = "debug-render")]
+mod debug_render_pipeline;

src/pipeline/physics_pipeline.rs

@@ -541,6 +541,17 @@ impl PhysicsPipeline {
             self.clear_modified_colliders(colliders, &mut modified_colliders);
         }
 
+        // Finally, make sure we update the world mass-properties of the rigid-bodies
+        // that moved. Otherwise, users may end up applying forces wrt. an outdated
+        // center of mass.
+        // TODO: avoid updating the world mass properties twice (here, and
+        //       at the beginning of the next timestep) for bodies that were
+        //       not modified by the user in the mean time.
+        for handle in islands.active_dynamic_bodies() {
+            let rb = bodies.index_mut_internal(*handle);
+            rb.mprops.update_world_mass_properties(&rb.pos.position);
+        }
+
         self.counters.step_completed();
     }
 }

src_testbed/debug_render.rs

@@ -0,0 +1,81 @@
+use crate::harness::Harness;
+use bevy::prelude::*;
+use bevy_prototype_debug_lines::DebugLines;
+use rapier::math::{Point, Real, DIM};
+use rapier::pipeline::{
+    DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline,
+};
+use std::fmt::Debug;
+
+pub struct RapierDebugRenderPlugin {
+    depth_test: bool,
+}
+
+impl Default for RapierDebugRenderPlugin {
+    fn default() -> Self {
+        Self {
+            depth_test: cfg!(feature = "dim3"),
+        }
+    }
+}
+
+impl RapierDebugRenderPlugin {
+    pub fn with_depth_test(enabled: bool) -> Self {
+        Self {
+            depth_test: enabled,
+        }
+    }
+}
+
+impl Plugin for RapierDebugRenderPlugin {
+    fn build(&self, app: &mut App) {
+        app.add_plugin(
+            bevy_prototype_debug_lines::DebugLinesPlugin::with_depth_test(self.depth_test),
+        )
+        .insert_resource(DebugRenderPipeline::new(
+            Default::default(),
+            !DebugRenderMode::RIGID_BODY_AXES,
+        ))
+        .add_system_to_stage(CoreStage::Update, debug_render_scene);
+    }
+}
+
+struct BevyLinesRenderBackend<'a> {
+    lines: &'a mut DebugLines,
+}
+
+impl<'a> DebugRenderBackend for BevyLinesRenderBackend<'a> {
+    #[cfg(feature = "dim2")]
+    fn draw_line(&mut self, _: DebugRenderObject, a: Point<Real>, b: Point<Real>, color: [f32; 4]) {
+        self.lines.line_colored(
+            [a.x, a.y, 1.0e-8].into(),
+            [b.x, b.y, 1.0e-8].into(),
+            0.0,
+            Color::hsla(color[0], color[1], color[2], color[3]),
+        )
+    }
+    #[cfg(feature = "dim3")]
+    fn draw_line(&mut self, _: DebugRenderObject, a: Point<Real>, b: Point<Real>, color: [f32; 4]) {
+        self.lines.line_colored(
+            [a.x, a.y, a.z].into(),
+            [b.x, b.y, b.z].into(),
+            0.0,
+            Color::hsla(color[0], color[1], color[2], color[3]),
+        )
+    }
+}
+
+fn debug_render_scene(
+    mut pipeline: ResMut<DebugRenderPipeline>,
+    harness: NonSend<Harness>,
+    mut lines: ResMut<DebugLines>,
+) {
+    let mut backend = BevyLinesRenderBackend { lines: &mut *lines };
+    pipeline.render(
+        &mut backend,
+        &harness.physics.bodies,
+        &harness.physics.colliders,
+        &harness.physics.impulse_joints,
+        &harness.physics.multibody_joints,
+    );
+}

src_testbed/lib.rs

@@ -19,6 +19,7 @@ mod box2d_backend;
 mod camera2d;
 #[cfg(feature = "dim3")]
 mod camera3d;
+mod debug_render;
 mod graphics;
 pub mod harness;
 pub mod objects;

src_testbed/testbed.rs

@@ -5,7 +5,7 @@ use bevy::prelude::*;
 
 use crate::physics::{PhysicsEvents, PhysicsSnapshot, PhysicsState};
 use crate::plugin::TestbedPlugin;
-use crate::ui;
+use crate::{debug_render, ui};
 use crate::{graphics::GraphicsManager, harness::RunState};
 
 use na::{self, Point2, Point3, Vector3};
@@ -17,7 +17,7 @@ use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 #[cfg(feature = "dim3")]
 use rapier::geometry::{InteractionGroups, Ray};
 use rapier::math::{Real, Vector};
-use rapier::pipeline::PhysicsHooks;
+use rapier::pipeline::{DebugRenderMode, PhysicsHooks};
 
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
 use crate::box2d_backend::Box2dWorld;
@@ -380,7 +380,8 @@ impl TestbedApp {
             .add_plugins(DefaultPlugins)
             .add_plugin(OrbitCameraPlugin)
             .add_plugin(WireframePlugin)
-            .add_plugin(bevy_egui::EguiPlugin);
+            .add_plugin(bevy_egui::EguiPlugin)
+            .add_plugin(debug_render::RapierDebugRenderPlugin::default());
 
             #[cfg(target_arch = "wasm32")]
             app.add_plugin(bevy_webgl2::WebGL2Plugin);