Merge pull request #315 from dimforge/debug-renderer

Add a basic lines-based debug-renderer

15 files changed, 865 insertions, 52 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 77a1ff7..bdde135 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -4,7 +4,7 @@ use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase};
 use crate::math::Real;
 use crate::parry::utils::SortedPair;
 use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
-use crate::prelude::ActiveEvents;
+use crate::prelude::{ActiveEvents, CollisionEventFlags};
 use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
 use parry::utils::hashmap::HashMap;
 use std::collections::BinaryHeap;
@@ -529,8 +529,18 @@ impl CCDSolver {
                     .contains(ActiveEvents::COLLISION_EVENTS)
             {
                 // Emit one intersection-started and one intersection-stopped event.
-                events.handle_collision_event(CollisionEvent::Started(toi.c1, toi.c2), None);
-                events.handle_collision_event(CollisionEvent::Stopped(toi.c1, toi.c2, false), None);
+                events.handle_collision_event(
+                    bodies,
+                    colliders,
+                    CollisionEvent::Started(toi.c1, toi.c2, CollisionEventFlags::SENSOR),
+                    None,
+                );
+                events.handle_collision_event(
+                    bodies,
+                    colliders,
+                    CollisionEvent::Stopped(toi.c1, toi.c2, CollisionEventFlags::SENSOR),
+                    None,
+                );
             }
         }
 
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 84c8117..6a86a2a 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -93,12 +93,12 @@ impl IntegrationParameters {
 
     /// The ERP coefficient, multiplied by the inverse timestep length.
     pub fn erp_inv_dt(&self) -> Real {
-        self.erp / self.dt
+        self.erp * self.inv_dt()
     }
 
     /// The joint ERP coefficient, multiplied by the inverse timestep length.
     pub fn joint_erp_inv_dt(&self) -> Real {
-        self.joint_erp / self.dt
+        self.joint_erp * self.inv_dt()
     }
 
     /// The CFM factor to be used in the constraints resolution.
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/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index cf52c1f..24e9754 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -305,20 +305,26 @@ impl RigidBody {
         self.ccd.ccd_active
     }
 
-    /// Sets the rigid-body's initial mass properties.
+    /// Sets the rigid-body's additional mass properties.
     ///
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
     /// put to sleep because it did not move for a while.
     #[inline]
-    pub fn set_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
-        if self.mprops.local_mprops != props {
-            if self.is_dynamic() && wake_up {
-                self.wake_up(true);
-            }
+    pub fn set_additional_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
+        if let Some(add_mprops) = &mut self.mprops.additional_local_mprops {
+            self.mprops.local_mprops += props;
+            self.mprops.local_mprops -= **add_mprops;
+            **add_mprops = props;
+        } else {
+            self.mprops.additional_local_mprops = Some(Box::new(props));
+            self.mprops.local_mprops += props;
+        }
 
-            self.mprops.local_mprops = props;
-            self.update_world_mass_properties();
+        if self.is_dynamic() && wake_up {
+            self.wake_up(true);
         }
+
+        self.update_world_mass_properties();
     }
 
     /// The handles of colliders attached to this rigid body.
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index a75d58d..551ffde 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -1,7 +1,8 @@
-use crate::dynamics::RigidBodyHandle;
-use crate::geometry::{ColliderHandle, Contact, ContactManifold};
+use crate::dynamics::{RigidBodyHandle, RigidBodySet};
+use crate::geometry::{ColliderHandle, ColliderSet, Contact, ContactManifold};
 use crate::math::{Point, Real, Vector};
 use crate::pipeline::EventHandler;
+use crate::prelude::CollisionEventFlags;
 use parry::query::ContactManifoldsWorkspace;
 
 use super::CollisionEvent;
@@ -69,22 +70,36 @@ impl IntersectionPair {
 
     pub(crate) fn emit_start_event(
         &mut self,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
         collider1: ColliderHandle,
         collider2: ColliderHandle,
         events: &dyn EventHandler,
     ) {
         self.start_event_emited = true;
-        events.handle_collision_event(CollisionEvent::new(collider1, collider2, true), None);
+        events.handle_collision_event(
+            bodies,
+            colliders,
+            CollisionEvent::Started(collider1, collider2, CollisionEventFlags::SENSOR),
+            None,
+        );
     }
 
     pub(crate) fn emit_stop_event(
         &mut self,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
         collider1: ColliderHandle,
         collider2: ColliderHandle,
         events: &dyn EventHandler,
     ) {
         self.start_event_emited = false;
-        events.handle_collision_event(CollisionEvent::new(collider1, collider2, false), None);
+        events.handle_collision_event(
+            bodies,
+            colliders,
+            CollisionEvent::Stopped(collider1, collider2, CollisionEventFlags::SENSOR),
+            None,
+        );
     }
 }
 
@@ -155,20 +170,34 @@ impl ContactPair {
         deepest
     }
 
-    pub(crate) fn emit_start_event(&mut self, events: &dyn EventHandler) {
+    pub(crate) fn emit_start_event(
+        &mut self,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        events: &dyn EventHandler,
+    ) {
         self.start_event_emited = true;
 
         events.handle_collision_event(
-            CollisionEvent::new(self.collider1, self.collider2, true),
+            bodies,
+            colliders,
+            CollisionEvent::Started(self.collider1, self.collider2, CollisionEventFlags::empty()),
             Some(self),
         );
     }
 
-    pub(crate) fn emit_stop_event(&mut self, events: &dyn EventHandler) {
+    pub(crate) fn emit_stop_event(
+        &mut self,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        events: &dyn EventHandler,
+    ) {
         self.start_event_emited = false;
 
         events.handle_collision_event(
-            CollisionEvent::new(self.collider1, self.collider2, false),
+            bodies,
+            colliders,
+            CollisionEvent::Stopped(self.collider1, self.collider2, CollisionEventFlags::empty()),
             Some(self),
         );
     }
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 7733ad2..34d3707 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -50,27 +50,29 @@ pub type PointProjection = parry::query::PointProjection;
 pub type TOI = parry::query::TOI;
 pub use parry::shape::SharedShape;
 
+bitflags::bitflags! {
+    /// Flags providing more information regarding a collision event.
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    pub struct CollisionEventFlags: u32 {
+        /// Flag set if at least one of the colliders involved in the
+        /// collision was a sensor when the event was fired.
+        const SENSOR = 0b0001;
+        /// Flag set if a `CollisionEvent::Stopped` was fired because
+        /// at least one of the colliders was removed.
+        const REMOVED = 0b0010;
+    }
+}
+
 #[derive(Copy, Clone, Hash, Debug)]
-/// Events occurring when two colliders start or stop being in contact (or intersecting)
+/// Events occurring when two colliders start or stop colliding
 pub enum CollisionEvent {
-    /// Event occurring when two colliders start being in contact (or intersecting)
-    Started(ColliderHandle, ColliderHandle),
-    /// Event occurring when two colliders stop being in contact (or intersecting).
-    ///
-    /// The boolean is set to `true` of this event originates from at least one of
-    /// the colliders being removed from the `ColliderSet`.
-    Stopped(ColliderHandle, ColliderHandle, bool),
+    /// Event occurring when two colliders start colliding
+    Started(ColliderHandle, ColliderHandle, CollisionEventFlags),
+    /// Event occurring when two colliders stop colliding.
+    Stopped(ColliderHandle, ColliderHandle, CollisionEventFlags),
 }
 
 impl CollisionEvent {
-    pub(crate) fn new(h1: ColliderHandle, h2: ColliderHandle, start: bool) -> Self {
-        if start {
-            Self::Started(h1, h2)
-        } else {
-            Self::Stopped(h1, h2, false)
-        }
-    }
-
     /// Is this a `Started` collision event?
     pub fn started(self) -> bool {
         matches!(self, CollisionEvent::Started(..))
@@ -84,14 +86,32 @@ impl CollisionEvent {
     /// The handle of the first collider involved in this collision event.
     pub fn collider1(self) -> ColliderHandle {
         match self {
-            Self::Started(h, _) | Self::Stopped(h, _, _) => h,
+            Self::Started(h, _, _) | Self::Stopped(h, _, _) => h,
         }
     }
 
     /// The handle of the second collider involved in this collision event.
     pub fn collider2(self) -> ColliderHandle {
         match self {
-            Self::Started(_, h) | Self::Stopped(_, h, _) => h,
+            Self::Started(_, h, _) | Self::Stopped(_, h, _) => h,
+        }
+    }
+
+    /// Was at least one of the colliders involved in the collision a sensor?
+    pub fn sensor(self) -> bool {
+        match self {
+            Self::Started(_, _, f) | Self::Stopped(_, _, f) => {
+                f.contains(CollisionEventFlags::SENSOR)
+            }
+        }
+    }
+
+    /// Was at least one of the colliders involved in the collision removed?
+    pub fn removed(self) -> bool {
+        match self {
+            Self::Started(_, _, f) | Self::Stopped(_, _, f) => {
+                f.contains(CollisionEventFlags::REMOVED)
+            }
         }
     }
 }
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index f4e8c58..3e7a13d 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -15,6 +15,7 @@ use crate::pipeline::{
     ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext,
     PhysicsHooks,
 };
+use crate::prelude::CollisionEventFlags;
 use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
 use parry::utils::IsometryOpt;
 use std::collections::HashMap;
@@ -317,14 +318,24 @@ impl NarrowPhase {
                 }
 
                 if pair.start_event_emited {
-                    events.handle_collision_event(CollisionEvent::Stopped(a, b, true), Some(pair));
+                    events.handle_collision_event(
+                        bodies,
+                        colliders,
+                        CollisionEvent::Stopped(a, b, CollisionEventFlags::REMOVED),
+                        Some(pair),
+                    );
                 }
             }
         } else {
             // If there is no island, don’t wake-up bodies, but do send the Stopped collision event.
             for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
                 if pair.start_event_emited {
-                    events.handle_collision_event(CollisionEvent::Stopped(a, b, true), Some(pair));
+                    events.handle_collision_event(
+                        bodies,
+                        colliders,
+                        CollisionEvent::Stopped(a, b, CollisionEventFlags::REMOVED),
+                        Some(pair),
+                    );
                 }
             }
         }
@@ -332,7 +343,16 @@ impl NarrowPhase {
         // Generate Stopped collision events for intersections.
         for (a, b, pair) in self.intersection_graph.interactions_with(contact_graph_id) {
             if pair.start_event_emited {
-                events.handle_collision_event(CollisionEvent::Stopped(a, b, true), None);
+                events.handle_collision_event(
+                    bodies,
+                    colliders,
+                    CollisionEvent::Stopped(
+                        a,
+                        b,
+                        CollisionEventFlags::REMOVED | CollisionEventFlags::SENSOR,
+                    ),
+                    None,
+                );
             }
         }
 
@@ -495,7 +515,13 @@ impl NarrowPhase {
                             if (co1.flags.active_events | co2.flags.active_events)
                                 .contains(ActiveEvents::COLLISION_EVENTS)
                             {
-                                intersection.emit_stop_event(pair.collider1, pair.collider2, events)
+                                intersection.emit_stop_event(
+                                    bodies,
+                                    colliders,
+                                    pair.collider1,
+                                    pair.collider2,
+                                    events,
+                                )
                             }
                         }
                     }
@@ -521,7 +547,7 @@ impl NarrowPhase {
                             if (co1.flags.active_events | co2.flags.active_events)
                                 .contains(ActiveEvents::COLLISION_EVENTS)
                             {
-                                ctct.emit_stop_event(events);
+                                ctct.emit_stop_event(bodies, colliders, events);
                             }
                         }
                     }
@@ -724,9 +750,11 @@ impl NarrowPhase {
                 && had_intersection != edge.weight.intersecting
             {
                 if edge.weight.intersecting {
-                    edge.weight.emit_start_event(handle1, handle2, events);
+                    edge.weight
+                        .emit_start_event(bodies, colliders, handle1, handle2, events);
                 } else {
-                    edge.weight.emit_stop_event(handle1, handle2, events);
+                    edge.weight
+                        .emit_stop_event(bodies, colliders, handle1, handle2, events);
                 }
             }
         });
@@ -928,9 +956,9 @@ impl NarrowPhase {
             if pair.has_any_active_contact != had_any_active_contact {
                 if active_events.contains(ActiveEvents::COLLISION_EVENTS) {
                     if pair.has_any_active_contact {
-                        pair.emit_start_event(events);
+                        pair.emit_start_event(bodies, colliders, events);
                     } else {
-                        pair.emit_stop_event(events);
+                        pair.emit_stop_event(bodies, colliders, events);
                     }
                 }
             }
diff --git a/src/pipeline/debug_render_pipeline/debug_render_backend.rs b/src/pipeline/debug_render_pipeline/debug_render_backend.rs
new file mode 100644
index 0000000..4fa86e4
--- /dev/null
+++ b/src/pipeline/debug_render_pipeline/debug_render_backend.rs
@@ -0,0 +1,83 @@
+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;
+
+/// The object currently being rendered by the debug-renderer.
+#[derive(Copy, Clone)]
+pub enum DebugRenderObject<'a> {
+    /// A rigid-body is being rendered.
+    RigidBody(RigidBodyHandle, &'a RigidBody),
+    /// A collider is being rendered.
+    Collider(ColliderHandle, &'a Collider),
+    /// An impulse-joint is being rendered.
+    ImpulseJoint(ImpulseJointHandle, &'a ImpulseJoint),
+    /// A multibody joint is being rendered.
+    MultibodyJoint(MultibodyJointHandle, &'a Multibody, &'a MultibodyLink),
+    /// Another element is being rendered.
+    Other,
+}
+
+/// Trait implemented by graphics backends responsible for rendering the physics scene.
+///
+/// The only thing that is required from the graphics backend is to be able to render
+/// a colored line. Note that the color is only a suggestion and is computed from the
+/// `DebugRenderStyle`. The backend is free to apply its own style, for example based on
+/// the `object` being rendered.
+pub trait DebugRenderBackend {
+    /// Draws a colored line.
+    ///
+    /// Note that this method can be called multiple time for the same `object`.
+    fn draw_line(
+        &mut self,
+        object: DebugRenderObject,
+        a: Point<Real>,
+        b: Point<Real>,
+        color: [f32; 4],
+    );
+
+    /// Draws a set of line.
+    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);
+        }
+    }
+
+    /// Draws a chain of line.
+    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);
+            }
+        }
+    }
+}
diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
new file mode 100644
index 0000000..14da40d
--- /dev/null
+++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
@@ -0,0 +1,481 @@
+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! {
+    /// Flags indicating what part of the physics engine should be rendered
+    /// by the debug-renderer.
+    pub struct DebugRenderMode: u32 {
+        /// If this flag is set, the collider shapes will be rendered.
+        const COLLIDER_SHAPES = 1 << 0;
+        /// If this flag is set, the local coordinate axes of rigid-bodies will be rendered.
+        const RIGID_BODY_AXES = 1 << 1;
+        /// If this flag is set, the multibody joints will be rendered.
+        const MULTIBODY_JOINTS = 1 << 2;
+        /// If this flag is set, the impulse joints will be rendered.
+        const IMPULSE_JOINTS = 1 << 3;
+    }
+}
+
+/// Pipeline responsible for rendering the state of the physics engine for debugging purpose.
+pub struct DebugRenderPipeline {
+    #[cfg(feature = "dim2")]
+    instances: HashMap<TypeId, Vec<Point<Real>>>,
+    #[cfg(feature = "dim3")]
+    instances: HashMap<TypeId, (Vec<Point<Real>>, Vec<[u32; 2]>)>,
+    /// The style used to compute the line colors for each element
+    /// to render.
+    pub style: DebugRenderStyle,
+    /// Flags controlling what part of the physics engine need to
+    /// be rendered.
+    pub mode: DebugRenderMode,
+}
+
+impl Default for DebugRenderPipeline {
+    fn default() -> Self {
+        Self::render_all(DebugRenderStyle::default())
+    }
+}
+
+impl DebugRenderPipeline {
+    /// Creates a new debug-render pipeline from a given style and flags.
+    pub fn new(style: DebugRenderStyle, mode: DebugRenderMode) -> Self {
+        Self {
+            instances: outlines::instances(style.subdivisions),
+            style,
+            mode,
+        }
+    }
+
+    /// Creates a new debug-render pipeline that renders everything
+    /// it can from the physics state.
+    pub fn render_all(style: DebugRenderStyle) -> Self {
+        Self::new(style, DebugRenderMode::all())
+    }
+
+    /// Render the scene.
+    pub fn render(
+        &mut self,
+        backend: &mut impl DebugRenderBackend,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
+    ) {
+        self.render_rigid_bodies(backend, bodies);
+        self.render_colliders(backend, bodies, colliders);
+        self.render_joints(backend, bodies, impulse_joints, multibody_joints);
+    }
+
+    /// Render only the joints from the scene.
+    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,
+                );
+            }
+        }
+    }
+
+    /// Render only the rigid-bodies from the scene.
+    pub fn render_rigid_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]);
+                }
+            }
+        }
+    }
+
+    /// Render only the colliders from the scene.
+    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) => {
+                let vtx = s.to_polyline(self.style.border_subdivisions);
+