Debug-renderer: add rendering of contacts, solver contacts, and collider AABBs

src/pipeline/debug_render_pipeline/debug_render_pipeline.rs

@@ -2,10 +2,12 @@ use super::{outlines, DebugRenderBackend};
 use crate::dynamics::{
     GenericJoint, ImpulseJointSet, MultibodyJointSet, RigidBodySet, RigidBodyType,
 };
-use crate::geometry::{Ball, ColliderSet, Cuboid, Shape, TypedShape};
+use crate::geometry::{
+    Ball, ColliderSet, Cuboid, NarrowPhase, OctantPattern, Shape, TypedShape, VoxelType, AABB,
+};
 #[cfg(feature = "dim3")]
 use crate::geometry::{Cone, Cylinder};
-use crate::math::{Isometry, Point, Real, Vector, DIM};
+use crate::math::{Isometry, Point, Real, Translation, Vector, DIM};
 use crate::pipeline::debug_render_pipeline::debug_render_backend::DebugRenderObject;
 use crate::pipeline::debug_render_pipeline::DebugRenderStyle;
 use crate::utils::WBasis;
@@ -24,6 +26,12 @@ bitflags::bitflags! {
         const MULTIBODY_JOINTS = 1 << 2;
         /// If this flag is set, the impulse joints will be rendered.
         const IMPULSE_JOINTS = 1 << 3;
+        /// If this flag is set, the solver contacts will be rendered.
+        const SOLVER_CONTACTS = 1 << 4;
+        /// If this flag is set, the geometric contacts will be rendered.
+        const CONTACTS = 1 << 5;
+        /// If this flag is set, the AABBs of colliders will be rendered.
+        const COLLIDER_AABBS = 1 << 6;
     }
 }
 
@@ -71,10 +79,61 @@ impl DebugRenderPipeline {
         colliders: &ColliderSet,
         impulse_joints: &ImpulseJointSet,
         multibody_joints: &MultibodyJointSet,
+        narrow_phase: &NarrowPhase,
     ) {
         self.render_rigid_bodies(backend, bodies);
         self.render_colliders(backend, bodies, colliders);
         self.render_joints(backend, bodies, impulse_joints, multibody_joints);
+        self.render_contacts(backend, colliders, narrow_phase);
+    }
+
+    pub fn render_contacts(
+        &mut self,
+        backend: &mut impl DebugRenderBackend,
+        colliders: &ColliderSet,
+        narrow_phase: &NarrowPhase,
+    ) {
+        if self.mode.contains(DebugRenderMode::CONTACTS) {
+            for pair in narrow_phase.contact_pairs() {
+                if let (Some(co1), Some(co2)) =
+                    (colliders.get(pair.collider1), colliders.get(pair.collider2))
+                {
+                    for manifold in &pair.manifolds {
+                        for contact in manifold.contacts() {
+                            backend.draw_line(
+                                DebugRenderObject::Other,
+                                co1.position() * contact.local_p1,
+                                co2.position() * contact.local_p2,
+                                self.style.contact_depth_color,
+                            );
+                            backend.draw_line(
+                                DebugRenderObject::Other,
+                                co1.position() * contact.local_p1,
+                                co1.position()
+                                    * (contact.local_p1
+                                        + manifold.local_n1 * self.style.contact_normal_length),
+                                self.style.contact_normal_color,
+                            );
+                        }
+                    }
+                }
+            }
+        }
+
+        if self.mode.contains(DebugRenderMode::SOLVER_CONTACTS) {
+            for pair in narrow_phase.contact_pairs() {
+                for manifold in &pair.manifolds {
+                    for contact in &manifold.data.solver_contacts {
+                        backend.draw_line(
+                            DebugRenderObject::Other,
+                            contact.point,
+                            contact.point + manifold.data.normal * self.style.contact_normal_length,
+                            self.style.contact_normal_color,
+                        );
+                    }
+                }
+            }
+        }
     }
 
     /// Render only the joints from the scene.
@@ -224,6 +283,20 @@ impl DebugRenderPipeline {
                 self.render_shape(object, backend, co.shape(), co.position(), color)
             }
         }
+
+        if self.mode.contains(DebugRenderMode::COLLIDER_AABBS) {
+            for (_, co) in colliders.iter() {
+                let aabb = co.compute_aabb();
+                let cuboid = Cuboid::new(aabb.half_extents());
+                self.render_shape(
+                    DebugRenderObject::Other,
+                    backend,
+                    &cuboid,
+                    &aabb.center().into(),
+                    self.style.collider_aabb_color,
+                );
+            }
+        }
     }
 
     #[cfg(feature = "dim2")]

src/pipeline/debug_render_pipeline/debug_render_style.rs

@@ -38,6 +38,14 @@ pub struct DebugRenderStyle {
     pub sleep_color_multiplier: [f32; 4],
     /// The length of the local coordinate axes rendered for a rigid-body.
     pub rigid_body_axes_length: Real,
+    /// The collor for the segments joining the two contact points.
+    pub contact_depth_color: DebugColor,
+    /// The color of the contact normals.
+    pub contact_normal_color: DebugColor,
+    /// The length of the contact normals.
+    pub contact_normal_length: Real,
+    /// The color of the colliders AABBs.
+    pub collider_aabb_color: DebugColor,
 }
 
 impl Default for DebugRenderStyle {
@@ -55,6 +63,10 @@ impl Default for DebugRenderStyle {
             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,
+            contact_depth_color: [120.0, 1.0, 0.4, 1.0],
+            contact_normal_color: [0.0, 1.0, 1.0, 1.0],
+            contact_normal_length: 0.3,
+            collider_aabb_color: [124.0, 1.0, 0.4, 1.0],
         }
     }
 }

src_testbed/debug_render.rs

@@ -25,7 +25,7 @@ impl Plugin for RapierDebugRenderPlugin {
         ))
         .insert_resource(DebugRenderPipeline::new(
             Default::default(),
-            !DebugRenderMode::RIGID_BODY_AXES,
+            !DebugRenderMode::RIGID_BODY_AXES & !DebugRenderMode::COLLIDER_AABBS,
         ))
         .add_system_to_stage(CoreStage::Update, debug_render_scene);
     }
@@ -68,5 +68,6 @@ fn debug_render_scene(
         &harness.physics.colliders,
         &harness.physics.impulse_joints,
         &harness.physics.multibody_joints,
+        &harness.physics.narrow_phase,
     );
 }

src_testbed/lines/debuglines.wgsl

@@ -25,9 +25,13 @@ struct FragmentOutput {
 fn vertex(vertex: Vertex) -> VertexOutput {
     var out: VertexOutput;
     out.clip_position = view.view_proj * vec4<f32>(vertex.pos, 1.0);
-    //out.color = vertex.color;
     // https://github.com/bevyengine/bevy/blob/328c26d02c50de0bc77f0d24a376f43ba89517b1/examples/2d/mesh2d_manual.rs#L234
-    out.color = vec4<f32>((vec4<u32>(vertex.color) >> vec4<u32>(8u, 8u, 16u, 24u)) & vec4<u32>(255u)) / 255.0;
+    // ... except the above doesn't seem to work in 3d.  Not sure what's going on there.
+    var r = f32(vertex.color & 255u) / 255.0;
+    var g = f32(vertex.color >> 8u & 255u) / 255.0;
+    var b = f32(vertex.color >> 16u & 255u) / 255.0;
+    var a = f32(vertex.color >> 24u & 255u) / 255.0;
+    out.color = vec4<f32>(r, g, b, a);
 
     return out;
 }

src_testbed/lines/debuglines2d.wgsl

@@ -17,10 +17,11 @@ struct VertexOutput {
 fn vertex(vertex: Vertex) -> VertexOutput {
     var out: VertexOutput;
     out.clip_position = view.view_proj * vec4<f32>(vertex.place, 1.0);
-    // What is this craziness?
+    var r = f32(vertex.color & 255u) / 255.0;
-    out.color = vec4<f32>((vec4<u32>(vertex.color) >> vec4<u32>(0u, 8u, 16u, 24u)) & vec4<u32>(255u)) / 255.0;
+    var g = f32(vertex.color >> 8u & 255u) / 255.0;
-    //out.color = vertex.color;
+    var b = f32(vertex.color >> 16u & 255u) / 255.0;
-    //out.color = vec4<f32>(1.0, 0.0, 0.0, 1.0);
+    var a = f32(vertex.color >> 24u & 255u) / 255.0;
+    out.color = vec4<f32>(r, g, b, a);
 
     return out;
 }

src_testbed/lines/mod.rs

@@ -1,4 +1,5 @@
 #![allow(warnings)]
+use bevy::render::view::NoFrustumCulling;
 /**
  *
  * NOTE: this module and its submodules are only temporary. It is a copy-paste of the bevy-debug-lines
@@ -33,7 +34,7 @@ mod render_dim;
 // gates-specific code.
 #[cfg(feature = "dim3")]
 mod dim {
-    pub(crate) use crate::lines::render_dim::r3d::{queue, DebugLinePipeline, DrawDebugLines};
+    pub(crate) use super::render_dim::r3d::{queue, DebugLinePipeline, DrawDebugLines};
     pub(crate) use bevy::core_pipeline::Opaque3d as Phase;
     use bevy::{asset::Handle, render::mesh::Mesh};
 
@@ -49,7 +50,7 @@ mod dim {
 }
 #[cfg(feature = "dim2")]
 mod dim {
-    pub(crate) use crate::lines::render_dim::r2d::{queue, DebugLinePipeline, DrawDebugLines};
+    pub(crate) use super::render_dim::r2d::{queue, DebugLinePipeline, DrawDebugLines};
     pub(crate) use bevy::core_pipeline::Transparent2d as Phase;
     use bevy::{asset::Handle, render::mesh::Mesh, sprite::Mesh2dHandle};
 
@@ -172,6 +173,7 @@ fn setup(mut cmds: Commands, mut meshes: ResMut<Assets<Mesh>>) {
             dim::into_handle(meshes.add(mesh)),
             NotShadowCaster,
             NotShadowReceiver,
+            NoFrustumCulling,
             Transform::default(),
             GlobalTransform::default(),
             Visibility::default(),
@@ -190,7 +192,7 @@ fn update(
     // For each debug line mesh, fill its buffers with the relevant positions/colors chunks.
     for (mesh_handle, debug_lines_idx) in debug_line_meshes.iter() {
         let mesh = meshes.get_mut(dim::from_handle(mesh_handle)).unwrap();
-        use VertexAttributeValues::{Float32x3, Float32x4, Uint32};
+        use VertexAttributeValues::{Float32x3, Uint32};
         if let Some(Float32x3(vbuffer)) = mesh.attribute_mut(Mesh::ATTRIBUTE_POSITION) {
             vbuffer.clear();
             if let Some(new_content) = lines
@@ -238,7 +240,7 @@ fn extract(mut commands: Commands, query: Query<Entity, With<DebugLinesMesh>>) {
 }
 
 #[derive(Component)]
-struct DebugLinesMesh(usize);
+pub(crate) struct DebugLinesMesh(usize);
 
 #[derive(Component)]
 pub(crate) struct RenderDebugLinesMesh;
@@ -320,7 +322,6 @@ impl DebugLines {
         end_color: Color,
     ) {
         if self.positions.len() >= MAX_POINTS {
-            warn!("Tried to add a new line when existing number of lines was already at maximum, ignoring.");
             return;
         }