From 0d05536ab66052af0dfc153e966c82b4e6d27075 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 30 May 2022 18:27:52 +0200 Subject: Debug-renderer: add rendering of contacts, solver contacts, and collider AABBs --- .../debug_render_pipeline/debug_render_pipeline.rs | 77 +++++++++++++++++++++- .../debug_render_pipeline/debug_render_style.rs | 12 ++++ 2 files changed, 87 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index 14da40d..017d26f 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/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")] diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs index ac2b3a3..987d95d 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_style.rs +++ b/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], } } } -- cgit