box2d-rust 1.0.0

// Port of the joint data model from box2d-cpp-reference/src/joint.h.
// Logic from joint.c and the per-joint .c files lands in the joints bring-up
// commits.
//
// SPDX-FileCopyrightText: 2023 Erin Catto
// SPDX-License-Identifier: MIT

use crate::core::NULL_INDEX;
use crate::math_functions::{Mat22, Transform, Vec2, MAT22_ZERO, TRANSFORM_IDENTITY, VEC2_ZERO};
use crate::solver::Softness;

/// Joint type enumeration. (types.h: b2JointType)
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub enum JointType {
    #[default]
    Distance,
    Filter,
    Motor,
    Prismatic,
    Revolute,
    Weld,
    Wheel,
}

/// A joint edge connects bodies and joints together in a joint graph where
/// each body is a node and each joint is an edge. (b2JointEdge)
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct JointEdge {
    pub body_id: i32,
    pub prev_key: i32,
    pub next_key: i32,
}

impl Default for JointEdge {
    fn default() -> Self {
        JointEdge {
            body_id: NULL_INDEX,
            prev_key: NULL_INDEX,
            next_key: NULL_INDEX,
        }
    }
}

/// Map from JointId to joint data in the solver sets. (b2Joint)
#[derive(Debug, Clone)]
pub struct Joint {
    pub user_data: u64,

    /// index of simulation set stored in World. NULL_INDEX when slot is free.
    pub set_index: i32,

    /// index into the constraint graph color array, may be NULL_INDEX for
    /// sleeping/disabled joints. NULL_INDEX when slot is free.
    pub color_index: i32,

    /// joint index within set or graph color. NULL_INDEX when slot is free.
    pub local_index: i32,

    pub edges: [JointEdge; 2],

    pub joint_id: i32,
    pub island_id: i32,

    /// Index into the island's joints array for O(1) swap-removal.
    /// NULL_INDEX when not in an island.
    pub island_index: i32,

    pub draw_scale: f32,

    pub type_: JointType,

    /// Monotonically advanced when a joint is allocated in this slot.
    pub generation: u16,

    pub collide_connected: bool,
}

impl Default for Joint {
    fn default() -> Self {
        Joint {
            user_data: 0,
            set_index: NULL_INDEX,
            color_index: NULL_INDEX,
            local_index: NULL_INDEX,
            edges: [JointEdge::default(); 2],
            joint_id: NULL_INDEX,
            island_id: NULL_INDEX,
            island_index: NULL_INDEX,
            draw_scale: 1.0,
            type_: JointType::Distance,
            generation: 0,
            collide_connected: false,
        }
    }
}

/// (b2DistanceJoint)
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct DistanceJoint {
    pub length: f32,
    pub hertz: f32,
    pub damping_ratio: f32,
    pub lower_spring_force: f32,
    pub upper_spring_force: f32,
    pub min_length: f32,
    pub max_length: f32,

    pub max_motor_force: f32,
    pub motor_speed: f32,

    pub impulse: f32,
    pub lower_impulse: f32,
    pub upper_impulse: f32,
    pub motor_impulse: f32,

    pub index_a: i32,
    pub index_b: i32,
    pub anchor_a: Vec2,
    pub anchor_b: Vec2,
    pub delta_center: Vec2,
    pub distance_softness: Softness,
    pub axial_mass: f32,

    pub enable_spring: bool,
    pub enable_limit: bool,
    pub enable_motor: bool,
}

/// (b2MotorJoint)
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MotorJoint {
    pub linear_velocity: Vec2,
    pub max_velocity_force: f32,
    pub angular_velocity: f32,
    pub max_velocity_torque: f32,
    pub linear_hertz: f32,
    pub linear_damping_ratio: f32,
    pub max_spring_force: f32,
    pub angular_hertz: f32,
    pub angular_damping_ratio: f32,
    pub max_spring_torque: f32,

    pub linear_velocity_impulse: Vec2,
    pub angular_velocity_impulse: f32,
    pub linear_spring_impulse: Vec2,
    pub angular_spring_impulse: f32,

    pub linear_spring: Softness,
    pub angular_spring: Softness,

    pub index_a: i32,
    pub index_b: i32,
    pub frame_a: Transform,
    pub frame_b: Transform,
    pub delta_center: Vec2,
    pub linear_mass: Mat22,
    pub angular_mass: f32,
}

impl Default for MotorJoint {
    fn default() -> Self {
        MotorJoint {
            linear_velocity: VEC2_ZERO,
            max_velocity_force: 0.0,
            angular_velocity: 0.0,
            max_velocity_torque: 0.0,
            linear_hertz: 0.0,
            linear_damping_ratio: 0.0,
            max_spring_force: 0.0,
            angular_hertz: 0.0,
            angular_damping_ratio: 0.0,
            max_spring_torque: 0.0,
            linear_velocity_impulse: VEC2_ZERO,
            angular_velocity_impulse: 0.0,
            linear_spring_impulse: VEC2_ZERO,
            angular_spring_impulse: 0.0,
            linear_spring: Softness::default(),
            angular_spring: Softness::default(),
            index_a: NULL_INDEX,
            index_b: NULL_INDEX,
            frame_a: TRANSFORM_IDENTITY,
            frame_b: TRANSFORM_IDENTITY,
            delta_center: VEC2_ZERO,
            linear_mass: MAT22_ZERO,
            angular_mass: 0.0,
        }
    }
}

/// (b2PrismaticJoint)
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PrismaticJoint {
    pub impulse: Vec2,
    pub spring_impulse: f32,
    pub motor_impulse: f32,
    pub lower_impulse: f32,
    pub upper_impulse: f32,
    pub hertz: f32,
    pub damping_ratio: f32,
    pub target_translation: f32,
    pub max_motor_force: f32,
    pub motor_speed: f32,
    pub lower_translation: f32,
    pub upper_translation: f32,

    pub index_a: i32,
    pub index_b: i32,
    pub frame_a: Transform,
    pub frame_b: Transform,
    pub delta_center: Vec2,
    pub spring_softness: Softness,

    pub enable_spring: bool,
    pub enable_limit: bool,
    pub enable_motor: bool,
}

impl Default for PrismaticJoint {
    fn default() -> Self {
        PrismaticJoint {
            impulse: VEC2_ZERO,
            spring_impulse: 0.0,
            motor_impulse: 0.0,
            lower_impulse: 0.0,
            upper_impulse: 0.0,
            hertz: 0.0,
            damping_ratio: 0.0,
            target_translation: 0.0,
            max_motor_force: 0.0,
            motor_speed: 0.0,
            lower_translation: 0.0,
            upper_translation: 0.0,
            index_a: NULL_INDEX,
            index_b: NULL_INDEX,
            frame_a: TRANSFORM_IDENTITY,
            frame_b: TRANSFORM_IDENTITY,
            delta_center: VEC2_ZERO,
            spring_softness: Softness::default(),
            enable_spring: false,
            enable_limit: false,
            enable_motor: false,
        }
    }
}

/// (b2RevoluteJoint)
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RevoluteJoint {
    pub linear_impulse: Vec2,
    pub spring_impulse: f32,
    pub motor_impulse: f32,
    pub lower_impulse: f32,
    pub upper_impulse: f32,
    pub hertz: f32,
    pub damping_ratio: f32,
    pub target_angle: f32,
    pub max_motor_torque: f32,
    pub motor_speed: f32,
    pub lower_angle: f32,
    pub upper_angle: f32,

    pub index_a: i32,
    pub index_b: i32,
    pub frame_a: Transform,
    pub frame_b: Transform,
    pub delta_center: Vec2,
    pub axial_mass: f32,
    pub spring_softness: Softness,

    pub enable_spring: bool,
    pub enable_motor: bool,
    pub enable_limit: bool,
}

impl Default for RevoluteJoint {
    fn default() -> Self {
        RevoluteJoint {
            linear_impulse: VEC2_ZERO,
            spring_impulse: 0.0,
            motor_impulse: 0.0,
            lower_impulse: 0.0,
            upper_impulse: 0.0,
            hertz: 0.0,
            damping_ratio: 0.0,
            target_angle: 0.0,
            max_motor_torque: 0.0,
            motor_speed: 0.0,
            lower_angle: 0.0,
            upper_angle: 0.0,
            index_a: NULL_INDEX,
            index_b: NULL_INDEX,
            frame_a: TRANSFORM_IDENTITY,
            frame_b: TRANSFORM_IDENTITY,
            delta_center: VEC2_ZERO,
            axial_mass: 0.0,
            spring_softness: Softness::default(),
            enable_spring: false,
            enable_motor: false,
            enable_limit: false,
        }
    }
}

/// (b2WeldJoint)
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct WeldJoint {
    pub linear_hertz: f32,
    pub linear_damping_ratio: f32,
    pub angular_hertz: f32,
    pub angular_damping_ratio: f32,

    pub linear_spring: Softness,
    pub angular_spring: Softness,
    pub linear_impulse: Vec2,
    pub angular_impulse: f32,

    pub index_a: i32,
    pub index_b: i32,
    pub frame_a: Transform,
    pub frame_b: Transform,
    pub delta_center: Vec2,
    pub axial_mass: f32,
}

impl Default for WeldJoint {
    fn default() -> Self {
        WeldJoint {
            linear_hertz: 0.0,
            linear_damping_ratio: 0.0,
            angular_hertz: 0.0,
            angular_damping_ratio: 0.0,
            linear_spring: Softness::default(),
            angular_spring: Softness::default(),
            linear_impulse: VEC2_ZERO,
            angular_impulse: 0.0,
            index_a: NULL_INDEX,
            index_b: NULL_INDEX,
            frame_a: TRANSFORM_IDENTITY,
            frame_b: TRANSFORM_IDENTITY,
            delta_center: VEC2_ZERO,
            axial_mass: 0.0,
        }
    }
}

/// (b2WheelJoint)
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct WheelJoint {
    pub perp_impulse: f32,
    pub motor_impulse: f32,
    pub spring_impulse: f32,
    pub lower_impulse: f32,
    pub upper_impulse: f32,
    pub max_motor_torque: f32,
    pub motor_speed: f32,
    pub lower_translation: f32,
    pub upper_translation: f32,
    pub hertz: f32,
    pub damping_ratio: f32,

    pub index_a: i32,
    pub index_b: i32,
    pub frame_a: Transform,
    pub frame_b: Transform,
    pub delta_center: Vec2,
    pub perp_mass: f32,
    pub motor_mass: f32,
    pub axial_mass: f32,
    pub spring_softness: Softness,

    pub enable_spring: bool,
    pub enable_motor: bool,
    pub enable_limit: bool,
}

impl Default for WheelJoint {
    fn default() -> Self {
        WheelJoint {
            perp_impulse: 0.0,
            motor_impulse: 0.0,
            spring_impulse: 0.0,
            lower_impulse: 0.0,
            upper_impulse: 0.0,
            max_motor_torque: 0.0,
            motor_speed: 0.0,
            lower_translation: 0.0,
            upper_translation: 0.0,
            hertz: 0.0,
            damping_ratio: 0.0,
            index_a: NULL_INDEX,
            index_b: NULL_INDEX,
            frame_a: TRANSFORM_IDENTITY,
            frame_b: TRANSFORM_IDENTITY,
            delta_center: VEC2_ZERO,
            perp_mass: 0.0,
            motor_mass: 0.0,
            axial_mass: 0.0,
            spring_softness: Softness::default(),
            enable_spring: false,
            enable_motor: false,
            enable_limit: false,
        }
    }
}

/// The per-type joint payload. The C `b2JointSim` stores a `b2JointType type`
/// tag plus a union; the Rust port stores this tagged enum. A filter joint has
/// no simulation data.
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum JointPayload {
    Distance(DistanceJoint),
    Filter,
    Motor(MotorJoint),
    Prismatic(PrismaticJoint),
    Revolute(RevoluteJoint),
    Weld(WeldJoint),
    Wheel(WheelJoint),
}

impl JointPayload {
    /// The joint type tag for this payload.
    pub fn joint_type(&self) -> JointType {
        match self {
            JointPayload::Distance(_) => JointType::Distance,
            JointPayload::Filter => JointType::Filter,
            JointPayload::Motor(_) => JointType::Motor,
            JointPayload::Prismatic(_) => JointType::Prismatic,
            JointPayload::Revolute(_) => JointType::Revolute,
            JointPayload::Weld(_) => JointType::Weld,
            JointPayload::Wheel(_) => JointType::Wheel,
        }
    }
}

/// The base joint simulation data. (b2JointSim)
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct JointSim {
    pub joint_id: i32,

    pub body_id_a: i32,
    pub body_id_b: i32,

    pub local_frame_a: Transform,
    pub local_frame_b: Transform,

    pub inv_mass_a: f32,
    pub inv_mass_b: f32,
    pub inv_i_a: f32,
    pub inv_i_b: f32,

    pub constraint_hertz: f32,
    pub constraint_damping_ratio: f32,

    pub constraint_softness: Softness,

    pub force_threshold: f32,
    pub torque_threshold: f32,

    /// The per-type data (C: type tag + union).
    pub payload: JointPayload,
}

impl JointSim {
    /// The joint type tag. (C: joint->type)
    pub fn joint_type(&self) -> JointType {
        self.payload.joint_type()
    }

    /// (C: &base->distanceJoint — union access checked by the payload tag)
    pub fn distance(&self) -> &DistanceJoint {
        match &self.payload {
            JointPayload::Distance(joint) => joint,
            _ => unreachable!("joint payload is not a distance joint"),
        }
    }

    pub fn distance_mut(&mut self) -> &mut DistanceJoint {
        match &mut self.payload {
            JointPayload::Distance(joint) => joint,
            _ => unreachable!("joint payload is not a distance joint"),
        }
    }

    /// (C: &base->motorJoint)
    pub fn motor(&self) -> &MotorJoint {
        match &self.payload {
            JointPayload::Motor(joint) => joint,
            _ => unreachable!("joint payload is not a motor joint"),
        }
    }

    pub fn motor_mut(&mut self) -> &mut MotorJoint {
        match &mut self.payload {
            JointPayload::Motor(joint) => joint,
            _ => unreachable!("joint payload is not a motor joint"),
        }
    }

    /// (C: &base->prismaticJoint)
    pub fn prismatic(&self) -> &PrismaticJoint {
        match &self.payload {
            JointPayload::Prismatic(joint) => joint,
            _ => unreachable!("joint payload is not a prismatic joint"),
        }
    }

    pub fn prismatic_mut(&mut self) -> &mut PrismaticJoint {
        match &mut self.payload {
            JointPayload::Prismatic(joint) => joint,
            _ => unreachable!("joint payload is not a prismatic joint"),
        }
    }

    /// (C: &base->revoluteJoint)
    pub fn revolute(&self) -> &RevoluteJoint {
        match &self.payload {
            JointPayload::Revolute(joint) => joint,
            _ => unreachable!("joint payload is not a revolute joint"),
        }
    }

    pub fn revolute_mut(&mut self) -> &mut RevoluteJoint {
        match &mut self.payload {
            JointPayload::Revolute(joint) => joint,
            _ => unreachable!("joint payload is not a revolute joint"),
        }
    }

    /// (C: &base->weldJoint)
    pub fn weld(&self) -> &WeldJoint {
        match &self.payload {
            JointPayload::Weld(joint) => joint,
            _ => unreachable!("joint payload is not a weld joint"),
        }
    }

    pub fn weld_mut(&mut self) -> &mut WeldJoint {
        match &mut self.payload {
            JointPayload::Weld(joint) => joint,
            _ => unreachable!("joint payload is not a weld joint"),
        }
    }

    /// (C: &base->wheelJoint)
    pub fn wheel(&self) -> &WheelJoint {
        match &self.payload {
            JointPayload::Wheel(joint) => joint,
            _ => unreachable!("joint payload is not a wheel joint"),
        }
    }

    pub fn wheel_mut(&mut self) -> &mut WheelJoint {
        match &mut self.payload {
            JointPayload::Wheel(joint) => joint,
            _ => unreachable!("joint payload is not a wheel joint"),
        }
    }
}

mod api;
mod draw;
mod lifecycle;
mod plumbing;
mod solve;

pub use api::*;
pub use draw::*;
pub use lifecycle::*;
pub use plumbing::*;
pub use solve::*;

#[cfg(test)]
mod tests {
    use super::*;
    use crate::body::{create_body, destroy_body, get_body_full_id};
    use crate::broad_phase::update_broad_phase_pairs;
    use crate::constraint_graph::OVERFLOW_INDEX;
    use crate::core::NULL_INDEX;
    use crate::geometry::make_box;
    use crate::shape::create_polygon_shape;
    use crate::solver_set::AWAKE_SET;
    use crate::types::{
        default_body_def, default_distance_joint_def, default_revolute_joint_def,
        default_shape_def, default_world_def, BodyType,
    };
    use crate::world::World;

    // Joint slice test: creation places joints in the constraint graph, links
    // islands, and filters contacts; collide_connected toggling and
    // destruction restore the previous state.
    #[test]
    fn create_and_destroy_joints() {
        let mut world = World::new(&default_world_def());

        let mut body_def = default_body_def();
        body_def.type_ = BodyType::Dynamic;
        let body_a = create_body(&mut world, &body_def);
        let body_b = create_body(&mut world, &body_def);
        let a_index = get_body_full_id(&world, body_a);
        let b_index = get_body_full_id(&world, body_b);

        let box_poly = make_box(0.5, 0.5);
        let shape_def = default_shape_def();
        let _sa = create_polygon_shape(&mut world, body_a, &shape_def, &box_poly);
        let _sb = create_polygon_shape(&mut world, body_b, &shape_def, &box_poly);

        update_broad_phase_pairs(&mut world);
        assert_eq!(world.contact_id_pool.id_count(), 1);

        // The two dynamic bodies start in separate islands.
        assert_ne!(
            world.bodies[a_index as usize].island_id,
            world.bodies[b_index as usize].island_id
        );

        // A revolute joint with collideConnected=false destroys the contact
        // and merges the islands.
        let mut revolute_def = default_revolute_joint_def();
        revolute_def.base.body_id_a = body_a;
        revolute_def.base.body_id_b = body_b;
        let revolute_id = create_revolute_joint(&mut world, &revolute_def);

        assert_eq!(world.contact_id_pool.id_count(), 0);
        assert_eq!(world.joint_id_pool.id_count(), 1);
        assert_eq!(world.bodies[a_index as usize].joint_count, 1);
        assert_eq!(world.bodies[b_index as usize].joint_count, 1);
        assert_eq!(
            world.bodies[a_index as usize].island_id,
            world.bodies[b_index as usize].island_id
        );

        let raw_revolute = get_joint_full_id(&world, revolute_id);
        {
            let joint = &world.joints[raw_revolute as usize];
            assert_eq!(joint.set_index, AWAKE_SET);
            assert!(joint.color_index != NULL_INDEX);
            assert!(joint.island_id != NULL_INDEX);
            assert_eq!(joint.type_, JointType::Revolute);
        }
        assert_eq!(joint_get_type(&world, revolute_id), JointType::Revolute);
        assert!(!joint_get_collide_connected(&world, revolute_id));

        // Broad-phase pair update does not recreate the filtered contact.
        update_broad_phase_pairs(&mut world);
        assert_eq!(world.contact_id_pool.id_count(), 0);

        // A distance joint between a static body and a dynamic body goes in
        // the awake graph with a static-priority color.
        let ground = create_body(&mut world, &default_body_def());
        let mut distance_def = default_distance_joint_def();
        distance_def.base.body_id_a = ground;
        distance_def.base.body_id_b = body_a;
        distance_def.length = 2.0;
        let distance_id = create_distance_joint(&mut world, &distance_def);

        assert_eq!(world.joint_id_pool.id_count(), 2);
        let raw_distance = get_joint_full_id(&world, distance_id);
        {
            let joint = &world.joints[raw_distance as usize];
            assert_eq!(joint.set_index, AWAKE_SET);
            assert!(joint.color_index != NULL_INDEX && joint.color_index <= OVERFLOW_INDEX);
        }
        assert_eq!(
            crate::distance_joint::distance_joint_get_length(&world, distance_id),
            2.0
        );
        assert_eq!(joint_get_body_a(&world, distance_id), ground);
        assert_eq!(joint_get_body_b(&world, distance_id), body_a);

        // Enabling collision on the revolute joint re-buffers the shapes so
        // the broad phase can recreate the contact.
        joint_set_collide_connected(&mut world, revolute_id, true);
        assert!(joint_get_collide_connected(&world, revolute_id));
        update_broad_phase_pairs(&mut world);
        assert_eq!(world.contact_id_pool.id_count(), 1);

        // Destroying the revolute joint unlinks bodies but keeps the contact.
        destroy_joint(&mut world, revolute_id, true);
        assert_eq!(world.joint_id_pool.id_count(), 1);
        assert_eq!(world.bodies[a_index as usize].joint_count, 1); // distance joint remains
        assert_eq!(world.bodies[b_index as usize].joint_count, 0);
        assert_eq!(world.contact_id_pool.id_count(), 1);

        // Destroying body A destroys the distance joint too.
        destroy_body(&mut world, body_a);
        assert_eq!(world.joint_id_pool.id_count(), 0);
        assert_eq!(world.contact_id_pool.id_count(), 0);

        world.validate_solver_sets();
    }
}