box2d_rust/body/
plumbing.rs

1// Body storage plumbing from body.c: accessors, island glue, sim removal.
2//
3// Borrow strategy: C returns interior pointers (b2GetBodySim). Rust callers
4// pass ids; accessors either return copies of small data (transforms) or
5// borrow through &/&mut World for the duration of one access.
6//
7// SPDX-FileCopyrightText: 2023 Erin Catto
8// SPDX-License-Identifier: MIT
9
10use super::{body_flags, Body, BodySim, BodyState};
11use crate::core::NULL_INDEX;
12use crate::id::BodyId;
13use crate::island::{create_island, destroy_island, validate_island};
14use crate::math_functions::WorldTransform;
15use crate::math_functions::{length_squared, mul_sv};
16use crate::solver_set::{AWAKE_SET, DISABLED_SET};
17use crate::world::World;
18
19/// (static b2LimitVelocity)
20pub(crate) fn limit_velocity(state: &mut BodyState, max_linear_speed: f32) {
21    let v2 = length_squared(state.linear_velocity);
22    if v2 > max_linear_speed * max_linear_speed {
23        state.linear_velocity = mul_sv(max_linear_speed / v2.sqrt(), state.linear_velocity);
24    }
25}
26
27/// Remove a body sim from a set with swap-removal, fixing the moved body's
28/// local index. (b2RemoveBodySim)
29pub fn remove_body_sim(body_sims: &mut Vec<BodySim>, bodies: &mut [Body], local_index: i32) {
30    debug_assert!(0 <= local_index && (local_index as usize) < body_sims.len());
31    let last_index = body_sims.len() - 1;
32    body_sims.swap_remove(local_index as usize);
33    if (local_index as usize) < body_sims.len() {
34        let moved_body = &mut bodies[body_sims[local_index as usize].body_id as usize];
35        debug_assert!(moved_body.local_index == last_index as i32);
36        moved_body.local_index = local_index;
37    }
38}
39
40/// Get a validated body index from an id. (b2GetBodyFullId — C returns a
41/// pointer; Rust returns the raw index into `world.bodies`)
42pub fn get_body_full_id(world: &World, body_id: BodyId) -> i32 {
43    debug_assert!(body_id.index1 >= 1);
44    let index = body_id.index1 - 1;
45    debug_assert!((index as usize) < world.bodies.len());
46    debug_assert!(world.bodies[index as usize].generation == body_id.generation);
47    // id index starts at one so that zero can represent null
48    index
49}
50
51/// (b2GetBodyTransformQuick)
52pub fn get_body_transform_quick(world: &World, body: &Body) -> WorldTransform {
53    let set = &world.solver_sets[body.set_index as usize];
54    set.body_sims[body.local_index as usize].transform
55}
56
57/// (b2GetBodyTransform)
58pub fn get_body_transform(world: &World, body_id: i32) -> WorldTransform {
59    let body = &world.bodies[body_id as usize];
60    get_body_transform_quick(world, body)
61}
62
63/// Create a BodyId from a raw id. (b2MakeBodyId)
64pub fn make_body_id(world: &World, body_id: i32) -> BodyId {
65    let body = &world.bodies[body_id as usize];
66    BodyId {
67        index1: body_id + 1,
68        world0: world.world_id,
69        generation: body.generation,
70    }
71}
72
73/// Location of a body's sim data: (set_index, local_index). Use to borrow the
74/// BodySim through `world.solver_sets`. (b2GetBodySim resolves to a pointer in
75/// C; the index pair is the borrow-safe equivalent.)
76pub fn body_sim_location(world: &World, body_id: i32) -> (i32, i32) {
77    let body = &world.bodies[body_id as usize];
78    (body.set_index, body.local_index)
79}
80
81/// Borrow a body's sim data mutably. (b2GetBodySim)
82pub fn get_body_sim<'a>(world: &'a mut World, body: &Body) -> &'a mut BodySim {
83    let set = &mut world.solver_sets[body.set_index as usize];
84    &mut set.body_sims[body.local_index as usize]
85}
86
87/// Borrow a body's state if it is in the awake set. (b2GetBodyState)
88pub fn get_body_state<'a>(world: &'a mut World, body: &Body) -> Option<&'a mut BodyState> {
89    if body.set_index == AWAKE_SET {
90        let set = &mut world.solver_sets[AWAKE_SET as usize];
91        return Some(&mut set.body_states[body.local_index as usize]);
92    }
93
94    None
95}
96
97/// (b2SyncBodyFlags)
98pub fn sync_body_flags(world: &mut World, body_id: i32) {
99    let body = &world.bodies[body_id as usize];
100    // Never sync transient flags
101    let flags = body.flags & !body_flags::BODY_TRANSIENT_FLAGS;
102    let (set_index, local_index) = (body.set_index, body.local_index);
103
104    let set = &mut world.solver_sets[set_index as usize];
105    set.body_sims[local_index as usize].flags = flags;
106
107    if set_index == AWAKE_SET {
108        set.body_states[local_index as usize].flags = flags;
109    }
110}
111
112/// Wake a body's solver set if it is sleeping. Returns true if the set was
113/// woken. (b2WakeBody)
114pub fn wake_body(world: &mut World, body_id: i32) -> bool {
115    let set_index = world.bodies[body_id as usize].set_index;
116    if set_index >= crate::solver_set::FIRST_SLEEPING_SET {
117        crate::solver_set::wake_solver_set(world, set_index);
118        world.validate_solver_sets();
119        return true;
120    }
121
122    false
123}
124
125/// (b2ShouldBodiesCollide)
126pub fn should_bodies_collide(world: &World, body_id_a: i32, body_id_b: i32) -> bool {
127    let body_a = &world.bodies[body_id_a as usize];
128    let body_b = &world.bodies[body_id_b as usize];
129
130    if body_a.type_ != crate::types::BodyType::Dynamic
131        && body_b.type_ != crate::types::BodyType::Dynamic
132    {
133        return false;
134    }
135
136    // Walk the smaller joint list
137    let (mut joint_key, other_body_id) = if body_a.joint_count < body_b.joint_count {
138        (body_a.head_joint_key, body_b.id)
139    } else {
140        (body_b.head_joint_key, body_a.id)
141    };
142
143    while joint_key != NULL_INDEX {
144        let joint_id = joint_key >> 1;
145        let edge_index = joint_key & 1;
146        let other_edge_index = edge_index ^ 1;
147
148        let joint = &world.joints[joint_id as usize];
149        if !joint.collide_connected
150            && joint.edges[other_edge_index as usize].body_id == other_body_id
151        {
152            return false;
153        }
154
155        joint_key = joint.edges[edge_index as usize].next_key;
156    }
157
158    true
159}
160
161/// (static b2CreateIslandForBody)
162pub(crate) fn create_island_for_body(world: &mut World, set_index: i32, body_id: i32) {
163    debug_assert!(world.bodies[body_id as usize].island_id == NULL_INDEX);
164    debug_assert!(set_index != DISABLED_SET);
165
166    let island_id = create_island(world, set_index);
167    world.islands[island_id as usize].bodies.push(body_id);
168    let body = &mut world.bodies[body_id as usize];
169    body.island_id = island_id;
170    body.island_index = 0;
171
172    validate_island(world, island_id);
173}
174
175/// (static b2RemoveBodyFromIsland)
176pub(crate) fn remove_body_from_island(world: &mut World, body_id: i32) {
177    let (island_id, island_index) = {
178        let body = &world.bodies[body_id as usize];
179        (body.island_id, body.island_index)
180    };
181    if island_id == NULL_INDEX {
182        debug_assert!(island_index == NULL_INDEX);
183        return;
184    }
185
186    {
187        let local_index = island_index;
188        let last = world.islands[island_id as usize].bodies.len() - 1;
189        let moved_body_id = world.islands[island_id as usize].bodies[last];
190        world.islands[island_id as usize].bodies[local_index as usize] = moved_body_id;
191        debug_assert!(world.bodies[moved_body_id as usize].island_index == last as i32);
192        world.bodies[moved_body_id as usize].island_index = local_index;
193        world.islands[island_id as usize].bodies.pop();
194    }
195
196    if world.islands[island_id as usize].bodies.is_empty() {
197        // Destroy empty island
198        debug_assert!(world.islands[island_id as usize].contacts.is_empty());
199        debug_assert!(world.islands[island_id as usize].joints.is_empty());
200
201        // Free the island
202        destroy_island(world, island_id);
203    } else {
204        validate_island(world, island_id);
205    }
206
207    let body = &mut world.bodies[body_id as usize];
208    body.island_id = NULL_INDEX;
209    body.island_index = NULL_INDEX;
210}
box2d_rust/body/plumbing.rs

box2d_rust/body/
plumbing.rs
