alice_physics/
debug_render.rs

1//! Debug Visualization API
2//!
3//! Abstract debug rendering interface for visualizing physics state.
4//! Outputs wireframe geometry (lines, points) that can be rendered
5//! by any graphics backend.
6//!
7//! # Usage
8//!
9//! Implement `DebugRenderer` trait for your graphics backend, then call
10//! `debug_draw_world()` each frame.
11
12use crate::collider::AABB;
13use crate::math::{Fix128, QuatFix, Vec3Fix};
14
15#[cfg(not(feature = "std"))]
16use alloc::vec::Vec;
17
18/// RGBA color for debug rendering (0-255 per channel)
19#[derive(Clone, Copy, Debug, PartialEq, Eq)]
20pub struct DebugColor {
21    /// Red channel
22    pub r: u8,
23    /// Green channel
24    pub g: u8,
25    /// Blue channel
26    pub b: u8,
27    /// Alpha channel
28    pub a: u8,
29}
30
31impl DebugColor {
32    /// Create a new color
33    #[must_use]
34    pub const fn new(r: u8, g: u8, b: u8, a: u8) -> Self {
35        Self { r, g, b, a }
36    }
37
38    /// Predefined colors
39    pub const RED: Self = Self::new(255, 50, 50, 255);
40    /// Green color
41    pub const GREEN: Self = Self::new(50, 255, 50, 255);
42    /// Blue color
43    pub const BLUE: Self = Self::new(50, 50, 255, 255);
44    /// Yellow color
45    pub const YELLOW: Self = Self::new(255, 255, 50, 255);
46    /// Cyan color
47    pub const CYAN: Self = Self::new(50, 255, 255, 255);
48    /// Magenta color
49    pub const MAGENTA: Self = Self::new(255, 50, 255, 255);
50    /// White color
51    pub const WHITE: Self = Self::new(255, 255, 255, 255);
52    /// Gray color
53    pub const GRAY: Self = Self::new(128, 128, 128, 255);
54    /// Orange color
55    pub const ORANGE: Self = Self::new(255, 165, 0, 255);
56}
57
58/// A debug line segment.
59///
60/// Marked `#[non_exhaustive]` so future rendering hint fields (line
61/// width, dash pattern, etc.) can be added without a breaking API change.
62#[derive(Clone, Copy, Debug, PartialEq, Eq)]
63#[non_exhaustive]
64pub struct DebugLine {
65    /// Start point
66    pub start: Vec3Fix,
67    /// End point
68    pub end: Vec3Fix,
69    /// Color
70    pub color: DebugColor,
71}
72
73impl DebugLine {
74    /// Construct a line segment (the struct is `#[non_exhaustive]`, so this
75    /// is the only way to build one outside the crate; 1.2.0).
76    #[must_use]
77    pub const fn new(start: Vec3Fix, end: Vec3Fix, color: DebugColor) -> Self {
78        Self { start, end, color }
79    }
80}
81
82/// A debug point.
83///
84/// Marked `#[non_exhaustive]` so future rendering hint fields can be
85/// added without a breaking API change.
86#[derive(Clone, Copy, Debug, PartialEq, Eq)]
87#[non_exhaustive]
88pub struct DebugPoint {
89    /// Position
90    pub position: Vec3Fix,
91    /// Color
92    pub color: DebugColor,
93    /// Size (for rendering)
94    pub size: Fix128,
95}
96
97impl DebugPoint {
98    /// Construct a point (the struct is `#[non_exhaustive]`, so this is the
99    /// only way to build one outside the crate; 1.2.0).
100    #[must_use]
101    pub const fn new(position: Vec3Fix, color: DebugColor, size: Fix128) -> Self {
102        Self {
103            position,
104            color,
105            size,
106        }
107    }
108}
109
110/// What to draw in debug mode
111#[derive(Clone, Copy, Debug, PartialEq, Eq)]
112#[allow(clippy::struct_excessive_bools)]
113pub struct DebugDrawFlags {
114    /// Draw body AABBs: the box the broad-phase tests for each body (a cube
115    /// centred on the body position with half-extent equal to its collision
116    /// radius), in the body color. Bodies without a collision radius take part
117    /// in no collision and draw no box.
118    pub draw_aabbs: bool,
119    /// Draw body centers of mass
120    pub draw_centers: bool,
121    /// Draw velocity vectors
122    pub draw_velocities: bool,
123    /// Draw contact points
124    pub draw_contacts: bool,
125    /// Draw contact normals
126    pub draw_contact_normals: bool,
127    /// Draw joint connections: a cyan line between the two bodies of every
128    /// distance constraint and every joint of the world's joint list.
129    pub draw_joints: bool,
130    /// Draw BVH nodes: the boxes of the bounding volume hierarchy the default
131    /// broad-phase builds from the body boxes of `draw_aabbs` in the current
132    /// state, in magenta (node boxes are kept on the integer lattice, so they
133    /// are the bodies' boxes rounded outward to whole units).
134    pub draw_bvh: bool,
135    /// Draw body axes (local coordinate frame)
136    pub draw_axes: bool,
137}
138
139impl Default for DebugDrawFlags {
140    fn default() -> Self {
141        Self {
142            draw_aabbs: true,
143            draw_centers: true,
144            draw_velocities: false,
145            draw_contacts: true,
146            draw_contact_normals: true,
147            draw_joints: true,
148            draw_bvh: false,
149            draw_axes: false,
150        }
151    }
152}
153
154/// Collected debug geometry for a frame.
155///
156/// Marked `#[non_exhaustive]` so additional geometry containers
157/// (triangles, meshes, labels) can be added post-v1.0 without a
158/// breaking API change.
159#[derive(Clone, Debug, Default)]
160#[non_exhaustive]
161pub struct DebugDrawData {
162    /// Line segments to draw
163    pub lines: Vec<DebugLine>,
164    /// Points to draw
165    pub points: Vec<DebugPoint>,
166}
167
168impl DebugDrawData {
169    /// Create empty draw data
170    #[must_use]
171    pub const fn new() -> Self {
172        Self {
173            lines: Vec::new(),
174            points: Vec::new(),
175        }
176    }
177
178    /// Clear all geometry
179    pub fn clear(&mut self) {
180        self.lines.clear();
181        self.points.clear();
182    }
183
184    /// Add a line
185    #[inline]
186    pub fn line(&mut self, start: Vec3Fix, end: Vec3Fix, color: DebugColor) {
187        self.lines.push(DebugLine::new(start, end, color));
188    }
189
190    /// Add a point
191    #[inline]
192    pub fn point(&mut self, position: Vec3Fix, color: DebugColor, size: Fix128) {
193        self.points.push(DebugPoint::new(position, color, size));
194    }
195
196    /// Draw an AABB wireframe (12 edges)
197    pub fn aabb(&mut self, aabb: &AABB, color: DebugColor) {
198        let min = aabb.min;
199        let max = aabb.max;
200
201        let corners = [
202            Vec3Fix::new(min.x, min.y, min.z),
203            Vec3Fix::new(max.x, min.y, min.z),
204            Vec3Fix::new(max.x, max.y, min.z),
205            Vec3Fix::new(min.x, max.y, min.z),
206            Vec3Fix::new(min.x, min.y, max.z),
207            Vec3Fix::new(max.x, min.y, max.z),
208            Vec3Fix::new(max.x, max.y, max.z),
209            Vec3Fix::new(min.x, max.y, max.z),
210        ];
211
212        // Bottom face
213        self.line(corners[0], corners[1], color);
214        self.line(corners[1], corners[2], color);
215        self.line(corners[2], corners[3], color);
216        self.line(corners[3], corners[0], color);
217        // Top face
218        self.line(corners[4], corners[5], color);
219        self.line(corners[5], corners[6], color);
220        self.line(corners[6], corners[7], color);
221        self.line(corners[7], corners[4], color);
222        // Verticals
223        self.line(corners[0], corners[4], color);
224        self.line(corners[1], corners[5], color);
225        self.line(corners[2], corners[6], color);
226        self.line(corners[3], corners[7], color);
227    }
228
229    /// Draw a sphere wireframe (3 rings: XY, XZ, YZ)
230    pub fn sphere(&mut self, center: Vec3Fix, radius: Fix128, color: DebugColor) {
231        let segments = 16;
232        self.draw_circle(
233            center,
234            Vec3Fix::UNIT_X,
235            Vec3Fix::UNIT_Y,
236            radius,
237            segments,
238            color,
239        );
240        self.draw_circle(
241            center,
242            Vec3Fix::UNIT_X,
243            Vec3Fix::UNIT_Z,
244            radius,
245            segments,
246            color,
247        );
248        self.draw_circle(
249            center,
250            Vec3Fix::UNIT_Y,
251            Vec3Fix::UNIT_Z,
252            radius,
253            segments,
254            color,
255        );
256    }
257
258    /// Draw a circle (ring)
259    fn draw_circle(
260        &mut self,
261        center: Vec3Fix,
262        axis_a: Vec3Fix,
263        axis_b: Vec3Fix,
264        radius: Fix128,
265        segments: usize,
266        color: DebugColor,
267    ) {
268        let mut prev = center + axis_a * radius;
269        for i in 1..=segments {
270            let angle = Fix128::TWO_PI * Fix128::from_ratio(i as i64, segments as i64);
271            let c = angle.cos();
272            let s = angle.sin();
273            let point = center + axis_a * (radius * c) + axis_b * (radius * s);
274            self.line(prev, point, color);
275            prev = point;
276        }
277    }
278
279    /// Draw an arrow: the shaft, an arrowhead of two barbs, and a point marker
280    /// at the tip.
281    ///
282    /// The head is `0.2 * |end - start|` long: each barb runs from `end` back to
283    /// `head_point ± side * head_len / 2`, where `head_point` lies `head_len`
284    /// before `end` on the shaft and `side` is a unit vector perpendicular to
285    /// the shaft (`dir × Y`, or `dir × X` when the shaft is within about
286    /// 1e-3 rad of Y, i.e. `|unit × Y| < 1e-3`). A zero-length arrow records
287    /// the shaft only.
288    pub fn arrow(&mut self, start: Vec3Fix, end: Vec3Fix, color: DebugColor) {
289        self.line(start, end, color);
290
291        let dir = end - start;
292        let len = dir.length();
293        if len.is_zero() {
294            return;
295        }
296
297        let head_len = len * Fix128::from_ratio(2, 10);
298        let unit = dir.normalize();
299        let head_point = end - unit * head_len;
300        let mut side = unit.cross(Vec3Fix::UNIT_Y);
301        if side.length_squared() < Fix128::from_ratio(1, 1_000_000) {
302            side = unit.cross(Vec3Fix::UNIT_X);
303        }
304        let half_width = side.normalize() * head_len.half();
305        self.line(end, head_point + half_width, color);
306        self.line(end, head_point - half_width, color);
307        self.point(end, color, head_len);
308    }
309
310    /// Draw local coordinate axes at a position
311    pub fn axes(&mut self, position: Vec3Fix, rotation: QuatFix, scale: Fix128) {
312        let x = rotation.rotate_vec(Vec3Fix::UNIT_X) * scale;
313        let y = rotation.rotate_vec(Vec3Fix::UNIT_Y) * scale;
314        let z = rotation.rotate_vec(Vec3Fix::UNIT_Z) * scale;
315
316        self.arrow(position, position + x, DebugColor::RED);
317        self.arrow(position, position + y, DebugColor::GREEN);
318        self.arrow(position, position + z, DebugColor::BLUE);
319    }
320
321    /// Total primitive count
322    #[must_use]
323    pub fn primitive_count(&self) -> usize {
324        self.lines.len() + self.points.len()
325    }
326}
327
328/// Draw debug visualization for a physics world
329pub fn debug_draw_world(
330    world: &crate::solver::PhysicsWorld,
331    flags: &DebugDrawFlags,
332    data: &mut DebugDrawData,
333) {
334    data.clear();
335
336    for (i, body) in world.bodies.iter().enumerate() {
337        let is_static = body.is_static();
338        let body_color = if is_static {
339            DebugColor::GRAY
340        } else {
341            DebugColor::GREEN
342        };
343
344        if flags.draw_aabbs {
345            if let Some((radius, _)) = world.ray_geometry(i) {
346                let half = Vec3Fix::new(radius, radius, radius);
347                data.aabb(&AABB::from_center_half(body.position, half), body_color);
348            }
349        }
350
351        if flags.draw_centers {
352            data.point(body.position, body_color, Fix128::from_ratio(1, 10));
353        }
354
355        if flags.draw_velocities && !is_static {
356            let vel_end = body.position + body.velocity;
357            data.arrow(body.position, vel_end, DebugColor::YELLOW);
358        }
359
360        if flags.draw_axes {
361            data.axes(body.position, body.rotation, Fix128::ONE);
362        }
363    }
364
365    if flags.draw_contacts {
366        for contact in &world.contact_constraints {
367            let c = &contact.contact;
368            data.point(c.point_a, DebugColor::RED, Fix128::from_ratio(5, 100));
369            data.point(c.point_b, DebugColor::BLUE, Fix128::from_ratio(5, 100));
370
371            if flags.draw_contact_normals {
372                data.arrow(c.point_a, c.point_a + c.normal * c.depth, DebugColor::RED);
373            }
374        }
375    }
376
377    if flags.draw_joints {
378        // Draw joint connections as colored lines
379        let position = |i: usize| world.bodies.get(i).map(|b| b.position);
380        for constraint in &world.distance_constraints {
381            if let (Some(a), Some(b)) = (position(constraint.body_a), position(constraint.body_b)) {
382                data.line(a, b, DebugColor::CYAN);
383            }
384        }
385        for joint in &world.joints {
386            let (ia, ib) = joint.bodies();
387            if let (Some(a), Some(b)) = (position(ia), position(ib)) {
388                data.line(a, b, DebugColor::CYAN);
389            }
390        }
391    }
392
393    if flags.draw_bvh {
394        // the hierarchy the default broad-phase builds from the body boxes
395        let primitives: Vec<crate::bvh::BvhPrimitive> = world
396            .bodies
397            .iter()
398            .enumerate()
399            .filter_map(|(i, body)| {
400                let (radius, _) = world.ray_geometry(i)?;
401                let half = Vec3Fix::new(radius, radius, radius);
402                Some(crate::bvh::BvhPrimitive {
403                    aabb: AABB::from_center_half(body.position, half),
404                    index: i as u32,
405                    morton: 0,
406                })
407            })
408            .collect();
409        if !primitives.is_empty() {
410            let bvh = crate::bvh::LinearBvh::build(primitives);
411            let corner =
412                |c: [i32; 3]| Vec3Fix::from_int(i64::from(c[0]), i64::from(c[1]), i64::from(c[2]));
413            for node in &bvh.nodes {
414                let aabb = AABB::new(corner(node.aabb_min), corner(node.aabb_max));
415                data.aabb(&aabb, DebugColor::MAGENTA);
416            }
417        }
418    }
419}
420
421impl crate::solver::PhysicsWorld {
422    /// Draw this world into `data` ([`debug_draw_world`]); `data` is cleared first.
423    pub fn debug_draw(&self, flags: &DebugDrawFlags, data: &mut DebugDrawData) {
424        debug_draw_world(self, flags, data);
425    }
426}
427
428#[cfg(test)]
429mod tests {
430    use super::*;
431    #[cfg(not(feature = "std"))]
432    use alloc::vec;
433
434    #[test]
435    fn test_debug_draw_data() {
436        let mut data = DebugDrawData::new();
437        data.line(Vec3Fix::ZERO, Vec3Fix::UNIT_X, DebugColor::RED);
438        data.point(Vec3Fix::ZERO, DebugColor::GREEN, Fix128::ONE);
439
440        assert_eq!(data.lines.len(), 1);
441        assert_eq!(data.points.len(), 1);
442        assert_eq!(data.primitive_count(), 2);
443
444        data.clear();
445        assert_eq!(data.primitive_count(), 0);
446    }
447
448    #[test]
449    fn test_debug_aabb() {
450        let mut data = DebugDrawData::new();
451        let aabb = AABB::new(Vec3Fix::ZERO, Vec3Fix::from_int(1, 1, 1));
452        data.aabb(&aabb, DebugColor::WHITE);
453        assert_eq!(data.lines.len(), 12); // 12 edges of a box
454    }
455
456    #[test]
457    fn test_debug_sphere() {
458        let mut data = DebugDrawData::new();
459        data.sphere(Vec3Fix::ZERO, Fix128::ONE, DebugColor::GREEN);
460        // 3 rings × 16 segments = 48 lines
461        assert_eq!(data.lines.len(), 48);
462    }
463
464    #[test]
465    fn test_debug_axes() {
466        let mut data = DebugDrawData::new();
467        data.axes(Vec3Fix::ZERO, QuatFix::IDENTITY, Fix128::ONE);
468        // 3 arrows = 3 x (shaft + two barbs) lines + 3 tip points
469        assert_eq!(data.lines.len(), 9);
470        assert_eq!(data.points.len(), 3);
471    }
472
473    #[test]
474    fn test_debug_flags_default() {
475        let flags = DebugDrawFlags::default();
476        assert!(flags.draw_aabbs);
477        assert!(flags.draw_contacts);
478        assert!(!flags.draw_bvh);
479    }
480
481    #[test]
482    fn test_debug_color() {
483        assert_eq!(DebugColor::RED, DebugColor::new(255, 50, 50, 255));
484    }
485
486    #[test]
487    fn debug_line_and_point_constructors_match_add_methods() {
488        let mut data = DebugDrawData::default();
489        data.line(Vec3Fix::ZERO, Vec3Fix::from_int(1, 0, 0), DebugColor::RED);
490        data.point(
491            Vec3Fix::from_int(0, 2, 0),
492            DebugColor::GREEN,
493            Fix128::from_ratio(1, 2),
494        );
495        assert_eq!(
496            data.lines[0],
497            DebugLine::new(Vec3Fix::ZERO, Vec3Fix::from_int(1, 0, 0), DebugColor::RED)
498        );
499        assert_eq!(
500            data.points[0],
501            DebugPoint::new(
502                Vec3Fix::from_int(0, 2, 0),
503                DebugColor::GREEN,
504                Fix128::from_ratio(1, 2)
505            )
506        );
507    }
508
509    fn only(set: impl Fn(&mut DebugDrawFlags)) -> DebugDrawFlags {
510        let mut flags = DebugDrawFlags {
511            draw_aabbs: false,
512            draw_centers: false,
513            draw_velocities: false,
514            draw_contacts: false,
515            draw_contact_normals: false,
516            draw_joints: false,
517            draw_bvh: false,
518            draw_axes: false,
519        };
520        set(&mut flags);
521        flags
522    }
523
524    /// Bodies 0 and 1 carry a collision radius of 1/4 at (1/2, 1/2, 1/2) and
525    /// (5/2, 1/2, 1/2); body 2 has none. A distance constraint joins 0–2, a
526    /// second one names a body that does not exist, and a ball joint joins 1–2.
527    fn joint_scene() -> crate::solver::PhysicsWorld {
528        use crate::joint::{BallJoint, Joint};
529        use crate::solver::{DistanceConstraint, PhysicsWorld, RigidBody, SolverConfig};
530        let mut world = PhysicsWorld::new(SolverConfig::default());
531        let half = Fix128::from_ratio(1, 2);
532        let quarter = Fix128::from_ratio(1, 4);
533        let a = world.add_body_with_radius(
534            RigidBody::new_static(Vec3Fix::new(half, half, half)),
535            quarter,
536        );
537        let b = world.add_body_with_radius(
538            RigidBody::new_dynamic(
539                Vec3Fix::new(Fix128::from_ratio(5, 2), half, half),
540                Fix128::ONE,
541            ),
542            quarter,
543        );
544        let c = world.add_body(RigidBody::new_dynamic(
545            Vec3Fix::from_int(0, 5, 0),
546            Fix128::ONE,
547        ));
548        let link =
549            |i, j| DistanceConstraint::new(i, j, Vec3Fix::ZERO, Vec3Fix::ZERO, Fix128::from_int(5));
550        world.add_distance_constraint(link(a, c));
551        world.add_distance_constraint(link(a, 99));
552        world.add_joint(Joint::Ball(BallJoint::new(
553            b,
554            c,
555            Vec3Fix::ZERO,
556            Vec3Fix::ZERO,
557        )));
558        world
559    }
560
561    #[test]
562    fn draw_joints_draws_one_cyan_line_per_resolvable_pair() {
563        let world = joint_scene();
564        let mut data = DebugDrawData::new();
565        world.debug_draw(&only(|f| f.draw_joints = true), &mut data);
566        // the constraint naming body 99 is skipped: 2 lines, no points
567        let p = |i: usize| world.bodies[i].position;
568        assert_eq!(
569            data.lines,
570            vec![
571                DebugLine::new(p(0), p(2), DebugColor::CYAN),
572                DebugLine::new(p(1), p(2), DebugColor::CYAN),
573            ]
574        );
575        assert!(data.points.is_empty());
576    }
577
578    #[test]
579    fn draw_bvh_draws_the_single_leaf_rounded_out_to_the_lattice() {
580        let world = joint_scene();
581        let mut data = DebugDrawData::new();
582        world.debug_draw(&only(|f| f.draw_bvh = true), &mut data);
583        // two boxes (≤ 4 primitives) make one leaf: their union
584        // [1/4, 11/4] × [1/4, 3/4]² rounded outward is [0, 3] × [0, 1]²
585        assert_eq!(data.lines.len(), 12);
586        assert!(data.lines.iter().all(|l| l.color == DebugColor::MAGENTA));
587        assert_eq!(data.lines[0].start, Vec3Fix::from_int(0, 0, 0));
588        assert_eq!(data.lines[0].end, Vec3Fix::from_int(3, 0, 0));
589        assert_eq!(data.lines[6].start, Vec3Fix::from_int(3, 1, 1));
590        assert_eq!(data.lines[6].end, Vec3Fix::from_int(0, 1, 1));
591
592        // no body with a radius: no hierarchy, nothing drawn
593        let mut empty = crate::solver::PhysicsWorld::new(crate::solver::SolverConfig::default());
594        empty.add_body(crate::solver::RigidBody::new_static(Vec3Fix::ZERO));
595        empty.debug_draw(&only(|f| f.draw_bvh = true), &mut data);
596        assert_eq!(data.primitive_count(), 0);
597    }
598
599    #[test]
600    fn draw_aabbs_centers_velocities_and_axes_counts() {
601        let mut world = joint_scene();
602        world.bodies[1].velocity = Vec3Fix::from_int(1, 0, 0);
603        let mut data = DebugDrawData::new();
604        // one 12-edge box per body with a radius (2), in the body colour
605        world.debug_draw(&only(|f| f.draw_aabbs = true), &mut data);
606        assert_eq!(data.lines.len(), 24);
607        assert_eq!(data.lines[0].color, DebugColor::GRAY);
608        assert_eq!(data.lines[12].color, DebugColor::GREEN);
609        // a centre point per body (3)
610        world.debug_draw(&only(|f| f.draw_centers = true), &mut data);
611        assert_eq!((data.lines.len(), data.points.len()), (0, 3));
612        // velocity arrows for the 2 dynamic bodies: body 1 has a full arrow
613        // (3 lines + tip point), body 2 has zero velocity (shaft only)
614        world.debug_draw(&only(|f| f.draw_velocities = true), &mut data);
615        assert_eq!((data.lines.len(), data.points.len()), (4, 1));
616        // axes: 3 arrows per body
617        world.debug_draw(&only(|f| f.draw_axes = true), &mut data);
618        assert_eq!((data.lines.len(), data.points.len()), (27, 9));
619    }
620
621    #[test]
622    fn draw_contacts_adds_two_points_and_a_normal_arrow_per_contact() {
623        use crate::collider::Contact;
624        use crate::solver::ContactConstraint;
625        let mut world = joint_scene();
626        world.add_contact(ContactConstraint::new(
627            0,
628            1,
629            Contact {
630                depth: Fix128::from_ratio(1, 10),
631                normal: Vec3Fix::UNIT_X,
632                point_a: Vec3Fix::ZERO,
633                point_b: Vec3Fix::UNIT_X,
634            },
635        ));
636        let mut data = DebugDrawData::new();
637        world.debug_draw(&only(|f| f.draw_contacts = true), &mut data);
638        assert_eq!((data.lines.len(), data.points.len()), (0, 2));
639        let flags = only(|f| {
640            f.draw_contacts = true;
641            f.draw_contact_normals = true;
642        });
643        world.debug_draw(&flags, &mut data);
644        assert_eq!((data.lines.len(), data.points.len()), (3, 3));
645    }
646}
alice_physics/debug_render.rs

alice_physics/
debug_render.rs
