box2d_rust/joint/
draw.rs

1// Debug draw dispatch for joints (b2DrawJoint in joint.c).
2//
3// SPDX-FileCopyrightText: 2023 Erin Catto
4// SPDX-License-Identifier: MIT
5
6use crate::body::get_body_transform_quick;
7use crate::core::NULL_INDEX;
8use crate::debug_draw::{DebugDraw, HexColor};
9use crate::joint::solve::{get_joint_constraint_force, get_joint_constraint_torque};
10use crate::joint::{get_joint_sim_ref, Joint, JointType};
11use crate::math_functions::{lerp_position, max_float, mul_sv, offset_pos, transform_world_point};
12use crate::solver_set::DISABLED_SET;
13use crate::world::World;
14
15/// (b2DrawJoint)
16pub fn draw_joint(draw: &mut dyn DebugDraw, world: &World, joint: &Joint) {
17    let body_a = &world.bodies[joint.edges[0].body_id as usize];
18    let body_b = &world.bodies[joint.edges[1].body_id as usize];
19    if body_a.set_index == DISABLED_SET || body_b.set_index == DISABLED_SET {
20        return;
21    }
22
23    let joint_sim = get_joint_sim_ref(world, joint.joint_id);
24
25    let xf_a = get_body_transform_quick(world, body_a);
26    let xf_b = get_body_transform_quick(world, body_b);
27
28    let p_a = transform_world_point(xf_a, joint_sim.local_frame_a.p);
29    let p_b = transform_world_point(xf_b, joint_sim.local_frame_b.p);
30
31    let scale = max_float(0.0001, draw.joint_scale() * joint.draw_scale);
32
33    match joint.type_ {
34        JointType::Distance => {
35            crate::distance_joint::draw_distance_joint(draw, joint_sim, xf_a, xf_b);
36        }
37
38        JointType::Filter => {
39            draw.draw_line(p_a, p_b, HexColor::GOLD);
40        }
41
42        JointType::Motor => {
43            draw.draw_point(p_a, 8.0, HexColor::YELLOW_GREEN);
44            draw.draw_point(p_b, 8.0, HexColor::PLUM);
45            draw.draw_line(p_a, p_b, HexColor::LIGHT_GRAY);
46        }
47
48        JointType::Prismatic => {
49            crate::prismatic_joint::draw_prismatic_joint(draw, joint_sim, xf_a, xf_b, scale);
50        }
51
52        JointType::Revolute => {
53            crate::revolute_joint::draw_revolute_joint(draw, joint_sim, xf_a, xf_b, scale);
54        }
55
56        JointType::Weld => {
57            crate::weld_joint::draw_weld_joint(draw, joint_sim, xf_a, xf_b, scale);
58        }
59
60        JointType::Wheel => {
61            crate::wheel_joint::draw_wheel_joint(draw, joint_sim, xf_a, xf_b, scale);
62        }
63    }
64
65    // The C switch has a default arm sketching three b2_colorDarkSeaGreen
66    // lines; JointType is exhaustive in Rust so it is unreachable here.
67
68    if draw.draw_graph_colors() {
69        let color_index = joint.color_index;
70        if color_index != NULL_INDEX {
71            let p = lerp_position(p_a, p_b, 0.5);
72            draw.draw_point(
73                p,
74                5.0,
75                crate::constraint_graph::get_graph_color(color_index),
76            );
77        }
78    }
79
80    if draw.draw_joint_extras() {
81        let joint_index = joint.joint_id;
82        let force = get_joint_constraint_force(world, joint_index);
83        let torque = get_joint_constraint_torque(world, joint_index);
84        let p = lerp_position(p_a, p_b, 0.5);
85
86        draw.draw_line(p, offset_pos(p, mul_sv(0.001, force)), HexColor::AZURE);
87
88        let buffer = format!("f = [{}, {}], t = {}", force.x, force.y, torque);
89        draw.draw_string(p, &buffer, HexColor::AZURE);
90    }
91}
box2d_rust/joint/draw.rs

box2d_rust/joint/
draw.rs
