box3d_rust/joint/
plumbing.rs

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

box3d_rust/joint/
plumbing.rs
