alice-physics 3.0.1

//! Debug Visualization API
//!
//! Abstract debug rendering interface for visualizing physics state.
//! Outputs wireframe geometry (lines, points) that can be rendered
//! by any graphics backend.
//!
//! # Usage
//!
//! Implement `DebugRenderer` trait for your graphics backend, then call
//! `debug_draw_world()` each frame.

use crate::collider::AABB;
use crate::math::{Fix128, QuatFix, Vec3Fix};

#[cfg(not(feature = "std"))]
use alloc::vec::Vec;

/// RGBA color for debug rendering (0-255 per channel)
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct DebugColor {
    /// Red channel
    pub r: u8,
    /// Green channel
    pub g: u8,
    /// Blue channel
    pub b: u8,
    /// Alpha channel
    pub a: u8,
}

impl DebugColor {
    /// Create a new color
    #[must_use]
    pub const fn new(r: u8, g: u8, b: u8, a: u8) -> Self {
        Self { r, g, b, a }
    }

    /// Predefined colors
    pub const RED: Self = Self::new(255, 50, 50, 255);
    /// Green color
    pub const GREEN: Self = Self::new(50, 255, 50, 255);
    /// Blue color
    pub const BLUE: Self = Self::new(50, 50, 255, 255);
    /// Yellow color
    pub const YELLOW: Self = Self::new(255, 255, 50, 255);
    /// Cyan color
    pub const CYAN: Self = Self::new(50, 255, 255, 255);
    /// Magenta color
    pub const MAGENTA: Self = Self::new(255, 50, 255, 255);
    /// White color
    pub const WHITE: Self = Self::new(255, 255, 255, 255);
    /// Gray color
    pub const GRAY: Self = Self::new(128, 128, 128, 255);
    /// Orange color
    pub const ORANGE: Self = Self::new(255, 165, 0, 255);
}

/// A debug line segment.
///
/// Marked `#[non_exhaustive]` so future rendering hint fields (line
/// width, dash pattern, etc.) can be added without a breaking API change.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub struct DebugLine {
    /// Start point
    pub start: Vec3Fix,
    /// End point
    pub end: Vec3Fix,
    /// Color
    pub color: DebugColor,
}

impl DebugLine {
    /// Construct a line segment (the struct is `#[non_exhaustive]`, so this
    /// is the only way to build one outside the crate; 1.2.0).
    #[must_use]
    pub const fn new(start: Vec3Fix, end: Vec3Fix, color: DebugColor) -> Self {
        Self { start, end, color }
    }
}

/// A debug point.
///
/// Marked `#[non_exhaustive]` so future rendering hint fields can be
/// added without a breaking API change.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub struct DebugPoint {
    /// Position
    pub position: Vec3Fix,
    /// Color
    pub color: DebugColor,
    /// Size (for rendering)
    pub size: Fix128,
}

impl DebugPoint {
    /// Construct a point (the struct is `#[non_exhaustive]`, so this is the
    /// only way to build one outside the crate; 1.2.0).
    #[must_use]
    pub const fn new(position: Vec3Fix, color: DebugColor, size: Fix128) -> Self {
        Self {
            position,
            color,
            size,
        }
    }
}

/// What to draw in debug mode
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[allow(clippy::struct_excessive_bools)]
pub struct DebugDrawFlags {
    /// Draw body AABBs: the box the broad-phase tests for each body (a cube
    /// centred on the body position with half-extent equal to its collision
    /// radius), in the body color. Bodies without a collision radius take part
    /// in no collision and draw no box.
    pub draw_aabbs: bool,
    /// Draw body centers of mass
    pub draw_centers: bool,
    /// Draw velocity vectors
    pub draw_velocities: bool,
    /// Draw contact points
    pub draw_contacts: bool,
    /// Draw contact normals
    pub draw_contact_normals: bool,
    /// Draw joint connections: a cyan line between the two bodies of every
    /// distance constraint and every joint of the world's joint list.
    pub draw_joints: bool,
    /// Draw BVH nodes: the boxes of the bounding volume hierarchy the default
    /// broad-phase builds from the body boxes of `draw_aabbs` in the current
    /// state, in magenta (node boxes are kept on the integer lattice, so they
    /// are the bodies' boxes rounded outward to whole units).
    pub draw_bvh: bool,
    /// Draw body axes (local coordinate frame)
    pub draw_axes: bool,
}

impl Default for DebugDrawFlags {
    fn default() -> Self {
        Self {
            draw_aabbs: true,
            draw_centers: true,
            draw_velocities: false,
            draw_contacts: true,
            draw_contact_normals: true,
            draw_joints: true,
            draw_bvh: false,
            draw_axes: false,
        }
    }
}

/// Collected debug geometry for a frame.
///
/// Marked `#[non_exhaustive]` so additional geometry containers
/// (triangles, meshes, labels) can be added post-v1.0 without a
/// breaking API change.
#[derive(Clone, Debug, Default)]
#[non_exhaustive]
pub struct DebugDrawData {
    /// Line segments to draw
    pub lines: Vec<DebugLine>,
    /// Points to draw
    pub points: Vec<DebugPoint>,
}

impl DebugDrawData {
    /// Create empty draw data
    #[must_use]
    pub const fn new() -> Self {
        Self {
            lines: Vec::new(),
            points: Vec::new(),
        }
    }

    /// Clear all geometry
    pub fn clear(&mut self) {
        self.lines.clear();
        self.points.clear();
    }

    /// Add a line
    #[inline]
    pub fn line(&mut self, start: Vec3Fix, end: Vec3Fix, color: DebugColor) {
        self.lines.push(DebugLine::new(start, end, color));
    }

    /// Add a point
    #[inline]
    pub fn point(&mut self, position: Vec3Fix, color: DebugColor, size: Fix128) {
        self.points.push(DebugPoint::new(position, color, size));
    }

    /// Draw an AABB wireframe (12 edges)
    pub fn aabb(&mut self, aabb: &AABB, color: DebugColor) {
        let min = aabb.min;
        let max = aabb.max;

        let corners = [
            Vec3Fix::new(min.x, min.y, min.z),
            Vec3Fix::new(max.x, min.y, min.z),
            Vec3Fix::new(max.x, max.y, min.z),
            Vec3Fix::new(min.x, max.y, min.z),
            Vec3Fix::new(min.x, min.y, max.z),
            Vec3Fix::new(max.x, min.y, max.z),
            Vec3Fix::new(max.x, max.y, max.z),
            Vec3Fix::new(min.x, max.y, max.z),
        ];

        // Bottom face
        self.line(corners[0], corners[1], color);
        self.line(corners[1], corners[2], color);
        self.line(corners[2], corners[3], color);
        self.line(corners[3], corners[0], color);
        // Top face
        self.line(corners[4], corners[5], color);
        self.line(corners[5], corners[6], color);
        self.line(corners[6], corners[7], color);
        self.line(corners[7], corners[4], color);
        // Verticals
        self.line(corners[0], corners[4], color);
        self.line(corners[1], corners[5], color);
        self.line(corners[2], corners[6], color);
        self.line(corners[3], corners[7], color);
    }

    /// Draw a sphere wireframe (3 rings: XY, XZ, YZ)
    pub fn sphere(&mut self, center: Vec3Fix, radius: Fix128, color: DebugColor) {
        let segments = 16;
        self.draw_circle(
            center,
            Vec3Fix::UNIT_X,
            Vec3Fix::UNIT_Y,
            radius,
            segments,
            color,
        );
        self.draw_circle(
            center,
            Vec3Fix::UNIT_X,
            Vec3Fix::UNIT_Z,
            radius,
            segments,
            color,
        );
        self.draw_circle(
            center,
            Vec3Fix::UNIT_Y,
            Vec3Fix::UNIT_Z,
            radius,
            segments,
            color,
        );
    }

    /// Draw a circle (ring)
    fn draw_circle(
        &mut self,
        center: Vec3Fix,
        axis_a: Vec3Fix,
        axis_b: Vec3Fix,
        radius: Fix128,
        segments: usize,
        color: DebugColor,
    ) {
        let mut prev = center + axis_a * radius;
        for i in 1..=segments {
            let angle = Fix128::TWO_PI * Fix128::from_ratio(i as i64, segments as i64);
            let c = angle.cos();
            let s = angle.sin();
            let point = center + axis_a * (radius * c) + axis_b * (radius * s);
            self.line(prev, point, color);
            prev = point;
        }
    }

    /// Draw an arrow: the shaft, an arrowhead of two barbs, and a point marker
    /// at the tip.
    ///
    /// The head is `0.2 * |end - start|` long: each barb runs from `end` back to
    /// `head_point ± side * head_len / 2`, where `head_point` lies `head_len`
    /// before `end` on the shaft and `side` is a unit vector perpendicular to
    /// the shaft (`dir × Y`, or `dir × X` when the shaft is within about
    /// 1e-3 rad of Y, i.e. `|unit × Y| < 1e-3`). A zero-length arrow records
    /// the shaft only.
    pub fn arrow(&mut self, start: Vec3Fix, end: Vec3Fix, color: DebugColor) {
        self.line(start, end, color);

        let dir = end - start;
        let len = dir.length();
        if len.is_zero() {
            return;
        }

        let head_len = len * Fix128::from_ratio(2, 10);
        let unit = dir.normalize();
        let head_point = end - unit * head_len;
        let mut side = unit.cross(Vec3Fix::UNIT_Y);
        if side.length_squared() < Fix128::from_ratio(1, 1_000_000) {
            side = unit.cross(Vec3Fix::UNIT_X);
        }
        let half_width = side.normalize() * head_len.half();
        self.line(end, head_point + half_width, color);
        self.line(end, head_point - half_width, color);
        self.point(end, color, head_len);
    }

    /// Draw local coordinate axes at a position
    pub fn axes(&mut self, position: Vec3Fix, rotation: QuatFix, scale: Fix128) {
        let x = rotation.rotate_vec(Vec3Fix::UNIT_X) * scale;
        let y = rotation.rotate_vec(Vec3Fix::UNIT_Y) * scale;
        let z = rotation.rotate_vec(Vec3Fix::UNIT_Z) * scale;

        self.arrow(position, position + x, DebugColor::RED);
        self.arrow(position, position + y, DebugColor::GREEN);
        self.arrow(position, position + z, DebugColor::BLUE);
    }

    /// Total primitive count
    #[must_use]
    pub fn primitive_count(&self) -> usize {
        self.lines.len() + self.points.len()
    }
}

/// Draw debug visualization for a physics world
pub fn debug_draw_world(
    world: &crate::solver::PhysicsWorld,
    flags: &DebugDrawFlags,
    data: &mut DebugDrawData,
) {
    data.clear();

    for (i, body) in world.bodies.iter().enumerate() {
        let is_static = body.is_static();
        let body_color = if is_static {
            DebugColor::GRAY
        } else {
            DebugColor::GREEN
        };

        if flags.draw_aabbs {
            if let Some((radius, _)) = world.ray_geometry(i) {
                let half = Vec3Fix::new(radius, radius, radius);
                data.aabb(&AABB::from_center_half(body.position, half), body_color);
            }
        }

        if flags.draw_centers {
            data.point(body.position, body_color, Fix128::from_ratio(1, 10));
        }

        if flags.draw_velocities && !is_static {
            let vel_end = body.position + body.velocity;
            data.arrow(body.position, vel_end, DebugColor::YELLOW);
        }

        if flags.draw_axes {
            data.axes(body.position, body.rotation, Fix128::ONE);
        }
    }

    if flags.draw_contacts {
        for contact in &world.contact_constraints {
            let c = &contact.contact;
            data.point(c.point_a, DebugColor::RED, Fix128::from_ratio(5, 100));
            data.point(c.point_b, DebugColor::BLUE, Fix128::from_ratio(5, 100));

            if flags.draw_contact_normals {
                data.arrow(c.point_a, c.point_a + c.normal * c.depth, DebugColor::RED);
            }
        }
    }

    if flags.draw_joints {
        // Draw joint connections as colored lines
        let position = |i: usize| world.bodies.get(i).map(|b| b.position);
        for constraint in &world.distance_constraints {
            if let (Some(a), Some(b)) = (position(constraint.body_a), position(constraint.body_b)) {
                data.line(a, b, DebugColor::CYAN);
            }
        }
        for joint in &world.joints {
            let (ia, ib) = joint.bodies();
            if let (Some(a), Some(b)) = (position(ia), position(ib)) {
                data.line(a, b, DebugColor::CYAN);
            }
        }
    }

    if flags.draw_bvh {
        // the hierarchy the default broad-phase builds from the body boxes
        let primitives: Vec<crate::bvh::BvhPrimitive> = world
            .bodies
            .iter()
            .enumerate()
            .filter_map(|(i, body)| {
                let (radius, _) = world.ray_geometry(i)?;
                let half = Vec3Fix::new(radius, radius, radius);
                Some(crate::bvh::BvhPrimitive {
                    aabb: AABB::from_center_half(body.position, half),
                    index: i as u32,
                    morton: 0,
                })
            })
            .collect();
        if !primitives.is_empty() {
            let bvh = crate::bvh::LinearBvh::build(primitives);
            let corner =
                |c: [i32; 3]| Vec3Fix::from_int(i64::from(c[0]), i64::from(c[1]), i64::from(c[2]));
            for node in &bvh.nodes {
                let aabb = AABB::new(corner(node.aabb_min), corner(node.aabb_max));
                data.aabb(&aabb, DebugColor::MAGENTA);
            }
        }
    }
}

impl crate::solver::PhysicsWorld {
    /// Draw this world into `data` ([`debug_draw_world`]); `data` is cleared first.
    pub fn debug_draw(&self, flags: &DebugDrawFlags, data: &mut DebugDrawData) {
        debug_draw_world(self, flags, data);
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    #[cfg(not(feature = "std"))]
    use alloc::vec;

    #[test]
    fn test_debug_draw_data() {
        let mut data = DebugDrawData::new();
        data.line(Vec3Fix::ZERO, Vec3Fix::UNIT_X, DebugColor::RED);
        data.point(Vec3Fix::ZERO, DebugColor::GREEN, Fix128::ONE);

        assert_eq!(data.lines.len(), 1);
        assert_eq!(data.points.len(), 1);
        assert_eq!(data.primitive_count(), 2);

        data.clear();
        assert_eq!(data.primitive_count(), 0);
    }

    #[test]
    fn test_debug_aabb() {
        let mut data = DebugDrawData::new();
        let aabb = AABB::new(Vec3Fix::ZERO, Vec3Fix::from_int(1, 1, 1));
        data.aabb(&aabb, DebugColor::WHITE);
        assert_eq!(data.lines.len(), 12); // 12 edges of a box
    }

    #[test]
    fn test_debug_sphere() {
        let mut data = DebugDrawData::new();
        data.sphere(Vec3Fix::ZERO, Fix128::ONE, DebugColor::GREEN);
        // 3 rings × 16 segments = 48 lines
        assert_eq!(data.lines.len(), 48);
    }

    #[test]
    fn test_debug_axes() {
        let mut data = DebugDrawData::new();
        data.axes(Vec3Fix::ZERO, QuatFix::IDENTITY, Fix128::ONE);
        // 3 arrows = 3 x (shaft + two barbs) lines + 3 tip points
        assert_eq!(data.lines.len(), 9);
        assert_eq!(data.points.len(), 3);
    }

    #[test]
    fn test_debug_flags_default() {
        let flags = DebugDrawFlags::default();
        assert!(flags.draw_aabbs);
        assert!(flags.draw_contacts);
        assert!(!flags.draw_bvh);
    }

    #[test]
    fn test_debug_color() {
        assert_eq!(DebugColor::RED, DebugColor::new(255, 50, 50, 255));
    }

    #[test]
    fn debug_line_and_point_constructors_match_add_methods() {
        let mut data = DebugDrawData::default();
        data.line(Vec3Fix::ZERO, Vec3Fix::from_int(1, 0, 0), DebugColor::RED);
        data.point(
            Vec3Fix::from_int(0, 2, 0),
            DebugColor::GREEN,
            Fix128::from_ratio(1, 2),
        );
        assert_eq!(
            data.lines[0],
            DebugLine::new(Vec3Fix::ZERO, Vec3Fix::from_int(1, 0, 0), DebugColor::RED)
        );
        assert_eq!(
            data.points[0],
            DebugPoint::new(
                Vec3Fix::from_int(0, 2, 0),
                DebugColor::GREEN,
                Fix128::from_ratio(1, 2)
            )
        );
    }

    fn only(set: impl Fn(&mut DebugDrawFlags)) -> DebugDrawFlags {
        let mut flags = DebugDrawFlags {
            draw_aabbs: false,
            draw_centers: false,
            draw_velocities: false,
            draw_contacts: false,
            draw_contact_normals: false,
            draw_joints: false,
            draw_bvh: false,
            draw_axes: false,
        };
        set(&mut flags);
        flags
    }

    /// Bodies 0 and 1 carry a collision radius of 1/4 at (1/2, 1/2, 1/2) and
    /// (5/2, 1/2, 1/2); body 2 has none. A distance constraint joins 0–2, a
    /// second one names a body that does not exist, and a ball joint joins 1–2.
    fn joint_scene() -> crate::solver::PhysicsWorld {
        use crate::joint::{BallJoint, Joint};
        use crate::solver::{DistanceConstraint, PhysicsWorld, RigidBody, SolverConfig};
        let mut world = PhysicsWorld::new(SolverConfig::default());
        let half = Fix128::from_ratio(1, 2);
        let quarter = Fix128::from_ratio(1, 4);
        let a = world.add_body_with_radius(
            RigidBody::new_static(Vec3Fix::new(half, half, half)),
            quarter,
        );
        let b = world.add_body_with_radius(
            RigidBody::new_dynamic(
                Vec3Fix::new(Fix128::from_ratio(5, 2), half, half),
                Fix128::ONE,
            ),
            quarter,
        );
        let c = world.add_body(RigidBody::new_dynamic(
            Vec3Fix::from_int(0, 5, 0),
            Fix128::ONE,
        ));
        let link =
            |i, j| DistanceConstraint::new(i, j, Vec3Fix::ZERO, Vec3Fix::ZERO, Fix128::from_int(5));
        world.add_distance_constraint(link(a, c));
        world.add_distance_constraint(link(a, 99));
        world.add_joint(Joint::Ball(BallJoint::new(
            b,
            c,
            Vec3Fix::ZERO,
            Vec3Fix::ZERO,
        )));
        world
    }

    #[test]
    fn draw_joints_draws_one_cyan_line_per_resolvable_pair() {
        let world = joint_scene();
        let mut data = DebugDrawData::new();
        world.debug_draw(&only(|f| f.draw_joints = true), &mut data);
        // the constraint naming body 99 is skipped: 2 lines, no points
        let p = |i: usize| world.bodies[i].position;
        assert_eq!(
            data.lines,
            vec![
                DebugLine::new(p(0), p(2), DebugColor::CYAN),
                DebugLine::new(p(1), p(2), DebugColor::CYAN),
            ]
        );
        assert!(data.points.is_empty());
    }

    #[test]
    fn draw_bvh_draws_the_single_leaf_rounded_out_to_the_lattice() {
        let world = joint_scene();
        let mut data = DebugDrawData::new();
        world.debug_draw(&only(|f| f.draw_bvh = true), &mut data);
        // two boxes (≤ 4 primitives) make one leaf: their union
        // [1/4, 11/4] × [1/4, 3/4]² rounded outward is [0, 3] × [0, 1]²
        assert_eq!(data.lines.len(), 12);
        assert!(data.lines.iter().all(|l| l.color == DebugColor::MAGENTA));
        assert_eq!(data.lines[0].start, Vec3Fix::from_int(0, 0, 0));
        assert_eq!(data.lines[0].end, Vec3Fix::from_int(3, 0, 0));
        assert_eq!(data.lines[6].start, Vec3Fix::from_int(3, 1, 1));
        assert_eq!(data.lines[6].end, Vec3Fix::from_int(0, 1, 1));

        // no body with a radius: no hierarchy, nothing drawn
        let mut empty = crate::solver::PhysicsWorld::new(crate::solver::SolverConfig::default());
        empty.add_body(crate::solver::RigidBody::new_static(Vec3Fix::ZERO));
        empty.debug_draw(&only(|f| f.draw_bvh = true), &mut data);
        assert_eq!(data.primitive_count(), 0);
    }

    #[test]
    fn draw_aabbs_centers_velocities_and_axes_counts() {
        let mut world = joint_scene();
        world.bodies[1].velocity = Vec3Fix::from_int(1, 0, 0);
        let mut data = DebugDrawData::new();
        // one 12-edge box per body with a radius (2), in the body colour
        world.debug_draw(&only(|f| f.draw_aabbs = true), &mut data);
        assert_eq!(data.lines.len(), 24);
        assert_eq!(data.lines[0].color, DebugColor::GRAY);
        assert_eq!(data.lines[12].color, DebugColor::GREEN);
        // a centre point per body (3)
        world.debug_draw(&only(|f| f.draw_centers = true), &mut data);
        assert_eq!((data.lines.len(), data.points.len()), (0, 3));
        // velocity arrows for the 2 dynamic bodies: body 1 has a full arrow
        // (3 lines + tip point), body 2 has zero velocity (shaft only)
        world.debug_draw(&only(|f| f.draw_velocities = true), &mut data);
        assert_eq!((data.lines.len(), data.points.len()), (4, 1));
        // axes: 3 arrows per body
        world.debug_draw(&only(|f| f.draw_axes = true), &mut data);
        assert_eq!((data.lines.len(), data.points.len()), (27, 9));
    }

    #[test]
    fn draw_contacts_adds_two_points_and_a_normal_arrow_per_contact() {
        use crate::collider::Contact;
        use crate::solver::ContactConstraint;
        let mut world = joint_scene();
        world.add_contact(ContactConstraint::new(
            0,
            1,
            Contact {
                depth: Fix128::from_ratio(1, 10),
                normal: Vec3Fix::UNIT_X,
                point_a: Vec3Fix::ZERO,
                point_b: Vec3Fix::UNIT_X,
            },
        ));
        let mut data = DebugDrawData::new();
        world.debug_draw(&only(|f| f.draw_contacts = true), &mut data);
        assert_eq!((data.lines.len(), data.points.len()), (0, 2));
        let flags = only(|f| {
            f.draw_contacts = true;
            f.draw_contact_normals = true;
        });
        world.debug_draw(&flags, &mut data);
        assert_eq!((data.lines.len(), data.points.len()), (3, 3));
    }
}