box2d_rust/joint/
plumbing.rs

1// Joint storage plumbing from joint.c: id validation, sim lookup, and
2// destruction.
3//
4// SPDX-FileCopyrightText: 2023 Erin Catto
5// SPDX-License-Identifier: MIT
6
7use super::*;
8
9/// Validate a JointId and return the raw joint index. (b2GetJointFullId — C
10/// returns a pointer; Rust returns the index into `world.joints`)
11pub fn get_joint_full_id(world: &crate::world::World, joint_id: crate::id::JointId) -> i32 {
12    let id = joint_id.index1 - 1;
13    debug_assert!((id as usize) < world.joints.len());
14    let joint = &world.joints[id as usize];
15    debug_assert!(joint.joint_id == id && joint.generation == joint_id.generation);
16    id
17}
18
19/// Borrow a joint's sim data mutably: constraint graph color for awake
20/// joints, otherwise the owning solver set. (b2GetJointSim)
21pub fn get_joint_sim(world: &mut crate::world::World, joint_id: i32) -> &mut JointSim {
22    use crate::constants::GRAPH_COLOR_COUNT;
23    use crate::solver_set::AWAKE_SET;
24
25    let (set_index, color_index, local_index) = {
26        let joint = &world.joints[joint_id as usize];
27        (joint.set_index, joint.color_index, joint.local_index)
28    };
29
30    if set_index == AWAKE_SET {
31        debug_assert!((0..GRAPH_COLOR_COUNT).contains(&color_index));
32        &mut world.constraint_graph.colors[color_index as usize].joint_sims[local_index as usize]
33    } else {
34        &mut world.solver_sets[set_index as usize].joint_sims[local_index as usize]
35    }
36}
37
38/// Shared-reference variant of get_joint_sim for read-only accessors. (The C
39/// b2GetJointSim is used for both; Rust needs the split.)
40pub fn get_joint_sim_ref(world: &crate::world::World, joint_id: i32) -> &JointSim {
41    use crate::constants::GRAPH_COLOR_COUNT;
42    use crate::solver_set::AWAKE_SET;
43
44    let joint = &world.joints[joint_id as usize];
45
46    if joint.set_index == AWAKE_SET {
47        debug_assert!(0 <= joint.color_index && joint.color_index < GRAPH_COLOR_COUNT);
48        &world.constraint_graph.colors[joint.color_index as usize].joint_sims
49            [joint.local_index as usize]
50    } else {
51        &world.solver_sets[joint.set_index as usize].joint_sims[joint.local_index as usize]
52    }
53}
54
55/// Shared-reference variant of get_joint_sim_check_type.
56pub fn get_joint_sim_check_type_ref(
57    world: &crate::world::World,
58    joint_id: crate::id::JointId,
59    joint_type: JointType,
60) -> &JointSim {
61    let id = get_joint_full_id(world, joint_id);
62    debug_assert!(world.joints[id as usize].type_ == joint_type);
63    let joint_sim = get_joint_sim_ref(world, id);
64    debug_assert!(joint_sim.joint_type() == joint_type);
65    joint_sim
66}
67
68/// (b2GetJointSimCheckType)
69pub fn get_joint_sim_check_type(
70    world: &mut crate::world::World,
71    joint_id: crate::id::JointId,
72    joint_type: JointType,
73) -> &mut JointSim {
74    let id = get_joint_full_id(world, joint_id);
75    debug_assert!(world.joints[id as usize].type_ == joint_type);
76    let joint_sim = get_joint_sim(world, id);
77    debug_assert!(joint_sim.joint_type() == joint_type);
78    joint_sim
79}
80
81impl Default for JointSim {
82    fn default() -> Self {
83        JointSim {
84            joint_id: NULL_INDEX,
85            body_id_a: NULL_INDEX,
86            body_id_b: NULL_INDEX,
87            local_frame_a: TRANSFORM_IDENTITY,
88            local_frame_b: TRANSFORM_IDENTITY,
89            inv_mass_a: 0.0,
90            inv_mass_b: 0.0,
91            inv_i_a: 0.0,
92            inv_i_b: 0.0,
93            constraint_hertz: 0.0,
94            constraint_damping_ratio: 0.0,
95            constraint_softness: Softness::default(),
96            force_threshold: 0.0,
97            torque_threshold: 0.0,
98            payload: JointPayload::Distance(DistanceJoint::default()),
99        }
100    }
101}
102
103/// Destroy a joint: unlink it from both bodies' joint lists, the island
104/// graph, and the solver set or constraint graph that owns its sim, then free
105/// the id. (b2DestroyJointInternal — C takes the joint pointer; the Rust port
106/// takes the id.)
107pub fn destroy_joint_internal(world: &mut crate::world::World, joint_id: i32, wake_bodies: bool) {
108    use crate::solver_set::{AWAKE_SET, DISABLED_SET};
109
110    let (edge_a, edge_b) = {
111        let joint = &world.joints[joint_id as usize];
112        (joint.edges[0], joint.edges[1])
113    };
114
115    let id_a = edge_a.body_id;
116    let id_b = edge_b.body_id;
117
118    // Remove from body A
119    if edge_a.prev_key != NULL_INDEX {
120        let prev_joint = &mut world.joints[(edge_a.prev_key >> 1) as usize];
121        prev_joint.edges[(edge_a.prev_key & 1) as usize].next_key = edge_a.next_key;
122    }
123
124    if edge_a.next_key != NULL_INDEX {
125        let next_joint = &mut world.joints[(edge_a.next_key >> 1) as usize];
126        next_joint.edges[(edge_a.next_key & 1) as usize].prev_key = edge_a.prev_key;
127    }
128
129    let edge_key_a = joint_id << 1;
130    {
131        let body_a = &mut world.bodies[id_a as usize];
132        if body_a.head_joint_key == edge_key_a {
133            body_a.head_joint_key = edge_a.next_key;
134        }
135        body_a.joint_count -= 1;
136    }
137
138    // Remove from body B
139    if edge_b.prev_key != NULL_INDEX {
140        let prev_joint = &mut world.joints[(edge_b.prev_key >> 1) as usize];
141        prev_joint.edges[(edge_b.prev_key & 1) as usize].next_key = edge_b.next_key;
142    }
143
144    if edge_b.next_key != NULL_INDEX {
145        let next_joint = &mut world.joints[(edge_b.next_key >> 1) as usize];
146        next_joint.edges[(edge_b.next_key & 1) as usize].prev_key = edge_b.prev_key;
147    }
148
149    let edge_key_b = (joint_id << 1) | 1;
150    {
151        let body_b = &mut world.bodies[id_b as usize];
152        if body_b.head_joint_key == edge_key_b {
153            body_b.head_joint_key = edge_b.next_key;
154        }
155        body_b.joint_count -= 1;
156    }
157
158    if world.joints[joint_id as usize].island_id != NULL_INDEX {
159        debug_assert!(world.joints[joint_id as usize].set_index > DISABLED_SET);
160        crate::island::unlink_joint(world, joint_id);
161    } else {
162        debug_assert!(world.joints[joint_id as usize].set_index <= DISABLED_SET);
163    }
164
165    // Remove joint from solver set that owns it
166    let (set_index, local_index, color_index) = {
167        let joint = &world.joints[joint_id as usize];
168        (joint.set_index, joint.local_index, joint.color_index)
169    };
170
171    if set_index == AWAKE_SET {
172        crate::constraint_graph::remove_joint_from_graph(
173            world,
174            id_a,
175            id_b,
176            color_index,
177            local_index,
178        );
179    } else {
180        let set = &mut world.solver_sets[set_index as usize];
181        let moved_index = set.joint_sims.len() as i32 - 1;
182        set.joint_sims.swap_remove(local_index as usize);
183        if moved_index != local_index {
184            // Fix moved joint
185            let moved_id =
186                world.solver_sets[set_index as usize].joint_sims[local_index as usize].joint_id;
187            let moved_joint = &mut world.joints[moved_id as usize];
188            debug_assert!(moved_joint.local_index == moved_index);
189            moved_joint.local_index = local_index;
190        }
191    }
192
193    // Free joint and id (preserve joint generation)
194    {
195        let joint = &mut world.joints[joint_id as usize];
196        joint.set_index = NULL_INDEX;
197        joint.local_index = NULL_INDEX;
198        joint.color_index = NULL_INDEX;
199        joint.joint_id = NULL_INDEX;
200    }
201    world.joint_id_pool.free_id(joint_id);
202
203    if wake_bodies {
204        crate::body::wake_body(world, id_a);
205        crate::body::wake_body(world, id_b);
206    }
207
208    world.validate_solver_sets();
209}
box2d_rust/joint/plumbing.rs

box2d_rust/joint/
plumbing.rs
